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> 63 #include <std_msgs/String.h> 64 #include <geometry_msgs/TransformStamped.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();
176 "Ready to take commands for planning group " << opt.
group_name_ <<
".");
179 template <
typename T>
182 ROS_DEBUG_NAMED(
"move_group_interface",
"Waiting for move_group action server (%s)...", name.c_str());
198 ROS_WARN_ONCE_NAMED(
"move_group_interface",
"Non-default CallbackQueue: Waiting for external queue " 216 ROS_WARN_ONCE_NAMED(
"move_group_interface",
"Non-default CallbackQueue: Waiting for external queue " 222 if (!action->isServerConnected())
224 std::stringstream error;
225 error <<
"Unable to connect to move_group action server '" << name <<
"' within allotted time (" << allotted_time
227 throw std::runtime_error(error.str());
231 ROS_DEBUG_NAMED(
"move_group_interface",
"Connected to '%s'", name.c_str());
241 const std::shared_ptr<tf2_ros::Buffer>&
getTF()
const 268 moveit_msgs::QueryPlannerInterfaces::Request req;
269 moveit_msgs::QueryPlannerInterfaces::Response
res;
271 if (!res.planner_interfaces.empty())
273 desc = res.planner_interfaces.front();
281 moveit_msgs::GetPlannerParams::Request req;
282 moveit_msgs::GetPlannerParams::Response
res;
283 req.planner_config = planner_id;
285 std::map<std::string, std::string> result;
288 for (
unsigned int i = 0, end = res.params.keys.size(); i < end; ++i)
289 result[res.params.keys[i]] = res.params.values[i];
295 const std::map<std::string, std::string>& params,
bool replace =
false)
297 moveit_msgs::SetPlannerParams::Request req;
298 moveit_msgs::SetPlannerParams::Response
res;
299 req.planner_config = planner_id;
301 req.replace = replace;
302 for (std::map<std::string, std::string>::const_iterator it = params.begin(), end = params.end(); it != end; ++it)
304 req.params.keys.push_back(it->first);
305 req.params.values.push_back(it->second);
312 std::stringstream param_name;
313 param_name <<
"move_group";
315 param_name <<
"/" << group;
316 param_name <<
"/default_planner_config";
318 std::string default_planner_config;
320 return default_planner_config;
369 robot_state::RobotStatePtr
s;
376 const std::string& frame,
bool approx)
378 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
405 if (c->knowsFrameTransform(frame))
416 ROS_ERROR_NAMED(
"move_group_interface",
"Unable to transform from frame '%s' to frame '%s'", frame.c_str(),
450 const std::vector<std::string>& possible_eefs =
452 for (std::size_t i = 0; i < possible_eefs.size(); ++i)
454 return possible_eefs[i];
456 static std::string empty;
460 bool setPoseTargets(
const std::vector<geometry_msgs::PoseStamped>& poses,
const std::string& end_effector_link)
462 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
465 ROS_ERROR_NAMED(
"move_group_interface",
"No end-effector to set the pose for");
472 std::vector<geometry_msgs::PoseStamped>& stored_poses =
pose_targets_[eef];
473 for (std::size_t i = 0; i < stored_poses.size(); ++i)
481 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
485 const geometry_msgs::PoseStamped&
getPoseTarget(
const std::string& end_effector_link)
const 487 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
490 std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt =
pose_targets_.find(eef);
492 if (!jt->second.empty())
493 return jt->second.at(0);
496 static const geometry_msgs::PoseStamped UNKNOWN;
497 ROS_ERROR_NAMED(
"move_group_interface",
"Pose for end-effector '%s' not known.", eef.c_str());
501 const std::vector<geometry_msgs::PoseStamped>&
getPoseTargets(
const std::string& end_effector_link)
const 503 const std::string& eef = end_effector_link.empty() ?
end_effector_link_ : end_effector_link;
505 std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt =
pose_targets_.find(eef);
507 if (!jt->second.empty())
511 static const std::vector<geometry_msgs::PoseStamped> EMPTY;
512 ROS_ERROR_NAMED(
"move_group_interface",
"Poses for end-effector '%s' are not known.", eef.c_str());
545 ROS_ERROR_NAMED(
"move_group_interface",
"Unable to monitor current robot state");
557 bool getCurrentState(robot_state::RobotStatePtr& current_state,
double wait_seconds = 1.0)
561 ROS_ERROR_NAMED(
"move_group_interface",
"Unable to get current robot state");
571 ROS_ERROR_NAMED(
"move_group_interface",
"Failed to fetch current robot state");
582 std::vector<moveit_msgs::PlaceLocation> locations;
583 for (std::size_t i = 0; i < poses.size(); ++i)
585 moveit_msgs::PlaceLocation location;
586 location.pre_place_approach.direction.vector.z = -1.0;
587 location.post_place_retreat.direction.vector.x = -1.0;
588 location.pre_place_approach.direction.header.frame_id =
getRobotModel()->getModelFrame();
591 location.pre_place_approach.min_distance = 0.1;
592 location.pre_place_approach.desired_distance = 0.2;
593 location.post_place_retreat.min_distance = 0.0;
594 location.post_place_retreat.desired_distance = 0.2;
597 location.place_pose = poses[i];
598 locations.push_back(location);
600 ROS_DEBUG_NAMED(
"move_group_interface",
"Move group interface has %u place locations",
601 (
unsigned int)locations.size());
619 ROS_DEBUG_NAMED(
"move_group_interface",
"Sent place goal with %d locations", (
int)goal.place_locations.size());
674 std::map<std::string, moveit_msgs::CollisionObject> objects = psi.
getObjects(std::vector<std::string>(1,
object));
679 "Asked for grasps for the object '" <<
object <<
"', but the object could not be found");
680 return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME);
691 << GRASP_PLANNING_SERVICE_NAME
692 <<
"' is not available." 693 " This has to be implemented and started separately.");
697 moveit_msgs::GraspPlanning::Request request;
698 moveit_msgs::GraspPlanning::Response
response;
701 request.target = object;
706 response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
708 ROS_ERROR_NAMED(
"move_group_interface",
"Grasp planning failed. Unable to pick.");
726 moveit_msgs::MoveGroupGoal goal;
728 goal.planning_options.plan_only =
true;
729 goal.planning_options.look_around =
false;
730 goal.planning_options.replan =
false;
731 goal.planning_options.planning_scene_diff.is_diff =
true;
732 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
765 moveit_msgs::MoveGroupGoal goal;
767 goal.planning_options.plan_only =
false;
768 goal.planning_options.look_around =
can_look_;
771 goal.planning_options.planning_scene_diff.is_diff =
true;
772 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
810 moveit_msgs::ExecuteTrajectoryGoal goal;
811 goal.trajectory = trajectory;
836 double computeCartesianPath(
const std::vector<geometry_msgs::Pose>& waypoints,
double step,
double jump_threshold,
837 moveit_msgs::RobotTrajectory& msg,
const moveit_msgs::Constraints& path_constraints,
838 bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code)
840 moveit_msgs::GetCartesianPath::Request req;
841 moveit_msgs::GetCartesianPath::Response
res;
846 req.start_state.is_diff =
true;
851 req.waypoints = waypoints;
853 req.jump_threshold = jump_threshold;
854 req.path_constraints = path_constraints;
855 req.avoid_collisions = avoid_collisions;
860 error_code = res.error_code;
861 if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
871 error_code.val = error_code.FAILURE;
880 std_msgs::String event;
886 bool attachObject(
const std::string&
object,
const std::string& link,
const std::vector<std::string>& touch_links)
897 ROS_ERROR_NAMED(
"move_group_interface",
"No known link to attach object '%s' to",
object.c_str());
900 moveit_msgs::AttachedCollisionObject aco;
901 aco.object.id = object;
902 aco.link_name.swap(l);
903 if (touch_links.empty())
904 aco.touch_links.push_back(aco.link_name);
906 aco.touch_links = touch_links;
907 aco.object.operation = moveit_msgs::CollisionObject::ADD;
914 moveit_msgs::AttachedCollisionObject aco;
917 aco.link_name = name;
919 aco.object.id = name;
920 aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
921 if (aco.link_name.empty() && aco.object.id.empty())
925 for (std::size_t i = 0; i < lnames.size(); ++i)
927 aco.link_name = lnames[i];
1013 request.start_state.is_diff =
true;
1017 request.goal_constraints.resize(1);
1024 std::size_t goal_count = 0;
1025 for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it =
pose_targets_.begin();
1027 goal_count = std::max(goal_count, it->second.size());
1033 request.goal_constraints.resize(goal_count);
1035 for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it =
pose_targets_.begin();
1038 for (std::size_t i = 0; i < it->second.size(); ++i)
1043 c.position_constraints.clear();
1045 c.orientation_constraints.clear();
1051 ROS_ERROR_NAMED(
"move_group_interface",
"Unable to construct MotionPlanRequest representation");
1064 moveit_msgs::PickupGoal
constructPickupGoal(
const std::string&
object, std::vector<moveit_msgs::Grasp>&& grasps,
1065 bool plan_only =
false)
1067 moveit_msgs::PickupGoal goal;
1068 goal.target_name = object;
1072 goal.possible_grasps = std::move(grasps);
1074 goal.allow_gripper_support_collision =
true;
1082 goal.planning_options.plan_only = plan_only;
1083 goal.planning_options.look_around =
can_look_;
1086 goal.planning_options.planning_scene_diff.is_diff =
true;
1087 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
1092 std::vector<moveit_msgs::PlaceLocation>&& locations,
bool plan_only =
false)
1094 moveit_msgs::PlaceGoal goal;
1096 goal.attached_object_name = object;
1098 goal.place_locations = std::move(locations);
1100 goal.allow_gripper_support_collision =
true;
1108 goal.planning_options.plan_only = plan_only;
1109 goal.planning_options.look_around =
can_look_;
1112 goal.planning_options.planning_scene_diff.is_diff =
true;
1113 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
1129 path_constraints_.reset(
new moveit_msgs::Constraints(static_cast<moveit_msgs::Constraints>(*msg_m)));
1162 std::vector<std::string> c;
1174 return moveit_msgs::Constraints();
1182 return moveit_msgs::TrajectoryConstraints();
1194 void setWorkspace(
double minx,
double miny,
double minz,
double maxx,
double maxy,
double maxz)
1213 conn->setParams(host, port);
1214 if (conn->connect())
1219 catch (std::exception& ex)
1283 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1287 throw std::runtime_error(
"ROS does not seem to be running");
1292 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1299 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1307 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
const ros::Duration& wait_for_servers)
1320 other.impl_ =
nullptr;
1329 impl_ = other.impl_;
1331 other.impl_ =
nullptr;
1344 const robot_model::RobotModelConstPtr& robot =
getRobotModel();
1346 const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group);
1350 return joint_group->getDefaultStateNames();
1353 std::vector<std::string> empty;
1368 moveit_msgs::PlannerInterfaceDescription& desc)
1373 std::map<std::string, std::string>
1380 const std::string&
group,
1381 const std::map<std::string, std::string>& params,
1413 double max_acceleration_scaling_factor)
1463 const std::string&
object, std::vector<moveit_msgs::Grasp> grasps,
bool plan_only =
false)
1469 const std::string&
object, std::vector<moveit_msgs::PlaceLocation> locations,
bool plan_only =
false)
1475 const std::vector<geometry_msgs::PoseStamped>& poses)
1506 const std::vector<geometry_msgs::Pose>& waypoints,
double eef_step,
double jump_threshold,
1507 moveit_msgs::RobotTrajectory& trajectory,
bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code)
1509 moveit_msgs::Constraints path_constraints_tmp;
1510 return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions,
1515 const std::vector<geometry_msgs::Pose>& waypoints,
double eef_step,
double jump_threshold,
1516 moveit_msgs::RobotTrajectory& trajectory,
const moveit_msgs::Constraints& path_constraints,
bool avoid_collisions,
1517 moveit_msgs::MoveItErrorCodes* error_code)
1522 avoid_collisions, *error_code);
1526 moveit_msgs::MoveItErrorCodes error_code_tmp;
1528 avoid_collisions, error_code_tmp);
1539 robot_state::RobotStatePtr rs;
1541 robot_state::robotStateMsgToRobotState(start_state, *rs);
1571 std::map<std::string, double>
1575 std::map<std::string, double> positions;
1580 for (
size_t x = 0;
x < names.size(); ++
x)
1582 positions[names[
x]] = it->second[
x];
1606 ROS_ERROR_NAMED(
"move_group_interface",
"The requested named target '%s' does not exist", name.c_str());
1621 const std::map<std::string, double>& joint_values)
1637 std::vector<double>
values(1, value);
1642 const std::vector<double>& values)
1646 if (jm && jm->getVariableCount() == values.size())
1662 const std::string& end_effector_link)
1668 const std::string& end_effector_link)
1674 const std::string& end_effector_link)
1676 geometry_msgs::Pose msg =
tf2::toMsg(eef_pose);
1681 const std::string& end_effector_link)
1687 const geometry_msgs::PoseStamped& eef_pose,
const std::string& end_effector_link)
1693 const std::string& end_effector_link)
1695 geometry_msgs::Pose msg =
tf2::toMsg(eef_pose);
1725 const robot_model::JointModelGroup* jmg =
impl_->
getRobotModel()->getEndEffector(eef_name);
1742 const std::string& end_effector_link)
1744 std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1752 const std::string& end_effector_link)
1754 std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1755 pose_msg[0].pose = target;
1762 const std::string& end_effector_link)
1764 std::vector<geometry_msgs::PoseStamped> targets(1, target);
1769 const std::string& end_effector_link)
1771 std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
1774 for (std::size_t i = 0; i < target.size(); ++i)
1777 pose_out[i].header.stamp = tm;
1778 pose_out[i].header.frame_id = frame_id;
1784 const std::string& end_effector_link)
1786 std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
1789 for (std::size_t i = 0; i < target.size(); ++i)
1791 target_stamped[i].pose = target[i];
1792 target_stamped[i].header.stamp = tm;
1793 target_stamped[i].header.frame_id = frame_id;
1799 const std::vector<geometry_msgs::PoseStamped>& target,
const std::string& end_effector_link)
1803 ROS_ERROR_NAMED(
"move_group_interface",
"No pose specified as goal target");
1813 const geometry_msgs::PoseStamped&
1819 const std::vector<geometry_msgs::PoseStamped>&
1828 geometry_msgs::PoseStamped& target)
1830 if (desired_frame != target.header.frame_id)
1832 geometry_msgs::PoseStamped target_in(target);
1833 tf_buffer.
transform(target_in, target, desired_frame);
1841 const std::string& end_effector_link)
1843 geometry_msgs::PoseStamped target;
1851 target.pose.orientation.x = 0.0;
1852 target.pose.orientation.y = 0.0;
1853 target.pose.orientation.z = 0.0;
1854 target.pose.orientation.w = 1.0;
1858 target.pose.position.x = x;
1859 target.pose.position.y = y;
1860 target.pose.position.z = z;
1867 const std::string& end_effector_link)
1869 geometry_msgs::PoseStamped target;
1877 target.pose.position.x = 0.0;
1878 target.pose.position.y = 0.0;
1879 target.pose.position.z = 0.0;
1891 const std::string& end_effector_link)
1893 geometry_msgs::PoseStamped target;
1901 target.pose.position.x = 0.0;
1902 target.pose.position.y = 0.0;
1903 target.pose.position.z = 0.0;
1907 target.pose.orientation.x = x;
1908 target.pose.orientation.y = y;
1909 target.pose.orientation.z = z;
1910 target.pose.orientation.w = w;
1975 robot_state::RobotStatePtr current_state;
1976 std::vector<double>
values;
1978 current_state->copyJointGroupPositions(
getName(), values);
1984 std::vector<double>
r;
1989 geometry_msgs::PoseStamped
1992 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
1993 Eigen::Isometry3d pose;
1999 robot_state::RobotStatePtr current_state;
2003 const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2005 pose = current_state->getGlobalLinkTransform(lm);
2008 geometry_msgs::PoseStamped pose_msg;
2015 geometry_msgs::PoseStamped
2018 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
2019 Eigen::Isometry3d pose;
2025 robot_state::RobotStatePtr current_state;
2028 const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2030 pose = current_state->getGlobalLinkTransform(lm);
2033 geometry_msgs::PoseStamped pose_msg;
2042 std::vector<double> result;
2043 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
2048 robot_state::RobotStatePtr current_state;
2051 const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2055 geometry_msgs::TransformStamped tfs =
tf2::eigenToTransform(current_state->getGlobalLinkTransform(lm));
2056 double pitch, roll, yaw;
2057 tf2::getEulerYPR<geometry_msgs::Quaternion>(tfs.transform.rotation, yaw, pitch, roll);
2084 robot_state::RobotStatePtr current_state;
2086 return current_state;
2090 const std::vector<double>& values)
2141 const moveit_msgs::TrajectoryConstraints& constraint)
2157 double maxy,
double maxz)
2191 return attachObject(
object, link, std::vector<std::string>());
2195 const std::vector<std::string>& touch_links)
const std::vector< geometry_msgs::PoseStamped > & getPoseTargets(const std::string &end_effector_link) const
bool startStateMonitor(double wait=1.0)
When reasoning about the current state of a robot, a CurrentStateMonitor instance is automatically co...
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_
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
void setPoseReferenceFrame(const std::string &pose_reference_frame)
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...
std::shared_ptr< tf2_ros::Buffer > getSharedTF()
moveit_msgs::WorkspaceParameters workspace_parameters_
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)
const std::shared_ptr< tf2_ros::Buffer > & getTF() const
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_
T & transform(const T &in, T &out, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) const
#define ROS_INFO_NAMED(name,...)
ActiveTargetType active_target_
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.
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...
const geometry_msgs::PoseStamped & getPoseTarget(const std::string &end_effector_link="") const
void setRandomTarget()
Set the joint state goal to a random joint configuration.
#define ROS_ERROR_STREAM_NAMED(name, args)
moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
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).
std::string end_effector_link_
const std::string & getEndEffectorLink() const
Get the current end-effector link. This returns the value set by setEndEffectorLink() (or indirectly ...
void allowLooking(bool flag)
MoveItErrorCode place(const std::string &object, bool plan_only=false)
Place an object somewhere safe in the world (a safe location will be detected)
const std::string & getPoseReferenceFrame() const
Get the reference frame set by setPoseReferenceFrame(). By default this is the reference frame of the...
std::vector< double > values
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
void clearPoseTargets()
Forget any poses specified for all end-effectors.
MoveGroupInterfaceImpl(const Options &opt, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer, const ros::WallDuration &wait_for_servers)
static const std::string SET_PLANNER_PARAMS_SERVICE_NAME
double getGoalPositionTolerance() const
Get the tolerance that is used for reaching a position goal. This is be the radius of a sphere where ...
moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
bool setJointValueTarget(const std::vector< double > &group_variable_values)
Set the JointValueTarget and use it for future planning requests.
void setStartStateToCurrentState()
std::map< std::string, std::vector< geometry_msgs::PoseStamped > > pose_targets_
void setPlanningTime(double seconds)
const std::vector< std::string > & getLinkNames()
Get vector of names of links available in move group.
double getGoalOrientationTolerance() const
std::vector< moveit_msgs::PlaceLocation > posesToPlaceLocations(const std::vector< geometry_msgs::PoseStamped > &poses)
Convert a vector of PoseStamped to a vector of PlaceLocation.
void clearTrajectoryConstraints()
std::vector< double > getCurrentJointValues()
Get the current joint values for the joints planned for by this instance (see getJoints()) ...
moveit_msgs::PickupGoal constructPickupGoal(const std::string &object, std::vector< moveit_msgs::Grasp > &&grasps, bool plan_only=false)
double goal_position_tolerance_
moveit_msgs::PlaceGoal constructPlaceGoal(const std::string &object, std::vector< moveit_msgs::PlaceLocation > &&locations, bool plan_only=false)
std::map< std::string, std::vector< double > > remembered_joint_values_
std::vector< std::string > getKnownConstraints() const
Get the names of the known constraints as read from the Mongo database, if a connection was achieved...
actionlib::SimpleActionClient< moveit_msgs::MoveGroupAction > & getMoveGroupClient() const
const std::string & getEndEffectorLink() 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)
moveit_msgs::Constraints getPathConstraints() const
bool call(MReq &req, MRes &res)
const robot_model::RobotModelConstPtr & getRobotModel() const
void setPoseReferenceFrame(const std::string &pose_reference_frame)
Specify which reference frame to assume for poses specified without a reference frame.
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...
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...
const std::vector< std::string > & getJointModelGroupNames() const
Get the available planning group names.
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_
const std::string & getPlannerId() const
void forgetJointValues(const std::string &name)
Forget the joint values remembered 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...
geometry_msgs::TransformStamped t
double getPlanningTime() const
Get the number of seconds set by setPlanningTime()
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.
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)
double goal_joint_tolerance_
MoveItErrorCode execute(const moveit_msgs::RobotTrajectory &trajectory, bool wait)
void setPlanningTime(double seconds)
Specify the maximum amount of time to use when planning.
const Options & getOptions() const
MoveItErrorCode place(const moveit_msgs::PlaceGoal &goal)
bool setPathConstraints(const std::string &constraint)
robot_model::RobotModelConstPtr getRobotModel() const
Get the RobotModel object.
ros::ServiceClient query_service_
void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest &request)
static const std::string CARTESIAN_PATH_SERVICE_NAME
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
bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc)
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,...)
const std::string & getEndEffector() const
const std::string & getName() const
Get the name of the group this instance operates on.
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::PlaceAction > > place_action_client_
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 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_
void publish(const boost::shared_ptr< M > &message) const
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...
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.
const std::string & getPlanningFrame() const
Get the name of the frame in which the robot is planning.
std::map< std::string, std::string > getPlannerParams(const std::string &planner_id, const std::string &group="")
void clearPathConstraints()
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)
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer)
getSharedStateMonitor is a simpler version of getSharedStateMonitor(const robot_model::RobotModelCons...
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 getGoalJointTolerance() const
static const std::string PLACE_ACTION
std::unique_ptr< moveit_warehouse::ConstraintsStorage > constraints_storage_
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...
#define ROS_FATAL_STREAM_NAMED(name, args)
const robot_model::JointModelGroup * joint_model_group_
void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &constraint)
bool getParam(const std::string &key, std::string &s) const
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)
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::ExecuteTrajectoryAction > > execute_action_client_
void setPathConstraints(const moveit_msgs::Constraints &constraint)
bool initializing_constraints_
void setTargetType(ActiveTargetType type)
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...
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.
void fromMsg(const A &, B &b)
moveit_msgs::PickupGoal constructPickupGoal(const std::string &object, std::vector< moveit_msgs::Grasp > grasps, bool plan_only)
Build a PickupGoal for an object named object and store it in goal_out.
void setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0...
actionlib::SimpleActionClient< moveit_msgs::MoveGroupAction > & getMoveGroupClient() const
Get the move_group action client used by the MoveGroupInterface. The client can be used for querying ...
const std::vector< geometry_msgs::PoseStamped > & getPoseTargets(const std::string &end_effector_link="") 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_
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.
MoveItErrorCode planGraspsAndPick(const std::string &object="", bool plan_only=false)
Pick up an object.
CallbackQueueInterface * getCallbackQueue() const
double getPlanningTime() const
MoveItErrorCode plan(Plan &plan)
Compute a motion plan that takes the group declared in the constructor from the current state to the ...
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
const std::string & getPoseReferenceFrame() const
#define ROS_WARN_ONCE_NAMED(name,...)
MoveGroupInterfaceImpl * impl_
bool setPoseTargets(const std::vector< geometry_msgs::PoseStamped > &poses, const std::string &end_effector_link)
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...
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.
bool hasPoseTarget(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.
MoveGroupInterface(const Options &opt, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >(), const ros::WallDuration &wait_for_servers=ros::WallDuration())
Construct a MoveGroupInterface instance call using a specified set of options opt.
void setPlannerId(const std::string &planner_id)
tf2_ros::Buffer * tf_buffer
const std::vector< std::string > & getActiveJoints() const
Get only the active (actuated) joints this instance operates on.
moveit_msgs::RobotState start_state_
The full starting state used for planning.
double allowed_planning_time_
double getGoalOrientationTolerance() const
Get the tolerance that is used for reaching an orientation goal. This is the tolerance for roll...
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_
MoveGroupInterface & operator=(const MoveGroupInterface &)=delete
ros::Publisher trajectory_event_publisher_
void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &constraint)
ros::ServiceClient set_params_service_
void allowReplanning(bool flag)
Specify whether the robot is allowed to replan if it detects changes in the environment.
std::vector< std::string > getKnownConstraints() const
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,...)
ActiveTargetType getTargetType() const
const std::vector< std::string > & getJoints() const
Get all the joints this instance operates on (including fixed joints)
std::vector< moveit_msgs::PlaceLocation > posesToPlaceLocations(const std::vector< geometry_msgs::PoseStamped > &poses)
Convert a vector of PoseStamped to a vector of PlaceLocation.
std::unique_ptr< moveit_msgs::TrajectoryConstraints > trajectory_constraints_
std::unique_ptr< moveit_msgs::Constraints > path_constraints_
double getGoalJointTolerance() const
Get the tolerance that is used for reaching a joint goal. This is distance for each joint in configur...
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
moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
void setGoalOrientationTolerance(double tolerance)
std::string getDefaultPlannerId(const std::string &group) const
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
const std::string & getEndEffector() const
Get the current end-effector name. This returns the value set by setEndEffector() (or indirectly by s...
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)
MoveItErrorCode pick(const moveit_msgs::PickupGoal &goal)
void allowLooking(bool flag)
Specify whether the robot is allowed to look around before moving if it determines it should (default...
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)
std::string getDefaultPlannerId(const std::string &group="") const
Get the default planner for a given group (or global default)
unsigned int num_planning_attempts_
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
moveit_msgs::PlaceGoal constructPlaceGoal(const std::string &object, std::vector< moveit_msgs::PlaceLocation > locations, bool plan_only)
Build a PlaceGoal for an object named object and store it in goal_out.
bool setPoseTarget(const Eigen::Isometry3d &end_effector_pose, const std::string &end_effector_link="")
Set the goal pose of the end-effector end_effector_link.
robot_state::RobotState & getJointStateTarget()
moveit_msgs::Constraints getPathConstraints() const
Get the actual set of constraints in use with this MoveGroupInterface.
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.
const geometry_msgs::PoseStamped & getPoseTarget(const std::string &end_effector_link) const
The representation of a motion plan (as ROS messasges)
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.
const std::string & getPlannerId() const
Get the current planner_id.
void setStartStateToCurrentState()
Set the starting state for planning to be that reported by the robot's joint state publication...
double getGoalPositionTolerance() const
const ros::NodeHandle & getNodeHandle() const
Get the ROS node handle of this instance operates on.
const std::string response
const robot_model::JointModelGroup * getJointModelGroup() const
robot_state::RobotStatePtr getStartState()
MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject &object, bool plan_only=false)
const robot_state::RobotState & getJointValueTarget() const
Get the currently set joint state goal.
void setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
MoveItErrorCode pick(const std::string &object, bool plan_only=false)
Pick up an object.
#define ROS_WARN_STREAM_NAMED(name, args)
ros::ServiceClient cartesian_path_service_
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::PickupAction > > pick_action_client_
double getReplanningDelay() const
bool setPoseTargets(const EigenSTL::vector_Isometry3d &end_effector_pose, const std::string &end_effector_link="")
Set goal poses for end_effector_link.
robot_state::RobotStatePtr considered_start_state_