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


planning_interface
Author(s): Ioan Sucan
autogenerated on Sun Nov 21 2021 03:27:11