moved implementations of property setters to header

This commit is contained in:
Robert Haschke 2018-04-15 20:26:36 +02:00
parent 17e487be06
commit 9e19f2187b
2 changed files with 22 additions and 35 deletions

View File

@ -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));

View File

@ -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<std::string>(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;
}