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/MoveGroupAction.h>
54 #include <moveit_msgs/PickupAction.h>
55 #include <moveit_msgs/ExecuteTrajectoryAction.h>
56 #include <moveit_msgs/PlaceAction.h>
57 #include <moveit_msgs/ExecuteKnownTrajectory.h>
58 #include <moveit_msgs/QueryPlannerInterfaces.h>
59 #include <moveit_msgs/GetCartesianPath.h>
60 #include <moveit_msgs/GraspPlanning.h>
61 #include <moveit_msgs/GetPlannerParams.h>
62 #include <moveit_msgs/SetPlannerParams.h>
63 
66 #include <std_msgs/String.h>
67 #include <tf/transform_listener.h>
68 #include <tf/transform_datatypes.h>
70 #include <ros/console.h>
71 #include <ros/ros.h>
72 
73 namespace moveit
74 {
75 namespace planning_interface
76 {
77 const std::string MoveGroupInterface::ROBOT_DESCRIPTION =
78  "robot_description"; // name of the robot description (a param name, so it can be changed externally)
79 
80 const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps
81 
82 namespace
83 {
85 {
86  JOINT,
87  POSE,
88  POSITION,
89  ORIENTATION
90 };
91 }
92 
94 {
95 public:
97  const ros::WallDuration& wait_for_servers)
98  : opt_(opt), node_handle_(opt.node_handle_), tf_(tf)
99  {
101  if (!getRobotModel())
102  {
103  std::string error = "Unable to construct robot model. Please make sure all needed information is on the "
104  "parameter server.";
105  ROS_FATAL_STREAM_NAMED("move_group_interface", error);
106  throw std::runtime_error(error);
107  }
108 
109  if (!getRobotModel()->hasJointModelGroup(opt.group_name_))
110  {
111  std::string error = "Group '" + opt.group_name_ + "' was not found.";
112  ROS_FATAL_STREAM_NAMED("move_group_interface", error);
113  throw std::runtime_error(error);
114  }
115 
116  joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_);
117 
118  joint_state_target_.reset(new robot_state::RobotState(getRobotModel()));
119  joint_state_target_->setToDefaultValues();
120  active_target_ = JOINT;
121  can_look_ = false;
122  can_replan_ = false;
123  replan_delay_ = 2.0;
124  goal_joint_tolerance_ = 1e-4;
125  goal_position_tolerance_ = 1e-4; // 0.1 mm
126  goal_orientation_tolerance_ = 1e-3; // ~0.1 deg
132 
133  if (joint_model_group_->isChain())
134  end_effector_link_ = joint_model_group_->getLinkModelNames().back();
135  pose_reference_frame_ = getRobotModel()->getModelFrame();
136 
139  attached_object_publisher_ = node_handle_.advertise<moveit_msgs::AttachedCollisionObject>(
141 
143 
144  ros::WallTime timeout_for_servers = ros::WallTime::now() + wait_for_servers;
145  if (wait_for_servers == ros::WallDuration())
146  timeout_for_servers = ros::WallTime(); // wait for ever
147  double allotted_time = wait_for_servers.toSec();
148 
149  move_action_client_.reset(
151  waitForAction(move_action_client_, move_group::MOVE_ACTION, timeout_for_servers, allotted_time);
152 
153  pick_action_client_.reset(
155  waitForAction(pick_action_client_, move_group::PICKUP_ACTION, timeout_for_servers, allotted_time);
156 
157  place_action_client_.reset(
159  waitForAction(place_action_client_, move_group::PLACE_ACTION, timeout_for_servers, allotted_time);
160 
163  // TODO: after deprecation period, i.e. for L-turtle, switch back to standard waitForAction function
164  // waitForAction(execute_action_client_, move_group::EXECUTE_ACTION_NAME, timeout_for_servers, allotted_time);
165  waitForExecuteActionOrService(timeout_for_servers);
166 
168  node_handle_.serviceClient<moveit_msgs::QueryPlannerInterfaces>(move_group::QUERY_PLANNERS_SERVICE_NAME);
173 
176 
178 
179  ROS_INFO_STREAM_NAMED("move_group_interface", "Ready to take commands for planning group " << opt.group_name_
180  << ".");
181  }
182 
183  template <typename T>
184  void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time)
185  {
186  ROS_DEBUG_NAMED("move_group_interface", "Waiting for move_group action server (%s)...", name.c_str());
187 
188  // wait for the server (and spin as needed)
189  if (timeout == ros::WallTime()) // wait forever
190  {
191  while (node_handle_.ok() && !action->isServerConnected())
192  {
193  ros::WallDuration(0.001).sleep();
194  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
196  if (queue)
197  {
198  queue->callAvailable();
199  }
200  else // in case of nodelets and specific callback queue implementations
201  {
202  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
203  "handling.");
204  }
205  }
206  }
207  else // wait with timeout
208  {
209  while (node_handle_.ok() && !action->isServerConnected() && timeout > ros::WallTime::now())
210  {
211  ros::WallDuration(0.001).sleep();
212  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
214  if (queue)
215  {
216  queue->callAvailable();
217  }
218  else // in case of nodelets and specific callback queue implementations
219  {
220  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
221  "handling.");
222  }
223  }
224  }
225 
226  if (!action->isServerConnected())
227  {
228  std::stringstream error;
229  error << "Unable to connect to move_group action server '" << name << "' within allotted time (" << allotted_time
230  << "s)";
231  throw std::runtime_error(error.str());
232  }
233  else
234  {
235  ROS_DEBUG_NAMED("move_group_interface", "Connected to '%s'", name.c_str());
236  }
237  }
238 
240  {
241  ROS_DEBUG("Waiting for move_group action server (%s)...", move_group::EXECUTE_ACTION_NAME.c_str());
242 
243  // Deprecated service
245  node_handle_.serviceClient<moveit_msgs::ExecuteKnownTrajectory>(move_group::EXECUTE_SERVICE_NAME);
246 
247  // wait for either of action or service
248  if (timeout == ros::WallTime()) // wait forever
249  {
250  while (!execute_action_client_->isServerConnected() && !execute_service_.exists())
251  {
252  ros::WallDuration(0.001).sleep();
253  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
255  if (queue)
256  {
257  queue->callAvailable();
258  }
259  else // in case of nodelets and specific callback queue implementations
260  {
261  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
262  "handling.");
263  }
264  }
265  }
266  else // wait with timeout
267  {
268  while (!execute_action_client_->isServerConnected() && !execute_service_.exists() &&
269  timeout > ros::WallTime::now())
270  {
271  ros::WallDuration(0.001).sleep();
272  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
274  if (queue)
275  {
276  queue->callAvailable();
277  }
278  else // in case of nodelets and specific callback queue implementations
279  {
280  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
281  "handling.");
282  }
283  }
284  }
285 
286  // issue warning
287  if (!execute_action_client_->isServerConnected())
288  {
289  if (execute_service_.exists())
290  {
291  ROS_WARN_NAMED("planning_interface",
292  "\nDeprecation warning: Trajectory execution service is deprecated (was replaced by an action)."
293  "\nReplace 'MoveGroupExecuteService' with 'MoveGroupExecuteTrajectoryAction' in "
294  "move_group.launch");
295  }
296  else
297  {
298  ROS_ERROR_STREAM_NAMED("planning_interface",
299  "Unable to find execution action on topic: "
302  throw std::runtime_error("No Trajectory execution capability available.");
303  }
304  execute_action_client_.reset();
305  }
306  }
307 
309  {
311  constraints_init_thread_->join();
312  }
313 
315  {
316  return tf_;
317  }
318 
319  const Options& getOptions() const
320  {
321  return opt_;
322  }
323 
324  const robot_model::RobotModelConstPtr& getRobotModel() const
325  {
326  return robot_model_;
327  }
328 
329  const robot_model::JointModelGroup* getJointModelGroup() const
330  {
331  return joint_model_group_;
332  }
333 
334  bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc)
335  {
336  moveit_msgs::QueryPlannerInterfaces::Request req;
337  moveit_msgs::QueryPlannerInterfaces::Response res;
338  if (query_service_.call(req, res))
339  if (!res.planner_interfaces.empty())
340  {
341  desc = res.planner_interfaces.front();
342  return true;
343  }
344  return false;
345  }
346 
347  std::map<std::string, std::string> getPlannerParams(const std::string& planner_id, const std::string& group = "")
348  {
349  moveit_msgs::GetPlannerParams::Request req;
350  moveit_msgs::GetPlannerParams::Response res;
351  req.planner_config = planner_id;
352  req.group = group;
353  std::map<std::string, std::string> result;
354  if (get_params_service_.call(req, res))
355  {
356  for (unsigned int i = 0, end = res.params.keys.size(); i < end; ++i)
357  result[res.params.keys[i]] = res.params.values[i];
358  }
359  return result;
360  }
361 
362  void setPlannerParams(const std::string& planner_id, const std::string& group,
363  const std::map<std::string, std::string>& params, bool replace = false)
364  {
365  moveit_msgs::SetPlannerParams::Request req;
366  moveit_msgs::SetPlannerParams::Response res;
367  req.planner_config = planner_id;
368  req.group = group;
369  req.replace = replace;
370  for (std::map<std::string, std::string>::const_iterator it = params.begin(), end = params.end(); it != end; ++it)
371  {
372  req.params.keys.push_back(it->first);
373  req.params.values.push_back(it->second);
374  }
375  set_params_service_.call(req, res);
376  }
377 
378  std::string getDefaultPlannerId(const std::string& group) const
379  {
380  std::stringstream param_name;
381  param_name << "move_group";
382  if (!group.empty())
383  param_name << "/" << group;
384  param_name << "/default_planner_config";
385 
386  std::string default_planner_config;
387  node_handle_.getParam(param_name.str(), default_planner_config);
388  return default_planner_config;
389  }
390 
391  void setPlannerId(const std::string& planner_id)
392  {
393  planner_id_ = planner_id;
394  }
395 
396  void setNumPlanningAttempts(unsigned int num_planning_attempts)
397  {
398  num_planning_attempts_ = num_planning_attempts;
399  }
400 
401  void setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
402  {
403  max_velocity_scaling_factor_ = max_velocity_scaling_factor;
404  }
405 
406  void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
407  {
408  max_acceleration_scaling_factor_ = max_acceleration_scaling_factor;
409  }
410 
411  robot_state::RobotState& getJointStateTarget()
412  {
413  return *joint_state_target_;
414  }
415 
416  void setStartState(const robot_state::RobotState& start_state)
417  {
418  considered_start_state_.reset(new robot_state::RobotState(start_state));
419  }
420 
422  {
423  considered_start_state_.reset();
424  }
425 
426  robot_state::RobotStatePtr getStartState()
427  {
430  else
431  {
432  robot_state::RobotStatePtr s;
433  getCurrentState(s);
434  return s;
435  }
436  }
437 
438  bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link,
439  const std::string& frame, bool approx)
440  {
441  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
442  // this only works if we have an end-effector
443  if (!eef.empty())
444  {
445  // first we set the goal to be the same as the start state
446  moveit::core::RobotStatePtr c = getStartState();
447  if (c)
448  {
449  setTargetType(JOINT);
450  c->enforceBounds();
451  getJointStateTarget() = *c;
452  if (!getJointStateTarget().satisfiesBounds(getGoalJointTolerance()))
453  return false;
454  }
455  else
456  return false;
457 
458  // we may need to do approximate IK
460  o.return_approximate_solution = approx;
461 
462  // if no frame transforms are needed, call IK directly
463  if (frame.empty() || moveit::core::Transforms::sameFrame(frame, getRobotModel()->getModelFrame()))
464  return getJointStateTarget().setFromIK(getJointModelGroup(), eef_pose, eef, 0, 0.0,
466  else
467  {
468  if (c->knowsFrameTransform(frame))
469  {
470  // transform the pose first if possible, then do IK
471  const Eigen::Affine3d& t = getJointStateTarget().getFrameTransform(frame);
472  Eigen::Affine3d p;
473  tf::poseMsgToEigen(eef_pose, p);
474  return getJointStateTarget().setFromIK(getJointModelGroup(), t * p, eef, 0, 0.0,
476  }
477  else
478  {
479  logError("Unable to transform from frame '%s' to frame '%s'", frame.c_str(),
480  getRobotModel()->getModelFrame().c_str());
481  return false;
482  }
483  }
484  }
485  else
486  return false;
487  }
488 
489  void setEndEffectorLink(const std::string& end_effector)
490  {
491  end_effector_link_ = end_effector;
492  }
493 
494  void clearPoseTarget(const std::string& end_effector_link)
495  {
496  pose_targets_.erase(end_effector_link);
497  }
498 
500  {
501  pose_targets_.clear();
502  }
503 
504  const std::string& getEndEffectorLink() const
505  {
506  return end_effector_link_;
507  }
508 
509  const std::string& getEndEffector() const
510  {
511  if (!end_effector_link_.empty())
512  {
513  const std::vector<std::string>& possible_eefs =
514  getRobotModel()->getJointModelGroup(opt_.group_name_)->getAttachedEndEffectorNames();
515  for (std::size_t i = 0; i < possible_eefs.size(); ++i)
516  if (getRobotModel()->getEndEffector(possible_eefs[i])->hasLinkModel(end_effector_link_))
517  return possible_eefs[i];
518  }
519  static std::string empty;
520  return empty;
521  }
522 
523  bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& poses, const std::string& end_effector_link)
524  {
525  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
526  if (eef.empty())
527  {
528  ROS_ERROR_NAMED("move_group_interface", "No end-effector to set the pose for");
529  return false;
530  }
531  else
532  {
533  pose_targets_[eef] = poses;
534  // make sure we don't store an actual stamp, since that will become stale can potentially cause tf errors
535  std::vector<geometry_msgs::PoseStamped>& stored_poses = pose_targets_[eef];
536  for (std::size_t i = 0; i < stored_poses.size(); ++i)
537  stored_poses[i].header.stamp = ros::Time(0);
538  }
539  return true;
540  }
541 
542  bool hasPoseTarget(const std::string& end_effector_link) const
543  {
544  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
545  return pose_targets_.find(eef) != pose_targets_.end();
546  }
547 
548  const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link) const
549  {
550  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
551 
552  // if multiple pose targets are set, return the first one
553  std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
554  if (jt != pose_targets_.end())
555  if (!jt->second.empty())
556  return jt->second.at(0);
557 
558  // or return an error
559  static const geometry_msgs::PoseStamped unknown;
560  ROS_ERROR_NAMED("move_group_interface", "Pose for end-effector '%s' not known.", eef.c_str());
561  return unknown;
562  }
563 
564  const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string& end_effector_link) const
565  {
566  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
567 
568  std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
569  if (jt != pose_targets_.end())
570  if (!jt->second.empty())
571  return jt->second;
572 
573  // or return an error
574  static const std::vector<geometry_msgs::PoseStamped> empty;
575  ROS_ERROR_NAMED("move_group_interface", "Poses for end-effector '%s' are not known.", eef.c_str());
576  return empty;
577  }
578 
579  void setPoseReferenceFrame(const std::string& pose_reference_frame)
580  {
581  pose_reference_frame_ = pose_reference_frame;
582  }
583 
584  void setSupportSurfaceName(const std::string& support_surface)
585  {
586  support_surface_ = support_surface;
587  }
588 
589  const std::string& getPoseReferenceFrame() const
590  {
591  return pose_reference_frame_;
592  }
593 
594  void setTargetType(ActiveTargetType type)
595  {
596  active_target_ = type;
597  }
598 
599  ActiveTargetType getTargetType() const
600  {
601  return active_target_;
602  }
603 
604  bool startStateMonitor(double wait)
605  {
607  {
608  ROS_ERROR_NAMED("move_group_interface", "Unable to monitor current robot state");
609  return false;
610  }
611 
612  // if needed, start the monitor and wait up to 1 second for a full robot state
613  if (!current_state_monitor_->isActive())
614  current_state_monitor_->startStateMonitor();
615 
616  current_state_monitor_->waitForCompleteState(opt_.group_name_, wait);
617  return true;
618  }
619 
620  bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds = 1.0)
621  {
623  {
624  ROS_ERROR_NAMED("move_group_interface", "Unable to get current robot state");
625  return false;
626  }
627 
628  // if needed, start the monitor and wait up to 1 second for a full robot state
629  if (!current_state_monitor_->isActive())
630  current_state_monitor_->startStateMonitor();
631 
632  if (!current_state_monitor_->waitForCurrentState(ros::Time::now(), wait_seconds))
633  {
634  ROS_ERROR_NAMED("move_group_interface", "Failed to fetch current robot state");
635  return false;
636  }
637 
638  current_state = current_state_monitor_->getCurrentState();
639  return true;
640  }
641 
643  MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses)
644  {
645  std::vector<moveit_msgs::PlaceLocation> locations;
646  for (std::size_t i = 0; i < poses.size(); ++i)
647  {
648  moveit_msgs::PlaceLocation location;
649  location.pre_place_approach.direction.vector.z = -1.0;
650  location.post_place_retreat.direction.vector.x = -1.0;
651  location.pre_place_approach.direction.header.frame_id = getRobotModel()->getModelFrame();
652  location.post_place_retreat.direction.header.frame_id = end_effector_link_;
653 
654  location.pre_place_approach.min_distance = 0.1;
655  location.pre_place_approach.desired_distance = 0.2;
656  location.post_place_retreat.min_distance = 0.0;
657  location.post_place_retreat.desired_distance = 0.2;
658  // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody
659 
660  location.place_pose = poses[i];
661  locations.push_back(location);
662  }
663  ROS_DEBUG_NAMED("move_group_interface", "Move group interface has %u place locations",
664  (unsigned int)locations.size());
665  return place(object, locations);
666  }
667 
668  MoveItErrorCode place(const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations)
669  {
671  {
672  ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action client not found");
673  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
674  }
675  if (!place_action_client_->isServerConnected())
676  {
677  ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action server not connected");
678  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
679  }
680  moveit_msgs::PlaceGoal goal;
681  constructGoal(goal, object);
682  goal.place_locations = locations;
683  goal.planning_options.plan_only = false;
684  goal.planning_options.look_around = can_look_;
685  goal.planning_options.replan = can_replan_;
686  goal.planning_options.replan_delay = replan_delay_;
687  goal.planning_options.planning_scene_diff.is_diff = true;
688  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
689 
690  place_action_client_->sendGoal(goal);
691  ROS_DEBUG_NAMED("move_group_interface", "Sent place goal with %d locations", (int)goal.place_locations.size());
692  if (!place_action_client_->waitForResult())
693  {
694  ROS_INFO_STREAM_NAMED("move_group_interface", "Place action returned early");
695  }
697  {
698  return MoveItErrorCode(place_action_client_->getResult()->error_code);
699  }
700  else
701  {
702  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": "
703  << place_action_client_->getState().getText());
704  return MoveItErrorCode(place_action_client_->getResult()->error_code);
705  }
706  }
707 
708  MoveItErrorCode pick(const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps)
709  {
710  if (!pick_action_client_)
711  {
712  ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action client not found");
713  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
714  }
715  if (!pick_action_client_->isServerConnected())
716  {
717  ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action server not connected");
718  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
719  }
720  moveit_msgs::PickupGoal goal;
721  constructGoal(goal, object);
722  goal.possible_grasps = grasps;
723  goal.planning_options.plan_only = false;
724  goal.planning_options.look_around = can_look_;
725  goal.planning_options.replan = can_replan_;
726  goal.planning_options.replan_delay = replan_delay_;
727  goal.planning_options.planning_scene_diff.is_diff = true;
728  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
729 
730  pick_action_client_->sendGoal(goal);
731  if (!pick_action_client_->waitForResult())
732  {
733  ROS_INFO_STREAM_NAMED("move_group_interface", "Pickup action returned early");
734  }
736  {
737  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
738  }
739  else
740  {
741  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": "
742  << pick_action_client_->getState().getText());
743  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
744  }
745  }
746 
747  MoveItErrorCode planGraspsAndPick(const std::string& object)
748  {
749  if (object.empty())
750  {
751  return planGraspsAndPick(moveit_msgs::CollisionObject());
752  }
754 
755  std::map<std::string, moveit_msgs::CollisionObject> objects = psi.getObjects(std::vector<std::string>(1, object));
756 
757  if (objects.size() < 1)
758  {
759  ROS_ERROR_STREAM_NAMED("move_group_interface", "Asked for grasps for the object '"
760  << object << "', but the object could not be found");
761  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME);
762  }
763 
764  return planGraspsAndPick(objects[object]);
765  }
766 
767  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object)
768  {
770  {
771  ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '"
772  << GRASP_PLANNING_SERVICE_NAME
773  << "' is not available."
774  " This has to be implemented and started separately.");
775  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
776  }
777 
778  moveit_msgs::GraspPlanning::Request request;
779  moveit_msgs::GraspPlanning::Response response;
780 
781  request.group_name = opt_.group_name_;
782  request.target = object;
783  request.support_surfaces.push_back(support_surface_);
784 
785  ROS_DEBUG_NAMED("move_group_interface", "Calling grasp planner...");
786  if (!plan_grasps_service_.call(request, response) ||
787  response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
788  {
789  ROS_ERROR_NAMED("move_group_interface", "Grasp planning failed. Unable to pick.");
790  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
791  }
792 
793  return pick(object.id, response.grasps);
794  }
795 
797  {
798  if (!move_action_client_)
799  {
800  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
801  }
802  if (!move_action_client_->isServerConnected())
803  {
804  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
805  }
806 
807  moveit_msgs::MoveGroupGoal goal;
808  constructGoal(goal);
809  goal.planning_options.plan_only = true;
810  goal.planning_options.look_around = false;
811  goal.planning_options.replan = false;
812  goal.planning_options.planning_scene_diff.is_diff = true;
813  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
814 
815  move_action_client_->sendGoal(goal);
816  if (!move_action_client_->waitForResult())
817  {
818  ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
819  }
821  {
822  plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
823  plan.start_state_ = move_action_client_->getResult()->trajectory_start;
824  plan.planning_time_ = move_action_client_->getResult()->planning_time;
825  return MoveItErrorCode(move_action_client_->getResult()->error_code);
826  }
827  else
828  {
829  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": "
830  << move_action_client_->getState().getText());
831  return MoveItErrorCode(move_action_client_->getResult()->error_code);
832  }
833  }
834 
836  {
837  if (!move_action_client_)
838  {
839  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
840  }
841  if (!move_action_client_->isServerConnected())
842  {
843  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
844  }
845 
846  moveit_msgs::MoveGroupGoal goal;
847  constructGoal(goal);
848  goal.planning_options.plan_only = false;
849  goal.planning_options.look_around = can_look_;
850  goal.planning_options.replan = can_replan_;
851  goal.planning_options.replan_delay = replan_delay_;
852  goal.planning_options.planning_scene_diff.is_diff = true;
853  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
854 
855  move_action_client_->sendGoal(goal);
856  if (!wait)
857  {
858  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
859  }
860 
861  if (!move_action_client_->waitForResult())
862  {
863  ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
864  }
865 
867  {
868  return MoveItErrorCode(move_action_client_->getResult()->error_code);
869  }
870  else
871  {
872  ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString()
873  << ": " << move_action_client_->getState().getText());
874  return MoveItErrorCode(move_action_client_->getResult()->error_code);
875  }
876  }
877 
878  MoveItErrorCode execute(const Plan& plan, bool wait)
879  {
881  {
882  // TODO: Remove this backwards compatibility code in L-turtle
883  moveit_msgs::ExecuteKnownTrajectory::Request req;
884  moveit_msgs::ExecuteKnownTrajectory::Response res;
885  req.trajectory = plan.trajectory_;
886  req.wait_for_execution = wait;
887  if (execute_service_.call(req, res))
888  {
889  return MoveItErrorCode(res.error_code);
890  }
891  else
892  {
893  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
894  }
895  }
896 
897  if (!execute_action_client_->isServerConnected())
898  {
899  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
900  }
901 
902  moveit_msgs::ExecuteTrajectoryGoal goal;
903  goal.trajectory = plan.trajectory_;
904 
905  execute_action_client_->sendGoal(goal);
906  if (!wait)
907  {
908  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
909  }
910 
911  if (!execute_action_client_->waitForResult())
912  {
913  ROS_INFO_STREAM_NAMED("move_group_interface", "ExecuteTrajectory action returned early");
914  }
915 
917  {
918  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
919  }
920  else
921  {
922  ROS_INFO_STREAM_NAMED("move_group_interface", execute_action_client_->getState().toString()
923  << ": " << execute_action_client_->getState().getText());
924  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
925  }
926  }
927 
928  double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double step, double jump_threshold,
929  moveit_msgs::RobotTrajectory& msg, const moveit_msgs::Constraints& path_constraints,
930  bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code)
931  {
932  moveit_msgs::GetCartesianPath::Request req;
933  moveit_msgs::GetCartesianPath::Response res;
934 
936  robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state);
937  else
938  req.start_state.is_diff = true;
939 
940  req.group_name = opt_.group_name_;
941  req.header.frame_id = getPoseReferenceFrame();
942  req.header.stamp = ros::Time::now();
943  req.waypoints = waypoints;
944  req.max_step = step;
945  req.jump_threshold = jump_threshold;
946  req.path_constraints = path_constraints;
947  req.avoid_collisions = avoid_collisions;
948  req.link_name = getEndEffectorLink();
949 
950  if (cartesian_path_service_.call(req, res))
951  {
952  error_code = res.error_code;
953  if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
954  {
955  msg = res.solution;
956  return res.fraction;
957  }
958  else
959  return -1.0;
960  }
961  else
962  {
963  error_code.val = error_code.FAILURE;
964  return -1.0;
965  }
966  }
967 
968  void stop()
969  {
971  {
972  std_msgs::String event;
973  event.data = "stop";
975  }
976  }
977 
978  bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links)
979  {
980  std::string l = link.empty() ? getEndEffectorLink() : link;
981  if (l.empty())
982  {
983  const std::vector<std::string>& links = joint_model_group_->getLinkModelNames();
984  if (!links.empty())
985  l = links[0];
986  }
987  if (l.empty())
988  {
989  ROS_ERROR_NAMED("move_group_interface", "No known link to attach object '%s' to", object.c_str());
990  return false;
991  }
992  moveit_msgs::AttachedCollisionObject aco;
993  aco.object.id = object;
994  aco.link_name.swap(l);
995  if (touch_links.empty())
996  aco.touch_links.push_back(aco.link_name);
997  else
998  aco.touch_links = touch_links;
999  aco.object.operation = moveit_msgs::CollisionObject::ADD;
1001  return true;
1002  }
1003 
1004  bool detachObject(const std::string& name)
1005  {
1006  moveit_msgs::AttachedCollisionObject aco;
1007  // if name is a link
1008  if (!name.empty() && joint_model_group_->hasLinkModel(name))
1009  aco.link_name = name;
1010  else
1011  aco.object.id = name;
1012  aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
1013  if (aco.link_name.empty() && aco.object.id.empty())
1014  {
1015  // we only want to detach objects for this group
1016  const std::vector<std::string>& lnames = joint_model_group_->getLinkModelNames();
1017  for (std::size_t i = 0; i < lnames.size(); ++i)
1018  {
1019  aco.link_name = lnames[i];
1021  }
1022  }
1023  else
1025  return true;
1026  }
1027 
1029  {
1030  return goal_position_tolerance_;
1031  }
1032 
1034  {
1036  }
1037 
1038  double getGoalJointTolerance() const
1039  {
1040  return goal_joint_tolerance_;
1041  }
1042 
1043  void setGoalJointTolerance(double tolerance)
1044  {
1045  goal_joint_tolerance_ = tolerance;
1046  }
1047 
1048  void setGoalPositionTolerance(double tolerance)
1049  {
1050  goal_position_tolerance_ = tolerance;
1051  }
1052 
1053  void setGoalOrientationTolerance(double tolerance)
1054  {
1055  goal_orientation_tolerance_ = tolerance;
1056  }
1057 
1058  void setPlanningTime(double seconds)
1059  {
1060  if (seconds > 0.0)
1061  allowed_planning_time_ = seconds;
1062  }
1063 
1064  double getPlanningTime() const
1065  {
1066  return allowed_planning_time_;
1067  }
1068 
1069  void allowLooking(bool flag)
1070  {
1071  can_look_ = flag;
1072  ROS_INFO_NAMED("move_group_interface", "Looking around: %s", can_look_ ? "yes" : "no");
1073  }
1074 
1075  void allowReplanning(bool flag)
1076  {
1077  can_replan_ = flag;
1078  ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no");
1079  }
1080 
1081  void setReplanningDelay(double delay)
1082  {
1083  if (delay >= 0.0)
1084  replan_delay_ = delay;
1085  }
1086 
1087  double getReplanningDelay() const
1088  {
1089  return replan_delay_;
1090  }
1091 
1092  void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request)
1093  {
1094  request.group_name = opt_.group_name_;
1095  request.num_planning_attempts = num_planning_attempts_;
1096  request.max_velocity_scaling_factor = max_velocity_scaling_factor_;
1097  request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_;
1098  request.allowed_planning_time = allowed_planning_time_;
1099  request.planner_id = planner_id_;
1100  request.workspace_parameters = workspace_parameters_;
1101 
1103  robot_state::robotStateToRobotStateMsg(*considered_start_state_, request.start_state);
1104  else
1105  request.start_state.is_diff = true;
1106 
1107  if (active_target_ == JOINT)
1108  {
1109  request.goal_constraints.resize(1);
1110  request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
1112  }
1113  else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION)
1114  {
1115  // find out how many goals are specified
1116  std::size_t goal_count = 0;
1117  for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
1118  it != pose_targets_.end(); ++it)
1119  goal_count = std::max(goal_count, it->second.size());
1120 
1121  // start filling the goals;
1122  // each end effector has a number of possible poses (K) as valid goals
1123  // but there could be multiple end effectors specified, so we want each end effector
1124  // to reach the goal that corresponds to the goals of the other end effectors
1125  request.goal_constraints.resize(goal_count);
1126 
1127  for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
1128  it != pose_targets_.end(); ++it)
1129  {
1130  for (std::size_t i = 0; i < it->second.size(); ++i)
1131  {
1132  moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(
1133  it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_);
1134  if (active_target_ == ORIENTATION)
1135  c.position_constraints.clear();
1136  if (active_target_ == POSITION)
1137  c.orientation_constraints.clear();
1138  request.goal_constraints[i] = kinematic_constraints::mergeConstraints(request.goal_constraints[i], c);
1139  }
1140  }
1141  }
1142  else
1143  ROS_ERROR_NAMED("move_group_interface", "Unable to construct MotionPlanRequest representation");
1144 
1145  if (path_constraints_)
1146  request.path_constraints = *path_constraints_;
1147  }
1148 
1149  void constructGoal(moveit_msgs::MoveGroupGoal& goal)
1150  {
1151  constructMotionPlanRequest(goal.request);
1152  }
1153 
1154  void constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object)
1155  {
1156  moveit_msgs::PickupGoal goal;
1157  goal.target_name = object;
1158  goal.group_name = opt_.group_name_;
1159  goal.end_effector = getEndEffector();
1160  goal.allowed_planning_time = allowed_planning_time_;
1161  goal.support_surface_name = support_surface_;
1162  goal.planner_id = planner_id_;
1163  if (!support_surface_.empty())
1164  goal.allow_gripper_support_collision = true;
1165 
1166  if (path_constraints_)
1167  goal.path_constraints = *path_constraints_;
1168 
1169  goal_out = goal;
1170  }
1171 
1172  void constructGoal(moveit_msgs::PlaceGoal& goal_out, const std::string& object)
1173  {
1174  moveit_msgs::PlaceGoal goal;
1175  goal.attached_object_name = object;
1176  goal.group_name = opt_.group_name_;
1177  goal.allowed_planning_time = allowed_planning_time_;
1178  goal.support_surface_name = support_surface_;
1179  goal.planner_id = planner_id_;
1180  if (!support_surface_.empty())
1181  goal.allow_gripper_support_collision = true;
1182 
1183  if (path_constraints_)
1184  goal.path_constraints = *path_constraints_;
1185 
1186  goal_out = goal;
1187  }
1188 
1189  void setPathConstraints(const moveit_msgs::Constraints& constraint)
1190  {
1191  path_constraints_.reset(new moveit_msgs::Constraints(constraint));
1192  }
1193 
1194  bool setPathConstraints(const std::string& constraint)
1195  {
1197  {
1199  if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_))
1200  {
1201  path_constraints_.reset(new moveit_msgs::Constraints(static_cast<moveit_msgs::Constraints>(*msg_m)));
1202  return true;
1203  }
1204  else
1205  return false;
1206  }
1207  else
1208  return false;
1209  }
1210 
1212  {
1213  path_constraints_.reset();
1214  }
1215 
1216  std::vector<std::string> getKnownConstraints() const
1217  {
1219  {
1220  static ros::WallDuration d(0.01);
1221  d.sleep();
1222  }
1223 
1224  std::vector<std::string> c;
1226  constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_);
1227 
1228  return c;
1229  }
1230 
1231  moveit_msgs::Constraints getPathConstraints() const
1232  {
1233  if (path_constraints_)
1234  return *path_constraints_;
1235  else
1236  return moveit_msgs::Constraints();
1237  }
1238 
1239  void initializeConstraintsStorage(const std::string& host, unsigned int port)
1240  {
1243  constraints_init_thread_->join();
1245  new boost::thread(boost::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port)));
1246  }
1247 
1248  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
1249  {
1250  workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame();
1251  workspace_parameters_.header.stamp = ros::Time::now();
1252  workspace_parameters_.min_corner.x = minx;
1253  workspace_parameters_.min_corner.y = miny;
1254  workspace_parameters_.min_corner.z = minz;
1255  workspace_parameters_.max_corner.x = maxx;
1256  workspace_parameters_.max_corner.y = maxy;
1257  workspace_parameters_.max_corner.z = maxz;
1258  }
1259 
1260 private:
1261  void initializeConstraintsStorageThread(const std::string& host, unsigned int port)
1262  {
1263  // Set up db
1264  try
1265  {
1267  conn->setParams(host, port);
1268  if (conn->connect())
1269  {
1271  }
1272  }
1273  catch (std::exception& ex)
1274  {
1275  ROS_ERROR_NAMED("move_group_interface", "%s", ex.what());
1276  }
1277  initializing_constraints_ = false;
1278  }
1279 
1283  robot_model::RobotModelConstPtr robot_model_;
1284  planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
1285  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> > move_action_client_;
1286  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction> > execute_action_client_;
1287  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction> > pick_action_client_;
1288  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PlaceAction> > place_action_client_;
1289 
1290  // general planning params
1291  robot_state::RobotStatePtr considered_start_state_;
1292  moveit_msgs::WorkspaceParameters workspace_parameters_;
1294  std::string planner_id_;
1304 
1305  // joint state goal
1306  robot_state::RobotStatePtr joint_state_target_;
1307  const robot_model::JointModelGroup* joint_model_group_;
1308 
1309  // pose goal;
1310  // for each link we have a set of possible goal locations;
1311  std::map<std::string, std::vector<geometry_msgs::PoseStamped> > pose_targets_;
1312 
1313  // common properties for goals
1314  ActiveTargetType active_target_;
1315  std::unique_ptr<moveit_msgs::Constraints> path_constraints_;
1316  std::string end_effector_link_;
1318  std::string support_surface_;
1319 
1320  // ROS communication
1329  std::unique_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
1330  std::unique_ptr<boost::thread> constraints_init_thread_;
1332 };
1333 }
1334 }
1335 
1338  const ros::WallDuration& wait_for_servers)
1339 {
1340  if (!ros::ok())
1341  throw std::runtime_error("ROS does not seem to be running");
1342  impl_ = new MoveGroupInterfaceImpl(Options(group_name), tf ? tf : getSharedTF(), wait_for_servers);
1343 }
1344 
1347  const ros::Duration& wait_for_servers)
1348  : MoveGroupInterface(group, tf, ros::WallDuration(wait_for_servers.toSec()))
1349 {
1350 }
1351 
1354  const ros::WallDuration& wait_for_servers)
1355 {
1356  impl_ = new MoveGroupInterfaceImpl(opt, tf ? tf : getSharedTF(), wait_for_servers);
1357 }
1358 
1361  const ros::Duration& wait_for_servers)
1362  : MoveGroupInterface(opt, tf, ros::WallDuration(wait_for_servers.toSec()))
1363 {
1364 }
1365 
1367 {
1368  delete impl_;
1369 }
1370 
1373 {
1374  other.impl_ = nullptr;
1375 }
1376 
1379 {
1380  if (this != &other)
1381  {
1382  delete impl_;
1383  impl_ = std::move(other.impl_);
1384  remembered_joint_values_ = std::move(other.remembered_joint_values_);
1385  other.impl_ = nullptr;
1386  }
1387 
1388  return *this;
1389 }
1390 
1392 {
1393  return impl_->getOptions().group_name_;
1394 }
1395 
1397 {
1398  const robot_model::RobotModelConstPtr& robot = getRobotModel();
1399  const std::string& group = getName();
1400  const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group);
1401 
1402  if (joint_group)
1403  {
1404  return joint_group->getDefaultStateNames();
1405  }
1406 
1407  std::vector<std::string> empty;
1408  return empty;
1409 }
1410 
1412 {
1413  return impl_->getRobotModel();
1414 }
1415 
1417 {
1418  return impl_->getOptions().node_handle_;
1419 }
1420 
1422  moveit_msgs::PlannerInterfaceDescription& desc)
1423 {
1424  return impl_->getInterfaceDescription(desc);
1425 }
1426 
1428  const std::string& planner_id, const std::string& group)
1429 {
1430  return impl_->getPlannerParams(planner_id, group);
1431 }
1432 
1434  const std::string& group,
1435  const std::map<std::string, std::string>& params,
1436  bool replace)
1437 {
1438  impl_->setPlannerParams(planner_id, group, params, replace);
1439 }
1440 
1442 {
1443  return impl_->getDefaultPlannerId(group);
1444 }
1445 
1447 {
1448  impl_->setPlannerId(planner_id);
1449 }
1450 
1452 {
1453  impl_->setNumPlanningAttempts(num_planning_attempts);
1454 }
1455 
1457 {
1458  impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
1459 }
1460 
1462  double max_acceleration_scaling_factor)
1463 {
1464  impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
1465 }
1466 
1468 {
1469  return impl_->move(false);
1470 }
1471 
1473 {
1474  return impl_->move(true);
1475 }
1476 
1479 {
1480  return impl_->execute(plan, false);
1481 }
1482 
1484 {
1485  return impl_->execute(plan, true);
1486 }
1487 
1489 {
1490  return impl_->plan(plan);
1491 }
1492 
1495 {
1496  return impl_->pick(object, std::vector<moveit_msgs::Grasp>());
1497 }
1498 
1500 moveit::planning_interface::MoveGroupInterface::pick(const std::string& object, const moveit_msgs::Grasp& grasp)
1501 {
1502  return impl_->pick(object, std::vector<moveit_msgs::Grasp>(1, grasp));
1503 }
1504 
1506  const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps)
1507 {
1508  return impl_->pick(object, grasps);
1509 }
1510 
1513 {
1514  return impl_->planGraspsAndPick(object);
1515 }
1516 
1519 {
1520  return impl_->planGraspsAndPick(object);
1521 }
1522 
1525 {
1526  return impl_->place(object, std::vector<moveit_msgs::PlaceLocation>());
1527 }
1528 
1530  const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations)
1531 {
1532  return impl_->place(object, locations);
1533 }
1534 
1536  const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses)
1537 {
1538  return impl_->place(object, poses);
1539 }
1540 
1542 moveit::planning_interface::MoveGroupInterface::place(const std::string& object, const geometry_msgs::PoseStamped& pose)
1543 {
1544  return impl_->place(object, std::vector<geometry_msgs::PoseStamped>(1, pose));
1545 }
1546 
1548  const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
1549  moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code)
1550 {
1551  moveit_msgs::Constraints path_constraints_tmp;
1552  return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions,
1553  error_code);
1554 }
1555 
1557  const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
1558  moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions,
1559  moveit_msgs::MoveItErrorCodes* error_code)
1560 {
1561  if (error_code)
1562  {
1563  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1564  avoid_collisions, *error_code);
1565  }
1566  else
1567  {
1568  moveit_msgs::MoveItErrorCodes error_code_tmp;
1569  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1570  avoid_collisions, error_code_tmp);
1571  }
1572 }
1573 
1575 {
1576  impl_->stop();
1577 }
1578 
1579 void moveit::planning_interface::MoveGroupInterface::setStartState(const moveit_msgs::RobotState& start_state)
1580 {
1581  robot_state::RobotStatePtr rs;
1582  impl_->getCurrentState(rs);
1583  robot_state::robotStateMsgToRobotState(start_state, *rs);
1584  setStartState(*rs);
1585 }
1586 
1587 void moveit::planning_interface::MoveGroupInterface::setStartState(const robot_state::RobotState& start_state)
1588 {
1589  impl_->setStartState(start_state);
1590 }
1591 
1593 {
1595 }
1596 
1598 {
1599  impl_->getJointStateTarget().setToRandomPositions();
1600  impl_->setTargetType(JOINT);
1601 }
1602 
1604 {
1605  return impl_->getJointModelGroup()->getVariableNames();
1606 }
1607 
1609 {
1610  return impl_->getJointModelGroup()->getLinkModelNames();
1611 }
1612 
1613 std::map<std::string, double>
1615 {
1616  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1617  std::map<std::string, double> positions;
1618 
1619  if (it != remembered_joint_values_.end())
1620  {
1621  std::vector<std::string> names = impl_->getJointModelGroup()->getVariableNames();
1622  for (size_t x = 0; x < names.size(); ++x)
1623  {
1624  positions[names[x]] = it->second[x];
1625  }
1626  }
1627  else
1628  {
1629  impl_->getJointModelGroup()->getVariableDefaultPositions(name, positions);
1630  }
1631  return positions;
1632 }
1633 
1635 {
1636  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1637  if (it != remembered_joint_values_.end())
1638  {
1639  return setJointValueTarget(it->second);
1640  }
1641  else
1642  {
1643  if (impl_->getJointStateTarget().setToDefaultValues(impl_->getJointModelGroup(), name))
1644  {
1645  impl_->setTargetType(JOINT);
1646  return true;
1647  }
1648  ROS_ERROR_NAMED("move_group_interface", "The requested named target '%s' does not exist", name.c_str());
1649  return false;
1650  }
1651 }
1652 
1653 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::vector<double>& joint_values)
1654 {
1655  if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount())
1656  return false;
1657  impl_->setTargetType(JOINT);
1658  impl_->getJointStateTarget().setJointGroupPositions(impl_->getJointModelGroup(), joint_values);
1660 }
1661 
1663  const std::map<std::string, double>& joint_values)
1664 {
1665  impl_->setTargetType(JOINT);
1666  impl_->getJointStateTarget().setVariablePositions(joint_values);
1667  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1668 }
1669 
1671 {
1672  impl_->setTargetType(JOINT);
1673  impl_->getJointStateTarget() = rstate;
1674  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1675 }
1676 
1677 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::string& joint_name, double value)
1678 {
1679  std::vector<double> values(1, value);
1680  return setJointValueTarget(joint_name, values);
1681 }
1682 
1684  const std::vector<double>& values)
1685 {
1686  impl_->setTargetType(JOINT);
1687  const robot_model::JointModel* jm = impl_->getJointStateTarget().getJointModel(joint_name);
1688  if (jm && jm->getVariableCount() == values.size())
1689  {
1690  impl_->getJointStateTarget().setJointPositions(jm, values);
1691  return impl_->getJointStateTarget().satisfiesBounds(jm, impl_->getGoalJointTolerance());
1692  }
1693  return false;
1694 }
1695 
1697 {
1698  impl_->setTargetType(JOINT);
1699  impl_->getJointStateTarget().setVariableValues(state);
1700  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1701 }
1702 
1704  const std::string& end_effector_link)
1705 {
1706  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false);
1707 }
1708 
1709 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
1710  const std::string& end_effector_link)
1711 {
1712  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false);
1713 }
1714 
1716  const std::string& end_effector_link)
1717 {
1718  geometry_msgs::Pose msg;
1719  tf::poseEigenToMsg(eef_pose, msg);
1720  return setJointValueTarget(msg, end_effector_link);
1721 }
1722 
1724  const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link)
1725 {
1726  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true);
1727 }
1728 
1730  const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link)
1731 {
1732  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true);
1733 }
1734 
1736  const Eigen::Affine3d& eef_pose, const std::string& end_effector_link)
1737 {
1738  geometry_msgs::Pose msg;
1739  tf::poseEigenToMsg(eef_pose, msg);
1740  return setApproximateJointValueTarget(msg, end_effector_link);
1741 }
1742 
1744 {
1745  return impl_->getJointStateTarget();
1746 }
1747 
1749 {
1750  return impl_->getEndEffectorLink();
1751 }
1752 
1754 {
1755  return impl_->getEndEffector();
1756 }
1757 
1759 {
1760  if (impl_->getEndEffectorLink().empty() || link_name.empty())
1761  return false;
1762  impl_->setEndEffectorLink(link_name);
1763  impl_->setTargetType(POSE);
1764  return true;
1765 }
1766 
1768 {
1769  const robot_model::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name);
1770  if (jmg)
1771  return setEndEffectorLink(jmg->getEndEffectorParentGroup().second);
1772  return false;
1773 }
1774 
1776 {
1777  impl_->clearPoseTarget(end_effector_link);
1778 }
1779 
1781 {
1783 }
1784 
1786  const std::string& end_effector_link)
1787 {
1788  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1789  tf::poseEigenToMsg(pose, pose_msg[0].pose);
1790  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1791  pose_msg[0].header.stamp = ros::Time::now();
1792  return setPoseTargets(pose_msg, end_effector_link);
1793 }
1794 
1796  const std::string& end_effector_link)
1797 {
1798  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1799  pose_msg[0].pose = target;
1800  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1801  pose_msg[0].header.stamp = ros::Time::now();
1802  return setPoseTargets(pose_msg, end_effector_link);
1803 }
1804 
1805 bool moveit::planning_interface::MoveGroupInterface::setPoseTarget(const geometry_msgs::PoseStamped& target,
1806  const std::string& end_effector_link)
1807 {
1808  std::vector<geometry_msgs::PoseStamped> targets(1, target);
1809  return setPoseTargets(targets, end_effector_link);
1810 }
1811 
1813  const std::string& end_effector_link)
1814 {
1815  std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
1816  ros::Time tm = ros::Time::now();
1817  const std::string& frame_id = getPoseReferenceFrame();
1818  for (std::size_t i = 0; i < target.size(); ++i)
1819  {
1820  tf::poseEigenToMsg(target[i], pose_out[i].pose);
1821  pose_out[i].header.stamp = tm;
1822  pose_out[i].header.frame_id = frame_id;
1823  }
1824  return setPoseTargets(pose_out, end_effector_link);
1825 }
1826 
1827 bool moveit::planning_interface::MoveGroupInterface::setPoseTargets(const std::vector<geometry_msgs::Pose>& target,
1828  const std::string& end_effector_link)
1829 {
1830  std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
1831  ros::Time tm = ros::Time::now();
1832  const std::string& frame_id = getPoseReferenceFrame();
1833  for (std::size_t i = 0; i < target.size(); ++i)
1834  {
1835  target_stamped[i].pose = target[i];
1836  target_stamped[i].header.stamp = tm;
1837  target_stamped[i].header.frame_id = frame_id;
1838  }
1839  return setPoseTargets(target_stamped, end_effector_link);
1840 }
1841 
1843  const std::vector<geometry_msgs::PoseStamped>& target, const std::string& end_effector_link)
1844 {
1845  if (target.empty())
1846  {
1847  ROS_ERROR_NAMED("move_group_interface", "No pose specified as goal target");
1848  return false;
1849  }
1850  else
1851  {
1852  impl_->setTargetType(POSE);
1853  return impl_->setPoseTargets(target, end_effector_link);
1854  }
1855 }
1856 
1857 const geometry_msgs::PoseStamped&
1858 moveit::planning_interface::MoveGroupInterface::getPoseTarget(const std::string& end_effector_link) const
1859 {
1860  return impl_->getPoseTarget(end_effector_link);
1861 }
1862 
1863 const std::vector<geometry_msgs::PoseStamped>&
1864 moveit::planning_interface::MoveGroupInterface::getPoseTargets(const std::string& end_effector_link) const
1865 {
1866  return impl_->getPoseTargets(end_effector_link);
1867 }
1868 
1869 namespace
1870 {
1871 inline void transformPose(const tf::Transformer& tf, const std::string& desired_frame,
1872  geometry_msgs::PoseStamped& target)
1873 {
1874  if (desired_frame != target.header.frame_id)
1875  {
1876  tf::Pose pose;
1877  tf::poseMsgToTF(target.pose, pose);
1878  tf::Stamped<tf::Pose> stamped_target(pose, target.header.stamp, target.header.frame_id);
1879  tf::Stamped<tf::Pose> stamped_target_out;
1880  tf.transformPose(desired_frame, stamped_target, stamped_target_out);
1881  target.header.frame_id = stamped_target_out.frame_id_;
1882  // target.header.stamp = stamped_target_out.stamp_; // we leave the stamp to ros::Time(0) on purpose
1883  tf::poseTFToMsg(stamped_target_out, target.pose);
1884  }
1885 }
1886 }
1887 
1889  const std::string& end_effector_link)
1890 {
1891  geometry_msgs::PoseStamped target;
1892  if (impl_->hasPoseTarget(end_effector_link))
1893  {
1894  target = getPoseTarget(end_effector_link);
1895  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1896  }
1897  else
1898  {
1899  target.pose.orientation.x = 0.0;
1900  target.pose.orientation.y = 0.0;
1901  target.pose.orientation.z = 0.0;
1902  target.pose.orientation.w = 1.0;
1903  target.header.frame_id = impl_->getPoseReferenceFrame();
1904  }
1905 
1906  target.pose.position.x = x;
1907  target.pose.position.y = y;
1908  target.pose.position.z = z;
1909  bool result = setPoseTarget(target, end_effector_link);
1910  impl_->setTargetType(POSITION);
1911  return result;
1912 }
1913 
1915  const std::string& end_effector_link)
1916 {
1917  geometry_msgs::PoseStamped target;
1918  if (impl_->hasPoseTarget(end_effector_link))
1919  {
1920  target = getPoseTarget(end_effector_link);
1921  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1922  }
1923  else
1924  {
1925  target.pose.position.x = 0.0;
1926  target.pose.position.y = 0.0;
1927  target.pose.position.z = 0.0;
1928  target.header.frame_id = impl_->getPoseReferenceFrame();
1929  }
1930 
1931  tf::quaternionTFToMsg(tf::createQuaternionFromRPY(r, p, y), target.pose.orientation);
1932  bool result = setPoseTarget(target, end_effector_link);
1933  impl_->setTargetType(ORIENTATION);
1934  return result;
1935 }
1936 
1938  const std::string& end_effector_link)
1939 {
1940  geometry_msgs::PoseStamped target;
1941  if (impl_->hasPoseTarget(end_effector_link))
1942  {
1943  target = getPoseTarget(end_effector_link);
1944  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1945  }
1946  else
1947  {
1948  target.pose.position.x = 0.0;
1949  target.pose.position.y = 0.0;
1950  target.pose.position.z = 0.0;
1951  target.header.frame_id = impl_->getPoseReferenceFrame();
1952  }
1953 
1954  target.pose.orientation.x = x;
1955  target.pose.orientation.y = y;
1956  target.pose.orientation.z = z;
1957  target.pose.orientation.w = w;
1958  bool result = setPoseTarget(target, end_effector_link);
1959  impl_->setTargetType(ORIENTATION);
1960  return result;
1961 }
1962 
1964 {
1965  impl_->setPoseReferenceFrame(pose_reference_frame);
1966 }
1967 
1969 {
1970  return impl_->getPoseReferenceFrame();
1971 }
1972 
1974 {
1975  return impl_->getGoalJointTolerance();
1976 }
1977 
1979 {
1980  return impl_->getGoalPositionTolerance();
1981 }
1982 
1984 {
1986 }
1987 
1989 {
1990  setGoalJointTolerance(tolerance);
1991  setGoalPositionTolerance(tolerance);
1992  setGoalOrientationTolerance(tolerance);
1993 }
1994 
1996 {
1997  impl_->setGoalJointTolerance(tolerance);
1998 }
1999 
2001 {
2002  impl_->setGoalPositionTolerance(tolerance);
2003 }
2004 
2006 {
2007  impl_->setGoalOrientationTolerance(tolerance);
2008 }
2009 
2011 {
2013 }
2014 
2016 {
2017  return impl_->startStateMonitor(wait);
2018 }
2019 
2021 {
2022  robot_state::RobotStatePtr current_state;
2023  std::vector<double> values;
2024  if (impl_->getCurrentState(current_state))
2025  current_state->copyJointGroupPositions(getName(), values);
2026  return values;
2027 }
2028 
2030 {
2031  std::vector<double> r;
2032  impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getJointStateTarget().getRandomNumberGenerator(), r);
2033  return r;
2034 }
2035 
2036 geometry_msgs::PoseStamped
2038 {
2039  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2040  Eigen::Affine3d pose;
2041  pose.setIdentity();
2042  if (eef.empty())
2043  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2044  else
2045  {
2046  robot_state::RobotStatePtr current_state;
2047  if (impl_->getCurrentState(current_state))
2048  {
2049  current_state->setToRandomPositions(impl_->getJointModelGroup());
2050  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2051  if (lm)
2052  pose = current_state->getGlobalLinkTransform(lm);
2053  }
2054  }
2055  geometry_msgs::PoseStamped pose_msg;
2056  pose_msg.header.stamp = ros::Time::now();
2057  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2058  tf::poseEigenToMsg(pose, pose_msg.pose);
2059  return pose_msg;
2060 }
2061 
2062 geometry_msgs::PoseStamped
2064 {
2065  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2066  Eigen::Affine3d pose;
2067  pose.setIdentity();
2068  if (eef.empty())
2069  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2070  else
2071  {
2072  robot_state::RobotStatePtr current_state;
2073  if (impl_->getCurrentState(current_state))
2074  {
2075  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2076  if (lm)
2077  pose = current_state->getGlobalLinkTransform(lm);
2078  }
2079  }
2080  geometry_msgs::PoseStamped pose_msg;
2081  pose_msg.header.stamp = ros::Time::now();
2082  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2083  tf::poseEigenToMsg(pose, pose_msg.pose);
2084  return pose_msg;
2085 }
2086 
2087 std::vector<double> moveit::planning_interface::MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link)
2088 {
2089  std::vector<double> result;
2090  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2091  if (eef.empty())
2092  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2093  else
2094  {
2095  robot_state::RobotStatePtr current_state;
2096  if (impl_->getCurrentState(current_state))
2097  {
2098  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2099  if (lm)
2100  {
2101  result.resize(3);
2102  tf::Matrix3x3 ptf;
2103  tf::matrixEigenToTF(current_state->getGlobalLinkTransform(lm).rotation(), ptf);
2104  tfScalar pitch, roll, yaw;
2105  ptf.getRPY(roll, pitch, yaw);
2106  result[0] = roll;
2107  result[1] = pitch;
2108  result[2] = yaw;
2109  }
2110  }
2111  }
2112  return result;
2113 }
2114 
2116 {
2117  return impl_->getJointModelGroup()->getActiveJointModelNames();
2118 }
2119 
2120 const std::vector<std::string>& moveit::planning_interface::MoveGroupInterface::getJoints() const
2121 {
2122  return impl_->getJointModelGroup()->getJointModelNames();
2123 }
2124 
2126 {
2127  return impl_->getJointModelGroup()->getVariableCount();
2128 }
2129 
2131 {
2132  robot_state::RobotStatePtr current_state;
2133  impl_->getCurrentState(current_state);
2134  return current_state;
2135 }
2136 
2138  const std::vector<double>& values)
2139 {
2140  remembered_joint_values_[name] = values;
2141 }
2142 
2144 {
2145  remembered_joint_values_.erase(name);
2146 }
2147 
2149 {
2150  impl_->allowLooking(flag);
2151 }
2152 
2154 {
2155  impl_->allowReplanning(flag);
2156 }
2157 
2159 {
2160  return impl_->getKnownConstraints();
2161 }
2162 
2164 {
2165  return impl_->getPathConstraints();
2166 }
2167 
2169 {
2170  return impl_->setPathConstraints(constraint);
2171 }
2172 
2173 void moveit::planning_interface::MoveGroupInterface::setPathConstraints(const moveit_msgs::Constraints& constraint)
2174 {
2175  impl_->setPathConstraints(constraint);
2176 }
2177 
2179 {
2181 }
2182 
2183 void moveit::planning_interface::MoveGroupInterface::setConstraintsDatabase(const std::string& host, unsigned int port)
2184 {
2185  impl_->initializeConstraintsStorage(host, port);
2186 }
2187 
2188 void moveit::planning_interface::MoveGroupInterface::setWorkspace(double minx, double miny, double minz, double maxx,
2189  double maxy, double maxz)
2190 {
2191  impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz);
2192 }
2193 
2196 {
2197  impl_->setPlanningTime(seconds);
2198 }
2199 
2202 {
2203  return impl_->getPlanningTime();
2204 }
2205 
2207 {
2209 }
2210 
2212 {
2213  return impl_->getRobotModel()->getModelFrame();
2214 }
2215 
2216 bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link)
2217 {
2218  return attachObject(object, link, std::vector<std::string>());
2219 }
2220 
2221 bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link,
2222  const std::vector<std::string>& touch_links)
2223 {
2224  return impl_->attachObject(object, link, touch_links);
2225 }
2226 
2228 {
2229  return impl_->detachObject(name);
2230 }
2231 
2233  moveit_msgs::MotionPlanRequest& goal_out)
2234 {
2235  impl_->constructMotionPlanRequest(goal_out);
2236 }
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, const std::vector< moveit_msgs::Grasp > &grasps)
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
MoveItErrorCode place(const std::string &object)
Place an object somewhere safe in the world (a safe location will be detected)
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.
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...
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.
MoveItErrorCode place(const std::string &object, const std::vector< geometry_msgs::PoseStamped > &poses)
Place an object at one of the specified possible locations.
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...
MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject &object)
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
#define ROS_FATAL_STREAM_NAMED(name, args)
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
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()
const std::string & getNamespace() const
MoveItErrorCode place(const std::string &object, const std::vector< moveit_msgs::PlaceLocation > &locations)
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 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,...)
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...
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.
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()) ...
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
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()
robot_state::RobotStatePtr getCurrentState()
Get the current state of the robot.
robot_model::RobotModelConstPtr getSharedRobotModel(const std::string &robot_description)
void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
MoveItErrorCode pick(const std::string &object)
Pick up an object.
static const std::string GET_PLANNER_PARAMS_SERVICE_NAME
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.
r
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)
MoveItErrorCode planGraspsAndPick(const std::string &object="")
Pick up an object.
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...
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_
#define ROS_DEBUG(...)
static const std::string EXECUTE_SERVICE_NAME


planning_interface
Author(s): Ioan Sucan
autogenerated on Sun Jan 21 2018 03:56:10