From 97c2312d67c982abf73ce9dab8355e38a57c4288 Mon Sep 17 00:00:00 2001 From: llach Date: Thu, 5 Apr 2018 16:17:03 +0200 Subject: [PATCH] MoveTo: can now take RobotState msg as goal --- .../moveit/task_constructor/stages/move_to.h | 6 ++- core/src/stages/move_to.cpp | 37 +++++++++++++------ 2 files changed, 29 insertions(+), 14 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index cbd6e701..4a545952 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -61,8 +61,10 @@ public: void setGoal(const geometry_msgs::PoseStamped& pose); /// move link to given point, keeping current orientation void setGoal(const geometry_msgs::PointStamped& point); - /// move joint model group to given named pose - void setGoal(const std::string& joint_pose); + /// move joint model group to given named 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){ setProperty("path_constraints", std::move(path_constraints)); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 7b916f6f..f8ec2be8 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -54,7 +54,8 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan p.declare("pose", "Cartesian target pose"); p.declare("point", "Cartesian target point"); - p.declare("joint_pose", "named joint pose"); + p.declare("joint_pose", "named joint pose"); + p.declare("robot_state", "target robot state"); p.declare("path_constraints", moveit_msgs::Constraints(), "constraints to maintain during trajectory"); @@ -80,7 +81,12 @@ void MoveTo::setGoal(const geometry_msgs::PointStamped &point) 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) @@ -104,7 +110,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP } // 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 == 0) ROS_WARN("MoveTo: no goal defined"); else ROS_WARN("MoveTo: cannot plan to multiple goals"); @@ -116,15 +122,22 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP robot_trajectory::RobotTrajectoryPtr robot_trajectory; bool success = false; - boost::any goal = props.get("joint_pose"); - if (!goal.empty()) { - const std::string& named_joint_pose = boost::any_cast(goal); - if (!scene->getCurrentStateNonConst().setToDefaultValues(jmg, named_joint_pose)) { - ROS_WARN("MoveTo: unknown joint pose '%s' for jmg '%s'", named_joint_pose.c_str(), group.c_str()); - return false; - } - success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); - } else { + boost::any goal = props.get("robot_state"); + if (!goal.empty()) { + const moveit_msgs::RobotState& target_state = boost::any_cast(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()) { + const std::string& named_joint_pose = boost::any_cast(goal); + if (!scene->getCurrentStateNonConst().setToDefaultValues(jmg, named_joint_pose)) { + ROS_WARN("MoveTo: unknown joint pose '%s' for jmg '%s'", named_joint_pose.c_str(), group.c_str()); + return false; + } + success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + } else { // Cartesian targets require the link name std::string link_name = props.get("link"); const moveit::core::LinkModel* link;