moveit2
The MoveIt Motion Planning Framework for ROS 2.
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>
42 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
53 #include <moveit_msgs/action/execute_trajectory.hpp>
54 #include <moveit_msgs/srv/query_planner_interfaces.hpp>
55 #include <moveit_msgs/srv/get_cartesian_path.hpp>
56 #include <moveit_msgs/srv/grasp_planning.hpp>
57 #include <moveit_msgs/srv/get_planner_params.hpp>
58 #include <moveit_msgs/srv/set_planner_params.hpp>
60 
61 #include <std_msgs/msg/string.hpp>
62 #include <geometry_msgs/msg/transform_stamped.hpp>
63 #include <tf2/utils.h>
64 #include <tf2_eigen/tf2_eigen.hpp>
65 #include <tf2_ros/transform_listener.h>
66 #include <rclcpp/rclcpp.hpp>
67 #include <rclcpp/version.h>
68 
69 namespace moveit
70 {
71 namespace planning_interface
72 {
73 const std::string MoveGroupInterface::ROBOT_DESCRIPTION =
74  "robot_description"; // name of the robot description (a param name, so it can be changed externally)
75 
76 const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps
77 const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_interface");
78 
79 namespace
80 {
81 enum ActiveTargetType
82 {
83  JOINT,
84  POSE,
85  POSITION,
87 };
88 
89 // Function to support both Rolling and Humble on the main branch
90 // Rolling has deprecated the version of the create_client method that takes
91 // rmw_qos_profile_services_default for the QoS argument
92 #if RCLCPP_VERSION_GTE(17, 0, 0) // Rolling
93 auto qos_default()
94 {
95  return rclcpp::SystemDefaultsQoS();
96 }
97 #else // Humble
98 auto qos_default()
99 {
100  return rmw_qos_profile_services_default;
101 }
102 #endif
103 
104 } // namespace
105 
107 {
108  friend MoveGroupInterface;
109 
110 public:
111  MoveGroupInterfaceImpl(const rclcpp::Node::SharedPtr& node, const Options& opt,
112  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const rclcpp::Duration& wait_for_servers)
113  : opt_(opt), node_(node), tf_buffer_(tf_buffer)
114  {
115  // We have no control on how the passed node is getting executed. To make sure MGI is functional, we're creating
116  // our own callback group which is managed in a separate callback thread
117  callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive,
118  false /* don't spin with node executor */);
119  callback_executor_.add_callback_group(callback_group_, node->get_node_base_interface());
120  callback_thread_ = std::thread([this]() { callback_executor_.spin(); });
121 
122  robot_model_ = opt.robot_model ? opt.robot_model : getSharedRobotModel(node_, opt.robot_description);
123  if (!getRobotModel())
124  {
125  std::string error = "Unable to construct robot model. Please make sure all needed information is on the "
126  "parameter server.";
127  RCLCPP_FATAL_STREAM(LOGGER, error);
128  throw std::runtime_error(error);
129  }
130 
131  if (!getRobotModel()->hasJointModelGroup(opt.group_name))
132  {
133  std::string error = "Group '" + opt.group_name + "' was not found.";
134  RCLCPP_FATAL_STREAM(LOGGER, error);
135  throw std::runtime_error(error);
136  }
137 
138  joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name);
139 
140  joint_state_target_ = std::make_shared<moveit::core::RobotState>(getRobotModel());
141  joint_state_target_->setToDefaultValues();
142  active_target_ = JOINT;
143  can_look_ = false;
144  look_around_attempts_ = 0;
145  can_replan_ = false;
146  replan_delay_ = 2.0;
147  replan_attempts_ = 1;
148  goal_joint_tolerance_ = 1e-4;
149  goal_position_tolerance_ = 1e-4; // 0.1 mm
150  goal_orientation_tolerance_ = 1e-3; // ~0.1 deg
151  allowed_planning_time_ = 5.0;
152  num_planning_attempts_ = 1;
153  node_->get_parameter_or<double>("robot_description_planning.default_velocity_scaling_factor",
154  max_velocity_scaling_factor_, 0.1);
155  node_->get_parameter_or<double>("robot_description_planning.default_acceleration_scaling_factor",
156  max_acceleration_scaling_factor_, 0.1);
157  initializing_constraints_ = false;
158 
159  if (joint_model_group_->isChain())
160  end_effector_link_ = joint_model_group_->getLinkModelNames().back();
161  pose_reference_frame_ = getRobotModel()->getModelFrame();
162  // Append the slash between two topic components
163  trajectory_event_publisher_ = node_->create_publisher<std_msgs::msg::String>(
166  1);
167  attached_object_publisher_ = node_->create_publisher<moveit_msgs::msg::AttachedCollisionObject>(
170  1);
171 
172  current_state_monitor_ = getSharedStateMonitor(node_, robot_model_, tf_buffer_);
173 
174  move_action_client_ = rclcpp_action::create_client<moveit_msgs::action::MoveGroup>(
175  node_, rclcpp::names::append(opt_.move_group_namespace, move_group::MOVE_ACTION), callback_group_);
176  move_action_client_->wait_for_action_server(wait_for_servers.to_chrono<std::chrono::duration<double>>());
177  execute_action_client_ = rclcpp_action::create_client<moveit_msgs::action::ExecuteTrajectory>(
178  node_, rclcpp::names::append(opt_.move_group_namespace, move_group::EXECUTE_ACTION_NAME), callback_group_);
179  execute_action_client_->wait_for_action_server(wait_for_servers.to_chrono<std::chrono::duration<double>>());
180 
181  query_service_ = node_->create_client<moveit_msgs::srv::QueryPlannerInterfaces>(
182  rclcpp::names::append(opt_.move_group_namespace, move_group::QUERY_PLANNERS_SERVICE_NAME), qos_default(),
183  callback_group_);
184  get_params_service_ = node_->create_client<moveit_msgs::srv::GetPlannerParams>(
185  rclcpp::names::append(opt_.move_group_namespace, move_group::GET_PLANNER_PARAMS_SERVICE_NAME), qos_default(),
186  callback_group_);
187  set_params_service_ = node_->create_client<moveit_msgs::srv::SetPlannerParams>(
188  rclcpp::names::append(opt_.move_group_namespace, move_group::SET_PLANNER_PARAMS_SERVICE_NAME), qos_default(),
189  callback_group_);
190  cartesian_path_service_ = node_->create_client<moveit_msgs::srv::GetCartesianPath>(
191  rclcpp::names::append(opt_.move_group_namespace, move_group::CARTESIAN_PATH_SERVICE_NAME), qos_default(),
192  callback_group_);
193 
194  RCLCPP_INFO_STREAM(LOGGER, "Ready to take commands for planning group " << opt.group_name << '.');
195  }
196 
198  {
199  if (constraints_init_thread_)
200  constraints_init_thread_->join();
201 
202  callback_executor_.cancel();
203 
204  if (callback_thread_.joinable())
205  callback_thread_.join();
206  }
207 
208  const std::shared_ptr<tf2_ros::Buffer>& getTF() const
209  {
210  return tf_buffer_;
211  }
212 
213  const Options& getOptions() const
214  {
215  return opt_;
216  }
217 
218  const moveit::core::RobotModelConstPtr& getRobotModel() const
219  {
220  return robot_model_;
221  }
222 
224  {
225  return joint_model_group_;
226  }
227 
228  rclcpp_action::Client<moveit_msgs::action::MoveGroup>& getMoveGroupClient() const
229  {
230  return *move_action_client_;
231  }
232 
233  bool getInterfaceDescription(moveit_msgs::msg::PlannerInterfaceDescription& desc)
234  {
235  auto req = std::make_shared<moveit_msgs::srv::QueryPlannerInterfaces::Request>();
236  auto future_response = query_service_->async_send_request(req);
237 
238  if (future_response.valid())
239  {
240  const auto& response = future_response.get();
241  if (!response->planner_interfaces.empty())
242  {
243  desc = response->planner_interfaces.front();
244  return true;
245  }
246  }
247  return false;
248  }
249 
250  bool getInterfaceDescriptions(std::vector<moveit_msgs::msg::PlannerInterfaceDescription>& desc)
251  {
252  auto req = std::make_shared<moveit_msgs::srv::QueryPlannerInterfaces::Request>();
253  auto future_response = query_service_->async_send_request(req);
254  if (future_response.valid())
255  {
256  const auto& response = future_response.get();
257  if (!response->planner_interfaces.empty())
258  {
259  desc = response->planner_interfaces;
260  return true;
261  }
262  }
263  return false;
264  }
265 
266  std::map<std::string, std::string> getPlannerParams(const std::string& planner_id, const std::string& group = "")
267  {
268  auto req = std::make_shared<moveit_msgs::srv::GetPlannerParams::Request>();
269  moveit_msgs::srv::GetPlannerParams::Response::SharedPtr response;
270  req->planner_config = planner_id;
271  req->group = group;
272  std::map<std::string, std::string> result;
273 
274  auto future_response = get_params_service_->async_send_request(req);
275  if (future_response.valid())
276  {
277  response = future_response.get();
278  for (unsigned int i = 0, end = response->params.keys.size(); i < end; ++i)
279  result[response->params.keys[i]] = response->params.values[i];
280  }
281  return result;
282  }
283 
284  void setPlannerParams(const std::string& planner_id, const std::string& group,
285  const std::map<std::string, std::string>& params, bool replace = false)
286  {
287  auto req = std::make_shared<moveit_msgs::srv::SetPlannerParams::Request>();
288  req->planner_config = planner_id;
289  req->group = group;
290  req->replace = replace;
291  for (const std::pair<const std::string, std::string>& param : params)
292  {
293  req->params.keys.push_back(param.first);
294  req->params.values.push_back(param.second);
295  }
296  set_params_service_->async_send_request(req);
297  }
298 
299  std::string getDefaultPlanningPipelineId() const
300  {
301  std::string default_planning_pipeline;
302  node_->get_parameter("move_group.default_planning_pipeline", default_planning_pipeline);
303  return default_planning_pipeline;
304  }
305 
306  void setPlanningPipelineId(const std::string& pipeline_id)
307  {
308  if (pipeline_id != planning_pipeline_id_)
309  {
310  planning_pipeline_id_ = pipeline_id;
311 
312  // Reset planner_id if planning pipeline changed
313  planner_id_ = "";
314  }
315  }
316 
317  const std::string& getPlanningPipelineId() const
318  {
319  return planning_pipeline_id_;
320  }
321 
322  std::string getDefaultPlannerId(const std::string& group) const
323  {
324  // Check what planning pipeline config should be used
325  std::string pipeline_id = getDefaultPlanningPipelineId();
326  if (!planning_pipeline_id_.empty())
327  pipeline_id = planning_pipeline_id_;
328 
329  std::stringstream param_name;
330  param_name << "move_group";
331  if (!pipeline_id.empty())
332  param_name << "/planning_pipelines/" << pipeline_id;
333  if (!group.empty())
334  param_name << '.' << group;
335  param_name << ".default_planner_config";
336 
337  std::string default_planner_config;
338  node_->get_parameter(param_name.str(), default_planner_config);
339  return default_planner_config;
340  }
341 
342  void setPlannerId(const std::string& planner_id)
343  {
344  planner_id_ = planner_id;
345  }
346 
347  const std::string& getPlannerId() const
348  {
349  return planner_id_;
350  }
351 
352  void setNumPlanningAttempts(unsigned int num_planning_attempts)
353  {
354  num_planning_attempts_ = num_planning_attempts;
355  }
356 
357  void setMaxVelocityScalingFactor(double value)
358  {
359  setMaxScalingFactor(max_velocity_scaling_factor_, value, "velocity_scaling_factor", 0.1);
360  }
361 
363  {
364  setMaxScalingFactor(max_acceleration_scaling_factor_, value, "acceleration_scaling_factor", 0.1);
365  }
366 
367  void setMaxScalingFactor(double& variable, const double target_value, const char* factor_name, double fallback_value)
368  {
369  if (target_value > 1.0)
370  {
371  RCLCPP_WARN(rclcpp::get_logger("move_group_interface"), "Limiting max_%s (%.2f) to 1.0.", factor_name,
372  target_value);
373  variable = 1.0;
374  }
375  else if (target_value <= 0.0)
376  {
377  node_->get_parameter_or<double>(std::string("robot_description_planning.default_") + factor_name, variable,
378  fallback_value);
379  if (target_value < 0.0)
380  {
381  RCLCPP_WARN(rclcpp::get_logger("move_group_interface"), "max_%s < 0.0! Setting to default: %.2f.", factor_name,
382  variable);
383  }
384  }
385  else
386  {
387  variable = target_value;
388  }
389  }
390 
392  {
393  return *joint_state_target_;
394  }
395 
397  {
398  return *joint_state_target_;
399  }
400 
401  void setStartState(const moveit::core::RobotState& start_state)
402  {
403  considered_start_state_ = std::make_shared<moveit::core::RobotState>(start_state);
404  }
405 
407  {
408  considered_start_state_.reset();
409  }
410 
411  moveit::core::RobotStatePtr getStartState()
412  {
413  if (considered_start_state_)
414  {
415  return considered_start_state_;
416  }
417  else
418  {
419  moveit::core::RobotStatePtr s;
420  getCurrentState(s);
421  return s;
422  }
423  }
424 
425  bool setJointValueTarget(const geometry_msgs::msg::Pose& eef_pose, const std::string& end_effector_link,
426  const std::string& frame, bool approx)
427  {
428  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
429  // this only works if we have an end-effector
430  if (!eef.empty())
431  {
432  // first we set the goal to be the same as the start state
433  moveit::core::RobotStatePtr c = getStartState();
434  if (c)
435  {
437  c->enforceBounds();
438  getTargetRobotState() = *c;
439  if (!getTargetRobotState().satisfiesBounds(getGoalJointTolerance()))
440  return false;
441  }
442  else
443  return false;
444 
445  // we may need to do approximate IK
447  o.return_approximate_solution = approx;
448 
449  // if no frame transforms are needed, call IK directly
450  if (frame.empty() || moveit::core::Transforms::sameFrame(frame, getRobotModel()->getModelFrame()))
451  {
452  return getTargetRobotState().setFromIK(getJointModelGroup(), eef_pose, eef, 0.0,
454  }
455  else
456  {
457  // transform the pose into the model frame, then do IK
458  bool frame_found;
459  const Eigen::Isometry3d& t = getTargetRobotState().getFrameTransform(frame, &frame_found);
460  if (frame_found)
461  {
462  Eigen::Isometry3d p;
463  tf2::fromMsg(eef_pose, p);
464  return getTargetRobotState().setFromIK(getJointModelGroup(), t * p, eef, 0.0,
466  }
467  else
468  {
469  RCLCPP_ERROR(LOGGER, "Unable to transform from frame '%s' to frame '%s'", frame.c_str(),
470  getRobotModel()->getModelFrame().c_str());
471  return false;
472  }
473  }
474  }
475  else
476  return false;
477  }
478 
479  void setEndEffectorLink(const std::string& end_effector)
480  {
481  end_effector_link_ = end_effector;
482  }
483 
484  void clearPoseTarget(const std::string& end_effector_link)
485  {
486  pose_targets_.erase(end_effector_link);
487  }
488 
490  {
491  pose_targets_.clear();
492  }
493 
494  const std::string& getEndEffectorLink() const
495  {
496  return end_effector_link_;
497  }
498 
499  const std::string& getEndEffector() const
500  {
501  if (!end_effector_link_.empty())
502  {
503  const std::vector<std::string>& possible_eefs =
504  getRobotModel()->getJointModelGroup(opt_.group_name)->getAttachedEndEffectorNames();
505  for (const std::string& possible_eef : possible_eefs)
506  {
507  if (getRobotModel()->getEndEffector(possible_eef)->hasLinkModel(end_effector_link_))
508  return possible_eef;
509  }
510  }
511  static std::string empty;
512  return empty;
513  }
514 
515  bool setPoseTargets(const std::vector<geometry_msgs::msg::PoseStamped>& poses, const std::string& end_effector_link)
516  {
517  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
518  if (eef.empty())
519  {
520  RCLCPP_ERROR(LOGGER, "No end-effector to set the pose for");
521  return false;
522  }
523  else
524  {
525  pose_targets_[eef] = poses;
526  // make sure we don't store an actual stamp, since that will become stale can potentially cause tf errors
527  std::vector<geometry_msgs::msg::PoseStamped>& stored_poses = pose_targets_[eef];
528  for (geometry_msgs::msg::PoseStamped& stored_pose : stored_poses)
529  stored_pose.header.stamp = rclcpp::Time(0);
530  }
531  return true;
532  }
533 
534  bool hasPoseTarget(const std::string& end_effector_link) const
535  {
536  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
537  return pose_targets_.find(eef) != pose_targets_.end();
538  }
539 
540  const geometry_msgs::msg::PoseStamped& getPoseTarget(const std::string& end_effector_link) const
541  {
542  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
543 
544  // if multiple pose targets are set, return the first one
545  std::map<std::string, std::vector<geometry_msgs::msg::PoseStamped>>::const_iterator jt = pose_targets_.find(eef);
546  if (jt != pose_targets_.end())
547  {
548  if (!jt->second.empty())
549  return jt->second.at(0);
550  }
551 
552  // or return an error
553  static const geometry_msgs::msg::PoseStamped UNKNOWN;
554  RCLCPP_ERROR(LOGGER, "Pose for end-effector '%s' not known.", eef.c_str());
555  return UNKNOWN;
556  }
557 
558  const std::vector<geometry_msgs::msg::PoseStamped>& getPoseTargets(const std::string& end_effector_link) const
559  {
560  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
561 
562  std::map<std::string, std::vector<geometry_msgs::msg::PoseStamped>>::const_iterator jt = pose_targets_.find(eef);
563  if (jt != pose_targets_.end())
564  {
565  if (!jt->second.empty())
566  return jt->second;
567  }
568 
569  // or return an error
570  static const std::vector<geometry_msgs::msg::PoseStamped> EMPTY;
571  RCLCPP_ERROR(LOGGER, "Poses for end-effector '%s' are not known.", eef.c_str());
572  return EMPTY;
573  }
574 
575  void setPoseReferenceFrame(const std::string& pose_reference_frame)
576  {
577  pose_reference_frame_ = pose_reference_frame;
578  }
579 
580  void setSupportSurfaceName(const std::string& support_surface)
581  {
582  support_surface_ = support_surface;
583  }
584 
585  const std::string& getPoseReferenceFrame() const
586  {
587  return pose_reference_frame_;
588  }
589 
590  void setTargetType(ActiveTargetType type)
591  {
592  active_target_ = type;
593  }
594 
595  ActiveTargetType getTargetType() const
596  {
597  return active_target_;
598  }
599 
600  bool startStateMonitor(double wait)
601  {
602  if (!current_state_monitor_)
603  {
604  RCLCPP_ERROR(LOGGER, "Unable to monitor current robot state");
605  return false;
606  }
607 
608  // if needed, start the monitor and wait up to 1 second for a full robot state
609  if (!current_state_monitor_->isActive())
610  current_state_monitor_->startStateMonitor();
611 
612  current_state_monitor_->waitForCompleteState(opt_.group_name, wait);
613  return true;
614  }
615 
616  bool getCurrentState(moveit::core::RobotStatePtr& current_state, double wait_seconds = 1.0)
617  {
618  if (!current_state_monitor_)
619  {
620  RCLCPP_ERROR(LOGGER, "Unable to get current robot state");
621  return false;
622  }
623 
624  // if needed, start the monitor and wait up to 1 second for a full robot state
625  if (!current_state_monitor_->isActive())
626  current_state_monitor_->startStateMonitor();
627 
628  if (!current_state_monitor_->waitForCurrentState(node_->now(), wait_seconds))
629  {
630  RCLCPP_ERROR(LOGGER, "Failed to fetch current robot state");
631  return false;
632  }
633 
634  current_state = current_state_monitor_->getCurrentState();
635  return true;
636  }
637 
639  {
640  if (!move_action_client_ || !move_action_client_->action_server_is_ready())
641  {
642  RCLCPP_INFO_STREAM(LOGGER, "MoveGroup action client/server not ready");
643  return moveit::core::MoveItErrorCode::FAILURE;
644  }
645  RCLCPP_INFO_STREAM(LOGGER, "MoveGroup action client/server ready");
646 
647  moveit_msgs::action::MoveGroup::Goal goal;
648  constructGoal(goal);
649  goal.planning_options.plan_only = true;
650  goal.planning_options.look_around = false;
651  goal.planning_options.replan = false;
652  goal.planning_options.planning_scene_diff.is_diff = true;
653  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
654 
655  bool done = false;
656  rclcpp_action::ResultCode code = rclcpp_action::ResultCode::UNKNOWN;
657  std::shared_ptr<moveit_msgs::action::MoveGroup::Result> res;
658  auto send_goal_opts = rclcpp_action::Client<moveit_msgs::action::MoveGroup>::SendGoalOptions();
659 
660  send_goal_opts.goal_response_callback =
661  [&](const rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::SharedPtr& goal_handle) {
662  if (!goal_handle)
663  {
664  done = true;
665  RCLCPP_INFO(LOGGER, "Planning request rejected");
666  }
667  else
668  RCLCPP_INFO(LOGGER, "Planning request accepted");
669  };
670  send_goal_opts.result_callback =
671  [&](const rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::WrappedResult& result) {
672  res = result.result;
673  code = result.code;
674  done = true;
675 
676  switch (result.code)
677  {
678  case rclcpp_action::ResultCode::SUCCEEDED:
679  RCLCPP_INFO(LOGGER, "Planning request complete!");
680  break;
681  case rclcpp_action::ResultCode::ABORTED:
682  RCLCPP_INFO(LOGGER, "Planning request aborted");
683  return;
684  case rclcpp_action::ResultCode::CANCELED:
685  RCLCPP_INFO(LOGGER, "Planning request canceled");
686  return;
687  default:
688  RCLCPP_INFO(LOGGER, "Planning request unknown result code");
689  return;
690  }
691  };
692 
693  auto goal_handle_future = move_action_client_->async_send_goal(goal, send_goal_opts);
694 
695  // wait until send_goal_opts.result_callback is called
696  while (!done)
697  {
698  std::this_thread::sleep_for(std::chrono::milliseconds(1));
699  }
700 
701  if (code != rclcpp_action::ResultCode::SUCCEEDED)
702  {
703  RCLCPP_ERROR_STREAM(LOGGER, "MoveGroupInterface::plan() failed or timeout reached");
704  return res->error_code;
705  }
706 
707  plan.trajectory = res->planned_trajectory;
708  plan.start_state = res->trajectory_start;
709  plan.planning_time = res->planning_time;
710  RCLCPP_INFO(LOGGER, "time taken to generate plan: %g seconds", plan.planning_time);
711 
712  return res->error_code;
713  }
714 
716  {
717  if (!move_action_client_ || !move_action_client_->action_server_is_ready())
718  {
719  RCLCPP_INFO_STREAM(LOGGER, "MoveGroup action client/server not ready");
720  return moveit::core::MoveItErrorCode::FAILURE;
721  }
722 
723  moveit_msgs::action::MoveGroup::Goal goal;
724  constructGoal(goal);
725  goal.planning_options.plan_only = false;
726  goal.planning_options.look_around = can_look_;
727  goal.planning_options.replan = can_replan_;
728  goal.planning_options.replan_delay = replan_delay_;
729  goal.planning_options.planning_scene_diff.is_diff = true;
730  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
731 
732  bool done = false;
733  rclcpp_action::ResultCode code = rclcpp_action::ResultCode::UNKNOWN;
734  std::shared_ptr<moveit_msgs::action::MoveGroup_Result> res;
735  auto send_goal_opts = rclcpp_action::Client<moveit_msgs::action::MoveGroup>::SendGoalOptions();
736 
737  send_goal_opts.goal_response_callback =
738  [&](const rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::SharedPtr& goal_handle) {
739  if (!goal_handle)
740  {
741  done = true;
742  RCLCPP_INFO(LOGGER, "Plan and Execute request rejected");
743  }
744  else
745  RCLCPP_INFO(LOGGER, "Plan and Execute request accepted");
746  };
747  send_goal_opts.result_callback =
748  [&](const rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::WrappedResult& result) {
749  res = result.result;
750  code = result.code;
751  done = true;
752 
753  switch (result.code)
754  {
755  case rclcpp_action::ResultCode::SUCCEEDED:
756  RCLCPP_INFO(LOGGER, "Plan and Execute request complete!");
757  break;
758  case rclcpp_action::ResultCode::ABORTED:
759  RCLCPP_INFO(LOGGER, "Plan and Execute request aborted");
760  return;
761  case rclcpp_action::ResultCode::CANCELED:
762  RCLCPP_INFO(LOGGER, "Plan and Execute request canceled");
763  return;
764  default:
765  RCLCPP_INFO(LOGGER, "Plan and Execute request unknown result code");
766  return;
767  }
768  };
769  auto goal_handle_future = move_action_client_->async_send_goal(goal, send_goal_opts);
770  if (!wait)
771  return moveit::core::MoveItErrorCode::SUCCESS;
772 
773  // wait until send_goal_opts.result_callback is called
774  while (!done)
775  {
776  std::this_thread::sleep_for(std::chrono::milliseconds(1));
777  }
778 
779  if (code != rclcpp_action::ResultCode::SUCCEEDED)
780  {
781  RCLCPP_ERROR_STREAM(LOGGER, "MoveGroupInterface::move() failed or timeout reached");
782  }
783  return res->error_code;
784  }
785 
786  moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory, bool wait,
787  const std::vector<std::string>& controllers = std::vector<std::string>())
788  {
789  if (!execute_action_client_ || !execute_action_client_->action_server_is_ready())
790  {
791  RCLCPP_INFO_STREAM(LOGGER, "execute_action_client_ client/server not ready");
792  return moveit::core::MoveItErrorCode::FAILURE;
793  }
794 
795  bool done = false;
796  rclcpp_action::ResultCode code = rclcpp_action::ResultCode::UNKNOWN;
797  std::shared_ptr<moveit_msgs::action::ExecuteTrajectory_Result> res;
798  auto send_goal_opts = rclcpp_action::Client<moveit_msgs::action::ExecuteTrajectory>::SendGoalOptions();
799 
800  send_goal_opts.goal_response_callback =
801  [&](const rclcpp_action::ClientGoalHandle<moveit_msgs::action::ExecuteTrajectory>::SharedPtr& goal_handle) {
802  if (!goal_handle)
803  {
804  done = true;
805  RCLCPP_INFO(LOGGER, "Execute request rejected");
806  }
807  else
808  RCLCPP_INFO(LOGGER, "Execute request accepted");
809  };
810  send_goal_opts.result_callback =
811  [&](const rclcpp_action::ClientGoalHandle<moveit_msgs::action::ExecuteTrajectory>::WrappedResult& result) {
812  res = result.result;
813  code = result.code;
814  done = true;
815 
816  switch (result.code)
817  {
818  case rclcpp_action::ResultCode::SUCCEEDED:
819  RCLCPP_INFO(LOGGER, "Execute request success!");
820  break;
821  case rclcpp_action::ResultCode::ABORTED:
822  RCLCPP_INFO(LOGGER, "Execute request aborted");
823  return;
824  case rclcpp_action::ResultCode::CANCELED:
825  RCLCPP_INFO(LOGGER, "Execute request canceled");
826  return;
827  default:
828  RCLCPP_INFO(LOGGER, "Execute request unknown result code");
829  return;
830  }
831  };
832 
833  moveit_msgs::action::ExecuteTrajectory::Goal goal;
834  goal.trajectory = trajectory;
835  goal.controller_names = controllers;
836 
837  auto goal_handle_future = execute_action_client_->async_send_goal(goal, send_goal_opts);
838  if (!wait)
839  return moveit::core::MoveItErrorCode::SUCCESS;
840 
841  // wait until send_goal_opts.result_callback is called
842  while (!done)
843  {
844  std::this_thread::sleep_for(std::chrono::milliseconds(1));
845  }
846 
847  if (code != rclcpp_action::ResultCode::SUCCEEDED)
848  {
849  RCLCPP_ERROR_STREAM(LOGGER, "MoveGroupInterface::execute() failed or timeout reached");
850  }
851  return res->error_code;
852  }
853 
854  double computeCartesianPath(const std::vector<geometry_msgs::msg::Pose>& waypoints, double step,
855  double jump_threshold, moveit_msgs::msg::RobotTrajectory& msg,
856  const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions,
857  moveit_msgs::msg::MoveItErrorCodes& error_code)
858  {
859  auto req = std::make_shared<moveit_msgs::srv::GetCartesianPath::Request>();
860  moveit_msgs::srv::GetCartesianPath::Response::SharedPtr response;
861 
862  if (considered_start_state_)
863  {
864  moveit::core::robotStateToRobotStateMsg(*considered_start_state_, req->start_state);
865  }
866  else
867  {
868  req->start_state.is_diff = true;
869  }
870 
871  req->group_name = opt_.group_name;
872  req->header.frame_id = getPoseReferenceFrame();
873  req->header.stamp = getClock()->now();
874  req->waypoints = waypoints;
875  req->max_step = step;
876  req->jump_threshold = jump_threshold;
877  req->path_constraints = path_constraints;
878  req->avoid_collisions = avoid_collisions;
879  req->link_name = getEndEffectorLink();
880  req->max_velocity_scaling_factor = max_velocity_scaling_factor_;
881  req->max_acceleration_scaling_factor = max_acceleration_scaling_factor_;
882 
883  auto future_response = cartesian_path_service_->async_send_request(req);
884  if (future_response.valid())
885  {
886  response = future_response.get();
887  error_code = response->error_code;
888  if (response->error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
889  {
890  msg = response->solution;
891  return response->fraction;
892  }
893  else
894  return -1.0;
895  }
896  else
897  {
898  error_code.val = error_code.FAILURE;
899  return -1.0;
900  }
901  }
902 
903  void stop()
904  {
905  if (trajectory_event_publisher_)
906  {
907  std_msgs::msg::String event;
908  event.data = "stop";
909  trajectory_event_publisher_->publish(event);
910  }
911  }
912 
913  bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links)
914  {
915  std::string l = link.empty() ? getEndEffectorLink() : link;
916  if (l.empty())
917  {
918  const std::vector<std::string>& links = joint_model_group_->getLinkModelNames();
919  if (!links.empty())
920  l = links[0];
921  }
922  if (l.empty())
923  {
924  RCLCPP_ERROR(LOGGER, "No known link to attach object '%s' to", object.c_str());
925  return false;
926  }
927  moveit_msgs::msg::AttachedCollisionObject aco;
928  aco.object.id = object;
929  aco.link_name.swap(l);
930  if (touch_links.empty())
931  {
932  aco.touch_links.push_back(aco.link_name);
933  }
934  else
935  {
936  aco.touch_links = touch_links;
937  }
938  aco.object.operation = moveit_msgs::msg::CollisionObject::ADD;
939  attached_object_publisher_->publish(aco);
940  return true;
941  }
942 
943  bool detachObject(const std::string& name)
944  {
945  moveit_msgs::msg::AttachedCollisionObject aco;
946  // if name is a link
947  if (!name.empty() && joint_model_group_->hasLinkModel(name))
948  {
949  aco.link_name = name;
950  }
951  else
952  {
953  aco.object.id = name;
954  }
955  aco.object.operation = moveit_msgs::msg::CollisionObject::REMOVE;
956  if (aco.link_name.empty() && aco.object.id.empty())
957  {
958  // we only want to detach objects for this group
959  const std::vector<std::string>& lnames = joint_model_group_->getLinkModelNames();
960  for (const std::string& lname : lnames)
961  {
962  aco.link_name = lname;
963  attached_object_publisher_->publish(aco);
964  }
965  }
966  else
967  {
968  attached_object_publisher_->publish(aco);
969  }
970  return true;
971  }
972 
974  {
975  return goal_position_tolerance_;
976  }
977 
979  {
980  return goal_orientation_tolerance_;
981  }
982 
983  double getGoalJointTolerance() const
984  {
985  return goal_joint_tolerance_;
986  }
987 
988  void setGoalJointTolerance(double tolerance)
989  {
990  goal_joint_tolerance_ = tolerance;
991  }
992 
993  void setGoalPositionTolerance(double tolerance)
994  {
995  goal_position_tolerance_ = tolerance;
996  }
997 
998  void setGoalOrientationTolerance(double tolerance)
999  {
1000  goal_orientation_tolerance_ = tolerance;
1001  }
1002 
1003  void setPlanningTime(double seconds)
1004  {
1005  if (seconds > 0.0)
1006  allowed_planning_time_ = seconds;
1007  }
1008 
1009  double getPlanningTime() const
1010  {
1011  return allowed_planning_time_;
1012  }
1013 
1015  {
1016  request.group_name = opt_.group_name;
1017  request.num_planning_attempts = num_planning_attempts_;
1018  request.max_velocity_scaling_factor = max_velocity_scaling_factor_;
1019  request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_;
1020  request.allowed_planning_time = allowed_planning_time_;
1021  request.pipeline_id = planning_pipeline_id_;
1022  request.planner_id = planner_id_;
1023  request.workspace_parameters = workspace_parameters_;
1024 
1025  if (considered_start_state_)
1026  {
1027  moveit::core::robotStateToRobotStateMsg(*considered_start_state_, request.start_state);
1028  }
1029  else
1030  {
1031  request.start_state.is_diff = true;
1032  }
1033 
1034  if (active_target_ == JOINT)
1035  {
1036  request.goal_constraints.resize(1);
1037  request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
1038  getTargetRobotState(), joint_model_group_, goal_joint_tolerance_);
1039  }
1040  else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION)
1041  {
1042  // find out how many goals are specified
1043  std::size_t goal_count = 0;
1044  for (const auto& pose_target : pose_targets_)
1045  goal_count = std::max(goal_count, pose_target.second.size());
1046 
1047  // start filling the goals;
1048  // each end effector has a number of possible poses (K) as valid goals
1049  // but there could be multiple end effectors specified, so we want each end effector
1050  // to reach the goal that corresponds to the goals of the other end effectors
1051  request.goal_constraints.resize(goal_count);
1052 
1053  for (const auto& pose_target : pose_targets_)
1054  {
1055  for (std::size_t i = 0; i < pose_target.second.size(); ++i)
1056  {
1057  moveit_msgs::msg::Constraints c = kinematic_constraints::constructGoalConstraints(
1058  pose_target.first, pose_target.second[i], goal_position_tolerance_, goal_orientation_tolerance_);
1059  if (active_target_ == ORIENTATION)
1060  c.position_constraints.clear();
1061  if (active_target_ == POSITION)
1062  c.orientation_constraints.clear();
1063  request.goal_constraints[i] = kinematic_constraints::mergeConstraints(request.goal_constraints[i], c);
1064  }
1065  }
1066  }
1067  else
1068  RCLCPP_ERROR(LOGGER, "Unable to construct MotionPlanRequest representation");
1069 
1070  if (path_constraints_)
1071  request.path_constraints = *path_constraints_;
1072  if (trajectory_constraints_)
1073  request.trajectory_constraints = *trajectory_constraints_;
1074  }
1075 
1076  void constructGoal(moveit_msgs::action::MoveGroup::Goal& goal) const
1077  {
1078  constructMotionPlanRequest(goal.request);
1079  }
1080 
1081  // moveit_msgs::action::Pickup::Goal constructPickupGoal(const std::string& object,
1082  // std::vector<moveit_msgs::msg::Grasp>&& grasps,
1083  // bool plan_only = false) const
1084  // {
1085  // moveit_msgs::action::Pickup::Goal goal;
1086  // goal.target_name = object;
1087  // goal.group_name = opt_.group_name;
1088  // goal.end_effector = getEndEffector();
1089  // goal.support_surface_name = support_surface_;
1090  // goal.possible_grasps = std::move(grasps);
1091  // if (!support_surface_.empty())
1092  // goal.allow_gripper_support_collision = true;
1093  //
1094  // if (path_constraints_)
1095  // goal.path_constraints = *path_constraints_;
1096  //
1097  // goal.planner_id = planner_id_;
1098  // goal.allowed_planning_time = allowed_planning_time_;
1099  //
1100  // goal.planning_options.plan_only = plan_only;
1101  // goal.planning_options.look_around = can_look_;
1102  // goal.planning_options.replan = can_replan_;
1103  // goal.planning_options.replan_delay = replan_delay_;
1104  // goal.planning_options.planning_scene_diff.is_diff = true;
1105  // goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
1106  // return goal;
1107  // }
1108 
1109  // moveit_msgs::action::Place::Goal constructPlaceGoal(const std::string& object,
1110  // std::vector<moveit_msgs::msg::PlaceLocation>&& locations,
1111  // bool plan_only = false) const
1112  // {
1113  // moveit_msgs::action::Place::Goal goal;
1114  // goal.group_name = opt_.group_name;
1115  // goal.attached_object_name = object;
1116  // goal.support_surface_name = support_surface_;
1117  // goal.place_locations = std::move(locations);
1118  // if (!support_surface_.empty())
1119  // goal.allow_gripper_support_collision = true;
1120  //
1121  // if (path_constraints_)
1122  // goal.path_constraints = *path_constraints_;
1123  //
1124  // goal.planner_id = planner_id_;
1125  // goal.allowed_planning_time = allowed_planning_time_;
1126  //
1127  // goal.planning_options.plan_only = plan_only;
1128  // goal.planning_options.look_around = can_look_;
1129  // goal.planning_options.replan = can_replan_;
1130  // goal.planning_options.replan_delay = replan_delay_;
1131  // goal.planning_options.planning_scene_diff.is_diff = true;
1132  // goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
1133  // return goal;
1134  // }
1135 
1136  void setPathConstraints(const moveit_msgs::msg::Constraints& constraint)
1137  {
1138  path_constraints_ = std::make_unique<moveit_msgs::msg::Constraints>(constraint);
1139  }
1140 
1141  bool setPathConstraints(const std::string& constraint)
1142  {
1143  if (constraints_storage_)
1144  {
1146  if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name))
1147  {
1148  path_constraints_ =
1149  std::make_unique<moveit_msgs::msg::Constraints>(static_cast<moveit_msgs::msg::Constraints>(*msg_m));
1150  return true;
1151  }
1152  else
1153  return false;
1154  }
1155  else
1156  return false;
1157  }
1158 
1160  {
1161  path_constraints_.reset();
1162  }
1163 
1164  void setTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints& constraint)
1165  {
1166  trajectory_constraints_ = std::make_unique<moveit_msgs::msg::TrajectoryConstraints>(constraint);
1167  }
1168 
1170  {
1171  trajectory_constraints_.reset();
1172  }
1173 
1174  std::vector<std::string> getKnownConstraints() const
1175  {
1176  while (initializing_constraints_)
1177  {
1178  std::chrono::duration<double> d(0.01);
1179  rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(d), rclcpp::Context::SharedPtr(nullptr));
1180  }
1181 
1182  std::vector<std::string> c;
1183  if (constraints_storage_)
1184  constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name);
1185 
1186  return c;
1187  }
1188 
1189  moveit_msgs::msg::Constraints getPathConstraints() const
1190  {
1191  if (path_constraints_)
1192  {
1193  return *path_constraints_;
1194  }
1195  else
1196  {
1197  return moveit_msgs::msg::Constraints();
1198  }
1199  }
1200 
1201  moveit_msgs::msg::TrajectoryConstraints getTrajectoryConstraints() const
1202  {
1203  if (trajectory_constraints_)
1204  {
1205  return *trajectory_constraints_;
1206  }
1207  else
1208  {
1209  return moveit_msgs::msg::TrajectoryConstraints();
1210  }
1211  }
1212 
1213  void initializeConstraintsStorage(const std::string& host, unsigned int port)
1214  {
1215  initializing_constraints_ = true;
1216  if (constraints_init_thread_)
1217  constraints_init_thread_->join();
1218  constraints_init_thread_ =
1219  std::make_unique<std::thread>([this, host, port] { initializeConstraintsStorageThread(host, port); });
1220  }
1221 
1222  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
1223  {
1224  workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame();
1225  workspace_parameters_.header.stamp = getClock()->now();
1226  workspace_parameters_.min_corner.x = minx;
1227  workspace_parameters_.min_corner.y = miny;
1228  workspace_parameters_.min_corner.z = minz;
1229  workspace_parameters_.max_corner.x = maxx;
1230  workspace_parameters_.max_corner.y = maxy;
1231  workspace_parameters_.max_corner.z = maxz;
1232  }
1233 
1234  rclcpp::Clock::SharedPtr getClock()
1235  {
1236  return node_->get_clock();
1237  }
1238 
1239 private:
1240  void initializeConstraintsStorageThread(const std::string& host, unsigned int port)
1241  {
1242  // Set up db
1243  try
1244  {
1245  warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase(node_);
1246  conn->setParams(host, port);
1247  if (conn->connect())
1248  {
1249  constraints_storage_ = std::make_unique<moveit_warehouse::ConstraintsStorage>(conn);
1250  }
1251  }
1252  catch (std::exception& ex)
1253  {
1254  RCLCPP_ERROR(LOGGER, "%s", ex.what());
1255  }
1256  initializing_constraints_ = false;
1257  }
1258 
1259  Options opt_;
1260  rclcpp::Node::SharedPtr node_;
1261  rclcpp::CallbackGroup::SharedPtr callback_group_;
1262  rclcpp::executors::SingleThreadedExecutor callback_executor_;
1263  std::thread callback_thread_;
1264  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
1265  moveit::core::RobotModelConstPtr robot_model_;
1266  planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
1267 
1268  std::shared_ptr<rclcpp_action::Client<moveit_msgs::action::MoveGroup>> move_action_client_;
1269  // std::shared_ptr<rclcpp_action::Client<moveit_msgs::action::Pickup>> pick_action_client_;
1270  // std::shared_ptr<rclcpp_action::Client<moveit_msgs::action::Place>> place_action_client_;
1271  std::shared_ptr<rclcpp_action::Client<moveit_msgs::action::ExecuteTrajectory>> execute_action_client_;
1272 
1273  // general planning params
1274  moveit::core::RobotStatePtr considered_start_state_;
1275  moveit_msgs::msg::WorkspaceParameters workspace_parameters_;
1276  double allowed_planning_time_;
1277  std::string planning_pipeline_id_;
1278  std::string planner_id_;
1279  unsigned int num_planning_attempts_;
1280  double max_velocity_scaling_factor_;
1281  double max_acceleration_scaling_factor_;
1282  double goal_joint_tolerance_;
1283  double goal_position_tolerance_;
1284  double goal_orientation_tolerance_;
1285  bool can_look_;
1286  int32_t look_around_attempts_;
1287  bool can_replan_;
1288  int32_t replan_attempts_;
1289  double replan_delay_;
1290 
1291  // joint state goal
1292  moveit::core::RobotStatePtr joint_state_target_;
1293  const moveit::core::JointModelGroup* joint_model_group_;
1294 
1295  // pose goal;
1296  // for each link we have a set of possible goal locations;
1297  std::map<std::string, std::vector<geometry_msgs::msg::PoseStamped>> pose_targets_;
1298 
1299  // common properties for goals
1300  ActiveTargetType active_target_;
1301  std::unique_ptr<moveit_msgs::msg::Constraints> path_constraints_;
1302  std::unique_ptr<moveit_msgs::msg::TrajectoryConstraints> trajectory_constraints_;
1303  std::string end_effector_link_;
1304  std::string pose_reference_frame_;
1305  std::string support_surface_;
1306 
1307  // ROS communication
1308  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr trajectory_event_publisher_;
1309  rclcpp::Publisher<moveit_msgs::msg::AttachedCollisionObject>::SharedPtr attached_object_publisher_;
1310  rclcpp::Client<moveit_msgs::srv::QueryPlannerInterfaces>::SharedPtr query_service_;
1311  rclcpp::Client<moveit_msgs::srv::GetPlannerParams>::SharedPtr get_params_service_;
1312  rclcpp::Client<moveit_msgs::srv::SetPlannerParams>::SharedPtr set_params_service_;
1313  rclcpp::Client<moveit_msgs::srv::GetCartesianPath>::SharedPtr cartesian_path_service_;
1314  // rclcpp::Client<moveit_msgs::srv::GraspPlanning>::SharedPtr plan_grasps_service_;
1315  std::unique_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
1316  std::unique_ptr<std::thread> constraints_init_thread_;
1317  bool initializing_constraints_;
1318 };
1319 
1320 MoveGroupInterface::MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const std::string& group_name,
1321  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1322  const rclcpp::Duration& wait_for_servers)
1323 {
1324  if (!rclcpp::ok())
1325  throw std::runtime_error("ROS does not seem to be running");
1326  impl_ =
1327  new MoveGroupInterfaceImpl(node, Options(group_name), tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers);
1328 }
1329 
1330 MoveGroupInterface::MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const Options& opt,
1331  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1332  const rclcpp::Duration& wait_for_servers)
1333 {
1334  impl_ = new MoveGroupInterfaceImpl(node, opt, tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers);
1335 }
1336 
1338 {
1339  delete impl_;
1340 }
1341 
1343  : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_)
1344 {
1345  other.impl_ = nullptr;
1346 }
1347 
1349 {
1350  if (this != &other)
1351  {
1352  delete impl_;
1353  impl_ = other.impl_;
1354  remembered_joint_values_ = std::move(other.remembered_joint_values_);
1355  other.impl_ = nullptr;
1356  }
1357 
1358  return *this;
1359 }
1360 
1361 const std::string& MoveGroupInterface::getName() const
1362 {
1363  return impl_->getOptions().group_name;
1364 }
1365 
1366 const std::vector<std::string>& MoveGroupInterface::getNamedTargets() const
1367 {
1368  // The pointer returned by getJointModelGroup is guaranteed by the class
1369  // constructor to always be non-null
1370  return impl_->getJointModelGroup()->getDefaultStateNames();
1371 }
1372 
1373 moveit::core::RobotModelConstPtr MoveGroupInterface::getRobotModel() const
1374 {
1375  return impl_->getRobotModel();
1376 }
1377 
1378 bool MoveGroupInterface::getInterfaceDescription(moveit_msgs::msg::PlannerInterfaceDescription& desc) const
1379 {
1380  return impl_->getInterfaceDescription(desc);
1381 }
1382 
1383 bool MoveGroupInterface::getInterfaceDescriptions(std::vector<moveit_msgs::msg::PlannerInterfaceDescription>& desc) const
1384 {
1385  return impl_->getInterfaceDescriptions(desc);
1386 }
1387 
1388 std::map<std::string, std::string> MoveGroupInterface::getPlannerParams(const std::string& planner_id,
1389  const std::string& group) const
1390 {
1391  return impl_->getPlannerParams(planner_id, group);
1392 }
1393 
1394 void MoveGroupInterface::setPlannerParams(const std::string& planner_id, const std::string& group,
1395  const std::map<std::string, std::string>& params, bool replace)
1396 {
1397  impl_->setPlannerParams(planner_id, group, params, replace);
1398 }
1399 
1401 {
1402  return impl_->getDefaultPlanningPipelineId();
1403 }
1404 
1405 void MoveGroupInterface::setPlanningPipelineId(const std::string& pipeline_id)
1406 {
1407  impl_->setPlanningPipelineId(pipeline_id);
1408 }
1409 
1411 {
1412  return impl_->getPlanningPipelineId();
1413 }
1414 
1415 std::string MoveGroupInterface::getDefaultPlannerId(const std::string& group) const
1416 {
1417  return impl_->getDefaultPlannerId(group);
1418 }
1419 
1420 void MoveGroupInterface::setPlannerId(const std::string& planner_id)
1421 {
1422  impl_->setPlannerId(planner_id);
1423 }
1424 
1425 const std::string& MoveGroupInterface::getPlannerId() const
1426 {
1427  return impl_->getPlannerId();
1428 }
1429 
1430 void MoveGroupInterface::setNumPlanningAttempts(unsigned int num_planning_attempts)
1431 {
1432  impl_->setNumPlanningAttempts(num_planning_attempts);
1433 }
1434 
1435 void MoveGroupInterface::setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
1436 {
1437  impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
1438 }
1439 
1440 void MoveGroupInterface::setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
1441 {
1442  impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
1443 }
1444 
1446 {
1447  return impl_->move(false);
1448 }
1449 
1450 rclcpp_action::Client<moveit_msgs::action::MoveGroup>& MoveGroupInterface::getMoveGroupClient() const
1451 {
1452  return impl_->getMoveGroupClient();
1453 }
1454 
1456 {
1457  return impl_->move(true);
1458 }
1459 
1461  const std::vector<std::string>& controllers)
1462 {
1463  return impl_->execute(plan.trajectory, false, controllers);
1464 }
1465 
1466 moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory,
1467  const std::vector<std::string>& controllers)
1468 {
1469  return impl_->execute(trajectory, false, controllers);
1470 }
1471 
1472 moveit::core::MoveItErrorCode MoveGroupInterface::execute(const Plan& plan, const std::vector<std::string>& controllers)
1473 {
1474  return impl_->execute(plan.trajectory, true, controllers);
1475 }
1476 
1477 moveit::core::MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::msg::RobotTrajectory& trajectory,
1478  const std::vector<std::string>& controllers)
1479 {
1480  return impl_->execute(trajectory, true, controllers);
1481 }
1482 
1484 {
1485  return impl_->plan(plan);
1486 }
1487 
1488 // moveit_msgs::action::Pickup::Goal MoveGroupInterface::constructPickupGoal(const std::string& object,
1489 // std::vector<moveit_msgs::msg::Grasp> grasps,
1490 // bool plan_only = false) const
1491 //{
1492 // return impl_->constructPickupGoal(object, std::move(grasps), plan_only);
1493 //}
1494 //
1495 // moveit_msgs::action::Place::Goal MoveGroupInterface::constructPlaceGoal(
1496 // const std::string& object, std::vector<moveit_msgs::msg::PlaceLocation> locations, bool plan_only = false) const
1497 //{
1498 // return impl_->constructPlaceGoal(object, std::move(locations), plan_only);
1499 //}
1500 //
1501 // std::vector<moveit_msgs::msg::PlaceLocation>
1502 // MoveGroupInterface::posesToPlaceLocations(const std::vector<geometry_msgs::msg::PoseStamped>& poses) const
1503 //{
1504 // return impl_->posesToPlaceLocations(poses);
1505 //}
1506 //
1507 // moveit::core::MoveItErrorCode MoveGroupInterface::pick(const moveit_msgs::action::Pickup::Goal& goal)
1508 //{
1509 // return impl_->pick(goal);
1510 //}
1511 //
1512 // moveit::core::MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const std::string& object, bool plan_only)
1513 //{
1514 // return impl_->planGraspsAndPick(object, plan_only);
1515 //}
1516 //
1517 // moveit::core::MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const moveit_msgs::msg::CollisionObject& object,
1518 // bool plan_only)
1519 //{
1520 // return impl_->planGraspsAndPick(object, plan_only);
1521 //}
1522 //
1523 // moveit::core::MoveItErrorCode MoveGroupInterface::place(const moveit_msgs::action::Place::Goal& goal)
1524 //{
1525 // return impl_->place(goal);
1526 //}
1527 
1528 double MoveGroupInterface::computeCartesianPath(const std::vector<geometry_msgs::msg::Pose>& waypoints, double eef_step,
1529  double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory,
1530  bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code)
1531 {
1532  moveit_msgs::msg::Constraints path_constraints_tmp;
1533  return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, moveit_msgs::msg::Constraints(),
1534  avoid_collisions, error_code);
1535 }
1536 
1537 double MoveGroupInterface::computeCartesianPath(const std::vector<geometry_msgs::msg::Pose>& waypoints, double eef_step,
1538  double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory,
1539  const moveit_msgs::msg::Constraints& path_constraints,
1540  bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code)
1541 {
1542  if (error_code)
1543  {
1544  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1545  avoid_collisions, *error_code);
1546  }
1547  else
1548  {
1549  moveit_msgs::msg::MoveItErrorCodes err_tmp;
1550  err_tmp.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
1551  moveit_msgs::msg::MoveItErrorCodes& err = error_code ? *error_code : err_tmp;
1552  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1553  avoid_collisions, err);
1554  }
1555 }
1556 
1558 {
1559  impl_->stop();
1560 }
1561 
1562 void MoveGroupInterface::setStartState(const moveit_msgs::msg::RobotState& start_state)
1563 {
1564  moveit::core::RobotStatePtr rs;
1565  if (start_state.is_diff)
1566  {
1567  impl_->getCurrentState(rs);
1568  }
1569  else
1570  {
1571  rs = std::make_shared<moveit::core::RobotState>(getRobotModel());
1572  rs->setToDefaultValues(); // initialize robot state values for conversion
1573  }
1574  moveit::core::robotStateMsgToRobotState(start_state, *rs);
1575  setStartState(*rs);
1576 }
1577 
1579 {
1580  impl_->setStartState(start_state);
1581 }
1582 
1584 {
1585  impl_->setStartStateToCurrentState();
1586 }
1587 
1589 {
1591  impl_->setTargetType(JOINT);
1592 }
1593 
1594 const std::vector<std::string>& MoveGroupInterface::getJointNames() const
1595 {
1596  return impl_->getJointModelGroup()->getVariableNames();
1597 }
1598 
1599 const std::vector<std::string>& MoveGroupInterface::getLinkNames() const
1600 {
1601  return impl_->getJointModelGroup()->getLinkModelNames();
1602 }
1603 
1604 std::map<std::string, double> MoveGroupInterface::getNamedTargetValues(const std::string& name) const
1605 {
1606  std::map<std::string, std::vector<double>>::const_iterator it = remembered_joint_values_.find(name);
1607  std::map<std::string, double> positions;
1608 
1609  if (it != remembered_joint_values_.cend())
1610  {
1611  std::vector<std::string> names = impl_->getJointModelGroup()->getVariableNames();
1612  for (size_t x = 0; x < names.size(); ++x)
1613  {
1614  positions[names[x]] = it->second[x];
1615  }
1616  }
1617  else
1618  {
1619  if (!impl_->getJointModelGroup()->getVariableDefaultPositions(name, positions))
1620  {
1621  RCLCPP_ERROR(LOGGER, "The requested named target '%s' does not exist, returning empty positions.", name.c_str());
1622  }
1623  }
1624  return positions;
1625 }
1626 
1628 {
1629  std::map<std::string, std::vector<double>>::const_iterator it = remembered_joint_values_.find(name);
1630  if (it != remembered_joint_values_.end())
1631  {
1632  return setJointValueTarget(it->second);
1633  }
1634  else
1635  {
1637  {
1638  impl_->setTargetType(JOINT);
1639  return true;
1640  }
1641  RCLCPP_ERROR(LOGGER, "The requested named target '%s' does not exist", name.c_str());
1642  return false;
1643  }
1644 }
1645 
1646 void MoveGroupInterface::getJointValueTarget(std::vector<double>& group_variable_values) const
1647 {
1648  impl_->getTargetRobotState().copyJointGroupPositions(impl_->getJointModelGroup(), group_variable_values);
1649 }
1650 
1651 bool MoveGroupInterface::setJointValueTarget(const std::vector<double>& joint_values)
1652 {
1653  const auto n_group_joints = impl_->getJointModelGroup()->getVariableCount();
1654  if (joint_values.size() != n_group_joints)
1655  {
1656  RCLCPP_DEBUG_STREAM(LOGGER, "Provided joint value list has length " << joint_values.size() << " but group "
1657  << impl_->getJointModelGroup()->getName()
1658  << " has " << n_group_joints << " joints");
1659  return false;
1660  }
1661  impl_->setTargetType(JOINT);
1662  impl_->getTargetRobotState().setJointGroupPositions(impl_->getJointModelGroup(), joint_values);
1664 }
1665 
1666 bool MoveGroupInterface::setJointValueTarget(const std::map<std::string, double>& variable_values)
1667 {
1668  const auto& allowed = impl_->getJointModelGroup()->getVariableNames();
1669  for (const auto& pair : variable_values)
1670  {
1671  if (std::find(allowed.begin(), allowed.end(), pair.first) == allowed.end())
1672  {
1673  RCLCPP_ERROR_STREAM(LOGGER, "joint variable " << pair.first << " is not part of group "
1674  << impl_->getJointModelGroup()->getName());
1675  return false;
1676  }
1677  }
1678 
1679  impl_->setTargetType(JOINT);
1680  impl_->getTargetRobotState().setVariablePositions(variable_values);
1681  return impl_->getTargetRobotState().satisfiesBounds(impl_->getGoalJointTolerance());
1682 }
1683 
1684 bool MoveGroupInterface::setJointValueTarget(const std::vector<std::string>& variable_names,
1685  const std::vector<double>& variable_values)
1686 {
1687  if (variable_names.size() != variable_values.size())
1688  {
1689  RCLCPP_ERROR_STREAM(LOGGER, "sizes of name and position arrays do not match");
1690  return false;
1691  }
1692  const auto& allowed = impl_->getJointModelGroup()->getVariableNames();
1693  for (const auto& variable_name : variable_names)
1694  {
1695  if (std::find(allowed.begin(), allowed.end(), variable_name) == allowed.end())
1696  {
1697  RCLCPP_ERROR_STREAM(LOGGER, "joint variable " << variable_name << " is not part of group "
1698  << impl_->getJointModelGroup()->getName());
1699  return false;
1700  }
1701  }
1702 
1703  impl_->setTargetType(JOINT);
1704  impl_->getTargetRobotState().setVariablePositions(variable_names, variable_values);
1705  return impl_->getTargetRobotState().satisfiesBounds(impl_->getGoalJointTolerance());
1706 }
1707 
1709 {
1710  impl_->setTargetType(JOINT);
1711  impl_->getTargetRobotState() = rstate;
1712  return impl_->getTargetRobotState().satisfiesBounds(impl_->getGoalJointTolerance());
1713 }
1714 
1715 bool MoveGroupInterface::setJointValueTarget(const std::string& joint_name, double value)
1716 {
1717  std::vector<double> values(1, value);
1718  return setJointValueTarget(joint_name, values);
1719 }
1720 
1721 bool MoveGroupInterface::setJointValueTarget(const std::string& joint_name, const std::vector<double>& values)
1722 {
1723  impl_->setTargetType(JOINT);
1724  const moveit::core::JointModel* jm = impl_->getJointModelGroup()->getJointModel(joint_name);
1725  if (jm && jm->getVariableCount() == values.size())
1726  {
1727  impl_->getTargetRobotState().setJointPositions(jm, values);
1728  return impl_->getTargetRobotState().satisfiesBounds(jm, impl_->getGoalJointTolerance());
1729  }
1730 
1731  RCLCPP_ERROR_STREAM(LOGGER,
1732  "joint " << joint_name << " is not part of group " << impl_->getJointModelGroup()->getName());
1733  return false;
1734 }
1735 
1736 bool MoveGroupInterface::setJointValueTarget(const sensor_msgs::msg::JointState& state)
1737 {
1738  return setJointValueTarget(state.name, state.position);
1739 }
1740 
1741 bool MoveGroupInterface::setJointValueTarget(const geometry_msgs::msg::Pose& eef_pose,
1742  const std::string& end_effector_link)
1743 {
1744  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false);
1745 }
1746 
1747 bool MoveGroupInterface::setJointValueTarget(const geometry_msgs::msg::PoseStamped& eef_pose,
1748  const std::string& end_effector_link)
1749 {
1750  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false);
1751 }
1752 
1753 bool MoveGroupInterface::setJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link)
1754 {
1755  geometry_msgs::msg::Pose msg = tf2::toMsg(eef_pose);
1756  return setJointValueTarget(msg, end_effector_link);
1757 }
1758 
1759 bool MoveGroupInterface::setApproximateJointValueTarget(const geometry_msgs::msg::Pose& eef_pose,
1760  const std::string& end_effector_link)
1761 {
1762  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true);
1763 }
1764 
1765 bool MoveGroupInterface::setApproximateJointValueTarget(const geometry_msgs::msg::PoseStamped& eef_pose,
1766  const std::string& end_effector_link)
1767 {
1768  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true);
1769 }
1770 
1771 bool MoveGroupInterface::setApproximateJointValueTarget(const Eigen::Isometry3d& eef_pose,
1772  const std::string& end_effector_link)
1773 {
1774  geometry_msgs::msg::Pose msg = tf2::toMsg(eef_pose);
1775  return setApproximateJointValueTarget(msg, end_effector_link);
1776 }
1777 
1779 {
1780  return impl_->getTargetRobotState();
1781 }
1782 
1784 {
1785  return impl_->getTargetRobotState();
1786 }
1787 
1788 const std::string& MoveGroupInterface::getEndEffectorLink() const
1789 {
1790  return impl_->getEndEffectorLink();
1791 }
1792 
1793 const std::string& MoveGroupInterface::getEndEffector() const
1794 {
1795  return impl_->getEndEffector();
1796 }
1797 
1798 bool MoveGroupInterface::setEndEffectorLink(const std::string& link_name)
1799 {
1800  if (impl_->getEndEffectorLink().empty() || link_name.empty())
1801  return false;
1802  impl_->setEndEffectorLink(link_name);
1803  impl_->setTargetType(POSE);
1804  return true;
1805 }
1806 
1807 bool MoveGroupInterface::setEndEffector(const std::string& eef_name)
1808 {
1809  const moveit::core::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name);
1810  if (jmg)
1811  return setEndEffectorLink(jmg->getEndEffectorParentGroup().second);
1812  return false;
1813 }
1814 
1815 void MoveGroupInterface::clearPoseTarget(const std::string& end_effector_link)
1816 {
1817  impl_->clearPoseTarget(end_effector_link);
1818 }
1819 
1821 {
1822  impl_->clearPoseTargets();
1823 }
1824 
1825 bool MoveGroupInterface::setPoseTarget(const Eigen::Isometry3d& pose, const std::string& end_effector_link)
1826 {
1827  std::vector<geometry_msgs::msg::PoseStamped> pose_msg(1);
1828  pose_msg[0].pose = tf2::toMsg(pose);
1829  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1830  pose_msg[0].header.stamp = impl_->getClock()->now();
1831  return setPoseTargets(pose_msg, end_effector_link);
1832 }
1833 
1834 bool MoveGroupInterface::setPoseTarget(const geometry_msgs::msg::Pose& target, const std::string& end_effector_link)
1835 {
1836  std::vector<geometry_msgs::msg::PoseStamped> pose_msg(1);
1837  pose_msg[0].pose = target;
1838  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1839  pose_msg[0].header.stamp = impl_->getClock()->now();
1840  return setPoseTargets(pose_msg, end_effector_link);
1841 }
1842 
1843 bool MoveGroupInterface::setPoseTarget(const geometry_msgs::msg::PoseStamped& target,
1844  const std::string& end_effector_link)
1845 {
1846  std::vector<geometry_msgs::msg::PoseStamped> targets(1, target);
1847  return setPoseTargets(targets, end_effector_link);
1848 }
1849 
1850 bool MoveGroupInterface::setPoseTargets(const EigenSTL::vector_Isometry3d& target, const std::string& end_effector_link)
1851 {
1852  std::vector<geometry_msgs::msg::PoseStamped> pose_out(target.size());
1853  rclcpp::Time tm = impl_->getClock()->now();
1854  const std::string& frame_id = getPoseReferenceFrame();
1855  for (std::size_t i = 0; i < target.size(); ++i)
1856  {
1857  pose_out[i].pose = tf2::toMsg(target[i]);
1858  pose_out[i].header.stamp = tm;
1859  pose_out[i].header.frame_id = frame_id;
1860  }
1861  return setPoseTargets(pose_out, end_effector_link);
1862 }
1863 
1864 bool MoveGroupInterface::setPoseTargets(const std::vector<geometry_msgs::msg::Pose>& target,
1865  const std::string& end_effector_link)
1866 {
1867  std::vector<geometry_msgs::msg::PoseStamped> target_stamped(target.size());
1868  rclcpp::Time tm = impl_->getClock()->now();
1869  const std::string& frame_id = getPoseReferenceFrame();
1870  for (std::size_t i = 0; i < target.size(); ++i)
1871  {
1872  target_stamped[i].pose = target[i];
1873  target_stamped[i].header.stamp = tm;
1874  target_stamped[i].header.frame_id = frame_id;
1875  }
1876  return setPoseTargets(target_stamped, end_effector_link);
1877 }
1878 
1879 bool MoveGroupInterface::setPoseTargets(const std::vector<geometry_msgs::msg::PoseStamped>& target,
1880  const std::string& end_effector_link)
1881 {
1882  if (target.empty())
1883  {
1884  RCLCPP_ERROR(LOGGER, "No pose specified as goal target");
1885  return false;
1886  }
1887  else
1888  {
1889  impl_->setTargetType(POSE);
1890  return impl_->setPoseTargets(target, end_effector_link);
1891  }
1892 }
1893 
1894 const geometry_msgs::msg::PoseStamped& MoveGroupInterface::getPoseTarget(const std::string& end_effector_link) const
1895 {
1896  return impl_->getPoseTarget(end_effector_link);
1897 }
1898 
1899 const std::vector<geometry_msgs::msg::PoseStamped>&
1900 MoveGroupInterface::getPoseTargets(const std::string& end_effector_link) const
1901 {
1902  return impl_->getPoseTargets(end_effector_link);
1903 }
1904 
1905 namespace
1906 {
1907 inline void transformPose(const tf2_ros::Buffer& tf_buffer, const std::string& desired_frame,
1908  geometry_msgs::msg::PoseStamped& target)
1909 {
1910  if (desired_frame != target.header.frame_id)
1911  {
1912  geometry_msgs::msg::PoseStamped target_in(target);
1913  tf_buffer.transform(target_in, target, desired_frame);
1914  // we leave the stamp to ros::Time(0) on purpose
1915  target.header.stamp = rclcpp::Time(0);
1916  }
1917 }
1918 } // namespace
1919 
1920 bool MoveGroupInterface::setPositionTarget(double x, double y, double z, const std::string& end_effector_link)
1921 {
1922  geometry_msgs::msg::PoseStamped target;
1923  if (impl_->hasPoseTarget(end_effector_link))
1924  {
1925  target = getPoseTarget(end_effector_link);
1926  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1927  }
1928  else
1929  {
1930  target.pose.orientation.x = 0.0;
1931  target.pose.orientation.y = 0.0;
1932  target.pose.orientation.z = 0.0;
1933  target.pose.orientation.w = 1.0;
1934  target.header.frame_id = impl_->getPoseReferenceFrame();
1935  }
1936 
1937  target.pose.position.x = x;
1938  target.pose.position.y = y;
1939  target.pose.position.z = z;
1940  bool result = setPoseTarget(target, end_effector_link);
1941  impl_->setTargetType(POSITION);
1942  return result;
1943 }
1944 
1945 bool MoveGroupInterface::setRPYTarget(double r, double p, double y, const std::string& end_effector_link)
1946 {
1947  geometry_msgs::msg::PoseStamped target;
1948  if (impl_->hasPoseTarget(end_effector_link))
1949  {
1950  target = getPoseTarget(end_effector_link);
1951  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1952  }
1953  else
1954  {
1955  target.pose.position.x = 0.0;
1956  target.pose.position.y = 0.0;
1957  target.pose.position.z = 0.0;
1958  target.header.frame_id = impl_->getPoseReferenceFrame();
1959  }
1960  tf2::Quaternion q;
1961  q.setRPY(r, p, y);
1962  target.pose.orientation = tf2::toMsg(q);
1963  bool result = setPoseTarget(target, end_effector_link);
1964  impl_->setTargetType(ORIENTATION);
1965  return result;
1966 }
1967 
1968 bool MoveGroupInterface::setOrientationTarget(double x, double y, double z, double w,
1969  const std::string& end_effector_link)
1970 {
1971  geometry_msgs::msg::PoseStamped target;
1972  if (impl_->hasPoseTarget(end_effector_link))
1973  {
1974  target = getPoseTarget(end_effector_link);
1975  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1976  }
1977  else
1978  {
1979  target.pose.position.x = 0.0;
1980  target.pose.position.y = 0.0;
1981  target.pose.position.z = 0.0;
1982  target.header.frame_id = impl_->getPoseReferenceFrame();
1983  }
1984 
1985  target.pose.orientation.x = x;
1986  target.pose.orientation.y = y;
1987  target.pose.orientation.z = z;
1988  target.pose.orientation.w = w;
1989  bool result = setPoseTarget(target, end_effector_link);
1990  impl_->setTargetType(ORIENTATION);
1991  return result;
1992 }
1993 
1994 void MoveGroupInterface::setPoseReferenceFrame(const std::string& pose_reference_frame)
1995 {
1996  impl_->setPoseReferenceFrame(pose_reference_frame);
1997 }
1998 
2000 {
2001  return impl_->getPoseReferenceFrame();
2002 }
2003 
2005 {
2006  return impl_->getGoalJointTolerance();
2007 }
2008 
2010 {
2011  return impl_->getGoalPositionTolerance();
2012 }
2013 
2015 {
2016  return impl_->getGoalOrientationTolerance();
2017 }
2018 
2020 {
2021  setGoalJointTolerance(tolerance);
2022  setGoalPositionTolerance(tolerance);
2023  setGoalOrientationTolerance(tolerance);
2024 }
2025 
2027 {
2028  impl_->setGoalJointTolerance(tolerance);
2029 }
2030 
2032 {
2033  impl_->setGoalPositionTolerance(tolerance);
2034 }
2035 
2037 {
2038  impl_->setGoalOrientationTolerance(tolerance);
2039 }
2040 
2042 {
2044 }
2045 
2047 {
2048  return impl_->startStateMonitor(wait);
2049 }
2050 
2052 {
2053  moveit::core::RobotStatePtr current_state;
2054  std::vector<double> values;
2055  if (impl_->getCurrentState(current_state))
2056  current_state->copyJointGroupPositions(getName(), values);
2057  return values;
2058 }
2059 
2060 std::vector<double> MoveGroupInterface::getRandomJointValues() const
2061 {
2062  std::vector<double> r;
2064  return r;
2065 }
2066 
2067 geometry_msgs::msg::PoseStamped MoveGroupInterface::getRandomPose(const std::string& end_effector_link) const
2068 {
2069  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2070  Eigen::Isometry3d pose;
2071  pose.setIdentity();
2072  if (eef.empty())
2073  {
2074  RCLCPP_ERROR(LOGGER, "No end-effector specified");
2075  }
2076  else
2077  {
2078  moveit::core::RobotStatePtr current_state;
2079  if (impl_->getCurrentState(current_state))
2080  {
2081  current_state->setToRandomPositions(impl_->getJointModelGroup());
2082  const moveit::core::LinkModel* lm = current_state->getLinkModel(eef);
2083  if (lm)
2084  pose = current_state->getGlobalLinkTransform(lm);
2085  }
2086  }
2087  geometry_msgs::msg::PoseStamped pose_msg;
2088  pose_msg.header.stamp = impl_->getClock()->now();
2089  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2090  pose_msg.pose = tf2::toMsg(pose);
2091  return pose_msg;
2092 }
2093 
2094 geometry_msgs::msg::PoseStamped MoveGroupInterface::getCurrentPose(const std::string& end_effector_link) const
2095 {
2096  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2097  Eigen::Isometry3d pose;
2098  pose.setIdentity();
2099  if (eef.empty())
2100  {
2101  RCLCPP_ERROR(LOGGER, "No end-effector specified");
2102  }
2103  else
2104  {
2105  moveit::core::RobotStatePtr current_state;
2106  if (impl_->getCurrentState(current_state))
2107  {
2108  const moveit::core::LinkModel* lm = current_state->getLinkModel(eef);
2109  if (lm)
2110  pose = current_state->getGlobalLinkTransform(lm);
2111  }
2112  }
2113  geometry_msgs::msg::PoseStamped pose_msg;
2114  pose_msg.header.stamp = impl_->getClock()->now();
2115  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2116  pose_msg.pose = tf2::toMsg(pose);
2117  return pose_msg;
2118 }
2119 
2120 std::vector<double> MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link) const
2121 {
2122  std::vector<double> result;
2123  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2124  if (eef.empty())
2125  {
2126  RCLCPP_ERROR(LOGGER, "No end-effector specified");
2127  }
2128  else
2129  {
2130  moveit::core::RobotStatePtr current_state;
2131  if (impl_->getCurrentState(current_state))
2132  {
2133  const moveit::core::LinkModel* lm = current_state->getLinkModel(eef);
2134  if (lm)
2135  {
2136  result.resize(3);
2137  geometry_msgs::msg::TransformStamped tfs = tf2::eigenToTransform(current_state->getGlobalLinkTransform(lm));
2138  double pitch, roll, yaw;
2139  tf2::getEulerYPR<geometry_msgs::msg::Quaternion>(tfs.transform.rotation, yaw, pitch, roll);
2140  result[0] = roll;
2141  result[1] = pitch;
2142  result[2] = yaw;
2143  }
2144  }
2145  }
2146  return result;
2147 }
2148 
2149 const std::vector<std::string>& MoveGroupInterface::getActiveJoints() const
2150 {
2151  return impl_->getJointModelGroup()->getActiveJointModelNames();
2152 }
2153 
2154 const std::vector<std::string>& MoveGroupInterface::getJoints() const
2155 {
2156  return impl_->getJointModelGroup()->getJointModelNames();
2157 }
2158 
2160 {
2161  return impl_->getJointModelGroup()->getVariableCount();
2162 }
2163 
2164 moveit::core::RobotStatePtr MoveGroupInterface::getCurrentState(double wait) const
2165 {
2166  moveit::core::RobotStatePtr current_state;
2167  impl_->getCurrentState(current_state, wait);
2168  return current_state;
2169 }
2170 
2171 void MoveGroupInterface::rememberJointValues(const std::string& name, const std::vector<double>& values)
2172 {
2173  remembered_joint_values_[name] = values;
2174 }
2175 
2177 {
2178  remembered_joint_values_.erase(name);
2179 }
2180 
2182 {
2183  impl_->can_look_ = flag;
2184  RCLCPP_DEBUG(LOGGER, "Looking around: %s", flag ? "yes" : "no");
2185 }
2186 
2188 {
2189  if (attempts < 0)
2190  {
2191  RCLCPP_ERROR(LOGGER, "Tried to set negative number of look-around attempts");
2192  }
2193  else
2194  {
2195  RCLCPP_DEBUG_STREAM(LOGGER, "Look around attempts: " << attempts);
2196  impl_->look_around_attempts_ = attempts;
2197  }
2198 }
2199 
2201 {
2202  impl_->can_replan_ = flag;
2203  RCLCPP_DEBUG(LOGGER, "Replanning: %s", flag ? "yes" : "no");
2204 }
2205 
2207 {
2208  if (attempts < 0)
2209  {
2210  RCLCPP_ERROR(LOGGER, "Tried to set negative number of replan attempts");
2211  }
2212  else
2213  {
2214  RCLCPP_DEBUG_STREAM(LOGGER, "Replan Attempts: " << attempts);
2215  impl_->replan_attempts_ = attempts;
2216  }
2217 }
2218 
2220 {
2221  if (delay < 0.0)
2222  {
2223  RCLCPP_ERROR(LOGGER, "Tried to set negative replan delay");
2224  }
2225  else
2226  {
2227  RCLCPP_DEBUG_STREAM(LOGGER, "Replan Delay: " << delay);
2228  impl_->replan_delay_ = delay;
2229  }
2230 }
2231 
2232 std::vector<std::string> MoveGroupInterface::getKnownConstraints() const
2233 {
2234  return impl_->getKnownConstraints();
2235 }
2236 
2237 moveit_msgs::msg::Constraints MoveGroupInterface::getPathConstraints() const
2238 {
2239  return impl_->getPathConstraints();
2240 }
2241 
2242 bool MoveGroupInterface::setPathConstraints(const std::string& constraint)
2243 {
2244  return impl_->setPathConstraints(constraint);
2245 }
2246 
2247 void MoveGroupInterface::setPathConstraints(const moveit_msgs::msg::Constraints& constraint)
2248 {
2249  impl_->setPathConstraints(constraint);
2250 }
2251 
2253 {
2254  impl_->clearPathConstraints();
2255 }
2256 
2257 moveit_msgs::msg::TrajectoryConstraints MoveGroupInterface::getTrajectoryConstraints() const
2258 {
2259  return impl_->getTrajectoryConstraints();
2260 }
2261 
2262 void MoveGroupInterface::setTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints& constraint)
2263 {
2264  impl_->setTrajectoryConstraints(constraint);
2265 }
2266 
2268 {
2269  impl_->clearTrajectoryConstraints();
2270 }
2271 
2272 void MoveGroupInterface::setConstraintsDatabase(const std::string& host, unsigned int port)
2273 {
2274  impl_->initializeConstraintsStorage(host, port);
2275 }
2276 
2277 void MoveGroupInterface::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
2278 {
2279  impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz);
2280 }
2281 
2284 {
2285  impl_->setPlanningTime(seconds);
2286 }
2287 
2290 {
2291  return impl_->getPlanningTime();
2292 }
2293 
2295 {
2296  impl_->setSupportSurfaceName(name);
2297 }
2298 
2299 const std::string& MoveGroupInterface::getPlanningFrame() const
2300 {
2301  return impl_->getRobotModel()->getModelFrame();
2302 }
2303 
2304 const std::vector<std::string>& MoveGroupInterface::getJointModelGroupNames() const
2305 {
2306  return impl_->getRobotModel()->getJointModelGroupNames();
2307 }
2308 
2309 bool MoveGroupInterface::attachObject(const std::string& object, const std::string& link)
2310 {
2311  return attachObject(object, link, std::vector<std::string>());
2312 }
2313 
2314 bool MoveGroupInterface::attachObject(const std::string& object, const std::string& link,
2315  const std::vector<std::string>& touch_links)
2316 {
2317  return impl_->attachObject(object, link, touch_links);
2318 }
2319 
2320 bool MoveGroupInterface::detachObject(const std::string& name)
2321 {
2322  return impl_->detachObject(name);
2323 }
2324 
2326 {
2327  impl_->constructMotionPlanRequest(goal_out);
2328 }
2329 
2330 } // namespace planning_interface
2331 } // namespace moveit
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
bool isChain() const
Check if this group is a linear chain.
const std::string & getName() const
Get the name of the joint group.
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute random values for the state of the joint group.
const std::vector< std::string > & getJointModelNames() const
Get the names of the joints in this group. These are the names of the joints returned by getJointMode...
const std::pair< std::string, std::string > & getEndEffectorParentGroup() const
Get the name of the group this end-effector attaches to (first) and the name of the link in that grou...
const JointModel * getJointModel(const std::string &joint) const
Get a joint by its name. Throw an exception if the joint is not part of this group.
bool hasLinkModel(const std::string &link) const
Check if a link is part of this group.
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up the joints included in this group. The number of returned...
bool getVariableDefaultPositions(const std::string &name, std::map< std::string, double > &values) const
Get the values that correspond to a named state as read from the URDF. Return false on failure.
const std::vector< std::string > & getDefaultStateNames() const
Get the names of the known default states (as specified in the SRDF)
const std::vector< std::string > & getActiveJointModelNames() const
Get the names of the active joints in this group. These are the names of the joints returned by getJo...
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:117
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:210
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:72
a wrapper around moveit_msgs::MoveItErrorCodes to make it easier to return an error code message from...
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
Definition: robot_state.h:605
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
Definition: robot_state.h:691
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
bool satisfiesBounds(double margin=0.0) const
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
Return the instance of a random number generator.
Definition: robot_state.h:1717
bool setFromIK(const JointModelGroup *group, const geometry_msgs::msg::Pose &pose, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const kinematics::KinematicsBase::IKCostFn &cost_function=kinematics::KinematicsBase::IKCostFn())
If the group this state corresponds to is a chain and a solver is available, then the joint values ca...
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:515
static bool sameFrame(const std::string &frame1, const std::string &frame2)
Check if two frames end up being the same once the missing / are added as prefix (if they are missing...
Definition: transforms.cpp:64
void constructGoal(moveit_msgs::action::MoveGroup::Goal &goal) const
double computeCartesianPath(const std::vector< geometry_msgs::msg::Pose > &waypoints, double step, double jump_threshold, moveit_msgs::msg::RobotTrajectory &msg, const moveit_msgs::msg::Constraints &path_constraints, bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes &error_code)
void setMaxScalingFactor(double &variable, const double target_value, const char *factor_name, double fallback_value)
bool getCurrentState(moveit::core::RobotStatePtr &current_state, double wait_seconds=1.0)
rclcpp_action::Client< moveit_msgs::action::MoveGroup > & getMoveGroupClient() const
bool attachObject(const std::string &object, const std::string &link, const std::vector< std::string > &touch_links)
void initializeConstraintsStorage(const std::string &host, unsigned int port)
void setPathConstraints(const moveit_msgs::msg::Constraints &constraint)
std::map< std::string, std::string > getPlannerParams(const std::string &planner_id, const std::string &group="")
const geometry_msgs::msg::PoseStamped & getPoseTarget(const std::string &end_effector_link) const
void setTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints &constraint)
void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest &request) const
moveit_msgs::msg::TrajectoryConstraints getTrajectoryConstraints() const
MoveGroupInterfaceImpl(const rclcpp::Node::SharedPtr &node, const Options &opt, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer, const rclcpp::Duration &wait_for_servers)
bool setPoseTargets(const std::vector< geometry_msgs::msg::PoseStamped > &poses, const std::string &end_effector_link)
moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory &trajectory, bool wait, const std::vector< std::string > &controllers=std::vector< std::string >())
bool getInterfaceDescription(moveit_msgs::msg::PlannerInterfaceDescription &desc)
bool setJointValueTarget(const geometry_msgs::msg::Pose &eef_pose, const std::string &end_effector_link, const std::string &frame, bool approx)
const std::vector< geometry_msgs::msg::PoseStamped > & getPoseTargets(const std::string &end_effector_link) const
void setPlannerParams(const std::string &planner_id, const std::string &group, const std::map< std::string, std::string > &params, bool replace=false)
bool getInterfaceDescriptions(std::vector< moveit_msgs::msg::PlannerInterfaceDescription > &desc)
void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
Client class to conveniently use the ROS interfaces provided by the move_group node.
double computeCartesianPath(const std::vector< geometry_msgs::msg::Pose > &waypoints, double eef_step, double jump_threshold, moveit_msgs::msg::RobotTrajectory &trajectory, bool avoid_collisions=true, moveit_msgs::msg::MoveItErrorCodes *error_code=nullptr)
Compute a Cartesian path that follows specified waypoints with a step size of at most eef_step meters...
std::vector< double > getRandomJointValues() const
Get random joint values for the joints planned for by this instance (see getJoints())
void setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0,...
const std::string & getEndEffectorLink() const
Get the current end-effector link. This returns the value set by setEndEffectorLink() (or indirectly ...
static const std::string ROBOT_DESCRIPTION
Default ROS parameter name from where to read the robot's URDF. Set to 'robot_description'.
const std::vector< std::string > & getNamedTargets() const
Get the names of the named robot states available as targets, both either remembered states or defaul...
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.
void stop()
Stop any trajectory execution, if one is active.
MoveGroupInterface(const rclcpp::Node::SharedPtr &node, const Options &opt, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >(), const rclcpp::Duration &wait_for_servers=rclcpp::Duration::from_seconds(-1))
Construct a MoveGroupInterface instance call using a specified set of options opt.
const std::string & getPlannerId() const
Get the current planner_id.
void setReplanDelay(double delay)
Sleep this duration between replanning attempts (in walltime seconds)
moveit::core::MoveItErrorCode plan(Plan &plan)
Compute a motion plan that takes the group declared in the constructor from the current state to the ...
void setGoalTolerance(double tolerance)
Set the tolerance that is used for reaching the goal. For joint state goals, this will be distance fo...
void setGoalPositionTolerance(double tolerance)
Set the position tolerance that is used for reaching the goal when moving to a pose.
const std::string & getPlanningFrame() const
Get the name of the frame in which the robot is planning.
bool setPoseTargets(const EigenSTL::vector_Isometry3d &end_effector_pose, const std::string &end_effector_link="")
Set goal poses for end_effector_link.
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.
moveit::core::MoveItErrorCode asyncMove()
Plan and execute a trajectory that takes the group of joints declared in the constructor to the speci...
void setPlanningPipelineId(const std::string &pipeline_id)
Specify a planning pipeline to be used for further planning.
double getGoalJointTolerance() const
Get the tolerance that is used for reaching a joint goal. This is distance for each joint in configur...
bool setEndEffectorLink(const std::string &end_effector_link)
Specify the parent link of the end-effector. This end_effector_link will be used in calls to pose tar...
void setStartStateToCurrentState()
Set the starting state for planning to be that reported by the robot's joint state publication.
bool getInterfaceDescriptions(std::vector< moveit_msgs::msg::PlannerInterfaceDescription > &desc) const
Get the descriptions of all planning plugins loaded by the action server.
void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0...
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).
void setSupportSurfaceName(const std::string &name)
For pick/place operations, the name of the support surface is used to specify the fact that attached ...
const std::string & getEndEffector() const
Get the current end-effector name. This returns the value set by setEndEffector() (or indirectly by s...
void clearPathConstraints()
Specify that no path constraints are to be used. This removes any path constraints set in previous ca...
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....
moveit_msgs::msg::Constraints getPathConstraints() const
Get the actual set of constraints in use with this MoveGroupInterface.
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...
rclcpp_action::Client< moveit_msgs::action::MoveGroup > & getMoveGroupClient() const
Get the move_group action client used by the MoveGroupInterface. The client can be used for querying ...
MoveGroupInterface & operator=(const MoveGroupInterface &)=delete
std::vector< double > getCurrentJointValues() const
Get the current joint values for the joints planned for by this instance (see getJoints())
bool setNamedTarget(const std::string &name)
Set the current joint values to be ones previously remembered by rememberJointValues() or,...
const std::vector< std::string > & getJointNames() const
Get vector of names of joints available in move group.
moveit::core::MoveItErrorCode move()
Plan and execute a trajectory that takes the group of joints declared in the constructor to the speci...
void setReplanAttempts(int32_t attempts)
Maximum number of replanning attempts.
void allowReplanning(bool flag)
Specify whether the robot is allowed to replan if it detects changes in the environment.
moveit::core::RobotModelConstPtr getRobotModel() const
Get the RobotModel object.
moveit::core::MoveItErrorCode execute(const Plan &plan, const std::vector< std::string > &controllers=std::vector< std::string >())
Given a plan, execute it while waiting for completion.
void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest &request)
Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and sto...
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,...
bool setJointValueTarget(const std::vector< double > &group_variable_values)
Set the JointValueTarget and use it for future planning requests.
bool setRPYTarget(double roll, double pitch, double yaw, const std::string &end_effector_link="")
Set the goal orientation of the end-effector end_effector_link to be (roll,pitch,yaw) radians.
void setGoalOrientationTolerance(double tolerance)
Set the orientation tolerance that is used for reaching the goal when moving to a pose.
void clearPoseTarget(const std::string &end_effector_link="")
Forget pose(s) specified for end_effector_link.
const geometry_msgs::msg::PoseStamped & getPoseTarget(const std::string &end_effector_link="") const
void setGoalJointTolerance(double tolerance)
Set the joint tolerance (for each joint) that is used for reaching the goal when moving to a joint va...
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...
void setRandomTarget()
Set the joint state goal to a random joint configuration.
moveit::core::RobotStatePtr getCurrentState(double wait=1) const
Get the current state of the robot within the duration specified by wait.
const std::vector< std::string > & getActiveJoints() const
Get only the active (actuated) joints this instance operates on.
void rememberJointValues(const std::string &name)
Remember the current joint values (of the robot being monitored) under name. These can be used by set...
void setLookAroundAttempts(int32_t attempts)
How often is the system allowed to move the camera to update environment model when looking.
bool getInterfaceDescription(moveit_msgs::msg::PlannerInterfaceDescription &desc) const
Get the description of the default planning plugin loaded by the action server.
const std::string & getName() const
Get the name of the group this instance operates on.
moveit::core::MoveItErrorCode asyncExecute(const Plan &plan, const std::vector< std::string > &controllers=std::vector< std::string >())
Given a plan, execute it without waiting for completion.
const std::vector< std::string > & getJoints() const
Get all the joints this instance operates on (including fixed joints)
bool detachObject(const std::string &name="")
Detach an object. name specifies the name of the object attached to this group, or the name of the li...
void setPoseReferenceFrame(const std::string &pose_reference_frame)
Specify which reference frame to assume for poses specified without a reference frame.
const std::vector< geometry_msgs::msg::PoseStamped > & getPoseTargets(const std::string &end_effector_link="") const
moveit_msgs::msg::TrajectoryConstraints getTrajectoryConstraints() const
const std::vector< std::string > & getLinkNames() const
Get vector of names of links available in move group.
void setStartState(const moveit_msgs::msg::RobotState &start_state)
If a different start state should be considered instead of the current state of the robot,...
bool setPathConstraints(const std::string &constraint)
Specify a set of path constraints to use. The constraints are looked up by name from the Mongo databa...
void clearPoseTargets()
Forget any poses specified for all end-effectors.
void setPlannerId(const std::string &planner_id)
Specify a planner to be used for further planning.
bool setEndEffector(const std::string &eef_name)
Specify the name of the end-effector to use. This is equivalent to setting the EndEffectorLink to the...
const std::string & getPoseReferenceFrame() const
Get the reference frame set by setPoseReferenceFrame(). By default this is the reference frame of the...
const std::string & getPlanningPipelineId() const
Get the current planning_pipeline_id.
void setConstraintsDatabase(const std::string &host, unsigned int port)
Specify where the database server that holds known constraints resides.
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...
void forgetJointValues(const std::string &name)
Forget the joint values remembered under name.
std::vector< std::string > getKnownConstraints() const
Get the names of the known constraints as read from the Mongo database, if a connection was achieved.
const moveit::core::RobotState & getJointValueTarget() const
Get the currently set joint state goal, replaced by private getTargetRobotState()
void setTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints &constraint)
const moveit::core::RobotState & getTargetRobotState() const
geometry_msgs::msg::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...
double getGoalOrientationTolerance() const
Get the tolerance that is used for reaching an orientation goal. This is the tolerance for roll,...
void allowLooking(bool flag)
Specify whether the robot is allowed to look around before moving if it determines it should (default...
std::map< std::string, double > getNamedTargetValues(const std::string &name) const
Get the joint angles for targets specified by name.
geometry_msgs::msg::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...
bool startStateMonitor(double wait=1.0)
When reasoning about the current state of a robot, a CurrentStateMonitor instance is automatically co...
unsigned int getVariableCount() const
Get the number of variables used to describe the state of this group. This is larger or equal to the ...
double getGoalPositionTolerance() const
Get the tolerance that is used for reaching a position goal. This is be the radius of a sphere where ...
double getPlanningTime() const
Get the number of seconds set by setPlanningTime()
void setPlanningTime(double seconds)
Specify the maximum amount of time to use when planning.
bool setApproximateJointValueTarget(const geometry_msgs::msg::Pose &eef_pose, const std::string &end_effector_link="")
Set the joint state goal for a particular joint by computing IK.
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.
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....
const std::vector< std::string > & getJointModelGroupNames() const
Get the available planning group names.
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
The name of the topic used by default for attached collision objects.
moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
Generates a constraint message intended to be used as a goal constraint for a joint group....
Definition: utils.cpp:145
moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constraints &first, const moveit_msgs::msg::Constraints &second)
Merge two sets of constraints into one.
Definition: utils.cpp:58
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
std::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_...
Definition: robot_state.h:70
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::msg::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state.
moveit::core::RobotModelConstPtr getSharedRobotModel(const rclcpp::Node::SharedPtr &node, const std::string &robot_description)
std::shared_ptr< tf2_ros::Buffer > getSharedTF()
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const rclcpp::Node::SharedPtr &node, const moveit::core::RobotModelConstPtr &robot_model, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer)
getSharedStateMonitor
const std::string GRASP_PLANNING_SERVICE_NAME
planning_interface::MotionPlanResponse plan(std::shared_ptr< moveit_cpp::PlanningComponent > &planning_component, std::shared_ptr< moveit_cpp::PlanningComponent::PlanRequestParameters > &single_plan_parameters, std::shared_ptr< moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters > &multi_plan_parameters, std::optional< const moveit::planning_pipeline_interfaces::SolutionSelectionFunction > solution_selection_function, std::optional< moveit::planning_pipeline_interfaces::StoppingCriterionFunction > stopping_criterion_callback)
warehouse_ros::DatabaseConnection::Ptr loadDatabase(const rclcpp::Node::SharedPtr &node)
Load a database connection.
warehouse_ros::MessageWithMetadata< moveit_msgs::msg::Constraints >::ConstPtr ConstraintsWithMetadata
Main namespace for MoveIt.
Definition: exceptions.h:43
This namespace includes the base class for MoveIt planners.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
std::string append(const std::string &left, const std::string &right)
d
Definition: setup.py:4
name
Definition: setup.py:7
A set of options for the kinematics solver.
Specification of options to use when constructing the MoveGroupInterface class.
std::string move_group_namespace
The namespace for the move group node.
std::string robot_description
The robot description parameter name (if different from default)
moveit::core::RobotModelConstPtr robot_model
Optionally, an instance of the RobotModel to use can be also specified.
std::string group_name
The group to construct the class instance for.
The representation of a motion plan (as ROS messasges)