Overload setGoal to accept map of joint values as argument (#87)

This commit is contained in:
Michael Görner 2019-05-10 14:41:46 +02:00 committed by Robert Haschke
parent fee73bcd78
commit 267c214288
2 changed files with 17 additions and 0 deletions

View File

@ -95,6 +95,9 @@ public:
setProperty("goal", robot_state); setProperty("goal", robot_state);
} }
/// move joints by name to their mapped target value
void setGoal(const std::map<std::string, double>& joints);
void setPathConstraints(moveit_msgs::Constraints path_constraints){ void setPathConstraints(moveit_msgs::Constraints path_constraints){
setProperty("path_constraints", std::move(path_constraints)); setProperty("path_constraints", std::move(path_constraints));
} }

View File

@ -71,6 +71,20 @@ void MoveTo::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link)
setIKFrame(pose_msg); setIKFrame(pose_msg);
} }
void MoveTo::setGoal(const std::map<std::string, double>& joints) {
moveit_msgs::RobotState robot_state;
robot_state.joint_state.name.reserve(joints.size());
robot_state.joint_state.position.reserve(joints.size());
for (auto& joint : joints)
{
robot_state.joint_state.name.push_back(joint.first);
robot_state.joint_state.position.push_back(joint.second);
}
robot_state.is_diff = true;
setProperty("goal", robot_state);
}
void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model) void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model)
{ {
PropagatingEitherWay::init(robot_model); PropagatingEitherWay::init(robot_model);