MoveTo: can now take RobotState msg as goal

This commit is contained in:
llach 2018-04-05 16:17:03 +02:00 committed by Robert Haschke
parent 19fa7349d3
commit 97c2312d67
2 changed files with 29 additions and 14 deletions

View File

@ -63,6 +63,8 @@ public:
void setGoal(const geometry_msgs::PointStamped& point); void setGoal(const geometry_msgs::PointStamped& point);
/// move joint model group to given named pose /// move joint model group to given named pose
void setGoal(const std::string& joint_pose); void setGoal(const std::string& joint_pose);
/// move joints specified in msg to their target values
void setGoal(const moveit_msgs::RobotState& robot_state);
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

@ -55,6 +55,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
p.declare<geometry_msgs::PoseStamped>("pose", "Cartesian target pose"); p.declare<geometry_msgs::PoseStamped>("pose", "Cartesian target pose");
p.declare<geometry_msgs::PointStamped>("point", "Cartesian target point"); p.declare<geometry_msgs::PointStamped>("point", "Cartesian target point");
p.declare<std::string>("joint_pose", "named joint pose"); p.declare<std::string>("joint_pose", "named joint pose");
p.declare<moveit_msgs::RobotState>("robot_state", "target robot state");
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(), p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
"constraints to maintain during trajectory"); "constraints to maintain during trajectory");
@ -83,6 +84,11 @@ void MoveTo::setGoal(const std::string &joint_pose)
setProperty("joint_pose", joint_pose); setProperty("joint_pose", joint_pose);
} }
void MoveTo::setGoal(const moveit_msgs::RobotState &robot_state)
{
setProperty("robot_state", 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);
@ -104,7 +110,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
} }
// only allow single target // only allow single target
size_t count_goals = props.countDefined({"joint_pose", "pose", "point"}); size_t count_goals = props.countDefined({"joint_pose", "robot_state", "pose", "point"});
if (count_goals != 1) { if (count_goals != 1) {
if (count_goals == 0) ROS_WARN("MoveTo: no goal defined"); if (count_goals == 0) ROS_WARN("MoveTo: no goal defined");
else ROS_WARN("MoveTo: cannot plan to multiple goals"); else ROS_WARN("MoveTo: cannot plan to multiple goals");
@ -116,7 +122,14 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
robot_trajectory::RobotTrajectoryPtr robot_trajectory; robot_trajectory::RobotTrajectoryPtr robot_trajectory;
bool success = false; bool success = false;
boost::any goal = props.get("joint_pose"); boost::any goal = props.get("robot_state");
if (!goal.empty()) {
const moveit_msgs::RobotState& target_state = boost::any_cast<moveit_msgs::RobotState>(goal);
scene->setCurrentState(target_state);
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
}
goal = props.get("joint_pose");
if (!goal.empty()) { if (!goal.empty()) {
const std::string& named_joint_pose = boost::any_cast<std::string>(goal); const std::string& named_joint_pose = boost::any_cast<std::string>(goal);
if (!scene->getCurrentStateNonConst().setToDefaultValues(jmg, named_joint_pose)) { if (!scene->getCurrentStateNonConst().setToDefaultValues(jmg, named_joint_pose)) {