move_group_interface.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, SRI International
5  * Copyright (c) 2013, Ioan A. Sucan
6  * Copyright (c) 2012, Willow Garage, Inc.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *********************************************************************/
36 
37 /* Author: Ioan Sucan, Sachin Chitta */
38 
39 #include <stdexcept>
40 #include <sstream>
41 #include <memory>
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>
62 
64 #include <std_msgs/String.h>
65 #include <tf/transform_listener.h>
66 #include <tf/transform_datatypes.h>
68 #include <ros/console.h>
69 #include <ros/ros.h>
70 
71 namespace moveit
72 {
73 namespace planning_interface
74 {
75 const std::string MoveGroupInterface::ROBOT_DESCRIPTION =
76  "robot_description"; // name of the robot description (a param name, so it can be changed externally)
77 
78 const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps
79 
80 namespace
81 {
83 {
84  JOINT,
85  POSE,
86  POSITION,
87  ORIENTATION
88 };
89 }
90 
92 {
93 public:
95  const ros::WallDuration& wait_for_servers)
96  : opt_(opt), node_handle_(opt.node_handle_), tf_(tf)
97  {
99  if (!getRobotModel())
100  {
101  std::string error = "Unable to construct robot model. Please make sure all needed information is on the "
102  "parameter server.";
103  ROS_FATAL_STREAM_NAMED("move_group_interface", error);
104  throw std::runtime_error(error);
105  }
106 
107  if (!getRobotModel()->hasJointModelGroup(opt.group_name_))
108  {
109  std::string error = "Group '" + opt.group_name_ + "' was not found.";
110  ROS_FATAL_STREAM_NAMED("move_group_interface", error);
111  throw std::runtime_error(error);
112  }
113 
114  joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_);
115 
116  joint_state_target_.reset(new robot_state::RobotState(getRobotModel()));
117  joint_state_target_->setToDefaultValues();
118  active_target_ = JOINT;
119  can_look_ = false;
120  can_replan_ = false;
121  replan_delay_ = 2.0;
122  goal_joint_tolerance_ = 1e-4;
123  goal_position_tolerance_ = 1e-4; // 0.1 mm
124  goal_orientation_tolerance_ = 1e-3; // ~0.1 deg
130 
131  if (joint_model_group_->isChain())
132  end_effector_link_ = joint_model_group_->getLinkModelNames().back();
133  pose_reference_frame_ = getRobotModel()->getModelFrame();
134 
137  attached_object_publisher_ = node_handle_.advertise<moveit_msgs::AttachedCollisionObject>(
139 
141 
142  ros::WallTime timeout_for_servers = ros::WallTime::now() + wait_for_servers;
143  if (wait_for_servers == ros::WallDuration())
144  timeout_for_servers = ros::WallTime(); // wait for ever
145  double allotted_time = wait_for_servers.toSec();
146 
147  move_action_client_.reset(
149  waitForAction(move_action_client_, move_group::MOVE_ACTION, timeout_for_servers, allotted_time);
150 
151  pick_action_client_.reset(
153  waitForAction(pick_action_client_, move_group::PICKUP_ACTION, timeout_for_servers, allotted_time);
154 
155  place_action_client_.reset(
157  waitForAction(place_action_client_, move_group::PLACE_ACTION, timeout_for_servers, allotted_time);
158 
161  // TODO: after deprecation period, i.e. for L-turtle, switch back to standard waitForAction function
162  // waitForAction(execute_action_client_, move_group::EXECUTE_ACTION_NAME, timeout_for_servers, allotted_time);
163  waitForExecuteActionOrService(timeout_for_servers);
164 
166  node_handle_.serviceClient<moveit_msgs::QueryPlannerInterfaces>(move_group::QUERY_PLANNERS_SERVICE_NAME);
171 
174 
176 
177  ROS_INFO_STREAM_NAMED("move_group_interface", "Ready to take commands for planning group " << opt.group_name_
178  << ".");
179  }
180 
181  template <typename T>
182  void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time)
183  {
184  ROS_DEBUG_NAMED("move_group_interface", "Waiting for move_group action server (%s)...", name.c_str());
185 
186  // wait for the server (and spin as needed)
187  if (timeout == ros::WallTime()) // wait forever
188  {
189  while (node_handle_.ok() && !action->isServerConnected())
190  {
191  ros::WallDuration(0.001).sleep();
192  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
194  if (queue)
195  {
196  queue->callAvailable();
197  }
198  else // in case of nodelets and specific callback queue implementations
199  {
200  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
201  "handling.");
202  }
203  }
204  }
205  else // wait with timeout
206  {
207  while (node_handle_.ok() && !action->isServerConnected() && timeout > ros::WallTime::now())
208  {
209  ros::WallDuration(0.001).sleep();
210  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
212  if (queue)
213  {
214  queue->callAvailable();
215  }
216  else // in case of nodelets and specific callback queue implementations
217  {
218  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
219  "handling.");
220  }
221  }
222  }
223 
224  if (!action->isServerConnected())
225  {
226  std::stringstream error;
227  error << "Unable to connect to move_group action server '" << name << "' within allotted time (" << allotted_time
228  << "s)";
229  throw std::runtime_error(error.str());
230  }
231  else
232  {
233  ROS_DEBUG_NAMED("move_group_interface", "Connected to '%s'", name.c_str());
234  }
235  }
236 
238  {
239  ROS_DEBUG("Waiting for move_group action server (%s)...", move_group::EXECUTE_ACTION_NAME.c_str());
240 
241  // Deprecated service
243  node_handle_.serviceClient<moveit_msgs::ExecuteKnownTrajectory>(move_group::EXECUTE_SERVICE_NAME);
244 
245  // wait for either of action or service
246  if (timeout == ros::WallTime()) // wait forever
247  {
248  while (!execute_action_client_->isServerConnected() && !execute_service_.exists())
249  {
250  ros::WallDuration(0.001).sleep();
251  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
253  if (queue)
254  {
255  queue->callAvailable();
256  }
257  else // in case of nodelets and specific callback queue implementations
258  {
259  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
260  "handling.");
261  }
262  }
263  }
264  else // wait with timeout
265  {
266  while (!execute_action_client_->isServerConnected() && !execute_service_.exists() &&
267  timeout > ros::WallTime::now())
268  {
269  ros::WallDuration(0.001).sleep();
270  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
272  if (queue)
273  {
274  queue->callAvailable();
275  }
276  else // in case of nodelets and specific callback queue implementations
277  {
278  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
279  "handling.");
280  }
281  }
282  }
283 
284  // issue warning
285  if (!execute_action_client_->isServerConnected())
286  {
287  if (execute_service_.exists())
288  {
289  ROS_WARN_NAMED("move_group_interface",
290  "\nDeprecation warning: Trajectory execution service is deprecated (was replaced by an action)."
291  "\nReplace 'MoveGroupExecuteService' with 'MoveGroupExecuteTrajectoryAction' in "
292  "move_group.launch");
293  }
294  else
295  {
296  ROS_ERROR_STREAM_NAMED("move_group_interface",
297  "Unable to find execution action on topic: "
300  throw std::runtime_error("No Trajectory execution capability available.");
301  }
302  execute_action_client_.reset();
303  }
304  }
305 
307  {
309  constraints_init_thread_->join();
310  }
311 
313  {
314  return tf_;
315  }
316 
317  const Options& getOptions() const
318  {
319  return opt_;
320  }
321 
322  const robot_model::RobotModelConstPtr& getRobotModel() const
323  {
324  return robot_model_;
325  }
326 
327  const robot_model::JointModelGroup* getJointModelGroup() const
328  {
329  return joint_model_group_;
330  }
331 
333  {
334  return *move_action_client_;
335  }
336 
337  bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc)
338  {
339  moveit_msgs::QueryPlannerInterfaces::Request req;
340  moveit_msgs::QueryPlannerInterfaces::Response res;
341  if (query_service_.call(req, res))
342  if (!res.planner_interfaces.empty())
343  {
344  desc = res.planner_interfaces.front();
345  return true;
346  }
347  return false;
348  }
349 
350  std::map<std::string, std::string> getPlannerParams(const std::string& planner_id, const std::string& group = "")
351  {
352  moveit_msgs::GetPlannerParams::Request req;
353  moveit_msgs::GetPlannerParams::Response res;
354  req.planner_config = planner_id;
355  req.group = group;
356  std::map<std::string, std::string> result;
357  if (get_params_service_.call(req, res))
358  {
359  for (unsigned int i = 0, end = res.params.keys.size(); i < end; ++i)
360  result[res.params.keys[i]] = res.params.values[i];
361  }
362  return result;
363  }
364 
365  void setPlannerParams(const std::string& planner_id, const std::string& group,
366  const std::map<std::string, std::string>& params, bool replace = false)
367  {
368  moveit_msgs::SetPlannerParams::Request req;
369  moveit_msgs::SetPlannerParams::Response res;
370  req.planner_config = planner_id;
371  req.group = group;
372  req.replace = replace;
373  for (std::map<std::string, std::string>::const_iterator it = params.begin(), end = params.end(); it != end; ++it)
374  {
375  req.params.keys.push_back(it->first);
376  req.params.values.push_back(it->second);
377  }
378  set_params_service_.call(req, res);
379  }
380 
381  std::string getDefaultPlannerId(const std::string& group) const
382  {
383  std::stringstream param_name;
384  param_name << "move_group";
385  if (!group.empty())
386  param_name << "/" << group;
387  param_name << "/default_planner_config";
388 
389  std::string default_planner_config;
390  node_handle_.getParam(param_name.str(), default_planner_config);
391  return default_planner_config;
392  }
393 
394  void setPlannerId(const std::string& planner_id)
395  {
396  planner_id_ = planner_id;
397  }
398 
399  const std::string& getPlannerId() const
400  {
401  return planner_id_;
402  }
403 
404  void setNumPlanningAttempts(unsigned int num_planning_attempts)
405  {
406  num_planning_attempts_ = num_planning_attempts;
407  }
408 
409  void setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
410  {
411  max_velocity_scaling_factor_ = max_velocity_scaling_factor;
412  }
413 
414  void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
415  {
416  max_acceleration_scaling_factor_ = max_acceleration_scaling_factor;
417  }
418 
419  robot_state::RobotState& getJointStateTarget()
420  {
421  return *joint_state_target_;
422  }
423 
424  void setStartState(const robot_state::RobotState& start_state)
425  {
426  considered_start_state_.reset(new robot_state::RobotState(start_state));
427  }
428 
430  {
431  considered_start_state_.reset();
432  }
433 
434  robot_state::RobotStatePtr getStartState()
435  {
438  else
439  {
440  robot_state::RobotStatePtr s;
441  getCurrentState(s);
442  return s;
443  }
444  }
445 
446  bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link,
447  const std::string& frame, bool approx)
448  {
449  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
450  // this only works if we have an end-effector
451  if (!eef.empty())
452  {
453  // first we set the goal to be the same as the start state
454  moveit::core::RobotStatePtr c = getStartState();
455  if (c)
456  {
457  setTargetType(JOINT);
458  c->enforceBounds();
459  getJointStateTarget() = *c;
460  if (!getJointStateTarget().satisfiesBounds(getGoalJointTolerance()))
461  return false;
462  }
463  else
464  return false;
465 
466  // we may need to do approximate IK
468  o.return_approximate_solution = approx;
469 
470  // if no frame transforms are needed, call IK directly
471  if (frame.empty() || moveit::core::Transforms::sameFrame(frame, getRobotModel()->getModelFrame()))
472  return getJointStateTarget().setFromIK(getJointModelGroup(), eef_pose, eef, 0, 0.0,
474  else
475  {
476  if (c->knowsFrameTransform(frame))
477  {
478  // transform the pose first if possible, then do IK
479  const Eigen::Affine3d& t = getJointStateTarget().getFrameTransform(frame);
480  Eigen::Affine3d p;
481  tf::poseMsgToEigen(eef_pose, p);
482  return getJointStateTarget().setFromIK(getJointModelGroup(), t * p, eef, 0, 0.0,
484  }
485  else
486  {
487  ROS_ERROR_NAMED("move_group_interface", "Unable to transform from frame '%s' to frame '%s'", frame.c_str(),
488  getRobotModel()->getModelFrame().c_str());
489  return false;
490  }
491  }
492  }
493  else
494  return false;
495  }
496 
497  void setEndEffectorLink(const std::string& end_effector)
498  {
499  end_effector_link_ = end_effector;
500  }
501 
502  void clearPoseTarget(const std::string& end_effector_link)
503  {
504  pose_targets_.erase(end_effector_link);
505  }
506 
508  {
509  pose_targets_.clear();
510  }
511 
512  const std::string& getEndEffectorLink() const
513  {
514  return end_effector_link_;
515  }
516 
517  const std::string& getEndEffector() const
518  {
519  if (!end_effector_link_.empty())
520  {
521  const std::vector<std::string>& possible_eefs =
522  getRobotModel()->getJointModelGroup(opt_.group_name_)->getAttachedEndEffectorNames();
523  for (std::size_t i = 0; i < possible_eefs.size(); ++i)
524  if (getRobotModel()->getEndEffector(possible_eefs[i])->hasLinkModel(end_effector_link_))
525  return possible_eefs[i];
526  }
527  static std::string empty;
528  return empty;
529  }
530 
531  bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& poses, const std::string& end_effector_link)
532  {
533  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
534  if (eef.empty())
535  {
536  ROS_ERROR_NAMED("move_group_interface", "No end-effector to set the pose for");
537  return false;
538  }
539  else
540  {
541  pose_targets_[eef] = poses;
542  // make sure we don't store an actual stamp, since that will become stale can potentially cause tf errors
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);
546  }
547  return true;
548  }
549 
550  bool hasPoseTarget(const std::string& end_effector_link) const
551  {
552  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
553  return pose_targets_.find(eef) != pose_targets_.end();
554  }
555 
556  const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link) const
557  {
558  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
559 
560  // if multiple pose targets are set, return the first one
561  std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
562  if (jt != pose_targets_.end())
563  if (!jt->second.empty())
564  return jt->second.at(0);
565 
566  // or return an error
567  static const geometry_msgs::PoseStamped unknown;
568  ROS_ERROR_NAMED("move_group_interface", "Pose for end-effector '%s' not known.", eef.c_str());
569  return unknown;
570  }
571 
572  const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string& end_effector_link) const
573  {
574  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
575 
576  std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
577  if (jt != pose_targets_.end())
578  if (!jt->second.empty())
579  return jt->second;
580 
581  // or return an error
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());
584  return empty;
585  }
586 
587  void setPoseReferenceFrame(const std::string& pose_reference_frame)
588  {
589  pose_reference_frame_ = pose_reference_frame;
590  }
591 
592  void setSupportSurfaceName(const std::string& support_surface)
593  {
594  support_surface_ = support_surface;
595  }
596 
597  const std::string& getPoseReferenceFrame() const
598  {
599  return pose_reference_frame_;
600  }
601 
602  void setTargetType(ActiveTargetType type)
603  {
604  active_target_ = type;
605  }
606 
607  ActiveTargetType getTargetType() const
608  {
609  return active_target_;
610  }
611 
612  bool startStateMonitor(double wait)
613  {
615  {
616  ROS_ERROR_NAMED("move_group_interface", "Unable to monitor current robot state");
617  return false;
618  }
619 
620  // if needed, start the monitor and wait up to 1 second for a full robot state
621  if (!current_state_monitor_->isActive())
622  current_state_monitor_->startStateMonitor();
623 
624  current_state_monitor_->waitForCompleteState(opt_.group_name_, wait);
625  return true;
626  }
627 
628  bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds = 1.0)
629  {
631  {
632  ROS_ERROR_NAMED("move_group_interface", "Unable to get current robot state");
633  return false;
634  }
635 
636  // if needed, start the monitor and wait up to 1 second for a full robot state
637  if (!current_state_monitor_->isActive())
638  current_state_monitor_->startStateMonitor();
639 
640  if (!current_state_monitor_->waitForCurrentState(ros::Time::now(), wait_seconds))
641  {
642  ROS_ERROR_NAMED("move_group_interface", "Failed to fetch current robot state");
643  return false;
644  }
645 
646  current_state = current_state_monitor_->getCurrentState();
647  return true;
648  }
649 
651  MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses,
652  bool plan_only = false)
653  {
654  std::vector<moveit_msgs::PlaceLocation> locations;
655  for (std::size_t i = 0; i < poses.size(); ++i)
656  {
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();
661  location.post_place_retreat.direction.header.frame_id = end_effector_link_;
662 
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;
667  // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody
668 
669  location.place_pose = poses[i];
670  locations.push_back(location);
671  }
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);
675  }
676 
677  MoveItErrorCode place(const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations,
678  bool plan_only = false)
679  {
681  {
682  ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action client not found");
683  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
684  }
685  if (!place_action_client_->isServerConnected())
686  {
687  ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action server not connected");
688  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
689  }
690  moveit_msgs::PlaceGoal goal;
691  constructGoal(goal, object);
692  goal.place_locations = locations;
693  goal.planning_options.plan_only = plan_only;
694  goal.planning_options.look_around = can_look_;
695  goal.planning_options.replan = can_replan_;
696  goal.planning_options.replan_delay = replan_delay_;
697  goal.planning_options.planning_scene_diff.is_diff = true;
698  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
699 
700  place_action_client_->sendGoal(goal);
701  ROS_DEBUG_NAMED("move_group_interface", "Sent place goal with %d locations", (int)goal.place_locations.size());
702  if (!place_action_client_->waitForResult())
703  {
704  ROS_INFO_STREAM_NAMED("move_group_interface", "Place action returned early");
705  }
707  {
708  return MoveItErrorCode(place_action_client_->getResult()->error_code);
709  }
710  else
711  {
712  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": "
713  << place_action_client_->getState().getText());
714  return MoveItErrorCode(place_action_client_->getResult()->error_code);
715  }
716  }
717 
718  MoveItErrorCode pick(const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps, bool plan_only = false)
719  {
720  if (!pick_action_client_)
721  {
722  ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action client not found");
723  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
724  }
725  if (!pick_action_client_->isServerConnected())
726  {
727  ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action server not connected");
728  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
729  }
730  moveit_msgs::PickupGoal goal;
731  constructGoal(goal, object);
732  goal.possible_grasps = grasps;
733  goal.planning_options.plan_only = plan_only;
734  goal.planning_options.look_around = can_look_;
735  goal.planning_options.replan = can_replan_;
736  goal.planning_options.replan_delay = replan_delay_;
737  goal.planning_options.planning_scene_diff.is_diff = true;
738  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
739 
740  pick_action_client_->sendGoal(goal);
741  if (!pick_action_client_->waitForResult())
742  {
743  ROS_INFO_STREAM_NAMED("move_group_interface", "Pickup action returned early");
744  }
746  {
747  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
748  }
749  else
750  {
751  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": "
752  << pick_action_client_->getState().getText());
753  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
754  }
755  }
756 
757  MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false)
758  {
759  if (object.empty())
760  {
761  return planGraspsAndPick(moveit_msgs::CollisionObject());
762  }
764 
765  std::map<std::string, moveit_msgs::CollisionObject> objects = psi.getObjects(std::vector<std::string>(1, object));
766 
767  if (objects.size() < 1)
768  {
769  ROS_ERROR_STREAM_NAMED("move_group_interface", "Asked for grasps for the object '"
770  << object << "', but the object could not be found");
771  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME);
772  }
773 
774  return planGraspsAndPick(objects[object], plan_only);
775  }
776 
777  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false)
778  {
780  {
781  ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '"
782  << GRASP_PLANNING_SERVICE_NAME
783  << "' is not available."
784  " This has to be implemented and started separately.");
785  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
786  }
787 
788  moveit_msgs::GraspPlanning::Request request;
789  moveit_msgs::GraspPlanning::Response response;
790 
791  request.group_name = opt_.group_name_;
792  request.target = object;
793  request.support_surfaces.push_back(support_surface_);
794 
795  ROS_DEBUG_NAMED("move_group_interface", "Calling grasp planner...");
796  if (!plan_grasps_service_.call(request, response) ||
797  response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
798  {
799  ROS_ERROR_NAMED("move_group_interface", "Grasp planning failed. Unable to pick.");
800  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
801  }
802 
803  return pick(object.id, response.grasps, plan_only);
804  }
805 
807  {
808  if (!move_action_client_)
809  {
810  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
811  }
812  if (!move_action_client_->isServerConnected())
813  {
814  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
815  }
816 
817  moveit_msgs::MoveGroupGoal goal;
818  constructGoal(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;
824 
825  move_action_client_->sendGoal(goal);
826  if (!move_action_client_->waitForResult())
827  {
828  ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
829  }
831  {
832  plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
833  plan.start_state_ = move_action_client_->getResult()->trajectory_start;
834  plan.planning_time_ = move_action_client_->getResult()->planning_time;
835  return MoveItErrorCode(move_action_client_->getResult()->error_code);
836  }
837  else
838  {
839  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": "
840  << move_action_client_->getState().getText());
841  return MoveItErrorCode(move_action_client_->getResult()->error_code);
842  }
843  }
844 
846  {
847  if (!move_action_client_)
848  {
849  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
850  }
851  if (!move_action_client_->isServerConnected())
852  {
853  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
854  }
855 
856  moveit_msgs::MoveGroupGoal goal;
857  constructGoal(goal);
858  goal.planning_options.plan_only = false;
859  goal.planning_options.look_around = can_look_;
860  goal.planning_options.replan = can_replan_;
861  goal.planning_options.replan_delay = replan_delay_;
862  goal.planning_options.planning_scene_diff.is_diff = true;
863  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
864 
865  move_action_client_->sendGoal(goal);
866  if (!wait)
867  {
868  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
869  }
870 
871  if (!move_action_client_->waitForResult())
872  {
873  ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
874  }
875 
877  {
878  return MoveItErrorCode(move_action_client_->getResult()->error_code);
879  }
880  else
881  {
882  ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString()
883  << ": " << move_action_client_->getState().getText());
884  return MoveItErrorCode(move_action_client_->getResult()->error_code);
885  }
886  }
887 
888  MoveItErrorCode execute(const Plan& plan, bool wait)
889  {
891  {
892  // TODO: Remove this backwards compatibility code in L-turtle
893  moveit_msgs::ExecuteKnownTrajectory::Request req;
894  moveit_msgs::ExecuteKnownTrajectory::Response res;
895  req.trajectory = plan.trajectory_;
896  req.wait_for_execution = wait;
897  if (execute_service_.call(req, res))
898  {
899  return MoveItErrorCode(res.error_code);
900  }
901  else
902  {
903  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
904  }
905  }
906 
907  if (!execute_action_client_->isServerConnected())
908  {
909  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
910  }
911 
912  moveit_msgs::ExecuteTrajectoryGoal goal;
913  goal.trajectory = plan.trajectory_;
914 
915  execute_action_client_->sendGoal(goal);
916  if (!wait)
917  {
918  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
919  }
920 
921  if (!execute_action_client_->waitForResult())
922  {
923  ROS_INFO_STREAM_NAMED("move_group_interface", "ExecuteTrajectory action returned early");
924  }
925 
927  {
928  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
929  }
930  else
931  {
932  ROS_INFO_STREAM_NAMED("move_group_interface", execute_action_client_->getState().toString()
933  << ": " << execute_action_client_->getState().getText());
934  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
935  }
936  }
937 
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)
941  {
942  moveit_msgs::GetCartesianPath::Request req;
943  moveit_msgs::GetCartesianPath::Response res;
944 
946  robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state);
947  else
948  req.start_state.is_diff = true;
949 
950  req.group_name = opt_.group_name_;
951  req.header.frame_id = getPoseReferenceFrame();
952  req.header.stamp = ros::Time::now();
953  req.waypoints = waypoints;
954  req.max_step = step;
955  req.jump_threshold = jump_threshold;
956  req.path_constraints = path_constraints;
957  req.avoid_collisions = avoid_collisions;
958  req.link_name = getEndEffectorLink();
959 
960  if (cartesian_path_service_.call(req, res))
961  {
962  error_code = res.error_code;
963  if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
964  {
965  msg = res.solution;
966  return res.fraction;
967  }
968  else
969  return -1.0;
970  }
971  else
972  {
973  error_code.val = error_code.FAILURE;
974  return -1.0;
975  }
976  }
977 
978  void stop()
979  {
981  {
982  std_msgs::String event;
983  event.data = "stop";
985  }
986  }
987 
988  bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links)
989  {
990  std::string l = link.empty() ? getEndEffectorLink() : link;
991  if (l.empty())
992  {
993  const std::vector<std::string>& links = joint_model_group_->getLinkModelNames();
994  if (!links.empty())
995  l = links[0];
996  }
997  if (l.empty())
998  {
999  ROS_ERROR_NAMED("move_group_interface", "No known link to attach object '%s' to", object.c_str());
1000  return false;
1001  }
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);
1007  else
1008  aco.touch_links = touch_links;
1009  aco.object.operation = moveit_msgs::CollisionObject::ADD;
1011  return true;
1012  }
1013 
1014  bool detachObject(const std::string& name)
1015  {
1016  moveit_msgs::AttachedCollisionObject aco;
1017  // if name is a link
1018  if (!name.empty() && joint_model_group_->hasLinkModel(name))
1019  aco.link_name = name;
1020  else
1021  aco.object.id = name;
1022  aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
1023  if (aco.link_name.empty() && aco.object.id.empty())
1024  {
1025  // we only want to detach objects for this group
1026  const std::vector<std::string>& lnames = joint_model_group_->getLinkModelNames();
1027  for (std::size_t i = 0; i < lnames.size(); ++i)
1028  {
1029  aco.link_name = lnames[i];
1031  }
1032  }
1033  else
1035  return true;
1036  }
1037 
1039  {
1040  return goal_position_tolerance_;
1041  }
1042 
1044  {
1046  }
1047 
1048  double getGoalJointTolerance() const
1049  {
1050  return goal_joint_tolerance_;
1051  }
1052 
1053  void setGoalJointTolerance(double tolerance)
1054  {
1055  goal_joint_tolerance_ = tolerance;
1056  }
1057 
1058  void setGoalPositionTolerance(double tolerance)
1059  {
1060  goal_position_tolerance_ = tolerance;
1061  }
1062 
1063  void setGoalOrientationTolerance(double tolerance)
1064  {
1065  goal_orientation_tolerance_ = tolerance;
1066  }
1067 
1068  void setPlanningTime(double seconds)
1069  {
1070  if (seconds > 0.0)
1071  allowed_planning_time_ = seconds;
1072  }
1073 
1074  double getPlanningTime() const
1075  {
1076  return allowed_planning_time_;
1077  }
1078 
1079  void allowLooking(bool flag)
1080  {
1081  can_look_ = flag;
1082  ROS_INFO_NAMED("move_group_interface", "Looking around: %s", can_look_ ? "yes" : "no");
1083  }
1084 
1085  void allowReplanning(bool flag)
1086  {
1087  can_replan_ = flag;
1088  ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no");
1089  }
1090 
1091  void setReplanningDelay(double delay)
1092  {
1093  if (delay >= 0.0)
1094  replan_delay_ = delay;
1095  }
1096 
1097  double getReplanningDelay() const
1098  {
1099  return replan_delay_;
1100  }
1101 
1102  void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request)
1103  {
1104  request.group_name = opt_.group_name_;
1105  request.num_planning_attempts = num_planning_attempts_;
1106  request.max_velocity_scaling_factor = max_velocity_scaling_factor_;
1107  request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_;
1108  request.allowed_planning_time = allowed_planning_time_;
1109  request.planner_id = planner_id_;
1110  request.workspace_parameters = workspace_parameters_;
1111 
1113  robot_state::robotStateToRobotStateMsg(*considered_start_state_, request.start_state);
1114  else
1115  request.start_state.is_diff = true;
1116 
1117  if (active_target_ == JOINT)
1118  {
1119  request.goal_constraints.resize(1);
1120  request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
1122  }
1123  else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION)
1124  {
1125  // find out how many goals are specified
1126  std::size_t goal_count = 0;
1127  for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
1128  it != pose_targets_.end(); ++it)
1129  goal_count = std::max(goal_count, it->second.size());
1130 
1131  // start filling the goals;
1132  // each end effector has a number of possible poses (K) as valid goals
1133  // but there could be multiple end effectors specified, so we want each end effector
1134  // to reach the goal that corresponds to the goals of the other end effectors
1135  request.goal_constraints.resize(goal_count);
1136 
1137  for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
1138  it != pose_targets_.end(); ++it)
1139  {
1140  for (std::size_t i = 0; i < it->second.size(); ++i)
1141  {
1142  moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(
1143  it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_);
1144  if (active_target_ == ORIENTATION)
1145  c.position_constraints.clear();
1146  if (active_target_ == POSITION)
1147  c.orientation_constraints.clear();
1148  request.goal_constraints[i] = kinematic_constraints::mergeConstraints(request.goal_constraints[i], c);
1149  }
1150  }
1151  }
1152  else
1153  ROS_ERROR_NAMED("move_group_interface", "Unable to construct MotionPlanRequest representation");
1154 
1155  if (path_constraints_)
1156  request.path_constraints = *path_constraints_;
1158  request.trajectory_constraints = *trajectory_constraints_;
1159  }
1160 
1161  void constructGoal(moveit_msgs::MoveGroupGoal& goal)
1162  {
1163  constructMotionPlanRequest(goal.request);
1164  }
1165 
1166  void constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object)
1167  {
1168  moveit_msgs::PickupGoal goal;
1169  goal.target_name = object;
1170  goal.group_name = opt_.group_name_;
1171  goal.end_effector = getEndEffector();
1172  goal.allowed_planning_time = allowed_planning_time_;
1173  goal.support_surface_name = support_surface_;
1174  goal.planner_id = planner_id_;
1175  if (!support_surface_.empty())
1176  goal.allow_gripper_support_collision = true;
1177 
1178  if (path_constraints_)
1179  goal.path_constraints = *path_constraints_;
1180 
1181  goal_out = goal;
1182  }
1183 
1184  void constructGoal(moveit_msgs::PlaceGoal& goal_out, const std::string& object)
1185  {
1186  moveit_msgs::PlaceGoal goal;
1187  goal.attached_object_name = object;
1188  goal.group_name = opt_.group_name_;
1189  goal.allowed_planning_time = allowed_planning_time_;
1190  goal.support_surface_name = support_surface_;
1191  goal.planner_id = planner_id_;
1192  if (!support_surface_.empty())
1193  goal.allow_gripper_support_collision = true;
1194 
1195  if (path_constraints_)
1196  goal.path_constraints = *path_constraints_;
1197 
1198  goal_out = goal;
1199  }
1200 
1201  void setPathConstraints(const moveit_msgs::Constraints& constraint)
1202  {
1203  path_constraints_.reset(new moveit_msgs::Constraints(constraint));
1204  }
1205 
1206  bool setPathConstraints(const std::string& constraint)
1207  {
1209  {
1211  if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_))
1212  {
1213  path_constraints_.reset(new moveit_msgs::Constraints(static_cast<moveit_msgs::Constraints>(*msg_m)));
1214  return true;
1215  }
1216  else
1217  return false;
1218  }
1219  else
1220  return false;
1221  }
1222 
1224  {
1225  path_constraints_.reset();
1226  }
1227 
1228  void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint)
1229  {
1230  trajectory_constraints_.reset(new moveit_msgs::TrajectoryConstraints(constraint));
1231  }
1232 
1234  {
1235  trajectory_constraints_.reset();
1236  }
1237 
1238  std::vector<std::string> getKnownConstraints() const
1239  {
1241  {
1242  static ros::WallDuration d(0.01);
1243  d.sleep();
1244  }
1245 
1246  std::vector<std::string> c;
1248  constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_);
1249 
1250  return c;
1251  }
1252 
1253  moveit_msgs::Constraints getPathConstraints() const
1254  {
1255  if (path_constraints_)
1256  return *path_constraints_;
1257  else
1258  return moveit_msgs::Constraints();
1259  }
1260 
1261  moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
1262  {
1264  return *trajectory_constraints_;
1265  else
1266  return moveit_msgs::TrajectoryConstraints();
1267  }
1268 
1269  void initializeConstraintsStorage(const std::string& host, unsigned int port)
1270  {
1273  constraints_init_thread_->join();
1275  new boost::thread(boost::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port)));
1276  }
1277 
1278  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
1279  {
1280  workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame();
1281  workspace_parameters_.header.stamp = ros::Time::now();
1282  workspace_parameters_.min_corner.x = minx;
1283  workspace_parameters_.min_corner.y = miny;
1284  workspace_parameters_.min_corner.z = minz;
1285  workspace_parameters_.max_corner.x = maxx;
1286  workspace_parameters_.max_corner.y = maxy;
1287  workspace_parameters_.max_corner.z = maxz;
1288  }
1289 
1290 private:
1291  void initializeConstraintsStorageThread(const std::string& host, unsigned int port)
1292  {
1293  // Set up db
1294  try
1295  {
1297  conn->setParams(host, port);
1298  if (conn->connect())
1299  {
1301  }
1302  }
1303  catch (std::exception& ex)
1304  {
1305  ROS_ERROR_NAMED("move_group_interface", "%s", ex.what());
1306  }
1307  initializing_constraints_ = false;
1308  }
1309 
1313  robot_model::RobotModelConstPtr robot_model_;
1314  planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
1315  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> > move_action_client_;
1316  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction> > execute_action_client_;
1317  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction> > pick_action_client_;
1318  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PlaceAction> > place_action_client_;
1319 
1320  // general planning params
1321  robot_state::RobotStatePtr considered_start_state_;
1322  moveit_msgs::WorkspaceParameters workspace_parameters_;
1324  std::string planner_id_;
1334 
1335  // joint state goal
1336  robot_state::RobotStatePtr joint_state_target_;
1337  const robot_model::JointModelGroup* joint_model_group_;
1338 
1339  // pose goal;
1340  // for each link we have a set of possible goal locations;
1341  std::map<std::string, std::vector<geometry_msgs::PoseStamped> > pose_targets_;
1342 
1343  // common properties for goals
1344  ActiveTargetType active_target_;
1345  std::unique_ptr<moveit_msgs::Constraints> path_constraints_;
1346  std::unique_ptr<moveit_msgs::TrajectoryConstraints> trajectory_constraints_;
1347  std::string end_effector_link_;
1349  std::string support_surface_;
1350 
1351  // ROS communication
1360  std::unique_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
1361  std::unique_ptr<boost::thread> constraints_init_thread_;
1363 };
1364 }
1365 }
1366 
1369  const ros::WallDuration& wait_for_servers)
1370 {
1371  if (!ros::ok())
1372  throw std::runtime_error("ROS does not seem to be running");
1373  impl_ = new MoveGroupInterfaceImpl(Options(group_name), tf ? tf : getSharedTF(), wait_for_servers);
1374 }
1375 
1378  const ros::Duration& wait_for_servers)
1379  : MoveGroupInterface(group, tf, ros::WallDuration(wait_for_servers.toSec()))
1380 {
1381 }
1382 
1385  const ros::WallDuration& wait_for_servers)
1386 {
1387  impl_ = new MoveGroupInterfaceImpl(opt, tf ? tf : getSharedTF(), wait_for_servers);
1388 }
1389 
1392  const ros::Duration& wait_for_servers)
1393  : MoveGroupInterface(opt, tf, ros::WallDuration(wait_for_servers.toSec()))
1394 {
1395 }
1396 
1398 {
1399  delete impl_;
1400 }
1401 
1404 {
1405  other.impl_ = nullptr;
1406 }
1407 
1410 {
1411  if (this != &other)
1412  {
1413  delete impl_;
1414  impl_ = std::move(other.impl_);
1415  remembered_joint_values_ = std::move(other.remembered_joint_values_);
1416  other.impl_ = nullptr;
1417  }
1418 
1419  return *this;
1420 }
1421 
1423 {
1424  return impl_->getOptions().group_name_;
1425 }
1426 
1428 {
1429  const robot_model::RobotModelConstPtr& robot = getRobotModel();
1430  const std::string& group = getName();
1431  const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group);
1432 
1433  if (joint_group)
1434  {
1435  return joint_group->getDefaultStateNames();
1436  }
1437 
1438  std::vector<std::string> empty;
1439  return empty;
1440 }
1441 
1443 {
1444  return impl_->getRobotModel();
1445 }
1446 
1448 {
1449  return impl_->getOptions().node_handle_;
1450 }
1451 
1453  moveit_msgs::PlannerInterfaceDescription& desc)
1454 {
1455  return impl_->getInterfaceDescription(desc);
1456 }
1457 
1459  const std::string& planner_id, const std::string& group)
1460 {
1461  return impl_->getPlannerParams(planner_id, group);
1462 }
1463 
1465  const std::string& group,
1466  const std::map<std::string, std::string>& params,
1467  bool replace)
1468 {
1469  impl_->setPlannerParams(planner_id, group, params, replace);
1470 }
1471 
1473 {
1474  return impl_->getDefaultPlannerId(group);
1475 }
1476 
1478 {
1479  impl_->setPlannerId(planner_id);
1480 }
1481 
1483 {
1484  return impl_->getPlannerId();
1485 }
1486 
1488 {
1489  impl_->setNumPlanningAttempts(num_planning_attempts);
1490 }
1491 
1493 {
1494  impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
1495 }
1496 
1498  double max_acceleration_scaling_factor)
1499 {
1500  impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
1501 }
1502 
1504 {
1505  return impl_->move(false);
1506 }
1507 
1510 {
1511  return impl_->getMoveGroupClient();
1512 }
1513 
1515 {
1516  return impl_->move(true);
1517 }
1518 
1521 {
1522  return impl_->execute(plan, false);
1523 }
1524 
1526 {
1527  return impl_->execute(plan, true);
1528 }
1529 
1531 {
1532  return impl_->plan(plan);
1533 }
1534 
1536 moveit::planning_interface::MoveGroupInterface::pick(const std::string& object, bool plan_only)
1537 {
1538  return impl_->pick(object, std::vector<moveit_msgs::Grasp>(), plan_only);
1539 }
1540 
1542  const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only)
1543 {
1544  return impl_->pick(object, std::vector<moveit_msgs::Grasp>(1, grasp), plan_only);
1545 }
1546 
1548  const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps, bool plan_only)
1549 {
1550  return impl_->pick(object, grasps, plan_only);
1551 }
1552 
1555 {
1556  return impl_->planGraspsAndPick(object, plan_only);
1557 }
1558 
1560  const moveit_msgs::CollisionObject& object, bool plan_only)
1561 {
1562  return impl_->planGraspsAndPick(object, plan_only);
1563 }
1564 
1566 moveit::planning_interface::MoveGroupInterface::place(const std::string& object, bool plan_only)
1567 {
1568  return impl_->place(object, std::vector<moveit_msgs::PlaceLocation>(), plan_only);
1569 }
1570 
1572  const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations, bool plan_only)
1573 {
1574  return impl_->place(object, locations, plan_only);
1575 }
1576 
1578  const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses, bool plan_only)
1579 {
1580  return impl_->place(object, poses, plan_only);
1581 }
1582 
1584  const std::string& object, const geometry_msgs::PoseStamped& pose, bool plan_only)
1585 {
1586  return impl_->place(object, std::vector<geometry_msgs::PoseStamped>(1, pose), plan_only);
1587 }
1588 
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)
1592 {
1593  moveit_msgs::Constraints path_constraints_tmp;
1594  return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions,
1595  error_code);
1596 }
1597 
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)
1602 {
1603  if (error_code)
1604  {
1605  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1606  avoid_collisions, *error_code);
1607  }
1608  else
1609  {
1610  moveit_msgs::MoveItErrorCodes error_code_tmp;
1611  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1612  avoid_collisions, error_code_tmp);
1613  }
1614 }
1615 
1617 {
1618  impl_->stop();
1619 }
1620 
1621 void moveit::planning_interface::MoveGroupInterface::setStartState(const moveit_msgs::RobotState& start_state)
1622 {
1623  robot_state::RobotStatePtr rs;
1624  impl_->getCurrentState(rs);
1625  robot_state::robotStateMsgToRobotState(start_state, *rs);
1626  setStartState(*rs);
1627 }
1628 
1629 void moveit::planning_interface::MoveGroupInterface::setStartState(const robot_state::RobotState& start_state)
1630 {
1631  impl_->setStartState(start_state);
1632 }
1633 
1635 {
1637 }
1638 
1640 {
1641  impl_->getJointStateTarget().setToRandomPositions();
1642  impl_->setTargetType(JOINT);
1643 }
1644 
1646 {
1647  return impl_->getJointModelGroup()->getVariableNames();
1648 }
1649 
1651 {
1652  return impl_->getJointModelGroup()->getLinkModelNames();
1653 }
1654 
1655 std::map<std::string, double>
1657 {
1658  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1659  std::map<std::string, double> positions;
1660 
1661  if (it != remembered_joint_values_.end())
1662  {
1663  std::vector<std::string> names = impl_->getJointModelGroup()->getVariableNames();
1664  for (size_t x = 0; x < names.size(); ++x)
1665  {
1666  positions[names[x]] = it->second[x];
1667  }
1668  }
1669  else
1670  {
1671  impl_->getJointModelGroup()->getVariableDefaultPositions(name, positions);
1672  }
1673  return positions;
1674 }
1675 
1677 {
1678  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1679  if (it != remembered_joint_values_.end())
1680  {
1681  return setJointValueTarget(it->second);
1682  }
1683  else
1684  {
1685  if (impl_->getJointStateTarget().setToDefaultValues(impl_->getJointModelGroup(), name))
1686  {
1687  impl_->setTargetType(JOINT);
1688  return true;
1689  }
1690  ROS_ERROR_NAMED("move_group_interface", "The requested named target '%s' does not exist", name.c_str());
1691  return false;
1692  }
1693 }
1694 
1695 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::vector<double>& joint_values)
1696 {
1697  if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount())
1698  return false;
1699  impl_->setTargetType(JOINT);
1700  impl_->getJointStateTarget().setJointGroupPositions(impl_->getJointModelGroup(), joint_values);
1702 }
1703 
1705  const std::map<std::string, double>& joint_values)
1706 {
1707  impl_->setTargetType(JOINT);
1708  impl_->getJointStateTarget().setVariablePositions(joint_values);
1709  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1710 }
1711 
1713 {
1714  impl_->setTargetType(JOINT);
1715  impl_->getJointStateTarget() = rstate;
1716  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1717 }
1718 
1719 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::string& joint_name, double value)
1720 {
1721  std::vector<double> values(1, value);
1722  return setJointValueTarget(joint_name, values);
1723 }
1724 
1726  const std::vector<double>& values)
1727 {
1728  impl_->setTargetType(JOINT);
1729  const robot_model::JointModel* jm = impl_->getJointStateTarget().getJointModel(joint_name);
1730  if (jm && jm->getVariableCount() == values.size())
1731  {
1732  impl_->getJointStateTarget().setJointPositions(jm, values);
1733  return impl_->getJointStateTarget().satisfiesBounds(jm, impl_->getGoalJointTolerance());
1734  }
1735  return false;
1736 }
1737 
1739 {
1740  impl_->setTargetType(JOINT);
1741  impl_->getJointStateTarget().setVariableValues(state);
1742  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1743 }
1744 
1746  const std::string& end_effector_link)
1747 {
1748  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false);
1749 }
1750 
1751 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
1752  const std::string& end_effector_link)
1753 {
1754  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false);
1755 }
1756 
1758  const std::string& end_effector_link)
1759 {
1760  geometry_msgs::Pose msg;
1761  tf::poseEigenToMsg(eef_pose, msg);
1762  return setJointValueTarget(msg, end_effector_link);
1763 }
1764 
1766  const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link)
1767 {
1768  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true);
1769 }
1770 
1772  const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link)
1773 {
1774  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true);
1775 }
1776 
1778  const Eigen::Affine3d& eef_pose, const std::string& end_effector_link)
1779 {
1780  geometry_msgs::Pose msg;
1781  tf::poseEigenToMsg(eef_pose, msg);
1782  return setApproximateJointValueTarget(msg, end_effector_link);
1783 }
1784 
1786 {
1787  return impl_->getJointStateTarget();
1788 }
1789 
1791 {
1792  return impl_->getEndEffectorLink();
1793 }
1794 
1796 {
1797  return impl_->getEndEffector();
1798 }
1799 
1801 {
1802  if (impl_->getEndEffectorLink().empty() || link_name.empty())
1803  return false;
1804  impl_->setEndEffectorLink(link_name);
1805  impl_->setTargetType(POSE);
1806  return true;
1807 }
1808 
1810 {
1811  const robot_model::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name);
1812  if (jmg)
1813  return setEndEffectorLink(jmg->getEndEffectorParentGroup().second);
1814  return false;
1815 }
1816 
1818 {
1819  impl_->clearPoseTarget(end_effector_link);
1820 }
1821 
1823 {
1825 }
1826 
1828  const std::string& end_effector_link)
1829 {
1830  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1831  tf::poseEigenToMsg(pose, pose_msg[0].pose);
1832  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1833  pose_msg[0].header.stamp = ros::Time::now();
1834  return setPoseTargets(pose_msg, end_effector_link);
1835 }
1836 
1838  const std::string& end_effector_link)
1839 {
1840  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1841  pose_msg[0].pose = target;
1842  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1843  pose_msg[0].header.stamp = ros::Time::now();
1844  return setPoseTargets(pose_msg, end_effector_link);
1845 }
1846 
1847 bool moveit::planning_interface::MoveGroupInterface::setPoseTarget(const geometry_msgs::PoseStamped& target,
1848  const std::string& end_effector_link)
1849 {
1850  std::vector<geometry_msgs::PoseStamped> targets(1, target);
1851  return setPoseTargets(targets, end_effector_link);
1852 }
1853 
1855  const std::string& end_effector_link)
1856 {
1857  std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
1858  ros::Time tm = ros::Time::now();
1859  const std::string& frame_id = getPoseReferenceFrame();
1860  for (std::size_t i = 0; i < target.size(); ++i)
1861  {
1862  tf::poseEigenToMsg(target[i], pose_out[i].pose);
1863  pose_out[i].header.stamp = tm;
1864  pose_out[i].header.frame_id = frame_id;
1865  }
1866  return setPoseTargets(pose_out, end_effector_link);
1867 }
1868 
1869 bool moveit::planning_interface::MoveGroupInterface::setPoseTargets(const std::vector<geometry_msgs::Pose>& target,
1870  const std::string& end_effector_link)
1871 {
1872  std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
1873  ros::Time tm = ros::Time::now();
1874  const std::string& frame_id = getPoseReferenceFrame();
1875  for (std::size_t i = 0; i < target.size(); ++i)
1876  {
1877  target_stamped[i].pose = target[i];
1878  target_stamped[i].header.stamp = tm;
1879  target_stamped[i].header.frame_id = frame_id;
1880  }
1881  return setPoseTargets(target_stamped, end_effector_link);
1882 }
1883 
1885  const std::vector<geometry_msgs::PoseStamped>& target, const std::string& end_effector_link)
1886 {
1887  if (target.empty())
1888  {
1889  ROS_ERROR_NAMED("move_group_interface", "No pose specified as goal target");
1890  return false;
1891  }
1892  else
1893  {
1894  impl_->setTargetType(POSE);
1895  return impl_->setPoseTargets(target, end_effector_link);
1896  }
1897 }
1898 
1899 const geometry_msgs::PoseStamped&
1900 moveit::planning_interface::MoveGroupInterface::getPoseTarget(const std::string& end_effector_link) const
1901 {
1902  return impl_->getPoseTarget(end_effector_link);
1903 }
1904 
1905 const std::vector<geometry_msgs::PoseStamped>&
1906 moveit::planning_interface::MoveGroupInterface::getPoseTargets(const std::string& end_effector_link) const
1907 {
1908  return impl_->getPoseTargets(end_effector_link);
1909 }
1910 
1911 namespace
1912 {
1913 inline void transformPose(const tf::Transformer& tf, const std::string& desired_frame,
1914  geometry_msgs::PoseStamped& target)
1915 {
1916  if (desired_frame != target.header.frame_id)
1917  {
1918  tf::Pose pose;
1919  tf::poseMsgToTF(target.pose, pose);
1920  tf::Stamped<tf::Pose> stamped_target(pose, target.header.stamp, target.header.frame_id);
1921  tf::Stamped<tf::Pose> stamped_target_out;
1922  tf.transformPose(desired_frame, stamped_target, stamped_target_out);
1923  target.header.frame_id = stamped_target_out.frame_id_;
1924  // target.header.stamp = stamped_target_out.stamp_; // we leave the stamp to ros::Time(0) on purpose
1925  tf::poseTFToMsg(stamped_target_out, target.pose);
1926  }
1927 }
1928 }
1929 
1931  const std::string& end_effector_link)
1932 {
1933  geometry_msgs::PoseStamped target;
1934  if (impl_->hasPoseTarget(end_effector_link))
1935  {
1936  target = getPoseTarget(end_effector_link);
1937  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1938  }
1939  else
1940  {
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;
1945  target.header.frame_id = impl_->getPoseReferenceFrame();
1946  }
1947 
1948  target.pose.position.x = x;
1949  target.pose.position.y = y;
1950  target.pose.position.z = z;
1951  bool result = setPoseTarget(target, end_effector_link);
1952  impl_->setTargetType(POSITION);
1953  return result;
1954 }
1955 
1957  const std::string& end_effector_link)
1958 {
1959  geometry_msgs::PoseStamped target;
1960  if (impl_->hasPoseTarget(end_effector_link))
1961  {
1962  target = getPoseTarget(end_effector_link);
1963  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1964  }
1965  else
1966  {
1967  target.pose.position.x = 0.0;
1968  target.pose.position.y = 0.0;
1969  target.pose.position.z = 0.0;
1970  target.header.frame_id = impl_->getPoseReferenceFrame();
1971  }
1972 
1973  tf::quaternionTFToMsg(tf::createQuaternionFromRPY(r, p, y), target.pose.orientation);
1974  bool result = setPoseTarget(target, end_effector_link);
1975  impl_->setTargetType(ORIENTATION);
1976  return result;
1977 }
1978 
1980  const std::string& end_effector_link)
1981 {
1982  geometry_msgs::PoseStamped target;
1983  if (impl_->hasPoseTarget(end_effector_link))
1984  {
1985  target = getPoseTarget(end_effector_link);
1986  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1987  }
1988  else
1989  {
1990  target.pose.position.x = 0.0;
1991  target.pose.position.y = 0.0;
1992  target.pose.position.z = 0.0;
1993  target.header.frame_id = impl_->getPoseReferenceFrame();
1994  }
1995 
1996  target.pose.orientation.x = x;
1997  target.pose.orientation.y = y;
1998  target.pose.orientation.z = z;
1999  target.pose.orientation.w = w;
2000  bool result = setPoseTarget(target, end_effector_link);
2001  impl_->setTargetType(ORIENTATION);
2002  return result;
2003 }
2004 
2006 {
2007  impl_->setPoseReferenceFrame(pose_reference_frame);
2008 }
2009 
2011 {
2012  return impl_->getPoseReferenceFrame();
2013 }
2014 
2016 {
2017  return impl_->getGoalJointTolerance();
2018 }
2019 
2021 {
2022  return impl_->getGoalPositionTolerance();
2023 }
2024 
2026 {
2028 }
2029 
2031 {
2032  setGoalJointTolerance(tolerance);
2033  setGoalPositionTolerance(tolerance);
2034  setGoalOrientationTolerance(tolerance);
2035 }
2036 
2038 {
2039  impl_->setGoalJointTolerance(tolerance);
2040 }
2041 
2043 {
2044  impl_->setGoalPositionTolerance(tolerance);
2045 }
2046 
2048 {
2049  impl_->setGoalOrientationTolerance(tolerance);
2050 }
2051 
2053 {
2055 }
2056 
2058 {
2059  return impl_->startStateMonitor(wait);
2060 }
2061 
2063 {
2064  robot_state::RobotStatePtr current_state;
2065  std::vector<double> values;
2066  if (impl_->getCurrentState(current_state))
2067  current_state->copyJointGroupPositions(getName(), values);
2068  return values;
2069 }
2070 
2072 {
2073  std::vector<double> r;
2074  impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getJointStateTarget().getRandomNumberGenerator(), r);
2075  return r;
2076 }
2077 
2078 geometry_msgs::PoseStamped
2080 {
2081  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2082  Eigen::Affine3d pose;
2083  pose.setIdentity();
2084  if (eef.empty())
2085  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2086  else
2087  {
2088  robot_state::RobotStatePtr current_state;
2089  if (impl_->getCurrentState(current_state))
2090  {
2091  current_state->setToRandomPositions(impl_->getJointModelGroup());
2092  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2093  if (lm)
2094  pose = current_state->getGlobalLinkTransform(lm);
2095  }
2096  }
2097  geometry_msgs::PoseStamped pose_msg;
2098  pose_msg.header.stamp = ros::Time::now();
2099  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2100  tf::poseEigenToMsg(pose, pose_msg.pose);
2101  return pose_msg;
2102 }
2103 
2104 geometry_msgs::PoseStamped
2106 {
2107  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2108  Eigen::Affine3d pose;
2109  pose.setIdentity();
2110  if (eef.empty())
2111  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2112  else
2113  {
2114  robot_state::RobotStatePtr current_state;
2115  if (impl_->getCurrentState(current_state))
2116  {
2117  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2118  if (lm)
2119  pose = current_state->getGlobalLinkTransform(lm);
2120  }
2121  }
2122  geometry_msgs::PoseStamped pose_msg;
2123  pose_msg.header.stamp = ros::Time::now();
2124  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2125  tf::poseEigenToMsg(pose, pose_msg.pose);
2126  return pose_msg;
2127 }
2128 
2129 std::vector<double> moveit::planning_interface::MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link)
2130 {
2131  std::vector<double> result;
2132  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2133  if (eef.empty())
2134  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2135  else
2136  {
2137  robot_state::RobotStatePtr current_state;
2138  if (impl_->getCurrentState(current_state))
2139  {
2140  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2141  if (lm)
2142  {
2143  result.resize(3);
2144  tf::Matrix3x3 ptf;
2145  tf::matrixEigenToTF(current_state->getGlobalLinkTransform(lm).linear(), ptf);
2146  tfScalar pitch, roll, yaw;
2147  ptf.getRPY(roll, pitch, yaw);
2148  result[0] = roll;
2149  result[1] = pitch;
2150  result[2] = yaw;
2151  }
2152  }
2153  }
2154  return result;
2155 }
2156 
2158 {
2159  return impl_->getJointModelGroup()->getActiveJointModelNames();
2160 }
2161 
2162 const std::vector<std::string>& moveit::planning_interface::MoveGroupInterface::getJoints() const
2163 {
2164  return impl_->getJointModelGroup()->getJointModelNames();
2165 }
2166 
2168 {
2169  return impl_->getJointModelGroup()->getVariableCount();
2170 }
2171 
2173 {
2174  robot_state::RobotStatePtr current_state;
2175  impl_->getCurrentState(current_state, wait);
2176  return current_state;
2177 }
2178 
2180  const std::vector<double>& values)
2181 {
2182  remembered_joint_values_[name] = values;
2183 }
2184 
2186 {
2187  remembered_joint_values_.erase(name);
2188 }
2189 
2191 {
2192  impl_->allowLooking(flag);
2193 }
2194 
2196 {
2197  impl_->allowReplanning(flag);
2198 }
2199 
2201 {
2202  return impl_->getKnownConstraints();
2203 }
2204 
2206 {
2207  return impl_->getPathConstraints();
2208 }
2209 
2211 {
2212  return impl_->setPathConstraints(constraint);
2213 }
2214 
2215 void moveit::planning_interface::MoveGroupInterface::setPathConstraints(const moveit_msgs::Constraints& constraint)
2216 {
2217  impl_->setPathConstraints(constraint);
2218 }
2219 
2221 {
2223 }
2224 
2226 {
2227  return impl_->getTrajectoryConstraints();
2228 }
2229 
2231  const moveit_msgs::TrajectoryConstraints& constraint)
2232 {
2233  impl_->setTrajectoryConstraints(constraint);
2234 }
2235 
2237 {
2239 }
2240 
2241 void moveit::planning_interface::MoveGroupInterface::setConstraintsDatabase(const std::string& host, unsigned int port)
2242 {
2243  impl_->initializeConstraintsStorage(host, port);
2244 }
2245 
2246 void moveit::planning_interface::MoveGroupInterface::setWorkspace(double minx, double miny, double minz, double maxx,
2247  double maxy, double maxz)
2248 {
2249  impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz);
2250 }
2251 
2254 {
2255  impl_->setPlanningTime(seconds);
2256 }
2257 
2260 {
2261  return impl_->getPlanningTime();
2262 }
2263 
2265 {
2267 }
2268 
2270 {
2271  return impl_->getRobotModel()->getModelFrame();
2272 }
2273 
2274 bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link)
2275 {
2276  return attachObject(object, link, std::vector<std::string>());
2277 }
2278 
2279 bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link,
2280  const std::vector<std::string>& touch_links)
2281 {
2282  return impl_->attachObject(object, link, touch_links);
2283 }
2284 
2286 {
2287  return impl_->detachObject(name);
2288 }
2289 
2291  moveit_msgs::MotionPlanRequest& goal_out)
2292 {
2293  impl_->constructMotionPlanRequest(goal_out);
2294 }
bool startStateMonitor(double wait=1.0)
When reasoning about the current state of a robot, a CurrentStateMonitor instance is automatically co...
d
Definition: setup.py:4
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...
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
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...
void setGoalJointTolerance(double tolerance)
Set the joint tolerance (for each joint) that is used for reaching the goal when moving to a joint va...
moveit_msgs::RobotTrajectory trajectory_
The trajectory of the robot (may not contain joints that are the same as for the start_state_) ...
#define ROS_INFO_NAMED(name,...)
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&#39;s URDF. Set to &#39;robot_description&#39;.
void stop()
Stop any trajectory execution, if one is active.
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).
std::vector< double > values
void clearPoseTargets()
Forget any poses specified for all end-effectors.
double tfScalar
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.
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 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.
std::vector< double > getCurrentJointValues()
Get the current joint values for the joints planned for by this instance (see getJoints()) ...
XmlRpcServer s
std::map< std::string, std::vector< double > > remembered_joint_values_
const geometry_msgs::PoseStamped & getPoseTarget(const std::string &end_effector_link) const
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...
bool call(MReq &req, MRes &res)
static bool sameFrame(const std::string &frame1, const std::string &frame2)
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 > &params, bool bReplace=false)
Set the planner parameters for given group and planner_id.
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 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)
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)
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.
void setPlanningTime(double seconds)
Specify the maximum amount of time to use when planning.
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...
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.
#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_
bool getCurrentState(robot_state::RobotStatePtr &current_state, double wait_seconds=1.0)
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 sleep() const
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="")
MoveGroupInterfaceImpl(const Options &opt, const boost::shared_ptr< tf::Transformer > &tf, const ros::WallDuration &wait_for_servers)
warehouse_ros::DatabaseConnection::Ptr loadDatabase()
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.
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.
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...
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
#define ROS_FATAL_STREAM_NAMED(name, args)
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_
double getGoalOrientationTolerance() const
Get the tolerance that is used for reaching an orientation goal. This is the tolerance for roll...
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)
void transformPose(const std::string &target_frame, const Stamped< tf::Pose > &stamped_in, Stamped< tf::Pose > &stamped_out) const
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...
ROSCPP_DECL bool ok()
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...
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.
MoveItErrorCode plan(Plan &plan)
Compute a motion plan that takes the group declared in the constructor from the current state to the ...
#define ROS_WARN_ONCE_NAMED(name,...)
r
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 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.
moveit_msgs::RobotState start_state_
The full starting state used for planning.
res
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
std::string frame_id_
static WallTime now()
std::vector< double > getRandomJointValues()
Get random joint values for the joints planned for by this instance (see getJoints()) ...
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
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)
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 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...
static Time now()
std::unique_ptr< moveit_msgs::TrajectoryConstraints > trajectory_constraints_
const std::string & getPlannerId() const
Get the current planner_id.
robot_model::RobotModelConstPtr getSharedRobotModel(const std::string &robot_description)
void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
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 setPlannerParams(const std::string &planner_id, const std::string &group, const std::map< std::string, std::string > &params, bool replace=false)
std::map< std::string, double > getNamedTargetValues(const std::string &name)
Get the joint angles for targets specified by name.
bool ok() const
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)
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
std::string getDefaultPlannerId(const std::string &group="") const
Get the default planner for a given group (or global default)
double x
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 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
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&#39;s joint state publication...
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...
#define ROS_WARN_STREAM_NAMED(name, args)
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::PickupAction > > pick_action_client_
moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
#define ROS_DEBUG(...)
static const std::string EXECUTE_SERVICE_NAME


planning_interface
Author(s): Ioan Sucan
autogenerated on Sun Oct 18 2020 13:18:50