diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index e482a9d2..67b886ec 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -57,17 +57,32 @@ public: bool computeForward(const InterfaceState& from) override; bool computeBackward(const InterfaceState& to) override; - void setGroup(const std::string& group); - void setLink(const std::string& link); + void setGroup(const std::string& group) { + setProperty("group", group); + } + void setLink(const std::string& link) { + setProperty("link", link); + } /// move link to given pose - void setGoal(const geometry_msgs::PoseStamped& pose); + void setGoal(const geometry_msgs::PoseStamped& pose) { + setProperty("pose", pose); + } + /// move link to given point, keeping current orientation - void setGoal(const geometry_msgs::PointStamped& point); + void setGoal(const geometry_msgs::PointStamped& point) { + setProperty("point", point); + } + /// move joint model group to given named pose - void setGoal(const std::string& named_joint_pose); + void setGoal(const std::string& named_joint_pose) { + setProperty("named_joint_pose", named_joint_pose); + } + /// move joints specified in msg to their target values - void setGoal(const moveit_msgs::RobotState& robot_state); + void setGoal(const moveit_msgs::RobotState& robot_state) { + setProperty("joint_pose", robot_state); + } void setPathConstraints(moveit_msgs::Constraints path_constraints){ setProperty("path_constraints", std::move(path_constraints)); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index acf4d8f5..53e726e5 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -62,34 +62,6 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan "constraints to maintain during trajectory"); } -void MoveTo::setGroup(const std::string& group){ - setProperty("group", group); -} - -void MoveTo::setLink(const std::string& link){ - setProperty("link", link); -} - -void MoveTo::setGoal(const geometry_msgs::PoseStamped &pose) -{ - setProperty("pose", pose); -} - -void MoveTo::setGoal(const geometry_msgs::PointStamped &point) -{ - setProperty("point", point); -} - -void MoveTo::setGoal(const std::string &named_joint_pose) -{ - setProperty("named_joint_pose", named_joint_pose); -} - -void MoveTo::setGoal(const moveit_msgs::RobotState& robot_state) -{ - setProperty("joint_pose", robot_state); -} - void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model) { PropagatingEitherWay::init(robot_model); @@ -115,7 +87,7 @@ bool MoveTo::getJointStateGoal(moveit::core::RobotState& state, if (!goal.empty()) { const std::string& named_joint_pose = boost::any_cast(goal); if (!state.setToDefaultValues(jmg, named_joint_pose)) - throw InitStageException(*this, "Unknown joint pose:" + named_joint_pose); + throw InitStageException(*this, "Unknown joint pose: " + named_joint_pose); return true; }