53 #include <moveit_msgs/PickupAction.h> 54 #include <moveit_msgs/ExecuteTrajectoryAction.h> 55 #include <moveit_msgs/PlaceAction.h> 56 #include <moveit_msgs/ExecuteKnownTrajectory.h> 57 #include <moveit_msgs/QueryPlannerInterfaces.h> 58 #include <moveit_msgs/GetCartesianPath.h> 59 #include <moveit_msgs/GraspPlanning.h> 60 #include <moveit_msgs/GetPlannerParams.h> 61 #include <moveit_msgs/SetPlannerParams.h> 64 #include <std_msgs/String.h> 101 std::string error =
"Unable to construct robot model. Please make sure all needed information is on the " 104 throw std::runtime_error(error);
109 std::string error =
"Group '" + opt.
group_name_ +
"' was not found.";
111 throw std::runtime_error(error);
145 double allotted_time = wait_for_servers.
toSec();
181 template <
typename T>
184 ROS_DEBUG_NAMED(
"move_group_interface",
"Waiting for move_group action server (%s)...", name.c_str());
200 ROS_WARN_ONCE_NAMED(
"move_group_interface",
"Non-default CallbackQueue: Waiting for external queue " 218 ROS_WARN_ONCE_NAMED(
"move_group_interface",
"Non-default CallbackQueue: Waiting for external queue " 224 if (!action->isServerConnected())
226 std::stringstream error;
227 error <<
"Unable to connect to move_group action server '" << name <<
"' within allotted time (" << allotted_time
229 throw std::runtime_error(error.str());
233 ROS_DEBUG_NAMED(
"move_group_interface",
"Connected to '%s'", name.c_str());
259 ROS_WARN_ONCE_NAMED(
"move_group_interface",
"Non-default CallbackQueue: Waiting for external queue " 278 ROS_WARN_ONCE_NAMED(
"move_group_interface",
"Non-default CallbackQueue: Waiting for external queue " 290 "\nDeprecation warning: Trajectory execution service is deprecated (was replaced by an action)." 291 "\nReplace 'MoveGroupExecuteService' with 'MoveGroupExecuteTrajectoryAction' in " 292 "move_group.launch");
297 "Unable to find execution action on topic: " 300 throw std::runtime_error(
"No Trajectory execution capability available.");
339 moveit_msgs::QueryPlannerInterfaces::Request req;
340 moveit_msgs::QueryPlannerInterfaces::Response
res;
342 if (!res.planner_interfaces.empty())
344 desc = res.planner_interfaces.front();
352 moveit_msgs::GetPlannerParams::Request req;
353 moveit_msgs::GetPlannerParams::Response
res;
354 req.planner_config = planner_id;
356 std::map<std::string, std::string> result;
359 for (
unsigned int i = 0, end = res.params.keys.size(); i < end; ++i)
360 result[res.params.keys[i]] = res.params.values[i];
366 const std::map<std::string, std::string>& params,
bool replace =
false)
368 moveit_msgs::SetPlannerParams::Request req;
369 moveit_msgs::SetPlannerParams::Response
res;
370 req.planner_config = planner_id;
372 req.replace = replace;
373 for (std::map<std::string, std::string>::const_iterator it = params.begin(), end = params.end(); it != end; ++it)
375 req.params.keys.push_back(it->first);
376 req.params.values.push_back(it->second);
383 std::stringstream param_name;
384 param_name <<
"move_group";
386 param_name <<
"/" << group;
387 param_name <<
"/default_planner_config";
389 std::string default_planner_config;
391 return default_planner_config;
440 robot_state::RobotStatePtr
s;
447 const std::string& frame,
bool approx)
449 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
476 if (c->knowsFrameTransform(frame))
487 ROS_ERROR_NAMED(
"move_group_interface",
"Unable to transform from frame '%s' to frame '%s'", frame.c_str(),
521 const std::vector<std::string>& possible_eefs =
523 for (std::size_t i = 0; i < possible_eefs.size(); ++i)
525 return possible_eefs[i];
527 static std::string empty;
531 bool setPoseTargets(
const std::vector<geometry_msgs::PoseStamped>& poses,
const std::string& end_effector_link)
533 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
536 ROS_ERROR_NAMED(
"move_group_interface",
"No end-effector to set the pose for");
543 std::vector<geometry_msgs::PoseStamped>& stored_poses =
pose_targets_[eef];
544 for (std::size_t i = 0; i < stored_poses.size(); ++i)
545 stored_poses[i].header.stamp =
ros::Time(0);
552 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
556 const geometry_msgs::PoseStamped&
getPoseTarget(
const std::string& end_effector_link)
const 558 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
561 std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt =
pose_targets_.find(eef);
563 if (!jt->second.empty())
564 return jt->second.at(0);
567 static const geometry_msgs::PoseStamped unknown;
568 ROS_ERROR_NAMED(
"move_group_interface",
"Pose for end-effector '%s' not known.", eef.c_str());
572 const std::vector<geometry_msgs::PoseStamped>&
getPoseTargets(
const std::string& end_effector_link)
const 574 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
576 std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt =
pose_targets_.find(eef);
578 if (!jt->second.empty())
582 static const std::vector<geometry_msgs::PoseStamped> empty;
583 ROS_ERROR_NAMED(
"move_group_interface",
"Poses for end-effector '%s' are not known.", eef.c_str());
616 ROS_ERROR_NAMED(
"move_group_interface",
"Unable to monitor current robot state");
628 bool getCurrentState(robot_state::RobotStatePtr& current_state,
double wait_seconds = 1.0)
632 ROS_ERROR_NAMED(
"move_group_interface",
"Unable to get current robot state");
642 ROS_ERROR_NAMED(
"move_group_interface",
"Failed to fetch current robot state");
652 bool plan_only =
false)
654 std::vector<moveit_msgs::PlaceLocation> locations;
655 for (std::size_t i = 0; i < poses.size(); ++i)
657 moveit_msgs::PlaceLocation location;
658 location.pre_place_approach.direction.vector.z = -1.0;
659 location.post_place_retreat.direction.vector.x = -1.0;
660 location.pre_place_approach.direction.header.frame_id =
getRobotModel()->getModelFrame();
663 location.pre_place_approach.min_distance = 0.1;
664 location.pre_place_approach.desired_distance = 0.2;
665 location.post_place_retreat.min_distance = 0.0;
666 location.post_place_retreat.desired_distance = 0.2;
669 location.place_pose = poses[i];
670 locations.push_back(location);
672 ROS_DEBUG_NAMED(
"move_group_interface",
"Move group interface has %u place locations",
673 (
unsigned int)locations.size());
674 return place(
object, locations, plan_only);
678 bool plan_only =
false)
690 moveit_msgs::PlaceGoal goal;
692 goal.place_locations = locations;
693 goal.planning_options.plan_only = plan_only;
694 goal.planning_options.look_around =
can_look_;
697 goal.planning_options.planning_scene_diff.is_diff =
true;
698 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
701 ROS_DEBUG_NAMED(
"move_group_interface",
"Sent place goal with %d locations", (
int)goal.place_locations.size());
718 MoveItErrorCode pick(
const std::string&
object,
const std::vector<moveit_msgs::Grasp>& grasps,
bool plan_only =
false)
730 moveit_msgs::PickupGoal goal;
732 goal.possible_grasps = grasps;
733 goal.planning_options.plan_only = plan_only;
734 goal.planning_options.look_around =
can_look_;
737 goal.planning_options.planning_scene_diff.is_diff =
true;
738 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
765 std::map<std::string, moveit_msgs::CollisionObject> objects = psi.
getObjects(std::vector<std::string>(1,
object));
767 if (objects.size() < 1)
770 <<
object <<
"', but the object could not be found");
771 return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME);
782 << GRASP_PLANNING_SERVICE_NAME
783 <<
"' is not available." 784 " This has to be implemented and started separately.");
788 moveit_msgs::GraspPlanning::Request request;
789 moveit_msgs::GraspPlanning::Response response;
792 request.target = object;
797 response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
799 ROS_ERROR_NAMED(
"move_group_interface",
"Grasp planning failed. Unable to pick.");
803 return pick(
object.
id, response.grasps, plan_only);
817 moveit_msgs::MoveGroupGoal goal;
819 goal.planning_options.plan_only =
true;
820 goal.planning_options.look_around =
false;
821 goal.planning_options.replan =
false;
822 goal.planning_options.planning_scene_diff.is_diff =
true;
823 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
856 moveit_msgs::MoveGroupGoal goal;
858 goal.planning_options.plan_only =
false;
859 goal.planning_options.look_around =
can_look_;
862 goal.planning_options.planning_scene_diff.is_diff =
true;
863 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
893 moveit_msgs::ExecuteKnownTrajectory::Request req;
894 moveit_msgs::ExecuteKnownTrajectory::Response
res;
896 req.wait_for_execution = wait;
912 moveit_msgs::ExecuteTrajectoryGoal goal;
938 double computeCartesianPath(
const std::vector<geometry_msgs::Pose>& waypoints,
double step,
double jump_threshold,
939 moveit_msgs::RobotTrajectory& msg,
const moveit_msgs::Constraints& path_constraints,
940 bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code)
942 moveit_msgs::GetCartesianPath::Request req;
943 moveit_msgs::GetCartesianPath::Response
res;
948 req.start_state.is_diff =
true;
953 req.waypoints = waypoints;
955 req.jump_threshold = jump_threshold;
956 req.path_constraints = path_constraints;
957 req.avoid_collisions = avoid_collisions;
962 error_code = res.error_code;
963 if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
973 error_code.val = error_code.FAILURE;
982 std_msgs::String event;
988 bool attachObject(
const std::string&
object,
const std::string& link,
const std::vector<std::string>& touch_links)
999 ROS_ERROR_NAMED(
"move_group_interface",
"No known link to attach object '%s' to",
object.c_str());
1002 moveit_msgs::AttachedCollisionObject aco;
1003 aco.object.id = object;
1004 aco.link_name.swap(l);
1005 if (touch_links.empty())
1006 aco.touch_links.push_back(aco.link_name);
1008 aco.touch_links = touch_links;
1009 aco.object.operation = moveit_msgs::CollisionObject::ADD;
1016 moveit_msgs::AttachedCollisionObject aco;
1019 aco.link_name = name;
1021 aco.object.id = name;
1022 aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
1023 if (aco.link_name.empty() && aco.object.id.empty())
1027 for (std::size_t i = 0; i < lnames.size(); ++i)
1029 aco.link_name = lnames[i];
1115 request.start_state.is_diff =
true;
1119 request.goal_constraints.resize(1);
1126 std::size_t goal_count = 0;
1127 for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it =
pose_targets_.begin();
1129 goal_count = std::max(goal_count, it->second.size());
1135 request.goal_constraints.resize(goal_count);
1137 for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it =
pose_targets_.begin();
1140 for (std::size_t i = 0; i < it->second.size(); ++i)
1145 c.position_constraints.clear();
1147 c.orientation_constraints.clear();
1153 ROS_ERROR_NAMED(
"move_group_interface",
"Unable to construct MotionPlanRequest representation");
1166 void constructGoal(moveit_msgs::PickupGoal& goal_out,
const std::string&
object)
1168 moveit_msgs::PickupGoal goal;
1169 goal.target_name = object;
1176 goal.allow_gripper_support_collision =
true;
1186 moveit_msgs::PlaceGoal goal;
1187 goal.attached_object_name = object;
1193 goal.allow_gripper_support_collision =
true;
1213 path_constraints_.reset(
new moveit_msgs::Constraints(static_cast<moveit_msgs::Constraints>(*msg_m)));
1246 std::vector<std::string> c;
1258 return moveit_msgs::Constraints();
1266 return moveit_msgs::TrajectoryConstraints();
1278 void setWorkspace(
double minx,
double miny,
double minz,
double maxx,
double maxy,
double maxz)
1297 conn->setParams(host, port);
1298 if (conn->connect())
1303 catch (std::exception& ex)
1372 throw std::runtime_error(
"ROS does not seem to be running");
1405 other.impl_ =
nullptr;
1414 impl_ = std::move(other.impl_);
1416 other.impl_ =
nullptr;
1429 const robot_model::RobotModelConstPtr& robot =
getRobotModel();
1431 const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group);
1435 return joint_group->getDefaultStateNames();
1438 std::vector<std::string> empty;
1453 moveit_msgs::PlannerInterfaceDescription& desc)
1459 const std::string& planner_id,
const std::string&
group)
1465 const std::string&
group,
1466 const std::map<std::string, std::string>& params,
1498 double max_acceleration_scaling_factor)
1538 return impl_->
pick(
object, std::vector<moveit_msgs::Grasp>(), plan_only);
1542 const std::string&
object,
const moveit_msgs::Grasp& grasp,
bool plan_only)
1544 return impl_->
pick(
object, std::vector<moveit_msgs::Grasp>(1, grasp), plan_only);
1548 const std::string&
object,
const std::vector<moveit_msgs::Grasp>& grasps,
bool plan_only)
1550 return impl_->
pick(
object, grasps, plan_only);
1560 const moveit_msgs::CollisionObject&
object,
bool plan_only)
1568 return impl_->
place(
object, std::vector<moveit_msgs::PlaceLocation>(), plan_only);
1572 const std::string&
object,
const std::vector<moveit_msgs::PlaceLocation>& locations,
bool plan_only)
1574 return impl_->
place(
object, locations, plan_only);
1578 const std::string&
object,
const std::vector<geometry_msgs::PoseStamped>& poses,
bool plan_only)
1580 return impl_->
place(
object, poses, plan_only);
1584 const std::string&
object,
const geometry_msgs::PoseStamped& pose,
bool plan_only)
1586 return impl_->
place(
object, std::vector<geometry_msgs::PoseStamped>(1, pose), plan_only);
1590 const std::vector<geometry_msgs::Pose>& waypoints,
double eef_step,
double jump_threshold,
1591 moveit_msgs::RobotTrajectory& trajectory,
bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code)
1593 moveit_msgs::Constraints path_constraints_tmp;
1594 return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions,
1599 const std::vector<geometry_msgs::Pose>& waypoints,
double eef_step,
double jump_threshold,
1600 moveit_msgs::RobotTrajectory& trajectory,
const moveit_msgs::Constraints& path_constraints,
bool avoid_collisions,
1601 moveit_msgs::MoveItErrorCodes* error_code)
1606 avoid_collisions, *error_code);
1610 moveit_msgs::MoveItErrorCodes error_code_tmp;
1612 avoid_collisions, error_code_tmp);
1623 robot_state::RobotStatePtr rs;
1625 robot_state::robotStateMsgToRobotState(start_state, *rs);
1655 std::map<std::string, double>
1659 std::map<std::string, double> positions;
1664 for (
size_t x = 0;
x < names.size(); ++
x)
1666 positions[names[
x]] = it->second[
x];
1690 ROS_ERROR_NAMED(
"move_group_interface",
"The requested named target '%s' does not exist", name.c_str());
1705 const std::map<std::string, double>& joint_values)
1721 std::vector<double>
values(1, value);
1726 const std::vector<double>& values)
1730 if (jm && jm->getVariableCount() == values.size())
1746 const std::string& end_effector_link)
1752 const std::string& end_effector_link)
1758 const std::string& end_effector_link)
1760 geometry_msgs::Pose msg;
1766 const geometry_msgs::Pose& eef_pose,
const std::string& end_effector_link)
1772 const geometry_msgs::PoseStamped& eef_pose,
const std::string& end_effector_link)
1778 const Eigen::Affine3d& eef_pose,
const std::string& end_effector_link)
1780 geometry_msgs::Pose msg;
1811 const robot_model::JointModelGroup* jmg =
impl_->
getRobotModel()->getEndEffector(eef_name);
1828 const std::string& end_effector_link)
1830 std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1838 const std::string& end_effector_link)
1840 std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1841 pose_msg[0].pose = target;
1848 const std::string& end_effector_link)
1850 std::vector<geometry_msgs::PoseStamped> targets(1, target);
1855 const std::string& end_effector_link)
1857 std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
1860 for (std::size_t i = 0; i < target.size(); ++i)
1863 pose_out[i].header.stamp = tm;
1864 pose_out[i].header.frame_id = frame_id;
1870 const std::string& end_effector_link)
1872 std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
1875 for (std::size_t i = 0; i < target.size(); ++i)
1877 target_stamped[i].pose = target[i];
1878 target_stamped[i].header.stamp = tm;
1879 target_stamped[i].header.frame_id = frame_id;
1885 const std::vector<geometry_msgs::PoseStamped>& target,
const std::string& end_effector_link)
1889 ROS_ERROR_NAMED(
"move_group_interface",
"No pose specified as goal target");
1899 const geometry_msgs::PoseStamped&
1905 const std::vector<geometry_msgs::PoseStamped>&
1913 inline void transformPose(
const tf::Transformer&
tf,
const std::string& desired_frame,
1914 geometry_msgs::PoseStamped& target)
1916 if (desired_frame != target.header.frame_id)
1922 tf.
transformPose(desired_frame, stamped_target, stamped_target_out);
1923 target.header.frame_id = stamped_target_out.
frame_id_;
1931 const std::string& end_effector_link)
1933 geometry_msgs::PoseStamped target;
1941 target.pose.orientation.x = 0.0;
1942 target.pose.orientation.y = 0.0;
1943 target.pose.orientation.z = 0.0;
1944 target.pose.orientation.w = 1.0;
1948 target.pose.position.x = x;
1949 target.pose.position.y = y;
1950 target.pose.position.z = z;
1957 const std::string& end_effector_link)
1959 geometry_msgs::PoseStamped target;
1967 target.pose.position.x = 0.0;
1968 target.pose.position.y = 0.0;
1969 target.pose.position.z = 0.0;
1980 const std::string& end_effector_link)
1982 geometry_msgs::PoseStamped target;
1990 target.pose.position.x = 0.0;
1991 target.pose.position.y = 0.0;
1992 target.pose.position.z = 0.0;
1996 target.pose.orientation.x = x;
1997 target.pose.orientation.y = y;
1998 target.pose.orientation.z = z;
1999 target.pose.orientation.w = w;
2064 robot_state::RobotStatePtr current_state;
2065 std::vector<double>
values;
2067 current_state->copyJointGroupPositions(
getName(), values);
2073 std::vector<double>
r;
2078 geometry_msgs::PoseStamped
2081 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
2082 Eigen::Affine3d pose;
2088 robot_state::RobotStatePtr current_state;
2092 const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2094 pose = current_state->getGlobalLinkTransform(lm);
2097 geometry_msgs::PoseStamped pose_msg;
2104 geometry_msgs::PoseStamped
2107 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
2108 Eigen::Affine3d pose;
2114 robot_state::RobotStatePtr current_state;
2117 const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2119 pose = current_state->getGlobalLinkTransform(lm);
2122 geometry_msgs::PoseStamped pose_msg;
2131 std::vector<double> result;
2132 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
2137 robot_state::RobotStatePtr current_state;
2140 const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2147 ptf.
getRPY(roll, pitch, yaw);
2174 robot_state::RobotStatePtr current_state;
2176 return current_state;
2180 const std::vector<double>& values)
2231 const moveit_msgs::TrajectoryConstraints& constraint)
2247 double maxy,
double maxz)
2276 return attachObject(
object, link, std::vector<std::string>());
2280 const std::vector<std::string>& touch_links)
2291 moveit_msgs::MotionPlanRequest& goal_out)
bool startStateMonitor(double wait=1.0)
When reasoning about the current state of a robot, a CurrentStateMonitor instance is automatically co...
const std::string & getEndEffectorLink() const
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
bool attachObject(const std::string &object, const std::string &link, const std::vector< std::string > &touch_links)
MoveItErrorCode asyncMove()
Plan and execute a trajectory that takes the group of joints declared in the constructor to the speci...
ros::NodeHandle node_handle_
const std::string & getEndEffector() const
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
void setPoseReferenceFrame(const std::string &pose_reference_frame)
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
bool detachObject(const std::string &name="")
Detach an object. name specifies the name of the object attached to this group, or the name of the li...
moveit_msgs::WorkspaceParameters workspace_parameters_
double getGoalOrientationTolerance() const
const std::string & getPlannerId() const
void setGoalJointTolerance(double tolerance)
Set the joint tolerance (for each joint) that is used for reaching the goal when moving to a joint va...
MoveItErrorCode move(bool wait)
moveit_msgs::RobotTrajectory trajectory_
The trajectory of the robot (may not contain joints that are the same as for the start_state_) ...
robot_state::RobotStatePtr joint_state_target_
#define ROS_INFO_NAMED(name,...)
ActiveTargetType active_target_
boost::shared_ptr< tf::Transformer > getSharedTF()
robot_model::RobotModelConstPtr getRobotModel() const
Get the RobotModel object.
static const std::string ROBOT_DESCRIPTION
Default ROS parameter name from where to read the robot's URDF. Set to 'robot_description'.
ros::Publisher attached_object_publisher_
void stop()
Stop any trajectory execution, if one is active.
std::string getDefaultPlannerId(const std::string &group) const
bool setPoseTargets(const EigenSTL::vector_Affine3d &end_effector_pose, const std::string &end_effector_link="")
Set goal poses for end_effector_link.
void publish(const boost::shared_ptr< M > &message) const
bool setEndEffector(const std::string &eef_name)
Specify the name of the end-effector to use. This is equivalent to setting the EndEffectorLink to the...
void setRandomTarget()
Set the joint state goal to a random joint configuration.
#define ROS_ERROR_STREAM_NAMED(name, args)
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
#define ROS_WARN_NAMED(name,...)
static const std::string MOVE_ACTION
bool setPositionTarget(double x, double y, double z, const std::string &end_effector_link="")
Set the goal position of the end-effector end_effector_link to be (x, y, z).
const std::string & getPoseReferenceFrame() const
std::string end_effector_link_
void allowLooking(bool flag)
std::vector< double > values
void clearPoseTargets()
Forget any poses specified for all end-effectors.
static const std::string SET_PLANNER_PARAMS_SERVICE_NAME
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void matrixEigenToTF(const Eigen::Matrix3d &e, tf::Matrix3x3 &t)
bool setJointValueTarget(const std::vector< double > &group_variable_values)
Set the JointValueTarget and use it for future planning requests.
void setStartStateToCurrentState()
MoveItErrorCode pick(const std::string &object, bool plan_only=false)
Pick up an object.
std::map< std::string, std::vector< geometry_msgs::PoseStamped > > pose_targets_
void setPlanningTime(double seconds)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
const std::vector< std::string > & getLinkNames()
Get vector of names of links available in move group.
const robot_model::RobotModelConstPtr & getRobotModel() const
void clearTrajectoryConstraints()
std::vector< double > getCurrentJointValues()
Get the current joint values for the joints planned for by this instance (see getJoints()) ...
double goal_position_tolerance_
std::map< std::string, std::vector< double > > remembered_joint_values_
const geometry_msgs::PoseStamped & getPoseTarget(const std::string &end_effector_link) const
ros::ServiceClient get_params_service_
void clearPathConstraints()
Specify that no path constraints are to be used. This removes any path constraints set in previous ca...
void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest &request)
Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and sto...
~MoveGroupInterfaceImpl()
void setGoalJointTolerance(double tolerance)
bool call(MReq &req, MRes &res)
double getPlanningTime() const
void setPoseReferenceFrame(const std::string &pose_reference_frame)
Specify which reference frame to assume for poses specified without a reference frame.
const std::vector< geometry_msgs::PoseStamped > & getPoseTargets(const std::string &end_effector_link) const
void setPlannerParams(const std::string &planner_id, const std::string &group, const std::map< std::string, std::string > ¶ms, bool bReplace=false)
Set the planner parameters for given group and planner_id.
void clearPoseTarget(const std::string &end_effector_link)
void clearTrajectoryConstraints()
double computeCartesianPath(const std::vector< geometry_msgs::Pose > &waypoints, double eef_step, double jump_threshold, moveit_msgs::RobotTrajectory &trajectory, bool avoid_collisions=true, moveit_msgs::MoveItErrorCodes *error_code=NULL)
Compute a Cartesian path that follows specified waypoints with a step size of at most eef_step meters...
std::map< std::string, moveit_msgs::CollisionObject > getObjects(const std::vector< std::string > &object_ids=std::vector< std::string >())
Get the objects identified by the given object ids list. If no ids are provided, return all the known...
#define ROS_INFO_STREAM_NAMED(name, args)
const std::vector< std::string > getNamedTargets()
Get the names of the named robot states available as targets, both either remembered states or defaul...
double getGoalPositionTolerance() const
bool setPoseTarget(const Eigen::Affine3d &end_effector_pose, const std::string &end_effector_link="")
Set the goal pose of the end-effector end_effector_link.
bool setRPYTarget(double roll, double pitch, double yaw, const std::string &end_effector_link="")
Set the goal orientation of the end-effector end_effector_link to be (roll,pitch,yaw) radians...
void setPlannerId(const std::string &planner_id)
Specify a planner to be used for further planning.
MoveItErrorCode planGraspsAndPick(const std::string &object, bool plan_only=false)
double max_velocity_scaling_factor_
void forgetJointValues(const std::string &name)
Forget the joint values remebered under name.
static const std::string PICKUP_ACTION
std::vector< double > getCurrentRPY(const std::string &end_effector_link="")
Get the roll-pitch-yaw (XYZ) for the end-effector end_effector_link. If end_effector_link is empty (t...
MoveItErrorCode place(const std::string &object, const std::vector< moveit_msgs::PlaceLocation > &locations, bool plan_only=false)
static const std::string EXECUTION_EVENT_TOPIC
void initializeConstraintsStorageThread(const std::string &host, unsigned int port)
Specification of options to use when constructing the MoveGroupInterface class.
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr< tf::Transformer > &tf)
getSharedStateMonitor is a simpler version of getSharedStateMonitor(const robot_model::RobotModelCons...
void setGoalOrientationTolerance(double tolerance)
Set the orientation tolerance that is used for reaching the goal when moving to a pose...
std::string robot_description_
The robot description parameter name (if different from default)
const std::string & getPlanningFrame() const
Get the name of the frame in which the robot is planning.
double goal_joint_tolerance_
void setPlanningTime(double seconds)
Specify the maximum amount of time to use when planning.
bool setPathConstraints(const std::string &constraint)
ros::ServiceClient query_service_
void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest &request)
static const std::string CARTESIAN_PATH_SERVICE_NAME
std::vector< std::string > getKnownConstraints() const
const std::string GRASP_PLANNING_SERVICE_NAME
bool setEndEffectorLink(const std::string &end_effector_link)
Specify the parent link of the end-effector. This end_effector_link will be used in calls to pose tar...
void setNumPlanningAttempts(unsigned int num_planning_attempts)
bool return_approximate_solution
const std::string & getEndEffectorLink() const
Get the current end-effector link. This returns the value set by setEndEffectorLink() (or indirectly ...
bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc)
const std::string & getName() const
Get the name of the group this instance operates on.
void setGoalPositionTolerance(double tolerance)
Set the position tolerance that is used for reaching the goal when moving to a pose.
void setSupportSurfaceName(const std::string &support_surface)
#define ROS_DEBUG_NAMED(name,...)
std::vector< std::string > getKnownConstraints() const
Get the names of the known constraints as read from the Mongo database, if a connection was achieved...
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::PlaceAction > > place_action_client_
void constructGoal(moveit_msgs::MoveGroupGoal &goal)
bool getCurrentState(robot_state::RobotStatePtr ¤t_state, double wait_seconds=1.0)
void setEndEffectorLink(const std::string &end_effector)
std::string support_surface_
bool attachObject(const std::string &object, const std::string &link="")
Given the name of an object in the planning scene, make the object attached to a link of the robot...
const ros::NodeHandle & getNodeHandle() const
Get the ROS node handle of this instance operates on.
bool setPathConstraints(const std::string &constraint)
Specify a set of path constraints to use. The constraints are looked up by name from the Mongo databa...
MoveItErrorCode asyncExecute(const Plan &plan)
Given a plan, execute it without waiting for completion. Return true on success.
std::map< std::string, std::string > getPlannerParams(const std::string &planner_id, const std::string &group="")
void clearPathConstraints()
MoveGroupInterfaceImpl(const Options &opt, const boost::shared_ptr< tf::Transformer > &tf, const ros::WallDuration &wait_for_servers)
warehouse_ros::DatabaseConnection::Ptr loadDatabase()
bool startStateMonitor(double wait)
bool setApproximateJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link="")
Set the joint state goal for a particular joint by computing IK.
void setGoalPositionTolerance(double tolerance)
double getGoalPositionTolerance() const
Get the tolerance that is used for reaching a position goal. This is be the radius of a sphere where ...
bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc)
Get the description of the planning plugin loaded by the action server.
bool detachObject(const std::string &name)
double getReplanningDelay() const
static const std::string PLACE_ACTION
std::unique_ptr< moveit_warehouse::ConstraintsStorage > constraints_storage_
MoveItErrorCode execute(const Plan &plan, bool wait)
moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState &state, const robot_model::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
void rememberJointValues(const std::string &name)
Remember the current joint values (of the robot being monitored) under name. These can be used by set...
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
#define ROS_FATAL_STREAM_NAMED(name, args)
moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
const robot_model::JointModelGroup * joint_model_group_
void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &constraint)
void setStartState(const moveit_msgs::RobotState &start_state)
If a different start state should be considered instead of the current state of the robot...
void initializeConstraintsStorage(const std::string &host, unsigned int port)
double getPlanningTime() const
Get the number of seconds set by setPlanningTime()
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::ExecuteTrajectoryAction > > execute_action_client_
void setPathConstraints(const moveit_msgs::Constraints &constraint)
bool initializing_constraints_
double getGoalOrientationTolerance() const
Get the tolerance that is used for reaching an orientation goal. This is the tolerance for roll...
void setTargetType(ActiveTargetType type)
const geometry_msgs::PoseStamped & getPoseTarget(const std::string &end_effector_link="") const
geometry_msgs::PoseStamped getRandomPose(const std::string &end_effector_link="")
Get a random reachable pose for the end-effector end_effector_link. If end_effector_link is empty (th...
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
bool setJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link, const std::string &frame, bool approx)
robot_state::RobotStatePtr getCurrentState(double wait=1)
Get the current state of the robot within the duration specified by wait.
std::map< std::string, std::string > getPlannerParams(const std::string &planner_id, const std::string &group="")
Get the planner parameters for given group and planner_id.
moveit_msgs::Constraints getPathConstraints() const
Get the actual set of constraints in use with this MoveGroupInterface.
void setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0...
MoveItErrorCode pick(const std::string &object, const std::vector< moveit_msgs::Grasp > &grasps, bool plan_only=false)
const std::string & getNamespace() const
void clearPoseTarget(const std::string &end_effector_link="")
Forget pose(s) specified for end_effector_link.
bool setOrientationTarget(double x, double y, double z, double w, const std::string &end_effector_link="")
Set the goal orientation of the end-effector end_effector_link to be the quaternion (x...
geometry_msgs::PoseStamped getCurrentPose(const std::string &end_effector_link="")
Get the pose for the end-effector end_effector_link. If end_effector_link is empty (the default value...
MoveItErrorCode move()
Plan and execute a trajectory that takes the group of joints declared in the constructor to the speci...
robot_model::RobotModelConstPtr robot_model_
MoveGroupInterface(const Options &opt, const boost::shared_ptr< tf::Transformer > &tf=boost::shared_ptr< tf::Transformer >(), const ros::WallDuration &wait_for_servers=ros::WallDuration())
Construct a MoveGroupInterface instance call using a specified set of options opt.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
MoveItErrorCode execute(const Plan &plan)
Given a plan, execute it while waiting for completion. Return true on success.
MoveItErrorCode place(const std::string &object, bool plan_only=false)
Place an object somewhere safe in the world (a safe location will be detected)
MoveItErrorCode planGraspsAndPick(const std::string &object="", bool plan_only=false)
Pick up an object.
double getGoalJointTolerance() const
MoveItErrorCode plan(Plan &plan)
Compute a motion plan that takes the group declared in the constructor from the current state to the ...
ros::ServiceClient execute_service_
#define ROS_WARN_ONCE_NAMED(name,...)
MoveGroupInterfaceImpl * impl_
moveit_msgs::Constraints getPathConstraints() const
bool setPoseTargets(const std::vector< geometry_msgs::PoseStamped > &poses, const std::string &end_effector_link)
const std::vector< std::string > & getActiveJoints() const
Get only the active (actuated) joints this instance operates on.
const std::vector< std::string > & getJoints() const
Get all the joints this instance operates on (including fixed joints)
static const std::string QUERY_PLANNERS_SERVICE_NAME
void setNumPlanningAttempts(unsigned int num_planning_attempts)
Set the number of times the motion plan is to be computed from scratch before the shortest solution i...
actionlib::SimpleActionClient< moveit_msgs::MoveGroupAction > & getMoveGroupClient() const
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::MoveGroupAction > > move_action_client_
Client class to conveniently use the ROS interfaces provided by the move_group node.
const robot_state::RobotState & getJointValueTarget() const
Get the currently set joint state goal.
const std::vector< geometry_msgs::PoseStamped > & getPoseTargets(const std::string &end_effector_link="") const
static const std::string EXECUTE_ACTION_NAME
void setGoalTolerance(double tolerance)
Set the tolerance that is used for reaching the goal. For joint state goals, this will be distance fo...
void setStartState(const robot_state::RobotState &start_state)
ros::ServiceClient plan_grasps_service_
void setConstraintsDatabase(const std::string &host, unsigned int port)
Specify where the database server that holds known constraints resides.
robot_model::RobotModelConstPtr robot_model_
Optionally, an instance of the RobotModel to use can be also specified.
void setPlannerId(const std::string &planner_id)
moveit_msgs::RobotState start_state_
The full starting state used for planning.
ActiveTargetType getTargetType() const
double allowed_planning_time_
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
std::vector< double > getRandomJointValues()
Get random joint values for the joints planned for by this instance (see getJoints()) ...
planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_
actionlib::SimpleActionClient< moveit_msgs::MoveGroupAction > & getMoveGroupClient() const
Get the move_group action client used by the MoveGroupInterface. The client can be used for querying ...
MoveGroupInterface & operator=(const MoveGroupInterface &)=delete
ros::Publisher trajectory_event_publisher_
void constructGoal(moveit_msgs::PickupGoal &goal_out, const std::string &object)
bool getParam(const std::string &key, std::string &s) const
void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &constraint)
ros::ServiceClient set_params_service_
const std::string & getEndEffector() const
Get the current end-effector name. This returns the value set by setEndEffector() (or indirectly by s...
void constructGoal(moveit_msgs::PlaceGoal &goal_out, const std::string &object)
void allowReplanning(bool flag)
Specify whether the robot is allowed to replan if it detects changes in the environment.
void allowReplanning(bool flag)
void setSupportSurfaceName(const std::string &name)
For pick/place operations, the name of the support surface is used to specify the fact that attached ...
const std::vector< std::string > & getJointNames()
Get vector of names of joints available in move group.
#define ROS_ERROR_NAMED(name,...)
double getGoalJointTolerance() const
Get the tolerance that is used for reaching a joint goal. This is distance for each joint in configur...
std::unique_ptr< moveit_msgs::TrajectoryConstraints > trajectory_constraints_
std::unique_ptr< moveit_msgs::Constraints > path_constraints_
const std::string & getPlannerId() const
Get the current planner_id.
MoveItErrorCode plan(Plan &plan)
robot_model::RobotModelConstPtr getSharedRobotModel(const std::string &robot_description)
void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
std::unique_ptr< boost::thread > constraints_init_thread_
std::string pose_reference_frame_
static const std::string GET_PLANNER_PARAMS_SERVICE_NAME
MoveItErrorCode place(const std::string &object, const std::vector< geometry_msgs::PoseStamped > &poses, bool plan_only=false)
Place an object at one of the specified possible locations.
moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
void setGoalOrientationTolerance(double tolerance)
void setPlannerParams(const std::string &planner_id, const std::string &group, const std::map< std::string, std::string > ¶ms, bool replace=false)
std::map< std::string, double > getNamedTargetValues(const std::string &name)
Get the joint angles for targets specified by name.
void setReplanningDelay(double delay)
void waitForAction(const T &action, const std::string &name, const ros::WallTime &timeout, double allotted_time)
void allowLooking(bool flag)
Specify whether the robot is allowed to look around before moving if it determines it should (default...
CallbackQueueInterface * getCallbackQueue() const
void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0...
void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
void waitForExecuteActionOrService(ros::WallTime timeout)
unsigned int num_planning_attempts_
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
const Options & getOptions() const
std::string getDefaultPlannerId(const std::string &group="") const
Get the default planner for a given group (or global default)
const robot_model::JointModelGroup * getJointModelGroup() const
robot_state::RobotState & getJointStateTarget()
std::string group_name_
The group to construct the class instance for.
double computeCartesianPath(const std::vector< geometry_msgs::Pose > &waypoints, double step, double jump_threshold, moveit_msgs::RobotTrajectory &msg, const moveit_msgs::Constraints &path_constraints, bool avoid_collisions, moveit_msgs::MoveItErrorCodes &error_code)
double planning_time_
The amount of time it took to generate the plan.
The representation of a motion plan (as ROS messasges)
bool hasPoseTarget(const std::string &end_effector_link) const
double max_acceleration_scaling_factor_
bool setNamedTarget(const std::string &name)
Set the current joint values to be ones previously remembered by rememberJointValues() or...
boost::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
double goal_orientation_tolerance_
ros::NodeHandle node_handle_
void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
Specify the workspace bounding box. The box is specified in the planning frame (i.e. relative to the robot root link start position). This is useful when the planning group contains the root joint of the robot – i.e. when planning motion for the robot relative to the world.
unsigned int getVariableCount() const
Get the number of variables used to describe the state of this group. This is larger or equal to the ...
void setStartStateToCurrentState()
Set the starting state for planning to be that reported by the robot's joint state publication...
robot_state::RobotStatePtr getStartState()
MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject &object, bool plan_only=false)
const boost::shared_ptr< tf::Transformer > & getTF() const
const std::string & getPoseReferenceFrame() const
Get the reference frame set by setPoseReferenceFrame(). By default this is the reference frame of the...
boost::shared_ptr< tf::Transformer > tf_
void setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
#define ROS_WARN_STREAM_NAMED(name, args)
ros::ServiceClient cartesian_path_service_
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::PickupAction > > pick_action_client_
moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
static const std::string EXECUTE_SERVICE_NAME
robot_state::RobotStatePtr considered_start_state_