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 
63 #include <std_msgs/String.h>
64 #include <geometry_msgs/TransformStamped.h>
65 #include <tf2/utils.h>
66 #include <tf2_eigen/tf2_eigen.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 const std::string LOGNAME = "move_group_interface";
81 
82 namespace
83 {
84 enum ActiveTargetType
85 {
86  JOINT,
87  POSE,
88  POSITION,
89  ORIENTATION
90 };
91 }
92 
93 class MoveGroupInterface::MoveGroupInterfaceImpl
94 {
95 public:
96  MoveGroupInterfaceImpl(const Options& opt, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
97  const ros::WallDuration& wait_for_servers)
99  {
100  robot_model_ = opt.robot_model_ ? opt.robot_model_ : getSharedRobotModel(opt.robot_description_);
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.";
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.";
113  throw std::runtime_error(error);
114  }
115 
116  joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_);
117 
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
129  node_handle_.param<double>("robot_description_planning/default_velocity_scaling_factor",
131  node_handle_.param<double>("robot_description_planning/default_acceleration_scaling_factor",
134 
137  pose_reference_frame_ = getRobotModel()->getModelFrame();
138 
141  attached_object_publisher_ = node_handle_.advertise<moveit_msgs::AttachedCollisionObject>(
143 
145 
146  ros::WallTime timeout_for_servers = ros::WallTime::now() + wait_for_servers;
147  if (wait_for_servers == ros::WallDuration())
148  timeout_for_servers = ros::WallTime(); // wait for ever
149  double allotted_time = wait_for_servers.toSec();
150 
151  move_action_client_.reset(
153  waitForAction(move_action_client_, move_group::MOVE_ACTION, timeout_for_servers, allotted_time);
154 
155  pick_action_client_.reset(
157  waitForAction(pick_action_client_, move_group::PICKUP_ACTION, timeout_for_servers, allotted_time);
158 
159  place_action_client_.reset(
161  waitForAction(place_action_client_, move_group::PLACE_ACTION, timeout_for_servers, allotted_time);
162 
165  waitForAction(execute_action_client_, move_group::EXECUTE_ACTION_NAME, timeout_for_servers, allotted_time);
166 
168  node_handle_.serviceClient<moveit_msgs::QueryPlannerInterfaces>(move_group::QUERY_PLANNERS_SERVICE_NAME);
173 
176 
178 
179  ROS_INFO_STREAM_NAMED(LOGNAME, "Ready to take commands for planning group " << opt.group_name_ << ".");
180  }
181 
182  template <typename T>
183  void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time) const
184  {
185  ROS_DEBUG_NAMED(LOGNAME, "Waiting for move_group action server (%s)...", name.c_str());
186 
187  // wait for the server (and spin as needed)
188  if (timeout == ros::WallTime()) // wait forever
189  {
190  while (node_handle_.ok() && !action->isServerConnected())
191  {
192  ros::WallDuration(0.001).sleep();
193  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
195  if (queue)
196  {
197  queue->callAvailable();
198  }
199  else // in case of nodelets and specific callback queue implementations
200  {
201  ROS_WARN_ONCE_NAMED(LOGNAME, "Non-default CallbackQueue: Waiting for external queue "
202  "handling.");
203  }
204  }
205  }
206  else // wait with timeout
207  {
208  while (node_handle_.ok() && !action->isServerConnected() && timeout > ros::WallTime::now())
209  {
210  ros::WallDuration(0.001).sleep();
211  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
213  if (queue)
214  {
215  queue->callAvailable();
216  }
217  else // in case of nodelets and specific callback queue implementations
218  {
219  ROS_WARN_ONCE_NAMED(LOGNAME, "Non-default CallbackQueue: Waiting for external queue "
220  "handling.");
221  }
222  }
223  }
224 
225  if (!action->isServerConnected())
226  {
227  std::stringstream error;
228  error << "Unable to connect to move_group action server '" << name << "' within allotted time (" << allotted_time
229  << "s)";
230  throw std::runtime_error(error.str());
231  }
232  else
233  {
234  ROS_DEBUG_NAMED(LOGNAME, "Connected to '%s'", name.c_str());
235  }
236  }
237 
239  {
241  constraints_init_thread_->join();
242  }
243 
244  const std::shared_ptr<tf2_ros::Buffer>& getTF() const
245  {
246  return tf_buffer_;
247  }
248 
249  const Options& getOptions() const
250  {
251  return opt_;
252  }
253 
254  const moveit::core::RobotModelConstPtr& getRobotModel() const
255  {
256  return robot_model_;
257  }
258 
260  {
261  return joint_model_group_;
262  }
263 
265  {
266  return *move_action_client_;
267  }
268 
269  bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc)
270  {
271  moveit_msgs::QueryPlannerInterfaces::Request req;
272  moveit_msgs::QueryPlannerInterfaces::Response res;
273  if (query_service_.call(req, res))
274  if (!res.planner_interfaces.empty())
275  {
276  desc = res.planner_interfaces.front();
277  return true;
278  }
279  return false;
280  }
281 
282  bool getInterfaceDescriptions(std::vector<moveit_msgs::PlannerInterfaceDescription>& desc)
283  {
284  moveit_msgs::QueryPlannerInterfaces::Request req;
285  moveit_msgs::QueryPlannerInterfaces::Response res;
286  if (query_service_.call(req, res))
287  if (!res.planner_interfaces.empty())
288  {
289  desc = res.planner_interfaces;
290  return true;
291  }
292  return false;
293  }
294 
295  std::map<std::string, std::string> getPlannerParams(const std::string& planner_id, const std::string& group = "")
296  {
297  moveit_msgs::GetPlannerParams::Request req;
298  moveit_msgs::GetPlannerParams::Response res;
299  req.planner_config = planner_id;
300  req.group = group;
301  std::map<std::string, std::string> result;
302  if (get_params_service_.call(req, res))
303  {
304  for (unsigned int i = 0, end = res.params.keys.size(); i < end; ++i)
305  result[res.params.keys[i]] = res.params.values[i];
306  }
307  return result;
308  }
309 
310  void setPlannerParams(const std::string& planner_id, const std::string& group,
311  const std::map<std::string, std::string>& params, bool replace = false)
312  {
313  moveit_msgs::SetPlannerParams::Request req;
314  moveit_msgs::SetPlannerParams::Response res;
315  req.planner_config = planner_id;
316  req.group = group;
317  req.replace = replace;
318  for (const std::pair<const std::string, std::string>& param : params)
319  {
320  req.params.keys.push_back(param.first);
321  req.params.values.push_back(param.second);
322  }
323  set_params_service_.call(req, res);
324  }
325 
326  std::string getDefaultPlanningPipelineId() const
327  {
328  std::string default_planning_pipeline;
329  node_handle_.getParam("move_group/default_planning_pipeline", default_planning_pipeline);
330  return default_planning_pipeline;
331  }
332 
333  void setPlanningPipelineId(const std::string& pipeline_id)
334  {
335  if (pipeline_id != planning_pipeline_id_)
336  {
337  planning_pipeline_id_ = pipeline_id;
338 
339  // Reset planner_id if planning pipeline changed
340  planner_id_ = "";
341  }
342  }
343 
344  const std::string& getPlanningPipelineId() const
345  {
346  return planning_pipeline_id_;
347  }
348 
349  std::string getDefaultPlannerId(const std::string& group) const
350  {
351  // Check what planning pipeline config should be used
352  std::string pipeline_id = getDefaultPlanningPipelineId();
353  if (!planning_pipeline_id_.empty())
354  pipeline_id = planning_pipeline_id_;
355 
356  std::stringstream param_name;
357  param_name << "move_group";
358  if (!pipeline_id.empty())
359  param_name << "/planning_pipelines/" << pipeline_id;
360  if (!group.empty())
361  param_name << "/" << group;
362  param_name << "/default_planner_config";
363 
364  std::string default_planner_config;
365  node_handle_.getParam(param_name.str(), default_planner_config);
366  return default_planner_config;
367  }
368 
369  void setPlannerId(const std::string& planner_id)
370  {
371  planner_id_ = planner_id;
372  }
373 
374  const std::string& getPlannerId() const
375  {
376  return planner_id_;
377  }
378 
379  void setNumPlanningAttempts(unsigned int num_planning_attempts)
380  {
381  num_planning_attempts_ = num_planning_attempts;
382  }
383 
384  void setMaxVelocityScalingFactor(double value)
385  {
386  setMaxScalingFactor(max_velocity_scaling_factor_, value, "velocity_scaling_factor", 0.1);
387  }
388 
389  void setMaxAccelerationScalingFactor(double value)
390  {
391  setMaxScalingFactor(max_acceleration_scaling_factor_, value, "acceleration_scaling_factor", 0.1);
392  }
393 
394  void setMaxScalingFactor(double& variable, const double target_value, const char* factor_name, double fallback_value)
395  {
396  if (target_value > 1.0)
397  {
398  ROS_WARN_NAMED(LOGNAME, "Limiting max_%s (%.2f) to 1.0.", factor_name, target_value);
399  variable = 1.0;
400  }
401  else if (target_value <= 0.0)
402  {
403  node_handle_.param<double>(std::string("robot_description_planning/default_") + factor_name, variable,
404  fallback_value);
405  if (target_value < 0.0)
406  {
407  ROS_WARN_NAMED(LOGNAME, "max_%s < 0.0! Setting to default: %.2f.", factor_name, variable);
408  }
409  }
410  else
411  {
412  variable = target_value;
413  }
414  }
415 
417  {
418  return *joint_state_target_;
419  }
420 
422  {
423  return *joint_state_target_;
424  }
425 
426  void setStartState(const moveit::core::RobotState& start_state)
427  {
428  considered_start_state_.reset(new moveit::core::RobotState(start_state));
429  }
430 
432  {
433  considered_start_state_.reset();
434  }
435 
436  moveit::core::RobotStatePtr getStartState()
437  {
440  else
441  {
442  moveit::core::RobotStatePtr s;
443  getCurrentState(s);
444  return s;
445  }
446  }
447 
448  bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link,
449  const std::string& frame, bool approx)
450  {
451  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
452  // this only works if we have an end-effector
453  if (!eef.empty())
454  {
455  // first we set the goal to be the same as the start state
456  moveit::core::RobotStatePtr c = getStartState();
457  if (c)
458  {
459  setTargetType(JOINT);
460  c->enforceBounds();
461  getTargetRobotState() = *c;
462  if (!getTargetRobotState().satisfiesBounds(getGoalJointTolerance()))
463  return false;
464  }
465  else
466  return false;
467 
468  // we may need to do approximate IK
471 
472  // if no frame transforms are needed, call IK directly
473  if (frame.empty() || moveit::core::Transforms::sameFrame(frame, getRobotModel()->getModelFrame()))
474  return getTargetRobotState().setFromIK(getJointModelGroup(), eef_pose, eef, 0.0,
476  else
477  {
478  // transform the pose into the model frame, then do IK
479  bool frame_found;
480  const Eigen::Isometry3d& t = getTargetRobotState().getFrameTransform(frame, &frame_found);
481  if (frame_found)
482  {
483  Eigen::Isometry3d p;
484  tf2::fromMsg(eef_pose, p);
485  return getTargetRobotState().setFromIK(getJointModelGroup(), t * p, eef, 0.0,
487  }
488  else
489  {
490  ROS_ERROR_NAMED(LOGNAME, "Unable to transform from frame '%s' to frame '%s'", frame.c_str(),
491  getRobotModel()->getModelFrame().c_str());
492  return false;
493  }
494  }
495  }
496  else
497  return false;
498  }
499 
500  void setEndEffectorLink(const std::string& end_effector)
501  {
502  end_effector_link_ = end_effector;
503  }
504 
505  void clearPoseTarget(const std::string& end_effector_link)
506  {
507  pose_targets_.erase(end_effector_link);
508  }
509 
510  void clearPoseTargets()
511  {
512  pose_targets_.clear();
513  }
514 
515  const std::string& getEndEffectorLink() const
516  {
517  return end_effector_link_;
518  }
519 
520  const std::string& getEndEffector() const
521  {
522  if (!end_effector_link_.empty())
523  {
524  const std::vector<std::string>& possible_eefs =
525  getRobotModel()->getJointModelGroup(opt_.group_name_)->getAttachedEndEffectorNames();
526  for (const std::string& possible_eef : possible_eefs)
527  if (getRobotModel()->getEndEffector(possible_eef)->hasLinkModel(end_effector_link_))
528  return possible_eef;
529  }
530  static std::string empty;
531  return empty;
532  }
533 
534  bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& poses, const std::string& end_effector_link)
535  {
536  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
537  if (eef.empty())
538  {
539  ROS_ERROR_NAMED(LOGNAME, "No end-effector to set the pose for");
540  return false;
541  }
542  else
543  {
544  pose_targets_[eef] = poses;
545  // make sure we don't store an actual stamp, since that will become stale can potentially cause tf errors
546  std::vector<geometry_msgs::PoseStamped>& stored_poses = pose_targets_[eef];
547  for (geometry_msgs::PoseStamped& stored_pose : stored_poses)
548  stored_pose.header.stamp = ros::Time(0);
549  }
550  return true;
551  }
552 
553  bool hasPoseTarget(const std::string& end_effector_link) const
554  {
555  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
556  return pose_targets_.find(eef) != pose_targets_.end();
557  }
558 
559  const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link) const
560  {
561  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
562 
563  // if multiple pose targets are set, return the first one
564  std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
565  if (jt != pose_targets_.end())
566  if (!jt->second.empty())
567  return jt->second.at(0);
568 
569  // or return an error
570  static const geometry_msgs::PoseStamped UNKNOWN;
571  ROS_ERROR_NAMED(LOGNAME, "Pose for end-effector '%s' not known.", eef.c_str());
572  return UNKNOWN;
573  }
574 
575  const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string& end_effector_link) const
576  {
577  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
578 
579  std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
580  if (jt != pose_targets_.end())
581  if (!jt->second.empty())
582  return jt->second;
583 
584  // or return an error
585  static const std::vector<geometry_msgs::PoseStamped> EMPTY;
586  ROS_ERROR_NAMED(LOGNAME, "Poses for end-effector '%s' are not known.", eef.c_str());
587  return EMPTY;
588  }
589 
590  void setPoseReferenceFrame(const std::string& pose_reference_frame)
591  {
592  pose_reference_frame_ = pose_reference_frame;
593  }
594 
595  void setSupportSurfaceName(const std::string& support_surface)
596  {
597  support_surface_ = support_surface;
598  }
599 
600  const std::string& getPoseReferenceFrame() const
601  {
603  }
604 
605  void setTargetType(ActiveTargetType type)
606  {
607  active_target_ = type;
608  }
609 
610  ActiveTargetType getTargetType() const
611  {
612  return active_target_;
613  }
614 
615  bool startStateMonitor(double wait)
616  {
618  {
619  ROS_ERROR_NAMED(LOGNAME, "Unable to monitor current robot state");
620  return false;
621  }
622 
623  // if needed, start the monitor and wait up to 1 second for a full robot state
624  if (!current_state_monitor_->isActive())
625  current_state_monitor_->startStateMonitor();
626 
627  current_state_monitor_->waitForCompleteState(opt_.group_name_, wait);
628  return true;
629  }
630 
631  bool getCurrentState(moveit::core::RobotStatePtr& current_state, double wait_seconds = 1.0)
632  {
634  {
635  ROS_ERROR_NAMED(LOGNAME, "Unable to get current robot state");
636  return false;
637  }
638 
639  // if needed, start the monitor and wait up to 1 second for a full robot state
640  if (!current_state_monitor_->isActive())
641  current_state_monitor_->startStateMonitor();
642 
643  if (!current_state_monitor_->waitForCurrentState(ros::Time::now(), wait_seconds))
644  {
645  ROS_ERROR_NAMED(LOGNAME, "Failed to fetch current robot state");
646  return false;
647  }
648 
649  current_state = current_state_monitor_->getCurrentState();
650  return true;
651  }
652 
654  std::vector<moveit_msgs::PlaceLocation>
655  posesToPlaceLocations(const std::vector<geometry_msgs::PoseStamped>& poses) const
656  {
657  std::vector<moveit_msgs::PlaceLocation> locations;
658  for (const geometry_msgs::PoseStamped& pose : poses)
659  {
660  moveit_msgs::PlaceLocation location;
661  location.pre_place_approach.direction.vector.z = -1.0;
662  location.post_place_retreat.direction.vector.x = -1.0;
663  location.pre_place_approach.direction.header.frame_id = getRobotModel()->getModelFrame();
664  location.post_place_retreat.direction.header.frame_id = end_effector_link_;
665 
666  location.pre_place_approach.min_distance = 0.1;
667  location.pre_place_approach.desired_distance = 0.2;
668  location.post_place_retreat.min_distance = 0.0;
669  location.post_place_retreat.desired_distance = 0.2;
670  // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody
671 
672  location.place_pose = pose;
673  locations.push_back(location);
674  }
675  ROS_DEBUG_NAMED(LOGNAME, "Move group interface has %u place locations", (unsigned int)locations.size());
676  return locations;
677  }
678 
679  MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal)
680  {
682  {
683  ROS_ERROR_STREAM_NAMED(LOGNAME, "place action client not found");
684  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
685  }
686  if (!place_action_client_->isServerConnected())
687  {
688  ROS_WARN_STREAM_NAMED(LOGNAME, "place action server not connected");
689  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
690  }
691 
692  place_action_client_->sendGoal(goal);
693  ROS_DEBUG_NAMED(LOGNAME, "Sent place goal with %d locations", (int)goal.place_locations.size());
694  if (!place_action_client_->waitForResult())
695  {
696  ROS_INFO_STREAM_NAMED(LOGNAME, "Place action returned early");
697  }
699  {
700  return MoveItErrorCode(place_action_client_->getResult()->error_code);
701  }
702  else
703  {
704  ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << place_action_client_->getState().toString() << ": "
705  << place_action_client_->getState().getText());
706  return MoveItErrorCode(place_action_client_->getResult()->error_code);
707  }
708  }
709 
710  MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal)
711  {
712  if (!pick_action_client_)
713  {
714  ROS_ERROR_STREAM_NAMED(LOGNAME, "pick action client not found");
715  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
716  }
717  if (!pick_action_client_->isServerConnected())
718  {
719  ROS_WARN_STREAM_NAMED(LOGNAME, "pick action server not connected");
720  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
721  }
722 
723  pick_action_client_->sendGoal(goal);
724  if (!pick_action_client_->waitForResult())
725  {
726  ROS_INFO_STREAM_NAMED(LOGNAME, "Pickup action returned early");
727  }
729  {
730  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
731  }
732  else
733  {
734  ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << pick_action_client_->getState().toString() << ": "
735  << pick_action_client_->getState().getText());
736  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
737  }
738  }
739 
740  MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false)
741  {
742  if (object.empty())
743  {
744  return planGraspsAndPick(moveit_msgs::CollisionObject());
745  }
746 
748  std::map<std::string, moveit_msgs::CollisionObject> objects = psi.getObjects(std::vector<std::string>(1, object));
749 
750  if (objects.empty())
751  {
753  "Asked for grasps for the object '" << object << "', but the object could not be found");
754  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME);
755  }
756 
757  return planGraspsAndPick(objects[object], plan_only);
758  }
759 
760  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false)
761  {
763  {
764  ROS_ERROR_STREAM_NAMED(LOGNAME, "Grasp planning service '"
766  << "' is not available."
767  " This has to be implemented and started separately.");
768  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
769  }
770 
771  moveit_msgs::GraspPlanning::Request request;
772  moveit_msgs::GraspPlanning::Response response;
773 
774  request.group_name = opt_.group_name_;
775  request.target = object;
776  request.support_surfaces.push_back(support_surface_);
777 
778  ROS_DEBUG_NAMED(LOGNAME, "Calling grasp planner...");
779  if (!plan_grasps_service_.call(request, response) ||
780  response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
781  {
782  ROS_ERROR_NAMED(LOGNAME, "Grasp planning failed. Unable to pick.");
783  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
784  }
785 
786  return pick(constructPickupGoal(object.id, std::move(response.grasps), plan_only));
787  }
788 
789  MoveItErrorCode plan(Plan& plan)
790  {
791  if (!move_action_client_)
792  {
793  ROS_ERROR_STREAM_NAMED(LOGNAME, "move action client not found");
794  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
795  }
796  if (!move_action_client_->isServerConnected())
797  {
798  ROS_WARN_STREAM_NAMED(LOGNAME, "move action server not connected");
799  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
800  }
801 
802  moveit_msgs::MoveGroupGoal goal;
803  constructGoal(goal);
804  goal.planning_options.plan_only = true;
805  goal.planning_options.look_around = false;
806  goal.planning_options.replan = false;
807  goal.planning_options.planning_scene_diff.is_diff = true;
808  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
809 
810  move_action_client_->sendGoal(goal);
811  if (!move_action_client_->waitForResult())
812  {
813  ROS_INFO_STREAM_NAMED(LOGNAME, "MoveGroup action returned early");
814  }
816  {
817  plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
818  plan.start_state_ = move_action_client_->getResult()->trajectory_start;
819  plan.planning_time_ = move_action_client_->getResult()->planning_time;
820  return MoveItErrorCode(move_action_client_->getResult()->error_code);
821  }
822  else
823  {
824  ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << move_action_client_->getState().toString() << ": "
825  << move_action_client_->getState().getText());
826  return MoveItErrorCode(move_action_client_->getResult()->error_code);
827  }
828  }
829 
830  MoveItErrorCode move(bool wait)
831  {
832  if (!move_action_client_)
833  {
834  ROS_ERROR_STREAM_NAMED(LOGNAME, "move action client not found");
835  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
836  }
837  if (!move_action_client_->isServerConnected())
838  {
839  ROS_WARN_STREAM_NAMED(LOGNAME, "move action server not connected");
840  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
841  }
842 
843  moveit_msgs::MoveGroupGoal goal;
844  constructGoal(goal);
845  goal.planning_options.plan_only = false;
846  goal.planning_options.look_around = can_look_;
847  goal.planning_options.replan = can_replan_;
848  goal.planning_options.replan_delay = replan_delay_;
849  goal.planning_options.planning_scene_diff.is_diff = true;
850  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
851 
852  move_action_client_->sendGoal(goal);
853  if (!wait)
854  {
855  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
856  }
857 
858  if (!move_action_client_->waitForResult())
859  {
860  ROS_INFO_STREAM_NAMED(LOGNAME, "MoveGroup action returned early");
861  }
862 
864  {
865  return MoveItErrorCode(move_action_client_->getResult()->error_code);
866  }
867  else
868  {
869  ROS_INFO_STREAM_NAMED(LOGNAME, move_action_client_->getState().toString()
870  << ": " << move_action_client_->getState().getText());
871  return MoveItErrorCode(move_action_client_->getResult()->error_code);
872  }
873  }
874 
875  MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory, bool wait)
876  {
878  {
879  ROS_ERROR_STREAM_NAMED(LOGNAME, "execute action client not found");
880  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
881  }
882  if (!execute_action_client_->isServerConnected())
883  {
884  ROS_WARN_STREAM_NAMED(LOGNAME, "execute action server not connected");
885  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
886  }
887 
888  moveit_msgs::ExecuteTrajectoryGoal goal;
889  goal.trajectory = trajectory;
890 
891  execute_action_client_->sendGoal(goal);
892  if (!wait)
893  {
894  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
895  }
896 
897  if (!execute_action_client_->waitForResult())
898  {
899  ROS_INFO_STREAM_NAMED(LOGNAME, "ExecuteTrajectory action returned early");
900  }
901 
903  {
904  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
905  }
906  else
907  {
909  << ": " << execute_action_client_->getState().getText());
910  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
911  }
912  }
913 
914  double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double step, double jump_threshold,
915  moveit_msgs::RobotTrajectory& msg, const moveit_msgs::Constraints& path_constraints,
916  bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code)
917  {
918  moveit_msgs::GetCartesianPath::Request req;
919  moveit_msgs::GetCartesianPath::Response res;
920 
923  else
924  req.start_state.is_diff = true;
925 
926  req.group_name = opt_.group_name_;
927  req.header.frame_id = getPoseReferenceFrame();
928  req.header.stamp = ros::Time::now();
929  req.waypoints = waypoints;
930  req.max_step = step;
931  req.jump_threshold = jump_threshold;
932  req.path_constraints = path_constraints;
933  req.avoid_collisions = avoid_collisions;
934  req.link_name = getEndEffectorLink();
935 
936  if (cartesian_path_service_.call(req, res))
937  {
938  error_code = res.error_code;
939  if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
940  {
941  msg = res.solution;
942  return res.fraction;
943  }
944  else
945  return -1.0;
946  }
947  else
948  {
949  error_code.val = error_code.FAILURE;
950  return -1.0;
951  }
952  }
953 
954  void stop()
955  {
957  {
958  std_msgs::String event;
959  event.data = "stop";
961  }
962  }
963 
964  bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links)
965  {
966  std::string l = link.empty() ? getEndEffectorLink() : link;
967  if (l.empty())
968  {
969  const std::vector<std::string>& links = joint_model_group_->getLinkModelNames();
970  if (!links.empty())
971  l = links[0];
972  }
973  if (l.empty())
974  {
975  ROS_ERROR_NAMED(LOGNAME, "No known link to attach object '%s' to", object.c_str());
976  return false;
977  }
978  moveit_msgs::AttachedCollisionObject aco;
979  aco.object.id = object;
980  aco.link_name.swap(l);
981  if (touch_links.empty())
982  aco.touch_links.push_back(aco.link_name);
983  else
984  aco.touch_links = touch_links;
985  aco.object.operation = moveit_msgs::CollisionObject::ADD;
987  return true;
988  }
989 
990  bool detachObject(const std::string& name)
991  {
992  moveit_msgs::AttachedCollisionObject aco;
993  // if name is a link
994  if (!name.empty() && joint_model_group_->hasLinkModel(name))
995  aco.link_name = name;
996  else
997  aco.object.id = name;
998  aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
999  if (aco.link_name.empty() && aco.object.id.empty())
1000  {
1001  // we only want to detach objects for this group
1002  const std::vector<std::string>& lnames = joint_model_group_->getLinkModelNames();
1003  for (const std::string& lname : lnames)
1004  {
1005  aco.link_name = lname;
1007  }
1008  }
1009  else
1011  return true;
1012  }
1013 
1014  double getGoalPositionTolerance() const
1015  {
1016  return goal_position_tolerance_;
1017  }
1018 
1019  double getGoalOrientationTolerance() const
1020  {
1022  }
1023 
1024  double getGoalJointTolerance() const
1025  {
1026  return goal_joint_tolerance_;
1027  }
1028 
1029  void setGoalJointTolerance(double tolerance)
1030  {
1032  }
1033 
1034  void setGoalPositionTolerance(double tolerance)
1035  {
1037  }
1038 
1039  void setGoalOrientationTolerance(double tolerance)
1040  {
1042  }
1043 
1044  void setPlanningTime(double seconds)
1045  {
1046  if (seconds > 0.0)
1048  }
1049 
1050  double getPlanningTime() const
1051  {
1052  return allowed_planning_time_;
1053  }
1054 
1055  void allowLooking(bool flag)
1056  {
1057  can_look_ = flag;
1058  ROS_INFO_NAMED(LOGNAME, "Looking around: %s", can_look_ ? "yes" : "no");
1059  }
1060 
1061  void allowReplanning(bool flag)
1062  {
1063  can_replan_ = flag;
1064  ROS_INFO_NAMED(LOGNAME, "Replanning: %s", can_replan_ ? "yes" : "no");
1065  }
1066 
1067  void setReplanningDelay(double delay)
1068  {
1069  if (delay >= 0.0)
1070  replan_delay_ = delay;
1071  }
1072 
1073  double getReplanningDelay() const
1074  {
1075  return replan_delay_;
1076  }
1077 
1078  void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request) const
1079  {
1080  request.group_name = opt_.group_name_;
1081  request.num_planning_attempts = num_planning_attempts_;
1082  request.max_velocity_scaling_factor = max_velocity_scaling_factor_;
1083  request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_;
1084  request.allowed_planning_time = allowed_planning_time_;
1085  request.pipeline_id = planning_pipeline_id_;
1086  request.planner_id = planner_id_;
1087  request.workspace_parameters = workspace_parameters_;
1088 
1091  else
1092  request.start_state.is_diff = true;
1093 
1094  if (active_target_ == JOINT)
1095  {
1096  request.goal_constraints.resize(1);
1099  }
1100  else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION)
1101  {
1102  // find out how many goals are specified
1103  std::size_t goal_count = 0;
1104  for (const auto& pose_target : pose_targets_)
1105  goal_count = std::max(goal_count, pose_target.second.size());
1106 
1107  // start filling the goals;
1108  // each end effector has a number of possible poses (K) as valid goals
1109  // but there could be multiple end effectors specified, so we want each end effector
1110  // to reach the goal that corresponds to the goals of the other end effectors
1111  request.goal_constraints.resize(goal_count);
1113  for (const auto& pose_target : pose_targets_)
1114  {
1115  for (std::size_t i = 0; i < pose_target.second.size(); ++i)
1116  {
1117  moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(
1118  pose_target.first, pose_target.second[i], goal_position_tolerance_, goal_orientation_tolerance_);
1119  if (active_target_ == ORIENTATION)
1120  c.position_constraints.clear();
1121  if (active_target_ == POSITION)
1122  c.orientation_constraints.clear();
1123  request.goal_constraints[i] = kinematic_constraints::mergeConstraints(request.goal_constraints[i], c);
1124  }
1125  }
1126  }
1127  else
1128  ROS_ERROR_NAMED(LOGNAME, "Unable to construct MotionPlanRequest representation");
1130  if (path_constraints_)
1131  request.path_constraints = *path_constraints_;
1133  request.trajectory_constraints = *trajectory_constraints_;
1134  }
1136  void constructGoal(moveit_msgs::MoveGroupGoal& goal) const
1137  {
1138  constructMotionPlanRequest(goal.request);
1139  }
1140 
1141  moveit_msgs::PickupGoal constructPickupGoal(const std::string& object, std::vector<moveit_msgs::Grasp>&& grasps,
1142  bool plan_only = false) const
1143  {
1144  moveit_msgs::PickupGoal goal;
1145  goal.target_name = object;
1146  goal.group_name = opt_.group_name_;
1147  goal.end_effector = getEndEffector();
1148  goal.support_surface_name = support_surface_;
1149  goal.possible_grasps = std::move(grasps);
1150  if (!support_surface_.empty())
1151  goal.allow_gripper_support_collision = true;
1152 
1153  if (path_constraints_)
1154  goal.path_constraints = *path_constraints_;
1155 
1156  goal.planner_id = planner_id_;
1157  goal.allowed_planning_time = allowed_planning_time_;
1158 
1159  goal.planning_options.plan_only = plan_only;
1160  goal.planning_options.look_around = can_look_;
1161  goal.planning_options.replan = can_replan_;
1162  goal.planning_options.replan_delay = replan_delay_;
1163  goal.planning_options.planning_scene_diff.is_diff = true;
1164  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
1165  return goal;
1166  }
1167 
1168  moveit_msgs::PlaceGoal constructPlaceGoal(const std::string& object,
1169  std::vector<moveit_msgs::PlaceLocation>&& locations,
1170  bool plan_only = false) const
1171  {
1172  moveit_msgs::PlaceGoal goal;
1173  goal.group_name = opt_.group_name_;
1174  goal.attached_object_name = object;
1175  goal.support_surface_name = support_surface_;
1176  goal.place_locations = std::move(locations);
1177  if (!support_surface_.empty())
1178  goal.allow_gripper_support_collision = true;
1179 
1180  if (path_constraints_)
1181  goal.path_constraints = *path_constraints_;
1182 
1183  goal.planner_id = planner_id_;
1184  goal.allowed_planning_time = allowed_planning_time_;
1185 
1186  goal.planning_options.plan_only = plan_only;
1187  goal.planning_options.look_around = can_look_;
1188  goal.planning_options.replan = can_replan_;
1189  goal.planning_options.replan_delay = replan_delay_;
1190  goal.planning_options.planning_scene_diff.is_diff = true;
1191  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
1192  return goal;
1193  }
1194 
1195  void setPathConstraints(const moveit_msgs::Constraints& constraint)
1196  {
1197  path_constraints_.reset(new moveit_msgs::Constraints(constraint));
1198  }
1199 
1200  bool setPathConstraints(const std::string& constraint)
1201  {
1203  {
1205  if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_))
1206  {
1207  path_constraints_.reset(new moveit_msgs::Constraints(static_cast<moveit_msgs::Constraints>(*msg_m)));
1208  return true;
1209  }
1210  else
1211  return false;
1212  }
1213  else
1214  return false;
1215  }
1216 
1217  void clearPathConstraints()
1218  {
1219  path_constraints_.reset();
1220  }
1221 
1222  void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint)
1223  {
1224  trajectory_constraints_.reset(new moveit_msgs::TrajectoryConstraints(constraint));
1225  }
1226 
1228  {
1229  trajectory_constraints_.reset();
1230  }
1231 
1232  std::vector<std::string> getKnownConstraints() const
1233  {
1235  {
1236  static ros::WallDuration d(0.01);
1237  d.sleep();
1238  }
1239 
1240  std::vector<std::string> c;
1242  constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_);
1243 
1244  return c;
1245  }
1246 
1247  moveit_msgs::Constraints getPathConstraints() const
1248  {
1249  if (path_constraints_)
1250  return *path_constraints_;
1251  else
1252  return moveit_msgs::Constraints();
1253  }
1254 
1255  moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
1256  {
1258  return *trajectory_constraints_;
1259  else
1260  return moveit_msgs::TrajectoryConstraints();
1261  }
1262 
1263  void initializeConstraintsStorage(const std::string& host, unsigned int port)
1264  {
1267  constraints_init_thread_->join();
1269  new boost::thread(boost::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port)));
1270  }
1271 
1272  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
1273  {
1274  workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame();
1275  workspace_parameters_.header.stamp = ros::Time::now();
1276  workspace_parameters_.min_corner.x = minx;
1277  workspace_parameters_.min_corner.y = miny;
1278  workspace_parameters_.min_corner.z = minz;
1279  workspace_parameters_.max_corner.x = maxx;
1280  workspace_parameters_.max_corner.y = maxy;
1281  workspace_parameters_.max_corner.z = maxz;
1282  }
1283 
1284 private:
1285  void initializeConstraintsStorageThread(const std::string& host, unsigned int port)
1286  {
1287  // Set up db
1288  try
1289  {
1291  conn->setParams(host, port);
1292  if (conn->connect())
1293  {
1295  }
1296  }
1297  catch (std::exception& ex)
1298  {
1299  ROS_ERROR_NAMED(LOGNAME, "%s", ex.what());
1300  }
1301  initializing_constraints_ = false;
1302  }
1303 
1304  Options opt_;
1306  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
1307  moveit::core::RobotModelConstPtr robot_model_;
1308  planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
1309  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> > move_action_client_;
1310  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction> > execute_action_client_;
1311  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction> > pick_action_client_;
1312  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PlaceAction> > place_action_client_;
1313 
1314  // general planning params
1315  moveit::core::RobotStatePtr considered_start_state_;
1316  moveit_msgs::WorkspaceParameters workspace_parameters_;
1317  double allowed_planning_time_;
1318  std::string planning_pipeline_id_;
1319  std::string planner_id_;
1320  unsigned int num_planning_attempts_;
1324  double goal_position_tolerance_;
1326  bool can_look_;
1327  bool can_replan_;
1328  double replan_delay_;
1329 
1330  // joint state goal
1331  moveit::core::RobotStatePtr joint_state_target_;
1333 
1334  // pose goal;
1335  // for each link we have a set of possible goal locations;
1336  std::map<std::string, std::vector<geometry_msgs::PoseStamped> > pose_targets_;
1337 
1338  // common properties for goals
1339  ActiveTargetType active_target_;
1340  std::unique_ptr<moveit_msgs::Constraints> path_constraints_;
1341  std::unique_ptr<moveit_msgs::TrajectoryConstraints> trajectory_constraints_;
1342  std::string end_effector_link_;
1343  std::string pose_reference_frame_;
1344  std::string support_surface_;
1345 
1346  // ROS communication
1354  std::unique_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
1355  std::unique_ptr<boost::thread> constraints_init_thread_;
1357 };
1358 
1359 MoveGroupInterface::MoveGroupInterface(const std::string& group_name, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1360  const ros::WallDuration& wait_for_servers)
1361 {
1362  if (!ros::ok())
1363  throw std::runtime_error("ROS does not seem to be running");
1364  impl_ = new MoveGroupInterfaceImpl(Options(group_name), tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers);
1365 }
1366 
1367 MoveGroupInterface::MoveGroupInterface(const std::string& group, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1368  const ros::Duration& wait_for_servers)
1369  : MoveGroupInterface(group, tf_buffer, ros::WallDuration(wait_for_servers.toSec()))
1370 {
1371 }
1373 MoveGroupInterface::MoveGroupInterface(const Options& opt, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1374  const ros::WallDuration& wait_for_servers)
1376  impl_ = new MoveGroupInterfaceImpl(opt, tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers);
1380  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1381  const ros::Duration& wait_for_servers)
1382  : MoveGroupInterface(opt, tf_buffer, ros::WallDuration(wait_for_servers.toSec()))
1388  delete impl_;
1392  : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_)
1394  other.impl_ = nullptr;
1398 {
1399  if (this != &other)
1400  {
1401  delete impl_;
1402  impl_ = other.impl_;
1403  remembered_joint_values_ = std::move(other.remembered_joint_values_);
1404  other.impl_ = nullptr;
1405  }
1406 
1407  return *this;
1410 const std::string& MoveGroupInterface::getName() const
1413 }
1414 
1415 const std::vector<std::string>& MoveGroupInterface::getNamedTargets() const
1417  // The pointer returned by getJointModelGroup is guaranteed by the class
1418  // constructor to always be non-null
1422 moveit::core::RobotModelConstPtr MoveGroupInterface::getRobotModel() const
1425 }
1426 
1428 {
1429  return impl_->getOptions().node_handle_;
1430 }
1431 
1432 bool MoveGroupInterface::getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc) const
1433 {
1434  return impl_->getInterfaceDescription(desc);
1436 
1437 bool MoveGroupInterface::getInterfaceDescriptions(std::vector<moveit_msgs::PlannerInterfaceDescription>& desc) const
1438 {
1439  return impl_->getInterfaceDescriptions(desc);
1440 }
1442 std::map<std::string, std::string> MoveGroupInterface::getPlannerParams(const std::string& planner_id,
1443  const std::string& group) const
1444 {
1445  return impl_->getPlannerParams(planner_id, group);
1446 }
1448 void MoveGroupInterface::setPlannerParams(const std::string& planner_id, const std::string& group,
1449  const std::map<std::string, std::string>& params, bool replace)
1450 {
1451  impl_->setPlannerParams(planner_id, group, params, replace);
1452 }
1453 
1455 {
1457 }
1458 
1459 void MoveGroupInterface::setPlanningPipelineId(const std::string& pipeline_id)
1460 {
1461  impl_->setPlanningPipelineId(pipeline_id);
1462 }
1463 
1464 const std::string& MoveGroupInterface::getPlanningPipelineId() const
1466  return impl_->getPlanningPipelineId();
1467 }
1468 
1469 std::string MoveGroupInterface::getDefaultPlannerId(const std::string& group) const
1470 {
1471  return impl_->getDefaultPlannerId(group);
1472 }
1473 
1474 void MoveGroupInterface::setPlannerId(const std::string& planner_id)
1475 {
1476  impl_->setPlannerId(planner_id);
1477 }
1479 const std::string& MoveGroupInterface::getPlannerId() const
1480 {
1481  return impl_->getPlannerId();
1482 }
1484 void MoveGroupInterface::setNumPlanningAttempts(unsigned int num_planning_attempts)
1485 {
1486  impl_->setNumPlanningAttempts(num_planning_attempts);
1487 }
1488 
1489 void MoveGroupInterface::setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
1491  impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
1492 }
1493 
1494 void MoveGroupInterface::setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
1496  impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
1497 }
1498 
1501  return impl_->move(false);
1502 }
1503 
1506  return impl_->getMoveGroupClient();
1507 }
1508 
1511  return impl_->move(true);
1512 }
1513 
1515 {
1516  return impl_->execute(plan.trajectory_, false);
1517 }
1518 
1519 MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::RobotTrajectory& trajectory)
1520 {
1521  return impl_->execute(trajectory, false);
1523 
1525 {
1526  return impl_->execute(plan.trajectory_, true);
1528 
1529 MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::RobotTrajectory& trajectory)
1530 {
1531  return impl_->execute(trajectory, true);
1533 
1535 {
1536  return impl_->plan(plan);
1538 
1539 moveit_msgs::PickupGoal MoveGroupInterface::constructPickupGoal(const std::string& object,
1540  std::vector<moveit_msgs::Grasp> grasps,
1541  bool plan_only = false) const
1543  return impl_->constructPickupGoal(object, std::move(grasps), plan_only);
1544 }
1545 
1546 moveit_msgs::PlaceGoal MoveGroupInterface::constructPlaceGoal(const std::string& object,
1547  std::vector<moveit_msgs::PlaceLocation> locations,
1548  bool plan_only = false) const
1549 {
1550  return impl_->constructPlaceGoal(object, std::move(locations), plan_only);
1551 }
1553 std::vector<moveit_msgs::PlaceLocation>
1554 MoveGroupInterface::posesToPlaceLocations(const std::vector<geometry_msgs::PoseStamped>& poses) const
1555 {
1556  return impl_->posesToPlaceLocations(poses);
1558 
1559 MoveItErrorCode MoveGroupInterface::pick(const moveit_msgs::PickupGoal& goal)
1560 {
1561  return impl_->pick(goal);
1563 
1564 MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const std::string& object, bool plan_only)
1565 {
1566  return impl_->planGraspsAndPick(object, plan_only);
1568 
1569 MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only)
1570 {
1571  return impl_->planGraspsAndPick(object, plan_only);
1573 
1574 MoveItErrorCode MoveGroupInterface::place(const moveit_msgs::PlaceGoal& goal)
1575 {
1576  return impl_->place(goal);
1578 
1579 double MoveGroupInterface::computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step,
1580  double jump_threshold, moveit_msgs::RobotTrajectory& trajectory,
1581  bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code)
1583  moveit_msgs::Constraints path_constraints_tmp;
1584  return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions,
1585  error_code);
1586 }
1588 double MoveGroupInterface::computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step,
1589  double jump_threshold, moveit_msgs::RobotTrajectory& trajectory,
1590  const moveit_msgs::Constraints& path_constraints, bool avoid_collisions,
1591  moveit_msgs::MoveItErrorCodes* error_code)
1593  if (error_code)
1594  {
1595  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1596  avoid_collisions, *error_code);
1597  }
1598  else
1599  {
1600  moveit_msgs::MoveItErrorCodes error_code_tmp;
1601  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1602  avoid_collisions, error_code_tmp);
1603  }
1604 }
1605 
1608  impl_->stop();
1609 }
1610 
1611 void MoveGroupInterface::setStartState(const moveit_msgs::RobotState& start_state)
1612 {
1613  moveit::core::RobotStatePtr rs;
1615  moveit::core::robotStateMsgToRobotState(start_state, *rs);
1616  setStartState(*rs);
1617 }
1618 
1620 {
1621  impl_->setStartState(start_state);
1623 
1625 {
1628 
1630 {
1633 }
1634 
1635 const std::vector<std::string>& MoveGroupInterface::getJointNames() const
1636 {
1638 }
1639 
1640 const std::vector<std::string>& MoveGroupInterface::getLinkNames() const
1641 {
1643 }
1644 
1645 std::map<std::string, double> MoveGroupInterface::getNamedTargetValues(const std::string& name) const
1646 {
1647  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1648  std::map<std::string, double> positions;
1649 
1650  if (it != remembered_joint_values_.cend())
1651  {
1652  std::vector<std::string> names = impl_->getJointModelGroup()->getVariableNames();
1653  for (size_t x = 0; x < names.size(); ++x)
1654  {
1655  positions[names[x]] = it->second[x];
1656  }
1657  }
1658  else
1659  {
1661  }
1662  return positions;
1663 }
1664 
1665 bool MoveGroupInterface::setNamedTarget(const std::string& name)
1666 {
1667  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1668  if (it != remembered_joint_values_.end())
1669  {
1670  return setJointValueTarget(it->second);
1671  }
1672  else
1673  {
1675  {
1676  impl_->setTargetType(JOINT);
1677  return true;
1678  }
1679  ROS_ERROR_NAMED(LOGNAME, "The requested named target '%s' does not exist", name.c_str());
1680  return false;
1681  }
1682 }
1683 
1684 void MoveGroupInterface::getJointValueTarget(std::vector<double>& group_variable_values) const
1685 {
1688 
1689 bool MoveGroupInterface::setJointValueTarget(const std::vector<double>& joint_values)
1690 {
1691  auto const n_group_joints = impl_->getJointModelGroup()->getVariableCount();
1692  if (joint_values.size() != n_group_joints)
1693  {
1694  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Provided joint value list has length " << joint_values.size() << " but group "
1696  << " has " << n_group_joints << " joints");
1697  return false;
1698  }
1699  impl_->setTargetType(JOINT);
1702 }
1704 bool MoveGroupInterface::setJointValueTarget(const std::map<std::string, double>& variable_values)
1705 {
1706  const auto& allowed = impl_->getJointModelGroup()->getVariableNames();
1707  for (const auto& pair : variable_values)
1708  {
1709  if (std::find(allowed.begin(), allowed.end(), pair.first) == allowed.end())
1710  {
1711  ROS_ERROR_STREAM("joint variable " << pair.first << " is not part of group "
1712  << impl_->getJointModelGroup()->getName());
1713  return false;
1714  }
1715  }
1716 
1717  impl_->setTargetType(JOINT);
1718  impl_->getTargetRobotState().setVariablePositions(variable_values);
1720 }
1721 
1722 bool MoveGroupInterface::setJointValueTarget(const std::vector<std::string>& variable_names,
1723  const std::vector<double>& variable_values)
1724 {
1725  const auto& allowed = impl_->getJointModelGroup()->getVariableNames();
1726  for (const auto& variable_name : variable_names)
1727  {
1728  if (std::find(allowed.begin(), allowed.end(), variable_name) == allowed.end())
1729  {
1730  ROS_ERROR_STREAM("joint variable " << variable_name << " is not part of group "
1731  << impl_->getJointModelGroup()->getName());
1732  return false;
1733  }
1734  }
1735 
1736  impl_->setTargetType(JOINT);
1737  impl_->getTargetRobotState().setVariablePositions(variable_names, variable_values);
1739 }
1740 
1742 {
1743  impl_->setTargetType(JOINT);
1744  impl_->getTargetRobotState() = rstate;
1746 }
1747 
1748 bool MoveGroupInterface::setJointValueTarget(const std::string& joint_name, double value)
1749 {
1750  std::vector<double> values(1, value);
1751  return setJointValueTarget(joint_name, values);
1753 
1754 bool MoveGroupInterface::setJointValueTarget(const std::string& joint_name, const std::vector<double>& values)
1755 {
1756  impl_->setTargetType(JOINT);
1758  if (jm && jm->getVariableCount() == values.size())
1759  {
1762  }
1763 
1764  ROS_ERROR_STREAM("joint " << joint_name << " is not part of group " << impl_->getJointModelGroup()->getName());
1765  return false;
1766 }
1767 
1768 bool MoveGroupInterface::setJointValueTarget(const sensor_msgs::JointState& state)
1769 {
1770  return setJointValueTarget(state.name, state.position);
1771 }
1773 bool MoveGroupInterface::setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link)
1774 {
1775  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false);
1776 }
1777 
1778 bool MoveGroupInterface::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
1779  const std::string& end_effector_link)
1780 {
1781  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false);
1782 }
1783 
1784 bool MoveGroupInterface::setJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link)
1785 {
1786  geometry_msgs::Pose msg = tf2::toMsg(eef_pose);
1787  return setJointValueTarget(msg, end_effector_link);
1788 }
1789 
1790 bool MoveGroupInterface::setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose,
1791  const std::string& end_effector_link)
1792 {
1793  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true);
1794 }
1795 
1796 bool MoveGroupInterface::setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
1797  const std::string& end_effector_link)
1798 {
1799  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true);
1800 }
1801 
1802 bool MoveGroupInterface::setApproximateJointValueTarget(const Eigen::Isometry3d& eef_pose,
1803  const std::string& end_effector_link)
1804 {
1805  geometry_msgs::Pose msg = tf2::toMsg(eef_pose);
1806  return setApproximateJointValueTarget(msg, end_effector_link);
1807 }
1808 
1810 {
1811  return impl_->getTargetRobotState();
1812 }
1813 
1815 {
1817 }
1818 
1819 const std::string& MoveGroupInterface::getEndEffectorLink() const
1820 {
1821  return impl_->getEndEffectorLink();
1823 
1824 const std::string& MoveGroupInterface::getEndEffector() const
1825 {
1826  return impl_->getEndEffector();
1827 }
1828 
1829 bool MoveGroupInterface::setEndEffectorLink(const std::string& link_name)
1830 {
1831  if (impl_->getEndEffectorLink().empty() || link_name.empty())
1832  return false;
1833  impl_->setEndEffectorLink(link_name);
1834  impl_->setTargetType(POSE);
1835  return true;
1837 
1838 bool MoveGroupInterface::setEndEffector(const std::string& eef_name)
1839 {
1840  const moveit::core::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name);
1841  if (jmg)
1842  return setEndEffectorLink(jmg->getEndEffectorParentGroup().second);
1843  return false;
1844 }
1845 
1846 void MoveGroupInterface::clearPoseTarget(const std::string& end_effector_link)
1847 {
1848  impl_->clearPoseTarget(end_effector_link);
1849 }
1850 
1854 }
1855 
1856 bool MoveGroupInterface::setPoseTarget(const Eigen::Isometry3d& pose, const std::string& end_effector_link)
1857 {
1858  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1859  pose_msg[0].pose = tf2::toMsg(pose);
1860  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1861  pose_msg[0].header.stamp = ros::Time::now();
1862  return setPoseTargets(pose_msg, end_effector_link);
1863 }
1865 bool MoveGroupInterface::setPoseTarget(const geometry_msgs::Pose& target, const std::string& end_effector_link)
1866 {
1867  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1868  pose_msg[0].pose = target;
1869  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1870  pose_msg[0].header.stamp = ros::Time::now();
1871  return setPoseTargets(pose_msg, end_effector_link);
1872 }
1873 
1874 bool MoveGroupInterface::setPoseTarget(const geometry_msgs::PoseStamped& target, const std::string& end_effector_link)
1875 {
1876  std::vector<geometry_msgs::PoseStamped> targets(1, target);
1877  return setPoseTargets(targets, end_effector_link);
1878 }
1879 
1880 bool MoveGroupInterface::setPoseTargets(const EigenSTL::vector_Isometry3d& target, const std::string& end_effector_link)
1881 {
1882  std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
1883  ros::Time tm = ros::Time::now();
1884  const std::string& frame_id = getPoseReferenceFrame();
1885  for (std::size_t i = 0; i < target.size(); ++i)
1886  {
1887  pose_out[i].pose = tf2::toMsg(target[i]);
1888  pose_out[i].header.stamp = tm;
1889  pose_out[i].header.frame_id = frame_id;
1890  }
1891  return setPoseTargets(pose_out, end_effector_link);
1893 
1894 bool MoveGroupInterface::setPoseTargets(const std::vector<geometry_msgs::Pose>& target,
1895  const std::string& end_effector_link)
1896 {
1897  std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
1898  ros::Time tm = ros::Time::now();
1899  const std::string& frame_id = getPoseReferenceFrame();
1900  for (std::size_t i = 0; i < target.size(); ++i)
1901  {
1902  target_stamped[i].pose = target[i];
1903  target_stamped[i].header.stamp = tm;
1904  target_stamped[i].header.frame_id = frame_id;
1905  }
1906  return setPoseTargets(target_stamped, end_effector_link);
1907 }
1908 
1909 bool MoveGroupInterface::setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& target,
1910  const std::string& end_effector_link)
1911 {
1912  if (target.empty())
1913  {
1914  ROS_ERROR_NAMED(LOGNAME, "No pose specified as goal target");
1915  return false;
1916  }
1917  else
1918  {
1920  return impl_->setPoseTargets(target, end_effector_link);
1921  }
1922 }
1923 
1924 const geometry_msgs::PoseStamped& MoveGroupInterface::getPoseTarget(const std::string& end_effector_link) const
1925 {
1926  return impl_->getPoseTarget(end_effector_link);
1927 }
1928 
1929 const std::vector<geometry_msgs::PoseStamped>&
1930 MoveGroupInterface::getPoseTargets(const std::string& end_effector_link) const
1931 {
1932  return impl_->getPoseTargets(end_effector_link);
1934 
1935 namespace
1936 {
1937 inline void transformPose(const tf2_ros::Buffer& tf_buffer, const std::string& desired_frame,
1938  geometry_msgs::PoseStamped& target)
1939 {
1940  if (desired_frame != target.header.frame_id)
1941  {
1942  geometry_msgs::PoseStamped target_in(target);
1943  tf_buffer.transform(target_in, target, desired_frame);
1944  // we leave the stamp to ros::Time(0) on purpose
1945  target.header.stamp = ros::Time(0);
1946  }
1947 }
1948 } // namespace
1949 
1950 bool MoveGroupInterface::setPositionTarget(double x, double y, double z, const std::string& end_effector_link)
1951 {
1952  geometry_msgs::PoseStamped target;
1953  if (impl_->hasPoseTarget(end_effector_link))
1954  {
1955  target = getPoseTarget(end_effector_link);
1956  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1957  }
1958  else
1959  {
1960  target.pose.orientation.x = 0.0;
1961  target.pose.orientation.y = 0.0;
1962  target.pose.orientation.z = 0.0;
1963  target.pose.orientation.w = 1.0;
1964  target.header.frame_id = impl_->getPoseReferenceFrame();
1965  }
1966 
1967  target.pose.position.x = x;
1968  target.pose.position.y = y;
1969  target.pose.position.z = z;
1970  bool result = setPoseTarget(target, end_effector_link);
1971  impl_->setTargetType(POSITION);
1972  return result;
1973 }
1974 
1975 bool MoveGroupInterface::setRPYTarget(double r, double p, double y, const std::string& end_effector_link)
1976 {
1977  geometry_msgs::PoseStamped target;
1978  if (impl_->hasPoseTarget(end_effector_link))
1979  {
1980  target = getPoseTarget(end_effector_link);
1981  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1982  }
1983  else
1984  {
1985  target.pose.position.x = 0.0;
1986  target.pose.position.y = 0.0;
1987  target.pose.position.z = 0.0;
1988  target.header.frame_id = impl_->getPoseReferenceFrame();
1989  }
1991  q.setRPY(r, p, y);
1992  target.pose.orientation = tf2::toMsg(q);
1993  bool result = setPoseTarget(target, end_effector_link);
1994  impl_->setTargetType(ORIENTATION);
1995  return result;
1996 }
1997 
1998 bool MoveGroupInterface::setOrientationTarget(double x, double y, double z, double w,
1999  const std::string& end_effector_link)
2000 {
2001  geometry_msgs::PoseStamped target;
2002  if (impl_->hasPoseTarget(end_effector_link))
2003  {
2004  target = getPoseTarget(end_effector_link);
2005  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
2006  }
2007  else
2008  {
2009  target.pose.position.x = 0.0;
2010  target.pose.position.y = 0.0;
2011  target.pose.position.z = 0.0;
2012  target.header.frame_id = impl_->getPoseReferenceFrame();
2013  }
2014 
2015  target.pose.orientation.x = x;
2016  target.pose.orientation.y = y;
2017  target.pose.orientation.z = z;
2018  target.pose.orientation.w = w;
2019  bool result = setPoseTarget(target, end_effector_link);
2020  impl_->setTargetType(ORIENTATION);
2021  return result;
2022 }
2023 
2024 void MoveGroupInterface::setPoseReferenceFrame(const std::string& pose_reference_frame)
2025 {
2026  impl_->setPoseReferenceFrame(pose_reference_frame);
2027 }
2028 
2029 const std::string& MoveGroupInterface::getPoseReferenceFrame() const
2030 {
2031  return impl_->getPoseReferenceFrame();
2032 }
2033 
2035 {
2036  return impl_->getGoalJointTolerance();
2037 }
2038 
2040 {
2041  return impl_->getGoalPositionTolerance();
2042 }
2045 {
2047 }
2048 
2049 void MoveGroupInterface::setGoalTolerance(double tolerance)
2050 {
2051  setGoalJointTolerance(tolerance);
2052  setGoalPositionTolerance(tolerance);
2053  setGoalOrientationTolerance(tolerance);
2054 }
2055 
2056 void MoveGroupInterface::setGoalJointTolerance(double tolerance)
2057 {
2058  impl_->setGoalJointTolerance(tolerance);
2059 }
2060 
2061 void MoveGroupInterface::setGoalPositionTolerance(double tolerance)
2062 {
2063  impl_->setGoalPositionTolerance(tolerance);
2064 }
2065 
2067 {
2069 }
2070 
2071 void MoveGroupInterface::rememberJointValues(const std::string& name)
2072 {
2074 }
2075 
2076 bool MoveGroupInterface::startStateMonitor(double wait)
2077 {
2078  return impl_->startStateMonitor(wait);
2079 }
2080 
2081 std::vector<double> MoveGroupInterface::getCurrentJointValues() const
2082 {
2083  moveit::core::RobotStatePtr current_state;
2084  std::vector<double> values;
2085  if (impl_->getCurrentState(current_state))
2086  current_state->copyJointGroupPositions(getName(), values);
2087  return values;
2088 }
2089 
2090 std::vector<double> MoveGroupInterface::getRandomJointValues() const
2091 {
2092  std::vector<double> r;
2094  return r;
2095 }
2096 
2097 geometry_msgs::PoseStamped MoveGroupInterface::getRandomPose(const std::string& end_effector_link) const
2098 {
2099  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2100  Eigen::Isometry3d pose;
2101  pose.setIdentity();
2102  if (eef.empty())
2103  ROS_ERROR_NAMED(LOGNAME, "No end-effector specified");
2104  else
2105  {
2106  moveit::core::RobotStatePtr current_state;
2107  if (impl_->getCurrentState(current_state))
2108  {
2109  current_state->setToRandomPositions(impl_->getJointModelGroup());
2110  const moveit::core::LinkModel* lm = current_state->getLinkModel(eef);
2111  if (lm)
2112  pose = current_state->getGlobalLinkTransform(lm);
2113  }
2114  }
2115  geometry_msgs::PoseStamped pose_msg;
2116  pose_msg.header.stamp = ros::Time::now();
2117  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2118  pose_msg.pose = tf2::toMsg(pose);
2119  return pose_msg;
2120 }
2121 
2122 geometry_msgs::PoseStamped MoveGroupInterface::getCurrentPose(const std::string& end_effector_link) const
2123 {
2124  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2125  Eigen::Isometry3d pose;
2126  pose.setIdentity();
2127  if (eef.empty())
2128  ROS_ERROR_NAMED(LOGNAME, "No end-effector specified");
2129  else
2130  {
2131  moveit::core::RobotStatePtr current_state;
2132  if (impl_->getCurrentState(current_state))
2133  {
2134  const moveit::core::LinkModel* lm = current_state->getLinkModel(eef);
2135  if (lm)
2136  pose = current_state->getGlobalLinkTransform(lm);
2137  }
2138  }
2139  geometry_msgs::PoseStamped pose_msg;
2140  pose_msg.header.stamp = ros::Time::now();
2141  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2142  pose_msg.pose = tf2::toMsg(pose);
2143  return pose_msg;
2145 
2146 std::vector<double> MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link) const
2147 {
2148  std::vector<double> result;
2149  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2150  if (eef.empty())
2151  ROS_ERROR_NAMED(LOGNAME, "No end-effector specified");
2152  else
2153  {
2154  moveit::core::RobotStatePtr current_state;
2155  if (impl_->getCurrentState(current_state))
2156  {
2157  const moveit::core::LinkModel* lm = current_state->getLinkModel(eef);
2158  if (lm)
2159  {
2160  result.resize(3);
2161  geometry_msgs::TransformStamped tfs = tf2::eigenToTransform(current_state->getGlobalLinkTransform(lm));
2162  double pitch, roll, yaw;
2163  tf2::getEulerYPR<geometry_msgs::Quaternion>(tfs.transform.rotation, yaw, pitch, roll);
2164  result[0] = roll;
2165  result[1] = pitch;
2166  result[2] = yaw;
2167  }
2168  }
2169  }
2170  return result;
2171 }
2172 
2173 const std::vector<std::string>& MoveGroupInterface::getActiveJoints() const
2174 {
2176 }
2177 
2178 const std::vector<std::string>& MoveGroupInterface::getJoints() const
2179 {
2181 }
2182 
2183 unsigned int MoveGroupInterface::getVariableCount() const
2184 {
2186 }
2187 
2188 moveit::core::RobotStatePtr MoveGroupInterface::getCurrentState(double wait) const
2189 {
2190  moveit::core::RobotStatePtr current_state;
2191  impl_->getCurrentState(current_state, wait);
2192  return current_state;
2193 }
2194 
2195 void MoveGroupInterface::rememberJointValues(const std::string& name, const std::vector<double>& values)
2196 {
2198 }
2199 
2200 void MoveGroupInterface::forgetJointValues(const std::string& name)
2201 {
2202  remembered_joint_values_.erase(name);
2203 }
2204 
2205 void MoveGroupInterface::allowLooking(bool flag)
2206 {
2207  impl_->allowLooking(flag);
2208 }
2209 
2211 {
2212  impl_->allowReplanning(flag);
2213 }
2215 std::vector<std::string> MoveGroupInterface::getKnownConstraints() const
2216 {
2217  return impl_->getKnownConstraints();
2218 }
2219 
2220 moveit_msgs::Constraints MoveGroupInterface::getPathConstraints() const
2221 {
2222  return impl_->getPathConstraints();
2223 }
2224 
2225 bool MoveGroupInterface::setPathConstraints(const std::string& constraint)
2226 {
2227  return impl_->setPathConstraints(constraint);
2228 }
2229 
2230 void MoveGroupInterface::setPathConstraints(const moveit_msgs::Constraints& constraint)
2231 {
2232  impl_->setPathConstraints(constraint);
2233 }
2234 
2236 {
2238 }
2239 
2240 moveit_msgs::TrajectoryConstraints MoveGroupInterface::getTrajectoryConstraints() const
2242  return impl_->getTrajectoryConstraints();
2243 }
2244 
2245 void MoveGroupInterface::setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint)
2247  impl_->setTrajectoryConstraints(constraint);
2248 }
2249 
2253 }
2254 
2255 void MoveGroupInterface::setConstraintsDatabase(const std::string& host, unsigned int port)
2257  impl_->initializeConstraintsStorage(host, port);
2258 }
2259 
2260 void MoveGroupInterface::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
2261 {
2262  impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz);
2264 
2266 void MoveGroupInterface::setPlanningTime(double seconds)
2267 {
2269 }
2270 
2274  return impl_->getPlanningTime();
2275 }
2276 
2277 void MoveGroupInterface::setSupportSurfaceName(const std::string& name)
2280 }
2281 
2282 const std::string& MoveGroupInterface::getPlanningFrame() const
2284  return impl_->getRobotModel()->getModelFrame();
2285 }
2286 
2287 const std::vector<std::string>& MoveGroupInterface::getJointModelGroupNames() const
2289  return impl_->getRobotModel()->getJointModelGroupNames();
2290 }
2291 
2292 bool MoveGroupInterface::attachObject(const std::string& object, const std::string& link)
2294  return attachObject(object, link, std::vector<std::string>());
2295 }
2296 
2297 bool MoveGroupInterface::attachObject(const std::string& object, const std::string& link,
2298  const std::vector<std::string>& touch_links)
2299 {
2300  return impl_->attachObject(object, link, touch_links);
2301 }
2302 
2303 bool MoveGroupInterface::detachObject(const std::string& name)
2304 {
2305  return impl_->detachObject(name);
2306 }
2307 
2308 void MoveGroupInterface::constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& goal_out)
2309 {
2310  impl_->constructMotionPlanRequest(goal_out);
2311 }
2312 
2313 } // namespace planning_interface
2314 } // namespace moveit
moveit::core::JointModelGroup::hasLinkModel
bool hasLinkModel(const std::string &link) const
moveit::core::LinkModel
response
const std::string response
moveit::planning_interface::MoveGroupInterface::place
MoveItErrorCode place(const std::string &object, bool plan_only=false)
Place an object somewhere safe in the world (a safe location will be detected)
Definition: move_group_interface.h:918
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setJointValueTarget
bool setJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link, const std::string &frame, bool approx)
Definition: move_group_interface.cpp:516
moveit::planning_interface::MoveGroupInterface::getEndEffector
const std::string & getEndEffector() const
Get the current end-effector name. This returns the value set by setEndEffector() (or indirectly by s...
Definition: move_group_interface.cpp:1892
moveit::planning_interface::MoveGroupInterface::setStartState
void setStartState(const moveit_msgs::RobotState &start_state)
If a different start state should be considered instead of the current state of the robot,...
Definition: move_group_interface.cpp:1679
moveit::planning_interface::MoveGroupInterface::getPlannerParams
std::map< std::string, std::string > getPlannerParams(const std::string &planner_id, const std::string &group="") const
Get the planner parameters for given group and planner_id.
Definition: move_group_interface.cpp:1510
moveit::core::GroupStateValidityCallbackFn
boost::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
moveit::planning_interface::MoveGroupInterface::plan
MoveItErrorCode plan(Plan &plan)
Compute a motion plan that takes the group declared in the constructor from the current state to the ...
Definition: move_group_interface.cpp:1602
moveit::planning_interface::MoveGroupInterface::setPoseTarget
bool setPoseTarget(const Eigen::Isometry3d &end_effector_pose, const std::string &end_effector_link="")
Set the goal pose of the end-effector end_effector_link.
Definition: move_group_interface.cpp:1924
ros::WallDuration::sleep
bool sleep() const
moveit::core::JointModelGroup::getJointModel
const JointModel * getJointModel(const std::string &joint) const
moveit::planning_interface::MoveGroupInterface::setJointValueTarget
bool setJointValueTarget(const std::vector< double > &group_variable_values)
Set the JointValueTarget and use it for future planning requests.
Definition: move_group_interface.cpp:1757
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::max_acceleration_scaling_factor_
double max_acceleration_scaling_factor_
Definition: move_group_interface.cpp:1390
moveit::core::RobotState::setJointPositions
void setJointPositions(const std::string &joint_name, const double *position)
moveit::planning_interface::MoveGroupInterface::getKnownConstraints
std::vector< std::string > getKnownConstraints() const
Get the names of the known constraints as read from the Mongo database, if a connection was achieved.
Definition: move_group_interface.cpp:2283
moveit::planning_interface::MoveGroupInterface::getDefaultPlanningPipelineId
std::string getDefaultPlanningPipelineId() const
Definition: move_group_interface.cpp:1522
moveit::planning_interface::MoveGroupInterface::getTrajectoryConstraints
moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
Definition: move_group_interface.cpp:2308
moveit::core::RobotState::setVariablePositions
void setVariablePositions(const double *position)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setPlanningTime
void setPlanningTime(double seconds)
Definition: move_group_interface.cpp:1112
kinematics::KinematicsQueryOptions::return_approximate_solution
bool return_approximate_solution
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getTargetRobotState
moveit::core::RobotState & getTargetRobotState()
Definition: move_group_interface.cpp:484
moveit::planning_interface::MoveGroupInterface::getJoints
const std::vector< std::string > & getJoints() const
Get all the joints this instance operates on (including fixed joints)
Definition: move_group_interface.cpp:2246
ros::Publisher
moveit::planning_interface::MoveGroupInterface::getMoveGroupClient
actionlib::SimpleActionClient< moveit_msgs::MoveGroupAction > & getMoveGroupClient() const
Get the move_group action client used by the MoveGroupInterface. The client can be used for querying ...
Definition: move_group_interface.cpp:1572
moveit::planning_interface::MoveGroupInterface::startStateMonitor
bool startStateMonitor(double wait=1.0)
When reasoning about the current state of a robot, a CurrentStateMonitor instance is automatically co...
Definition: move_group_interface.cpp:2144
moveit::core::JointModel::getVariableCount
std::size_t getVariableCount() const
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::hasPoseTarget
bool hasPoseTarget(const std::string &end_effector_link) const
Definition: move_group_interface.cpp:621
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::computeCartesianPath
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)
Definition: move_group_interface.cpp:982
moveit::planning_interface::MoveGroupInterface::setGoalOrientationTolerance
void setGoalOrientationTolerance(double tolerance)
Set the orientation tolerance that is used for reaching the goal when moving to a pose.
Definition: move_group_interface.cpp:2134
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::waitForAction
void waitForAction(const T &action, const std::string &name, const ros::WallTime &timeout, double allotted_time) const
Definition: move_group_interface.cpp:251
moveit::planning_interface::MoveGroupInterface::setOrientationTarget
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,...
Definition: move_group_interface.cpp:2066
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl
Definition: move_group_interface.cpp:161
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::opt_
Options opt_
Definition: move_group_interface.cpp:1372
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::MoveGroupInterfaceImpl
MoveGroupInterfaceImpl(const Options &opt, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer, const ros::WallDuration &wait_for_servers)
Definition: move_group_interface.cpp:164
boost::shared_ptr
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::startStateMonitor
bool startStateMonitor(double wait)
Definition: move_group_interface.cpp:683
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::clearTrajectoryConstraints
void clearTrajectoryConstraints()
Definition: move_group_interface.cpp:1295
moveit::planning_interface::MoveGroupInterface::setStartStateToCurrentState
void setStartStateToCurrentState()
Set the starting state for planning to be that reported by the robot's joint state publication.
Definition: move_group_interface.cpp:1692
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::get_params_service_
ros::ServiceClient get_params_service_
Definition: move_group_interface.cpp:1418
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getPoseTarget
const geometry_msgs::PoseStamped & getPoseTarget(const std::string &end_effector_link) const
Definition: move_group_interface.cpp:627
moveit::planning_interface::MoveGroupInterface::getCurrentState
moveit::core::RobotStatePtr getCurrentState(double wait=1) const
Get the current state of the robot within the duration specified by wait.
Definition: move_group_interface.cpp:2256
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getPlanningPipelineId
const std::string & getPlanningPipelineId() const
Definition: move_group_interface.cpp:412
moveit::core::JointModelGroup::getActiveJointModelNames
const std::vector< std::string > & getActiveJointModelNames() const
moveit::planning_interface::MoveGroupInterface
Client class to conveniently use the ROS interfaces provided by the move_group node.
Definition: move_group_interface.h:165
ros::ServiceClient::exists
bool exists()
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getTF
const std::shared_ptr< tf2_ros::Buffer > & getTF() const
Definition: move_group_interface.cpp:312
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::planGraspsAndPick
MoveItErrorCode planGraspsAndPick(const std::string &object, bool plan_only=false)
Definition: move_group_interface.cpp:808
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getGoalOrientationTolerance
double getGoalOrientationTolerance() const
Definition: move_group_interface.cpp:1087
tf2_eigen.h
tf2::fromMsg
void fromMsg(const A &, B &b)
moveit::planning_interface::MoveGroupInterface::allowReplanning
void allowReplanning(bool flag)
Specify whether the robot is allowed to replan if it detects changes in the environment.
Definition: move_group_interface.cpp:2278
utils.h
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
s
XmlRpcServer s
ros
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getGoalJointTolerance
double getGoalJointTolerance() const
Definition: move_group_interface.cpp:1092
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
moveit::planning_interface::MoveGroupInterface::getVariableCount
unsigned int getVariableCount() const
Get the number of variables used to describe the state of this group. This is larger or equal to the ...
Definition: move_group_interface.cpp:2251
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setReplanningDelay
void setReplanningDelay(double delay)
Definition: move_group_interface.cpp:1135
ros.h
moveit::planning_interface::MoveGroupInterface::ROBOT_DESCRIPTION
static const std::string ROBOT_DESCRIPTION
Default ROS parameter name from where to read the robot's URDF. Set to 'robot_description'.
Definition: move_group_interface.h:169
moveit::planning_interface::GRASP_PLANNING_SERVICE_NAME
const std::string GRASP_PLANNING_SERVICE_NAME
Definition: move_group_interface.cpp:146
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::workspace_parameters_
moveit_msgs::WorkspaceParameters workspace_parameters_
Definition: move_group_interface.cpp:1384
moveit::planning_interface::MoveGroupInterface::move
MoveItErrorCode move()
Plan and execute a trajectory that takes the group of joints declared in the constructor to the speci...
Definition: move_group_interface.cpp:1577
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getPathConstraints
moveit_msgs::Constraints getPathConstraints() const
Definition: move_group_interface.cpp:1315
moveit::planning_interface::LOGNAME
const std::string LOGNAME
Definition: move_group_interface.cpp:148
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getPoseTargets
const std::vector< geometry_msgs::PoseStamped > & getPoseTargets(const std::string &end_effector_link) const
Definition: move_group_interface.cpp:643
moveit::planning_interface::MoveGroupInterface::asyncExecute
MoveItErrorCode asyncExecute(const Plan &plan)
Given a plan, execute it without waiting for completion.
Definition: move_group_interface.cpp:1582
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setPlanningPipelineId
void setPlanningPipelineId(const std::string &pipeline_id)
Definition: move_group_interface.cpp:401
constraints_storage.h
moveit::planning_interface::MoveGroupInterface::forgetJointValues
void forgetJointValues(const std::string &name)
Forget the joint values remembered under name.
Definition: move_group_interface.cpp:2268
r
r
moveit::planning_interface::MoveGroupInterface::getPlanningFrame
const std::string & getPlanningFrame() const
Get the name of the frame in which the robot is planning.
Definition: move_group_interface.cpp:2350
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::clearPathConstraints
void clearPathConstraints()
Definition: move_group_interface.cpp:1285
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: move_group_interface.cpp:1375
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::stop
void stop()
Definition: move_group_interface.cpp:1022
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::plan_grasps_service_
ros::ServiceClient plan_grasps_service_
Definition: move_group_interface.cpp:1421
ros::NodeHandle::serviceClient
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
moveit::planning_interface::MoveGroupInterface::setPlannerId
void setPlannerId(const std::string &planner_id)
Specify a planner to be used for further planning.
Definition: move_group_interface.cpp:1542
moveit::planning_interface::MoveGroupInterface::setGoalJointTolerance
void setGoalJointTolerance(double tolerance)
Set the joint tolerance (for each joint) that is used for reaching the goal when moving to a joint va...
Definition: move_group_interface.cpp:2124
moveit::planning_interface::MoveGroupInterface::getCurrentRPY
std::vector< double > getCurrentRPY(const std::string &end_effector_link="") const
Get the roll-pitch-yaw (XYZ) for the end-effector end_effector_link. If end_effector_link is empty (t...
Definition: move_group_interface.cpp:2214
moveit::planning_interface::MoveGroupInterface::getJointModelGroupNames
const std::vector< std::string > & getJointModelGroupNames() const
Get the available planning group names.
Definition: move_group_interface.cpp:2355
kinematic_constraints::constructGoalConstraints
moveit_msgs::Constraints constructGoalConstraints(const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance=std::numeric_limits< double >::epsilon())
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::joint_model_group_
const moveit::core::JointModelGroup * joint_model_group_
Definition: move_group_interface.cpp:1400
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getOptions
const Options & getOptions() const
Definition: move_group_interface.cpp:317
moveit::planning_interface::PlanningSceneInterface
Definition: planning_scene_interface.h:116
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::can_look_
bool can_look_
Definition: move_group_interface.cpp:1394
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setPathConstraints
void setPathConstraints(const moveit_msgs::Constraints &constraint)
Definition: move_group_interface.cpp:1263
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::move
MoveItErrorCode move(bool wait)
Definition: move_group_interface.cpp:898
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::cartesian_path_service_
ros::ServiceClient cartesian_path_service_
Definition: move_group_interface.cpp:1420
ROS_WARN_ONCE_NAMED
#define ROS_WARN_ONCE_NAMED(name,...)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getGoalPositionTolerance
double getGoalPositionTolerance() const
Definition: move_group_interface.cpp:1082
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
moveit::core::RobotState::copyJointGroupPositions
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
moveit::planning_interface::MoveGroupInterface::setRandomTarget
void setRandomTarget()
Set the joint state goal to a random joint configuration.
Definition: move_group_interface.cpp:1697
moveit::core::RobotState::setFromIK
bool setFromIK(const JointModelGroup *group, const geometry_msgs::Pose &pose, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
moveit::core::RobotState::setToRandomPositions
void setToRandomPositions()
step
unsigned int step
moveit::planning_interface::MoveGroupInterface::getCurrentJointValues
std::vector< double > getCurrentJointValues() const
Get the current joint values for the joints planned for by this instance (see getJoints())
Definition: move_group_interface.cpp:2149
moveit::planning_interface::MoveGroupInterface::setPlanningTime
void setPlanningTime(double seconds)
Specify the maximum amount of time to use when planning.
Definition: move_group_interface.cpp:2334
moveit::planning_interface::MoveGroupInterface::clearPathConstraints
void clearPathConstraints()
Specify that no path constraints are to be used. This removes any path constraints set in previous ca...
Definition: move_group_interface.cpp:2303
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::constraints_init_thread_
std::unique_ptr< boost::thread > constraints_init_thread_
Definition: move_group_interface.cpp:1423
moveit_warehouse::ConstraintsStorage
moveit::planning_interface::MoveGroupInterface::planGraspsAndPick
MoveItErrorCode planGraspsAndPick(const std::string &object="", bool plan_only=false)
Pick up an object.
Definition: move_group_interface.cpp:1632
moveit::planning_interface::MoveGroupInterface::getEndEffectorLink
const std::string & getEndEffectorLink() const
Get the current end-effector link. This returns the value set by setEndEffectorLink() (or indirectly ...
Definition: move_group_interface.cpp:1887
moveit::planning_interface::MoveGroupInterface::clearPoseTargets
void clearPoseTargets()
Forget any poses specified for all end-effectors.
Definition: move_group_interface.cpp:1919
moveit::planning_interface::MoveGroupInterface::getPoseTargets
const std::vector< geometry_msgs::PoseStamped > & getPoseTargets(const std::string &end_effector_link="") const
Definition: move_group_interface.cpp:1998
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setEndEffectorLink
void setEndEffectorLink(const std::string &end_effector)
Definition: move_group_interface.cpp:568
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getEndEffector
const std::string & getEndEffector() const
Definition: move_group_interface.cpp:588
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
moveit::planning_interface::MoveGroupInterface::getTargetRobotState
const moveit::core::RobotState & getTargetRobotState() const
Definition: move_group_interface.cpp:1882
moveit::planning_interface::MoveGroupInterface::rememberJointValues
void rememberJointValues(const std::string &name)
Remember the current joint values (of the robot being monitored) under name. These can be used by set...
Definition: move_group_interface.cpp:2139
ros::ok
ROSCPP_DECL bool ok()
moveit::planning_interface::MoveGroupInterface::setPlannerParams
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.
Definition: move_group_interface.cpp:1516
moveit::planning_interface::MoveGroupInterface::Options
Specification of options to use when constructing the MoveGroupInterface class.
Definition: move_group_interface.h:172
moveit::planning_interface::MoveGroupInterface::setGoalPositionTolerance
void setGoalPositionTolerance(double tolerance)
Set the position tolerance that is used for reaching the goal when moving to a pose.
Definition: move_group_interface.cpp:2129
moveit::planning_interface::MoveGroupInterface::remembered_joint_values_
std::map< std::string, std::vector< double > > remembered_joint_values_
Definition: move_group_interface.h:1083
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::trajectory_event_publisher_
ros::Publisher trajectory_event_publisher_
Definition: move_group_interface.cpp:1415
tf2::eigenToTransform
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setMaxScalingFactor
void setMaxScalingFactor(double &variable, const double target_value, const char *factor_name, double fallback_value)
Definition: move_group_interface.cpp:462
moveit::planning_interface::getSharedTF
std::shared_ptr< tf2_ros::Buffer > getSharedTF()
Definition: common_objects.cpp:99
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::pick_action_client_
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::PickupAction > > pick_action_client_
Definition: move_group_interface.cpp:1379
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::~MoveGroupInterfaceImpl
~MoveGroupInterfaceImpl()
Definition: move_group_interface.cpp:306
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setGoalPositionTolerance
void setGoalPositionTolerance(double tolerance)
Definition: move_group_interface.cpp:1102
moveit::planning_interface::MoveGroupInterface::computeCartesianPath
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=nullptr)
Compute a Cartesian path that follows specified waypoints with a step size of at most eef_step meters...
Definition: move_group_interface.cpp:1647
moveit::planning_interface::MoveGroupInterface::allowLooking
void allowLooking(bool flag)
Specify whether the robot is allowed to look around before moving if it determines it should (default...
Definition: move_group_interface.cpp:2273
moveit::planning_interface::MoveGroupInterface::getDefaultPlannerId
std::string getDefaultPlannerId(const std::string &group="") const
Get the default planner of the current planning pipeline for the given group (or the pipeline's defau...
Definition: move_group_interface.cpp:1537
move_group::QUERY_PLANNERS_SERVICE_NAME
static const std::string QUERY_PLANNERS_SERVICE_NAME
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getTrajectoryConstraints
moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
Definition: move_group_interface.cpp:1323
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::execute
MoveItErrorCode execute(const moveit_msgs::RobotTrajectory &trajectory, bool wait)
Definition: move_group_interface.cpp:943
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getReplanningDelay
double getReplanningDelay() const
Definition: move_group_interface.cpp:1141
moveit::core::JointModelGroup::getVariableNames
const std::vector< std::string > & getVariableNames() const
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setTrajectoryConstraints
void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &constraint)
Definition: move_group_interface.cpp:1290
moveit::core::RobotState::getRandomNumberGenerator
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
actionlib::SimpleActionClient
moveit::core::JointModelGroup::getName
const std::string & getName() const
move_group::MOVE_ACTION
static const std::string MOVE_ACTION
console.h
moveit::planning_interface::MoveGroupInterface::getPoseReferenceFrame
const std::string & getPoseReferenceFrame() const
Get the reference frame set by setPoseReferenceFrame(). By default this is the reference frame of the...
Definition: move_group_interface.cpp:2097
moveit::planning_interface::MoveGroupInterface::setConstraintsDatabase
void setConstraintsDatabase(const std::string &host, unsigned int port)
Specify where the database server that holds known constraints resides.
Definition: move_group_interface.cpp:2323
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::pose_targets_
std::map< std::string, std::vector< geometry_msgs::PoseStamped > > pose_targets_
Definition: move_group_interface.cpp:1404
moveit::core::JointModelGroup::getVariableDefaultPositions
bool getVariableDefaultPositions(const std::string &name, std::map< std::string, double > &values) const
moveit::planning_interface::MoveGroupInterface::setPoseReferenceFrame
void setPoseReferenceFrame(const std::string &pose_reference_frame)
Specify which reference frame to assume for poses specified without a reference frame.
Definition: move_group_interface.cpp:2092
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getDefaultPlannerId
std::string getDefaultPlannerId(const std::string &group) const
Definition: move_group_interface.cpp:417
moveit_warehouse::loadDatabase
warehouse_ros::DatabaseConnection::Ptr loadDatabase()
res
res
move_group::SET_PLANNER_PARAMS_SERVICE_NAME
static const std::string SET_PLANNER_PARAMS_SERVICE_NAME
moveit::planning_interface::MoveGroupInterface::getRobotModel
moveit::core::RobotModelConstPtr getRobotModel() const
Get the RobotModel object.
Definition: move_group_interface.cpp:1490
moveit::planning_interface::MoveGroupInterface::getRandomPose
geometry_msgs::PoseStamped getRandomPose(const std::string &end_effector_link="") const
Get a random reachable pose for the end-effector end_effector_link. If end_effector_link is empty (th...
Definition: move_group_interface.cpp:2165
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::clearPoseTargets
void clearPoseTargets()
Definition: move_group_interface.cpp:578
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::end_effector_link_
std::string end_effector_link_
Definition: move_group_interface.cpp:1410
moveit::planning_interface::MoveGroupInterface::execute
MoveItErrorCode execute(const Plan &plan)
Given a plan, execute it while waiting for completion.
Definition: move_group_interface.cpp:1592
ros::CallbackQueue
moveit::planning_interface::MoveGroupInterface::getNodeHandle
const ros::NodeHandle & getNodeHandle() const
Get the ROS node handle of this instance operates on.
Definition: move_group_interface.cpp:1495
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::considered_start_state_
moveit::core::RobotStatePtr considered_start_state_
Definition: move_group_interface.cpp:1383
ROS_FATAL_STREAM_NAMED
#define ROS_FATAL_STREAM_NAMED(name, args)
moveit::planning_interface::MoveGroupInterface::~MoveGroupInterface
~MoveGroupInterface()
Definition: move_group_interface.cpp:1454
capability_names.h
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getCurrentState
bool getCurrentState(moveit::core::RobotStatePtr &current_state, double wait_seconds=1.0)
Definition: move_group_interface.cpp:699
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::constructGoal
void constructGoal(moveit_msgs::MoveGroupGoal &goal) const
Definition: move_group_interface.cpp:1204
planning_scene_monitor.h
moveit::planning_interface::MoveGroupInterface::setMaxVelocityScalingFactor
void setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0,...
Definition: move_group_interface.cpp:1557
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::num_planning_attempts_
unsigned int num_planning_attempts_
Definition: move_group_interface.cpp:1388
y
double y
ros::WallTime::now
static WallTime now()
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
moveit::planning_interface::MoveGroupInterface::getName
const std::string & getName() const
Get the name of the group this instance operates on.
Definition: move_group_interface.cpp:1478
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::planning_pipeline_id_
std::string planning_pipeline_id_
Definition: move_group_interface.cpp:1386
moveit::core::JointModelGroup::getJointModelNames
const std::vector< std::string > & getJointModelNames() const
trajectory_execution_manager.h
moveit::planning_interface::MoveGroupInterface::setMaxAccelerationScalingFactor
void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0...
Definition: move_group_interface.cpp:1562
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getKnownConstraints
std::vector< std::string > getKnownConstraints() const
Definition: move_group_interface.cpp:1300
moveit::planning_interface::MoveGroupInterface::Options::node_handle_
ros::NodeHandle node_handle_
Definition: move_group_interface.h:189
moveit::core::RobotState::setToDefaultValues
void setToDefaultValues()
kinematic_constraints::mergeConstraints
moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
moveit::planning_interface::MoveGroupInterface::setGoalTolerance
void setGoalTolerance(double tolerance)
Set the tolerance that is used for reaching the goal. For joint state goals, this will be distance fo...
Definition: move_group_interface.cpp:2117
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getJointModelGroup
const moveit::core::JointModelGroup * getJointModelGroup() const
Definition: move_group_interface.cpp:327
moveit::planning_interface::MoveGroupInterface::setApproximateJointValueTarget
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.
Definition: move_group_interface.cpp:1858
moveit::planning_interface::MoveGroupInterface::setEndEffectorLink
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...
Definition: move_group_interface.cpp:1897
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setMaxAccelerationScalingFactor
void setMaxAccelerationScalingFactor(double value)
Definition: move_group_interface.cpp:457
moveit::planning_interface::MoveGroupInterface::setPlanningPipelineId
void setPlanningPipelineId(const std::string &pipeline_id)
Specify a planning pipeline to be used for further planning.
Definition: move_group_interface.cpp:1527
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setSupportSurfaceName
void setSupportSurfaceName(const std::string &support_surface)
Definition: move_group_interface.cpp:663
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::tf_buffer_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
Definition: move_group_interface.cpp:1374
ros::ServiceClient
moveit::planning_interface::MoveGroupInterface::setRPYTarget
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.
Definition: move_group_interface.cpp:2043
setup.d
d
Definition: setup.py:4
moveit::planning_interface::MoveGroupInterface::posesToPlaceLocations
std::vector< moveit_msgs::PlaceLocation > posesToPlaceLocations(const std::vector< geometry_msgs::PoseStamped > &poses) const
Convert a vector of PoseStamped to a vector of PlaceLocation.
Definition: move_group_interface.cpp:1622
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::move_action_client_
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::MoveGroupAction > > move_action_client_
Definition: move_group_interface.cpp:1377
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::path_constraints_
std::unique_ptr< moveit_msgs::Constraints > path_constraints_
Definition: move_group_interface.cpp:1408
ros::NodeHandle::getCallbackQueue
CallbackQueueInterface * getCallbackQueue() const
tf2_ros::Buffer
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::pose_reference_frame_
std::string pose_reference_frame_
Definition: move_group_interface.cpp:1411
moveit::planning_interface::MoveItErrorCode
Definition: move_group_interface.h:129
actionlib::SimpleClientGoalState::SUCCEEDED
SUCCEEDED
moveit::core::JointModelGroup::getVariableRandomPositions
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::trajectory_constraints_
std::unique_ptr< moveit_msgs::TrajectoryConstraints > trajectory_constraints_
Definition: move_group_interface.cpp:1409
moveit::core::JointModelGroup::getDefaultStateNames
const std::vector< std::string > & getDefaultStateNames() const
moveit::planning_interface::MoveGroupInterface::setTrajectoryConstraints
void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &constraint)
Definition: move_group_interface.cpp:2313
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::allowLooking
void allowLooking(bool flag)
Definition: move_group_interface.cpp:1123
moveit::planning_interface::MoveGroupInterface::setPathConstraints
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...
Definition: move_group_interface.cpp:2293
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::place
MoveItErrorCode place(const moveit_msgs::PlaceGoal &goal)
Definition: move_group_interface.cpp:747
moveit::planning_interface::MoveGroupInterface::stop
void stop()
Stop any trajectory execution, if one is active.
Definition: move_group_interface.cpp:1674
trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC
static const std::string EXECUTION_EVENT_TOPIC
ros::WallTime
q
q
moveit::planning_interface::MoveGroupInterface::getPoseTarget
const geometry_msgs::PoseStamped & getPoseTarget(const std::string &end_effector_link="") const
Definition: move_group_interface.cpp:1992
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setPlannerParams
void setPlannerParams(const std::string &planner_id, const std::string &group, const std::map< std::string, std::string > &params, bool replace=false)
Definition: move_group_interface.cpp:378
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::active_target_
ActiveTargetType active_target_
Definition: move_group_interface.cpp:1407
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getStartState
moveit::core::RobotStatePtr getStartState()
Definition: move_group_interface.cpp:504
planning_scene_interface.h
moveit::planning_interface::MoveGroupInterface::getGoalJointTolerance
double getGoalJointTolerance() const
Get the tolerance that is used for reaching a joint goal. This is distance for each joint in configur...
Definition: move_group_interface.cpp:2102
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::joint_state_target_
moveit::core::RobotStatePtr joint_state_target_
Definition: move_group_interface.cpp:1399
moveit::core::Transforms::sameFrame
static bool sameFrame(const std::string &frame1, const std::string &frame2)
ROS_WARN_STREAM_NAMED
#define ROS_WARN_STREAM_NAMED(name, args)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setStartStateToCurrentState
void setStartStateToCurrentState()
Definition: move_group_interface.cpp:499
common_objects.h
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::initializeConstraintsStorage
void initializeConstraintsStorage(const std::string &host, unsigned int port)
Definition: move_group_interface.cpp:1331
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setTargetType
void setTargetType(ActiveTargetType type)
Definition: move_group_interface.cpp:673
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setGoalJointTolerance
void setGoalJointTolerance(double tolerance)
Definition: move_group_interface.cpp:1097
moveit::core::JointModelGroup::getLinkModelNames
const std::vector< std::string > & getLinkModelNames() const
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getDefaultPlanningPipelineId
std::string getDefaultPlanningPipelineId() const
Definition: move_group_interface.cpp:394
moveit::planning_interface::MoveGroupInterface::getRandomJointValues
std::vector< double > getRandomJointValues() const
Get random joint values for the joints planned for by this instance (see getJoints())
Definition: move_group_interface.cpp:2158
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::initializing_constraints_
bool initializing_constraints_
Definition: move_group_interface.cpp:1424
transform_listener.h
moveit::planning_interface::MoveGroupInterface::getPlannerId
const std::string & getPlannerId() const
Get the current planner_id.
Definition: move_group_interface.cpp:1547
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::constraints_storage_
std::unique_ptr< moveit_warehouse::ConstraintsStorage > constraints_storage_
Definition: move_group_interface.cpp:1422
moveit::planning_interface::MoveGroupInterface::getJointValueTarget
const moveit::core::RobotState & getJointValueTarget() const
Get the currently set joint state goal, replaced by private getTargetRobotState()
Definition: move_group_interface.cpp:1877
moveit::planning_interface::getSharedStateMonitor
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const moveit::core::RobotModelConstPtr &robot_model, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer)
getSharedStateMonitor is a simpler version of getSharedStateMonitor(const moveit::core::RobotModelCon...
Definition: common_objects.cpp:134
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getPlannerId
const std::string & getPlannerId() const
Definition: move_group_interface.cpp:442
ros::NodeHandle::ok
bool ok() const
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getTargetType
ActiveTargetType getTargetType() const
Definition: move_group_interface.cpp:678
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::constructMotionPlanRequest
void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest &request) const
Definition: move_group_interface.cpp:1146
moveit::planning_interface::MoveGroupInterface::operator=
MoveGroupInterface & operator=(const MoveGroupInterface &)=delete
moveit::planning_interface::MoveGroupInterface::clearTrajectoryConstraints
void clearTrajectoryConstraints()
Definition: move_group_interface.cpp:2318
moveit::core::RobotState::getFrameTransform
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setMaxVelocityScalingFactor
void setMaxVelocityScalingFactor(double value)
Definition: move_group_interface.cpp:452
moveit::planning_interface::MoveGroupInterface::constructPlaceGoal
moveit_msgs::PlaceGoal constructPlaceGoal(const std::string &object, std::vector< moveit_msgs::PlaceLocation > locations, bool plan_only) const
Build a PlaceGoal for an object named object and store it in goal_out.
Definition: move_group_interface.cpp:1614
moveit::planning_interface::MoveGroupInterface::getPlanningPipelineId
const std::string & getPlanningPipelineId() const
Get the current planning_pipeline_id.
Definition: move_group_interface.cpp:1532
tf2_ros::BufferInterface::transform
B & transform(const A &in, B &out, const std::string &target_frame, const ros::Time &target_time, const std::string &fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getInterfaceDescriptions
bool getInterfaceDescriptions(std::vector< moveit_msgs::PlannerInterfaceDescription > &desc)
Definition: move_group_interface.cpp:350
ros::Time
moveit
values
std::vector< double > values
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::can_replan_
bool can_replan_
Definition: move_group_interface.cpp:1395
moveit::planning_interface::MoveGroupInterface::getNamedTargets
const std::vector< std::string > & getNamedTargets() const
Get the names of the named robot states available as targets, both either remembered states or defaul...
Definition: move_group_interface.cpp:1483
moveit::planning_interface::MoveGroupInterface::getInterfaceDescriptions
bool getInterfaceDescriptions(std::vector< moveit_msgs::PlannerInterfaceDescription > &desc) const
Get the descriptions of all planning plugins loaded by the action server.
Definition: move_group_interface.cpp:1505
moveit::planning_interface::MoveGroupInterface::setSupportSurfaceName
void setSupportSurfaceName(const std::string &name)
For pick/place operations, the name of the support surface is used to specify the fact that attached ...
Definition: move_group_interface.cpp:2345
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::goal_joint_tolerance_
double goal_joint_tolerance_
Definition: move_group_interface.cpp:1391
current_state_monitor.h
kinematics::KinematicsQueryOptions
moveit::planning_interface::PlanningSceneInterface::getObjects
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...
Definition: planning_scene_interface.cpp:376
tf_buffer
tf2_ros::Buffer * tf_buffer
moveit::planning_interface::MoveGroupInterface::clearPoseTarget
void clearPoseTarget(const std::string &end_effector_link="")
Forget pose(s) specified for end_effector_link.
Definition: move_group_interface.cpp:1914
moveit::planning_interface::MoveGroupInterface::setNumPlanningAttempts
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...
Definition: move_group_interface.cpp:1552
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::constructPlaceGoal
moveit_msgs::PlaceGoal constructPlaceGoal(const std::string &object, std::vector< moveit_msgs::PlaceLocation > &&locations, bool plan_only=false) const
Definition: move_group_interface.cpp:1236
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::goal_orientation_tolerance_
double goal_orientation_tolerance_
Definition: move_group_interface.cpp:1393
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getPlannerParams
std::map< std::string, std::string > getPlannerParams(const std::string &planner_id, const std::string &group="")
Definition: move_group_interface.cpp:363
moveit::core::RobotState::satisfiesBounds
bool satisfiesBounds(double margin=0.0) const
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::query_service_
ros::ServiceClient query_service_
Definition: move_group_interface.cpp:1417
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::detachObject
bool detachObject(const std::string &name)
Definition: move_group_interface.cpp:1058
move_group_interface.h
moveit::planning_interface::getSharedRobotModel
moveit::core::RobotModelConstPtr getSharedRobotModel(const std::string &robot_description)
Definition: common_objects.cpp:116
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::replan_delay_
double replan_delay_
Definition: move_group_interface.cpp:1396
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setPlannerId
void setPlannerId(const std::string &planner_id)
Definition: move_group_interface.cpp:437
planning_interface
move_group::EXECUTE_ACTION_NAME
static const std::string EXECUTE_ACTION_NAME
moveit::planning_interface::MoveGroupInterface::setPositionTarget
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).
Definition: move_group_interface.cpp:2018
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::constructPickupGoal
moveit_msgs::PickupGoal constructPickupGoal(const std::string &object, std::vector< moveit_msgs::Grasp > &&grasps, bool plan_only=false) const
Definition: move_group_interface.cpp:1209
approx
bool approx(S x, S y)
moveit::planning_interface::MoveGroupInterface::getInterfaceDescription
bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc) const
Get the description of the default planning plugin loaded by the action server.
Definition: move_group_interface.cpp:1500
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::plan
MoveItErrorCode plan(Plan &plan)
Definition: move_group_interface.cpp:857
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setStartState
void setStartState(const moveit::core::RobotState &start_state)
Definition: move_group_interface.cpp:494
ros::ServiceClient::call
bool call(const MReq &req, MRes &resp, const std::string &service_md5sum)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Definition: move_group_interface.cpp:322
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getInterfaceDescription
bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc)
Definition: move_group_interface.cpp:337
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setPoseTargets
bool setPoseTargets(const std::vector< geometry_msgs::PoseStamped > &poses, const std::string &end_effector_link)
Definition: move_group_interface.cpp:602
moveit::core::JointModelGroup::isChain
bool isChain() const
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
moveit::core::JointModelGroup
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
tf2::Quaternion
tolerance
S tolerance()
ROS_INFO_STREAM_NAMED
#define ROS_INFO_STREAM_NAMED(name, args)
moveit::planning_interface::MoveGroupInterface::attachObject
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....
Definition: move_group_interface.cpp:2360
moveit::planning_interface::MoveGroupInterface::setWorkspace
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....
Definition: move_group_interface.cpp:2328
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::attached_object_publisher_
ros::Publisher attached_object_publisher_
Definition: move_group_interface.cpp:1416
move_group::PICKUP_ACTION
static const std::string PICKUP_ACTION
moveit::planning_interface::MoveGroupInterface::pick
MoveItErrorCode pick(const std::string &object, bool plan_only=false)
Pick up an object.
Definition: move_group_interface.h:884
tf2::toMsg
B toMsg(const A &a)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::current_state_monitor_
planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_
Definition: move_group_interface.cpp:1376
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getEndEffectorLink
const std::string & getEndEffectorLink() const
Definition: move_group_interface.cpp:583
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::allowReplanning
void allowReplanning(bool flag)
Definition: move_group_interface.cpp:1129
moveit::planning_interface::MoveGroupInterface::impl_
MoveGroupInterfaceImpl * impl_
Definition: move_group_interface.h:1084
param
T param(const std::string &param_name, const T &default_val)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::goal_position_tolerance_
double goal_position_tolerance_
Definition: move_group_interface.cpp:1392
conversions.h
moveit::planning_interface::MoveGroupInterface::getPathConstraints
moveit_msgs::Constraints getPathConstraints() const
Get the actual set of constraints in use with this MoveGroupInterface.
Definition: move_group_interface.cpp:2288
x
double x
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setGoalOrientationTolerance
void setGoalOrientationTolerance(double tolerance)
Definition: move_group_interface.cpp:1107
moveit::planning_interface::MoveGroupInterface::getPlanningTime
double getPlanningTime() const
Get the number of seconds set by setPlanningTime()
Definition: move_group_interface.cpp:2340
moveit::planning_interface::MoveGroupInterface::MoveGroupInterface
MoveGroupInterface(const Options &opt, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >(), const ros::WallDuration &wait_for_servers=ros::WallDuration())
Construct a MoveGroupInterface instance call using a specified set of options opt.
Definition: move_group_interface.cpp:1441
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::execute_action_client_
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::ExecuteTrajectoryAction > > execute_action_client_
Definition: move_group_interface.cpp:1378
moveit::planning_interface::MoveGroupInterface::getActiveJoints
const std::vector< std::string > & getActiveJoints() const
Get only the active (actuated) joints this instance operates on.
Definition: move_group_interface.cpp:2241
moveit::planning_interface::MoveGroupInterface::getLinkNames
const std::vector< std::string > & getLinkNames() const
Get vector of names of links available in move group.
Definition: move_group_interface.cpp:1708
moveit::planning_interface::MoveGroupInterface::getJointNames
const std::vector< std::string > & getJointNames() const
Get vector of names of joints available in move group.
Definition: move_group_interface.cpp:1703
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::place_action_client_
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::PlaceAction > > place_action_client_
Definition: move_group_interface.cpp:1380
DurationBase< WallDuration >::toSec
double toSec() const
moveit::planning_interface::MoveGroupInterface::setPoseTargets
bool setPoseTargets(const EigenSTL::vector_Isometry3d &end_effector_pose, const std::string &end_effector_link="")
Set goal poses for end_effector_link.
Definition: move_group_interface.cpp:1948
moveit::planning_interface::MoveGroupInterface::setNamedTarget
bool setNamedTarget(const std::string &name)
Set the current joint values to be ones previously remembered by rememberJointValues() or,...
Definition: move_group_interface.cpp:1733
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setWorkspace
void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
Definition: move_group_interface.cpp:1340
moveit::core::JointModelGroup::getVariableCount
unsigned int getVariableCount() const
move_group::CARTESIAN_PATH_SERVICE_NAME
static const std::string CARTESIAN_PATH_SERVICE_NAME
moveit::planning_interface::MoveGroupInterface::getGoalOrientationTolerance
double getGoalOrientationTolerance() const
Get the tolerance that is used for reaching an orientation goal. This is the tolerance for roll,...
Definition: move_group_interface.cpp:2112
ros::WallDuration
moveit::core::RobotState::setJointGroupPositions
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::posesToPlaceLocations
std::vector< moveit_msgs::PlaceLocation > posesToPlaceLocations(const std::vector< geometry_msgs::PoseStamped > &poses) const
Convert a vector of PoseStamped to a vector of PlaceLocation.
Definition: move_group_interface.cpp:723
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::max_velocity_scaling_factor_
double max_velocity_scaling_factor_
Definition: move_group_interface.cpp:1389
seconds
FCL_EXPORT double seconds(const duration &d)
moveit::core::JointModelGroup::getEndEffectorParentGroup
const std::pair< std::string, std::string > & getEndEffectorParentGroup() const
move_group::GET_PLANNER_PARAMS_SERVICE_NAME
static const std::string GET_PLANNER_PARAMS_SERVICE_NAME
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::initializeConstraintsStorageThread
void initializeConstraintsStorageThread(const std::string &host, unsigned int port)
Definition: move_group_interface.cpp:1353
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::clearPoseTarget
void clearPoseTarget(const std::string &end_effector_link)
Definition: move_group_interface.cpp:573
moveit::planning_interface::MoveGroupInterface::getNamedTargetValues
std::map< std::string, double > getNamedTargetValues(const std::string &name) const
Get the joint angles for targets specified by name.
Definition: move_group_interface.cpp:1713
moveit::planning_interface::MoveGroupInterface::asyncMove
MoveItErrorCode asyncMove()
Plan and execute a trajectory that takes the group of joints declared in the constructor to the speci...
Definition: move_group_interface.cpp:1567
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getPlanningTime
double getPlanningTime() const
Definition: move_group_interface.cpp:1118
moveit::planning_interface::MoveGroupInterface::constructMotionPlanRequest
void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest &request)
Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and sto...
Definition: move_group_interface.cpp:2376
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::pick
MoveItErrorCode pick(const moveit_msgs::PickupGoal &goal)
Definition: move_group_interface.cpp:778
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::attachObject
bool attachObject(const std::string &object, const std::string &link, const std::vector< std::string > &touch_links)
Definition: move_group_interface.cpp:1032
ros::Duration
moveit::planning_interface::MoveGroupInterface::getCurrentPose
geometry_msgs::PoseStamped getCurrentPose(const std::string &end_effector_link="") const
Get the pose for the end-effector end_effector_link. If end_effector_link is empty (the default value...
Definition: move_group_interface.cpp:2190
z
double z
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getMoveGroupClient
actionlib::SimpleActionClient< moveit_msgs::MoveGroupAction > & getMoveGroupClient() const
Definition: move_group_interface.cpp:332
moveit::planning_interface::MoveGroupInterface::detachObject
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...
Definition: move_group_interface.cpp:2371
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::support_surface_
std::string support_surface_
Definition: move_group_interface.cpp:1412
moveit::planning_interface::MoveGroupInterface::setEndEffector
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...
Definition: move_group_interface.cpp:1906
ros::CallbackQueue::callAvailable
void callAvailable()
moveit::core::robotStateToRobotStateMsg
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
t
geometry_msgs::TransformStamped t
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::allowed_planning_time_
double allowed_planning_time_
Definition: move_group_interface.cpp:1385
moveit::core::JointModel
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setNumPlanningAttempts
void setNumPlanningAttempts(unsigned int num_planning_attempts)
Definition: move_group_interface.cpp:447
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::set_params_service_
ros::ServiceClient set_params_service_
Definition: move_group_interface.cpp:1419
moveit::planning_interface::MoveGroupInterface::Options::group_name_
std::string group_name_
The group to construct the class instance for.
Definition: move_group_interface.h:181
ros::NodeHandle
moveit::core::robotStateMsgToRobotState
bool robotStateMsgToRobotState(const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
name
name
moveit::planning_interface::MoveGroupInterface::constructPickupGoal
moveit_msgs::PickupGoal constructPickupGoal(const std::string &object, std::vector< moveit_msgs::Grasp > grasps, bool plan_only) const
Build a PickupGoal for an object named object and store it in goal_out.
Definition: move_group_interface.cpp:1607
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::setPoseReferenceFrame
void setPoseReferenceFrame(const std::string &pose_reference_frame)
Definition: move_group_interface.cpp:658
move_group::PLACE_ACTION
static const std::string PLACE_ACTION
ros::Time::now
static Time now()
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
moveit::planning_interface::MoveGroupInterface::getGoalPositionTolerance
double getGoalPositionTolerance() const
Get the tolerance that is used for reaching a position goal. This is be the radius of a sphere where ...
Definition: move_group_interface.cpp:2107
test_cleanup.group
group
Definition: test_cleanup.py:8
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::node_handle_
ros::NodeHandle node_handle_
Definition: move_group_interface.cpp:1373
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::planner_id_
std::string planner_id_
Definition: move_group_interface.cpp:1387
moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::getPoseReferenceFrame
const std::string & getPoseReferenceFrame() const
Definition: move_group_interface.cpp:668


planning_interface
Author(s): Ioan Sucan
autogenerated on Thu May 6 2021 02:32:02