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>
62 #include <std_msgs/msg/string.hpp>
63 #include <geometry_msgs/msg/transform_stamped.hpp>
64 #include <tf2/utils.h>
65 #include <tf2_eigen/tf2_eigen.hpp>
66 #include <tf2_ros/transform_listener.h>
67 #include <rclcpp/rclcpp.hpp>
68 #include <rclcpp/version.h>
92 #if RCLCPP_VERSION_GTE(17, 0, 0)
95 return rclcpp::SystemDefaultsQoS();
100 return rmw_qos_profile_services_default;
112 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
const rclcpp::Duration& wait_for_servers)
113 : opt_(opt), node_(node), logger_(
moveit::
getLogger(
"move_group_interface")), tf_buffer_(tf_buffer)
117 callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive,
119 callback_executor_.add_callback_group(callback_group_, node->get_node_base_interface());
120 callback_thread_ = std::thread([
this]() { callback_executor_.spin(); });
125 std::string error =
"Unable to construct robot model. Please make sure all needed information is on the "
127 RCLCPP_FATAL_STREAM(logger_, error);
128 throw std::runtime_error(error);
133 std::string error =
"Group '" + opt.
group_name +
"' was not found.";
134 RCLCPP_FATAL_STREAM(logger_, error);
135 throw std::runtime_error(error);
140 joint_state_target_ = std::make_shared<moveit::core::RobotState>(
getRobotModel());
141 joint_state_target_->setToDefaultValues();
142 active_target_ =
JOINT;
144 look_around_attempts_ = 0;
147 replan_attempts_ = 1;
148 goal_joint_tolerance_ = 1e-4;
149 goal_position_tolerance_ = 1e-4;
150 goal_orientation_tolerance_ = 1e-3;
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;
159 if (joint_model_group_->
isChain())
163 trajectory_event_publisher_ = node_->create_publisher<std_msgs::msg::String>(
167 attached_object_publisher_ = node_->create_publisher<moveit_msgs::msg::AttachedCollisionObject>(
174 move_action_client_ = rclcpp_action::create_client<moveit_msgs::action::MoveGroup>(
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>(
179 execute_action_client_->wait_for_action_server(wait_for_servers.to_chrono<std::chrono::duration<double>>());
181 query_service_ = node_->create_client<moveit_msgs::srv::QueryPlannerInterfaces>(
184 get_params_service_ = node_->create_client<moveit_msgs::srv::GetPlannerParams>(
187 set_params_service_ = node_->create_client<moveit_msgs::srv::SetPlannerParams>(
190 cartesian_path_service_ = node_->create_client<moveit_msgs::srv::GetCartesianPath>(
194 RCLCPP_INFO_STREAM(logger_,
"Ready to take commands for planning group " << opt.
group_name <<
'.');
199 if (constraints_init_thread_)
200 constraints_init_thread_->join();
202 callback_executor_.cancel();
204 if (callback_thread_.joinable())
205 callback_thread_.join();
208 const std::shared_ptr<tf2_ros::Buffer>&
getTF()
const
225 return joint_model_group_;
230 return *move_action_client_;
235 auto req = std::make_shared<moveit_msgs::srv::QueryPlannerInterfaces::Request>();
236 auto future_response = query_service_->async_send_request(req);
238 if (future_response.valid())
240 const auto& response = future_response.get();
241 if (!response->planner_interfaces.empty())
243 desc = response->planner_interfaces.front();
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())
256 const auto& response = future_response.get();
257 if (!response->planner_interfaces.empty())
259 desc = response->planner_interfaces;
266 std::map<std::string, std::string>
getPlannerParams(
const std::string& planner_id,
const std::string& group =
"")
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;
272 std::map<std::string, std::string> result;
274 auto future_response = get_params_service_->async_send_request(req);
275 if (future_response.valid())
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];
285 const std::map<std::string, std::string>& params,
bool replace =
false)
287 auto req = std::make_shared<moveit_msgs::srv::SetPlannerParams::Request>();
288 req->planner_config = planner_id;
290 req->replace = replace;
291 for (
const std::pair<const std::string, std::string>& param : params)
293 req->params.keys.push_back(param.first);
294 req->params.values.push_back(param.second);
296 set_params_service_->async_send_request(req);
301 std::string default_planning_pipeline;
302 node_->get_parameter(
"move_group.default_planning_pipeline", default_planning_pipeline);
303 return default_planning_pipeline;
308 if (pipeline_id != planning_pipeline_id_)
310 planning_pipeline_id_ = pipeline_id;
319 return planning_pipeline_id_;
326 if (!planning_pipeline_id_.empty())
327 pipeline_id = planning_pipeline_id_;
329 std::stringstream param_name;
330 param_name <<
"move_group";
331 if (!pipeline_id.empty())
332 param_name <<
"/planning_pipelines/" << pipeline_id;
334 param_name <<
'.' << group;
335 param_name <<
".default_planner_config";
337 std::string default_planner_config;
338 node_->get_parameter(param_name.str(), default_planner_config);
339 return default_planner_config;
344 planner_id_ = planner_id;
354 num_planning_attempts_ = num_planning_attempts;
364 setMaxScalingFactor(max_acceleration_scaling_factor_, value,
"acceleration_scaling_factor", 0.1);
367 void setMaxScalingFactor(
double& variable,
const double target_value,
const char* factor_name,
double fallback_value)
369 if (target_value > 1.0)
371 RCLCPP_WARN(logger_,
"Limiting max_%s (%.2f) to 1.0.", factor_name, target_value);
374 else if (target_value <= 0.0)
376 node_->get_parameter_or<
double>(std::string(
"robot_description_planning.default_") + factor_name, variable,
378 if (target_value < 0.0)
380 RCLCPP_WARN(logger_,
"max_%s < 0.0! Setting to default: %.2f.", factor_name, variable);
385 variable = target_value;
391 return *joint_state_target_;
396 return *joint_state_target_;
401 considered_start_state_ = std::make_shared<moveit::core::RobotState>(start_state);
406 considered_start_state_.reset();
411 if (considered_start_state_)
413 return considered_start_state_;
417 moveit::core::RobotStatePtr s;
424 const std::string& frame,
bool approx)
426 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
461 tf2::fromMsg(eef_pose, p);
467 RCLCPP_ERROR(logger_,
"Unable to transform from frame '%s' to frame '%s'", frame.c_str(),
479 end_effector_link_ = end_effector;
484 pose_targets_.erase(end_effector_link);
489 pose_targets_.clear();
494 return end_effector_link_;
499 if (!end_effector_link_.empty())
501 const std::vector<std::string>& possible_eefs =
503 for (
const std::string& possible_eef : possible_eefs)
509 static std::string empty;
513 bool setPoseTargets(
const std::vector<geometry_msgs::msg::PoseStamped>& poses,
const std::string& end_effector_link)
515 const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
518 RCLCPP_ERROR(logger_,
"No end-effector to set the pose for");
523 pose_targets_[eef] = poses;
525 std::vector<geometry_msgs::msg::PoseStamped>& stored_poses = pose_targets_[eef];
526 for (geometry_msgs::msg::PoseStamped& stored_pose : stored_poses)
527 stored_pose.header.stamp = rclcpp::Time(0);
534 const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
535 return pose_targets_.find(eef) != pose_targets_.end();
538 const geometry_msgs::msg::PoseStamped&
getPoseTarget(
const std::string& end_effector_link)
const
540 const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
543 std::map<std::string, std::vector<geometry_msgs::msg::PoseStamped>>::const_iterator jt = pose_targets_.find(eef);
544 if (jt != pose_targets_.end())
546 if (!jt->second.empty())
547 return jt->second.at(0);
551 static const geometry_msgs::msg::PoseStamped UNKNOWN;
552 RCLCPP_ERROR(logger_,
"Pose for end-effector '%s' not known.", eef.c_str());
556 const std::vector<geometry_msgs::msg::PoseStamped>&
getPoseTargets(
const std::string& end_effector_link)
const
558 const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
560 std::map<std::string, std::vector<geometry_msgs::msg::PoseStamped>>::const_iterator jt = pose_targets_.find(eef);
561 if (jt != pose_targets_.end())
563 if (!jt->second.empty())
568 static const std::vector<geometry_msgs::msg::PoseStamped> EMPTY;
569 RCLCPP_ERROR(logger_,
"Poses for end-effector '%s' are not known.", eef.c_str());
575 pose_reference_frame_ = pose_reference_frame;
580 support_surface_ = support_surface;
585 return pose_reference_frame_;
590 active_target_ = type;
595 return active_target_;
600 if (!current_state_monitor_)
602 RCLCPP_ERROR(logger_,
"Unable to monitor current robot state");
607 if (!current_state_monitor_->isActive())
608 current_state_monitor_->startStateMonitor();
610 current_state_monitor_->waitForCompleteState(opt_.
group_name, wait);
614 bool getCurrentState(moveit::core::RobotStatePtr& current_state,
double wait_seconds = 1.0)
616 if (!current_state_monitor_)
618 RCLCPP_ERROR(logger_,
"Unable to get current robot state");
623 if (!current_state_monitor_->isActive())
624 current_state_monitor_->startStateMonitor();
626 if (!current_state_monitor_->waitForCurrentState(node_->now(), wait_seconds))
628 RCLCPP_ERROR(logger_,
"Failed to fetch current robot state");
632 current_state = current_state_monitor_->getCurrentState();
638 if (!move_action_client_ || !move_action_client_->action_server_is_ready())
640 RCLCPP_INFO_STREAM(logger_,
"MoveGroup action client/server not ready");
641 return moveit::core::MoveItErrorCode::FAILURE;
643 RCLCPP_INFO_STREAM(logger_,
"MoveGroup action client/server ready");
645 moveit_msgs::action::MoveGroup::Goal goal;
647 goal.planning_options.plan_only =
true;
648 goal.planning_options.look_around =
false;
649 goal.planning_options.replan =
false;
650 goal.planning_options.planning_scene_diff.is_diff =
true;
651 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
654 rclcpp_action::ResultCode code = rclcpp_action::ResultCode::UNKNOWN;
655 std::shared_ptr<moveit_msgs::action::MoveGroup::Result> res;
656 auto send_goal_opts = rclcpp_action::Client<moveit_msgs::action::MoveGroup>::SendGoalOptions();
658 send_goal_opts.goal_response_callback =
659 [&](
const rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::SharedPtr& goal_handle) {
663 RCLCPP_INFO(logger_,
"Planning request rejected");
666 RCLCPP_INFO(logger_,
"Planning request accepted");
668 send_goal_opts.result_callback =
669 [&](
const rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::WrappedResult& result) {
676 case rclcpp_action::ResultCode::SUCCEEDED:
677 RCLCPP_INFO(logger_,
"Planning request complete!");
679 case rclcpp_action::ResultCode::ABORTED:
680 RCLCPP_INFO(logger_,
"Planning request aborted");
682 case rclcpp_action::ResultCode::CANCELED:
683 RCLCPP_INFO(logger_,
"Planning request canceled");
686 RCLCPP_INFO(logger_,
"Planning request unknown result code");
691 auto goal_handle_future = move_action_client_->async_send_goal(goal, send_goal_opts);
696 std::this_thread::sleep_for(std::chrono::milliseconds(1));
699 if (code != rclcpp_action::ResultCode::SUCCEEDED)
701 RCLCPP_ERROR_STREAM(logger_,
"MoveGroupInterface::plan() failed or timeout reached");
702 return res->error_code;
705 plan.trajectory = res->planned_trajectory;
706 plan.start_state = res->trajectory_start;
707 plan.planning_time = res->planning_time;
708 RCLCPP_INFO(logger_,
"time taken to generate plan: %g seconds",
plan.planning_time);
710 return res->error_code;
715 if (!move_action_client_ || !move_action_client_->action_server_is_ready())
717 RCLCPP_INFO_STREAM(logger_,
"MoveGroup action client/server not ready");
718 return moveit::core::MoveItErrorCode::FAILURE;
721 moveit_msgs::action::MoveGroup::Goal goal;
723 goal.planning_options.plan_only =
false;
724 goal.planning_options.look_around = can_look_;
725 goal.planning_options.replan = can_replan_;
726 goal.planning_options.replan_delay = replan_delay_;
727 goal.planning_options.planning_scene_diff.is_diff =
true;
728 goal.planning_options.planning_scene_diff.robot_state.is_diff =
true;
731 rclcpp_action::ResultCode code = rclcpp_action::ResultCode::UNKNOWN;
732 std::shared_ptr<moveit_msgs::action::MoveGroup_Result> res;
733 auto send_goal_opts = rclcpp_action::Client<moveit_msgs::action::MoveGroup>::SendGoalOptions();
735 send_goal_opts.goal_response_callback =
736 [&](
const rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::SharedPtr& goal_handle) {
740 RCLCPP_INFO(logger_,
"Plan and Execute request rejected");
743 RCLCPP_INFO(logger_,
"Plan and Execute request accepted");
745 send_goal_opts.result_callback =
746 [&](
const rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::WrappedResult& result) {
753 case rclcpp_action::ResultCode::SUCCEEDED:
754 RCLCPP_INFO(logger_,
"Plan and Execute request complete!");
756 case rclcpp_action::ResultCode::ABORTED:
757 RCLCPP_INFO(logger_,
"Plan and Execute request aborted");
759 case rclcpp_action::ResultCode::CANCELED:
760 RCLCPP_INFO(logger_,
"Plan and Execute request canceled");
763 RCLCPP_INFO(logger_,
"Plan and Execute request unknown result code");
767 auto goal_handle_future = move_action_client_->async_send_goal(goal, send_goal_opts);
769 return moveit::core::MoveItErrorCode::SUCCESS;
774 std::this_thread::sleep_for(std::chrono::milliseconds(1));
777 if (code != rclcpp_action::ResultCode::SUCCEEDED)
779 RCLCPP_ERROR_STREAM(logger_,
"MoveGroupInterface::move() failed or timeout reached");
781 return res->error_code;
785 const std::vector<std::string>& controllers = std::vector<std::string>())
787 if (!execute_action_client_ || !execute_action_client_->action_server_is_ready())
789 RCLCPP_INFO_STREAM(logger_,
"execute_action_client_ client/server not ready");
790 return moveit::core::MoveItErrorCode::FAILURE;
794 rclcpp_action::ResultCode code = rclcpp_action::ResultCode::UNKNOWN;
795 std::shared_ptr<moveit_msgs::action::ExecuteTrajectory_Result> res;
796 auto send_goal_opts = rclcpp_action::Client<moveit_msgs::action::ExecuteTrajectory>::SendGoalOptions();
798 send_goal_opts.goal_response_callback =
799 [&](
const rclcpp_action::ClientGoalHandle<moveit_msgs::action::ExecuteTrajectory>::SharedPtr& goal_handle) {
803 RCLCPP_INFO(logger_,
"Execute request rejected");
806 RCLCPP_INFO(logger_,
"Execute request accepted");
808 send_goal_opts.result_callback =
809 [&](
const rclcpp_action::ClientGoalHandle<moveit_msgs::action::ExecuteTrajectory>::WrappedResult& result) {
816 case rclcpp_action::ResultCode::SUCCEEDED:
817 RCLCPP_INFO(logger_,
"Execute request success!");
819 case rclcpp_action::ResultCode::ABORTED:
820 RCLCPP_INFO(logger_,
"Execute request aborted");
822 case rclcpp_action::ResultCode::CANCELED:
823 RCLCPP_INFO(logger_,
"Execute request canceled");
826 RCLCPP_INFO(logger_,
"Execute request unknown result code");
831 moveit_msgs::action::ExecuteTrajectory::Goal goal;
832 goal.trajectory = trajectory;
833 goal.controller_names = controllers;
835 auto goal_handle_future = execute_action_client_->async_send_goal(goal, send_goal_opts);
837 return moveit::core::MoveItErrorCode::SUCCESS;
842 std::this_thread::sleep_for(std::chrono::milliseconds(1));
845 if (code != rclcpp_action::ResultCode::SUCCEEDED)
847 RCLCPP_ERROR_STREAM(logger_,
"MoveGroupInterface::execute() failed or timeout reached");
849 return res->error_code;
853 double jump_threshold, moveit_msgs::msg::RobotTrajectory& msg,
854 const moveit_msgs::msg::Constraints& path_constraints,
bool avoid_collisions,
855 moveit_msgs::msg::MoveItErrorCodes& error_code)
857 auto req = std::make_shared<moveit_msgs::srv::GetCartesianPath::Request>();
858 moveit_msgs::srv::GetCartesianPath::Response::SharedPtr response;
860 if (considered_start_state_)
866 req->start_state.is_diff =
true;
871 req->header.stamp =
getClock()->now();
872 req->waypoints = waypoints;
873 req->max_step = step;
874 req->jump_threshold = jump_threshold;
875 req->path_constraints = path_constraints;
876 req->avoid_collisions = avoid_collisions;
878 req->max_velocity_scaling_factor = max_velocity_scaling_factor_;
879 req->max_acceleration_scaling_factor = max_acceleration_scaling_factor_;
881 auto future_response = cartesian_path_service_->async_send_request(req);
882 if (future_response.valid())
884 response = future_response.get();
885 error_code = response->error_code;
886 if (response->error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
888 msg = response->solution;
889 return response->fraction;
896 error_code.val = error_code.FAILURE;
903 if (trajectory_event_publisher_)
905 std_msgs::msg::String event;
907 trajectory_event_publisher_->publish(event);
911 bool attachObject(
const std::string&
object,
const std::string& link,
const std::vector<std::string>& touch_links)
922 RCLCPP_ERROR(logger_,
"No known link to attach object '%s' to",
object.c_str());
925 moveit_msgs::msg::AttachedCollisionObject aco;
926 aco.object.id = object;
927 aco.link_name.swap(l);
928 if (touch_links.empty())
930 aco.touch_links.push_back(aco.link_name);
934 aco.touch_links = touch_links;
936 aco.object.operation = moveit_msgs::msg::CollisionObject::ADD;
937 attached_object_publisher_->publish(aco);
943 moveit_msgs::msg::AttachedCollisionObject aco;
947 aco.link_name =
name;
951 aco.object.id =
name;
953 aco.object.operation = moveit_msgs::msg::CollisionObject::REMOVE;
954 if (aco.link_name.empty() && aco.object.id.empty())
957 const std::vector<std::string>& lnames = joint_model_group_->
getLinkModelNames();
958 for (
const std::string& lname : lnames)
960 aco.link_name = lname;
961 attached_object_publisher_->publish(aco);
966 attached_object_publisher_->publish(aco);
973 return goal_position_tolerance_;
978 return goal_orientation_tolerance_;
983 return goal_joint_tolerance_;
988 goal_joint_tolerance_ = tolerance;
993 goal_position_tolerance_ = tolerance;
998 goal_orientation_tolerance_ = tolerance;
1004 allowed_planning_time_ = seconds;
1009 return allowed_planning_time_;
1015 request.num_planning_attempts = num_planning_attempts_;
1016 request.max_velocity_scaling_factor = max_velocity_scaling_factor_;
1017 request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_;
1018 request.allowed_planning_time = allowed_planning_time_;
1019 request.pipeline_id = planning_pipeline_id_;
1020 request.planner_id = planner_id_;
1021 request.workspace_parameters = workspace_parameters_;
1023 if (considered_start_state_)
1029 request.start_state.is_diff =
true;
1032 if (active_target_ ==
JOINT)
1034 request.goal_constraints.resize(1);
1038 else if (active_target_ == POSE || active_target_ ==
POSITION || active_target_ ==
ORIENTATION)
1041 std::size_t goal_count = 0;
1042 for (
const auto& pose_target : pose_targets_)
1043 goal_count = std::max(goal_count, pose_target.second.size());
1049 request.goal_constraints.resize(goal_count);
1051 for (
const auto& pose_target : pose_targets_)
1053 for (std::size_t i = 0; i < pose_target.second.size(); ++i)
1056 pose_target.first, pose_target.second[i], goal_position_tolerance_, goal_orientation_tolerance_);
1058 c.position_constraints.clear();
1060 c.orientation_constraints.clear();
1066 RCLCPP_ERROR(logger_,
"Unable to construct MotionPlanRequest representation");
1068 if (path_constraints_)
1069 request.path_constraints = *path_constraints_;
1070 if (trajectory_constraints_)
1071 request.trajectory_constraints = *trajectory_constraints_;
1136 path_constraints_ = std::make_unique<moveit_msgs::msg::Constraints>(constraint);
1141 if (constraints_storage_)
1144 if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.
group_name))
1147 std::make_unique<moveit_msgs::msg::Constraints>(
static_cast<moveit_msgs::msg::Constraints
>(*msg_m));
1159 path_constraints_.reset();
1164 trajectory_constraints_ = std::make_unique<moveit_msgs::msg::TrajectoryConstraints>(constraint);
1169 trajectory_constraints_.reset();
1174 while (initializing_constraints_)
1176 std::chrono::duration<double> d(0.01);
1177 rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(d), rclcpp::Context::SharedPtr(
nullptr));
1180 std::vector<std::string> c;
1181 if (constraints_storage_)
1182 constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.
group_name);
1189 if (path_constraints_)
1191 return *path_constraints_;
1195 return moveit_msgs::msg::Constraints();
1201 if (trajectory_constraints_)
1203 return *trajectory_constraints_;
1207 return moveit_msgs::msg::TrajectoryConstraints();
1213 initializing_constraints_ =
true;
1214 if (constraints_init_thread_)
1215 constraints_init_thread_->join();
1216 constraints_init_thread_ =
1217 std::make_unique<std::thread>([
this, host, port] { initializeConstraintsStorageThread(host, port); });
1220 void setWorkspace(
double minx,
double miny,
double minz,
double maxx,
double maxy,
double maxz)
1222 workspace_parameters_.header.frame_id =
getRobotModel()->getModelFrame();
1223 workspace_parameters_.header.stamp =
getClock()->now();
1224 workspace_parameters_.min_corner.x = minx;
1225 workspace_parameters_.min_corner.y = miny;
1226 workspace_parameters_.min_corner.z = minz;
1227 workspace_parameters_.max_corner.x = maxx;
1228 workspace_parameters_.max_corner.y = maxy;
1229 workspace_parameters_.max_corner.z = maxz;
1234 return node_->get_clock();
1238 void initializeConstraintsStorageThread(
const std::string& host,
unsigned int port)
1244 conn->setParams(host, port);
1245 if (conn->connect())
1247 constraints_storage_ = std::make_unique<moveit_warehouse::ConstraintsStorage>(conn);
1250 catch (std::exception& ex)
1252 RCLCPP_ERROR(logger_,
"%s", ex.what());
1254 initializing_constraints_ =
false;
1258 rclcpp::Node::SharedPtr node_;
1259 rclcpp::Logger logger_;
1260 rclcpp::CallbackGroup::SharedPtr callback_group_;
1261 rclcpp::executors::SingleThreadedExecutor callback_executor_;
1262 std::thread callback_thread_;
1263 std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
1264 moveit::core::RobotModelConstPtr robot_model_;
1265 planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
1267 std::shared_ptr<rclcpp_action::Client<moveit_msgs::action::MoveGroup>> move_action_client_;
1270 std::shared_ptr<rclcpp_action::Client<moveit_msgs::action::ExecuteTrajectory>> execute_action_client_;
1273 moveit::core::RobotStatePtr considered_start_state_;
1274 moveit_msgs::msg::WorkspaceParameters workspace_parameters_;
1275 double allowed_planning_time_;
1276 std::string planning_pipeline_id_;
1277 std::string planner_id_;
1278 unsigned int num_planning_attempts_;
1279 double max_velocity_scaling_factor_;
1280 double max_acceleration_scaling_factor_;
1281 double goal_joint_tolerance_;
1282 double goal_position_tolerance_;
1283 double goal_orientation_tolerance_;
1285 int32_t look_around_attempts_;
1287 int32_t replan_attempts_;
1288 double replan_delay_;
1291 moveit::core::RobotStatePtr joint_state_target_;
1296 std::map<std::string, std::vector<geometry_msgs::msg::PoseStamped>> pose_targets_;
1299 ActiveTargetType active_target_;
1300 std::unique_ptr<moveit_msgs::msg::Constraints> path_constraints_;
1301 std::unique_ptr<moveit_msgs::msg::TrajectoryConstraints> trajectory_constraints_;
1302 std::string end_effector_link_;
1303 std::string pose_reference_frame_;
1304 std::string support_surface_;
1307 rclcpp::Publisher<std_msgs::msg::String>::SharedPtr trajectory_event_publisher_;
1308 rclcpp::Publisher<moveit_msgs::msg::AttachedCollisionObject>::SharedPtr attached_object_publisher_;
1309 rclcpp::Client<moveit_msgs::srv::QueryPlannerInterfaces>::SharedPtr query_service_;
1310 rclcpp::Client<moveit_msgs::srv::GetPlannerParams>::SharedPtr get_params_service_;
1311 rclcpp::Client<moveit_msgs::srv::SetPlannerParams>::SharedPtr set_params_service_;
1312 rclcpp::Client<moveit_msgs::srv::GetCartesianPath>::SharedPtr cartesian_path_service_;
1314 std::unique_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
1315 std::unique_ptr<std::thread> constraints_init_thread_;
1316 bool initializing_constraints_;
1320 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1321 const rclcpp::Duration& wait_for_servers)
1325 throw std::runtime_error(
"ROS does not seem to be running");
1331 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1332 const rclcpp::Duration& wait_for_servers)
1344 : remembered_joint_values_(std::move(other.remembered_joint_values_))
1345 , impl_(other.impl_)
1346 , logger_(std::move(other.logger_))
1348 other.impl_ =
nullptr;
1356 impl_ = other.impl_;
1357 logger_ = other.logger_;
1358 remembered_joint_values_ = std::move(other.remembered_joint_values_);
1359 other.impl_ =
nullptr;
1393 const std::string& group)
const
1399 const std::map<std::string, std::string>& params,
bool replace)
1451 return impl_->
move(
false);
1461 return impl_->
move(
true);
1465 const std::vector<std::string>& controllers)
1467 return impl_->
execute(
plan.trajectory,
false, controllers);
1471 const std::vector<std::string>& controllers)
1473 return impl_->
execute(trajectory,
false, controllers);
1478 return impl_->
execute(
plan.trajectory,
true, controllers);
1482 const std::vector<std::string>& controllers)
1484 return impl_->
execute(trajectory,
true, controllers);
1533 double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory,
1534 bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code)
1536 moveit_msgs::msg::Constraints path_constraints_tmp;
1537 return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, moveit_msgs::msg::Constraints(),
1538 avoid_collisions, error_code);
1542 double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory,
1543 const moveit_msgs::msg::Constraints& path_constraints,
1544 bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code)
1548 return impl_->
computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1549 avoid_collisions, *error_code);
1553 moveit_msgs::msg::MoveItErrorCodes err_tmp;
1554 err_tmp.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
1555 moveit_msgs::msg::MoveItErrorCodes& err = error_code ? *error_code : err_tmp;
1556 return impl_->
computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1557 avoid_collisions, err);
1568 moveit::core::RobotStatePtr rs;
1569 if (start_state.is_diff)
1575 rs = std::make_shared<moveit::core::RobotState>(
getRobotModel());
1576 rs->setToDefaultValues();
1610 std::map<std::string, std::vector<double>>::const_iterator it = remembered_joint_values_.find(
name);
1611 std::map<std::string, double> positions;
1613 if (it != remembered_joint_values_.cend())
1616 for (
size_t x = 0; x < names.size(); ++x)
1618 positions[names[x]] = it->second[x];
1625 RCLCPP_ERROR(logger_,
"The requested named target '%s' does not exist, returning empty positions.",
name.c_str());
1633 std::map<std::string, std::vector<double>>::const_iterator it = remembered_joint_values_.find(
name);
1634 if (it != remembered_joint_values_.end())
1645 RCLCPP_ERROR(logger_,
"The requested named target '%s' does not exist",
name.c_str());
1658 if (joint_values.size() != n_group_joints)
1660 RCLCPP_DEBUG_STREAM(logger_,
"Provided joint value list has length " << joint_values.size() <<
" but group "
1662 <<
" has " << n_group_joints <<
" joints");
1673 for (
const auto& pair : variable_values)
1675 if (std::find(allowed.begin(), allowed.end(), pair.first) == allowed.end())
1677 RCLCPP_ERROR_STREAM(logger_,
"joint variable " << pair.first <<
" is not part of group "
1689 const std::vector<double>& variable_values)
1691 if (variable_names.size() != variable_values.size())
1693 RCLCPP_ERROR_STREAM(logger_,
"sizes of name and position arrays do not match");
1697 for (
const auto& variable_name : variable_names)
1699 if (std::find(allowed.begin(), allowed.end(), variable_name) == allowed.end())
1701 RCLCPP_ERROR_STREAM(logger_,
"joint variable " << variable_name <<
" is not part of group "
1721 std::vector<double> values(1, value);
1735 RCLCPP_ERROR_STREAM(logger_,
1746 const std::string& end_effector_link)
1752 const std::string& end_effector_link)
1754 return impl_->
setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id,
false);
1759 geometry_msgs::msg::Pose msg = tf2::toMsg(eef_pose);
1764 const std::string& end_effector_link)
1770 const std::string& end_effector_link)
1772 return impl_->
setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id,
true);
1776 const std::string& end_effector_link)
1778 geometry_msgs::msg::Pose msg = tf2::toMsg(eef_pose);
1826 std::vector<geometry_msgs::msg::PoseStamped> pose_msg(1);
1827 pose_msg[0].pose = tf2::toMsg(pose);
1829 pose_msg[0].header.stamp = impl_->
getClock()->now();
1835 std::vector<geometry_msgs::msg::PoseStamped> pose_msg(1);
1836 pose_msg[0].pose = target;
1838 pose_msg[0].header.stamp = impl_->
getClock()->now();
1843 const std::string& end_effector_link)
1845 std::vector<geometry_msgs::msg::PoseStamped> targets(1, target);
1851 std::vector<geometry_msgs::msg::PoseStamped> pose_out(target.size());
1852 rclcpp::Time tm = impl_->
getClock()->now();
1854 for (std::size_t i = 0; i < target.size(); ++i)
1856 pose_out[i].pose = tf2::toMsg(target[i]);
1857 pose_out[i].header.stamp = tm;
1858 pose_out[i].header.frame_id = frame_id;
1864 const std::string& end_effector_link)
1866 std::vector<geometry_msgs::msg::PoseStamped> target_stamped(target.size());
1867 rclcpp::Time tm = impl_->
getClock()->now();
1869 for (std::size_t i = 0; i < target.size(); ++i)
1871 target_stamped[i].pose = target[i];
1872 target_stamped[i].header.stamp = tm;
1873 target_stamped[i].header.frame_id = frame_id;
1879 const std::string& end_effector_link)
1883 RCLCPP_ERROR(logger_,
"No pose specified as goal target");
1898 const std::vector<geometry_msgs::msg::PoseStamped>&
1906 inline void transformPose(
const tf2_ros::Buffer& tf_buffer,
const std::string& desired_frame,
1907 geometry_msgs::msg::PoseStamped& target)
1909 if (desired_frame != target.header.frame_id)
1911 geometry_msgs::msg::PoseStamped target_in(target);
1912 tf_buffer.transform(target_in, target, desired_frame);
1914 target.header.stamp = rclcpp::Time(0);
1921 geometry_msgs::msg::PoseStamped target;
1929 target.pose.orientation.x = 0.0;
1930 target.pose.orientation.y = 0.0;
1931 target.pose.orientation.z = 0.0;
1932 target.pose.orientation.w = 1.0;
1936 target.pose.position.x = x;
1937 target.pose.position.y = y;
1938 target.pose.position.z = z;
1946 geometry_msgs::msg::PoseStamped target;
1954 target.pose.position.x = 0.0;
1955 target.pose.position.y = 0.0;
1956 target.pose.position.z = 0.0;
1961 target.pose.orientation = tf2::toMsg(q);
1968 const std::string& end_effector_link)
1970 geometry_msgs::msg::PoseStamped target;
1978 target.pose.position.x = 0.0;
1979 target.pose.position.y = 0.0;
1980 target.pose.position.z = 0.0;
1984 target.pose.orientation.x = x;
1985 target.pose.orientation.y = y;
1986 target.pose.orientation.z = z;
1987 target.pose.orientation.w = w;
2052 moveit::core::RobotStatePtr current_state;
2053 std::vector<double> values;
2055 current_state->copyJointGroupPositions(
getName(), values);
2061 std::vector<double> r;
2068 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
2069 Eigen::Isometry3d pose;
2073 RCLCPP_ERROR(logger_,
"No end-effector specified");
2077 moveit::core::RobotStatePtr current_state;
2083 pose = current_state->getGlobalLinkTransform(lm);
2086 geometry_msgs::msg::PoseStamped pose_msg;
2087 pose_msg.header.stamp = impl_->
getClock()->now();
2088 pose_msg.header.frame_id = impl_->
getRobotModel()->getModelFrame();
2089 pose_msg.pose = tf2::toMsg(pose);
2095 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
2096 Eigen::Isometry3d pose;
2100 RCLCPP_ERROR(logger_,
"No end-effector specified");
2104 moveit::core::RobotStatePtr current_state;
2109 pose = current_state->getGlobalLinkTransform(lm);
2112 geometry_msgs::msg::PoseStamped pose_msg;
2113 pose_msg.header.stamp = impl_->
getClock()->now();
2114 pose_msg.header.frame_id = impl_->
getRobotModel()->getModelFrame();
2115 pose_msg.pose = tf2::toMsg(pose);
2121 std::vector<double> result;
2122 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
2125 RCLCPP_ERROR(logger_,
"No end-effector specified");
2129 moveit::core::RobotStatePtr current_state;
2136 geometry_msgs::msg::TransformStamped tfs = tf2::eigenToTransform(current_state->getGlobalLinkTransform(lm));
2137 double pitch, roll, yaw;
2138 tf2::getEulerYPR<geometry_msgs::msg::Quaternion>(tfs.transform.rotation, yaw, pitch, roll);
2165 moveit::core::RobotStatePtr current_state;
2167 return current_state;
2172 remembered_joint_values_[
name] = values;
2177 remembered_joint_values_.erase(
name);
2182 impl_->can_look_ = flag;
2183 RCLCPP_DEBUG(logger_,
"Looking around: %s", flag ?
"yes" :
"no");
2190 RCLCPP_ERROR(logger_,
"Tried to set negative number of look-around attempts");
2194 RCLCPP_DEBUG_STREAM(logger_,
"Look around attempts: " << attempts);
2195 impl_->look_around_attempts_ = attempts;
2201 impl_->can_replan_ = flag;
2202 RCLCPP_DEBUG(logger_,
"Replanning: %s", flag ?
"yes" :
"no");
2209 RCLCPP_ERROR(logger_,
"Tried to set negative number of replan attempts");
2213 RCLCPP_DEBUG_STREAM(logger_,
"Replan Attempts: " << attempts);
2214 impl_->replan_attempts_ = attempts;
2222 RCLCPP_ERROR(logger_,
"Tried to set negative replan delay");
2226 RCLCPP_DEBUG_STREAM(logger_,
"Replan Delay: " << delay);
2227 impl_->replan_delay_ = delay;
2278 impl_->
setWorkspace(minx, miny, minz, maxx, maxy, maxz);
2310 return attachObject(
object, link, std::vector<std::string>());
2314 const std::vector<std::string>& touch_links)
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....
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
A link from the robot. Contains the constant transform applied to the link and its geometry.
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.
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...
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...
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.
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)
std::vector< std::string > getKnownConstraints() const
const std::string & getEndEffectorLink() const
bool startStateMonitor(double wait)
void setPlanningPipelineId(const std::string &pipeline_id)
const std::string & getEndEffector() const
void setPoseReferenceFrame(const std::string &pose_reference_frame)
double getGoalPositionTolerance() const
std::string getDefaultPlanningPipelineId() const
~MoveGroupInterfaceImpl()
rclcpp::Clock::SharedPtr getClock()
void setEndEffectorLink(const std::string &end_effector)
const Options & getOptions() const
void constructGoal(moveit_msgs::action::MoveGroup::Goal &goal) const
double getPlanningTime() 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)
std::string getDefaultPlannerId(const std::string &group) const
bool setPathConstraints(const std::string &constraint)
const std::string & getPlannerId() const
void setStartState(const moveit::core::RobotState &start_state)
double getGoalJointTolerance() const
void setMaxScalingFactor(double &variable, const double target_value, const char *factor_name, double fallback_value)
void setMaxAccelerationScalingFactor(double value)
void clearPathConstraints()
void setGoalOrientationTolerance(double tolerance)
void setSupportSurfaceName(const std::string &support_surface)
const moveit::core::RobotState & getTargetRobotState() const
bool getCurrentState(moveit::core::RobotStatePtr ¤t_state, double wait_seconds=1.0)
rclcpp_action::Client< moveit_msgs::action::MoveGroup > & getMoveGroupClient() const
void clearPoseTarget(const std::string &end_effector_link)
bool attachObject(const std::string &object, const std::string &link, const std::vector< std::string > &touch_links)
void setGoalPositionTolerance(double tolerance)
void initializeConstraintsStorage(const std::string &host, unsigned int port)
void setPlannerId(const std::string &planner_id)
const std::shared_ptr< tf2_ros::Buffer > & getTF() const
double getGoalOrientationTolerance() const
moveit_msgs::msg::Constraints getPathConstraints() const
void setTargetType(ActiveTargetType type)
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 moveit::core::JointModelGroup * getJointModelGroup() const
ActiveTargetType getTargetType() const
void setMaxVelocityScalingFactor(double value)
void setNumPlanningAttempts(unsigned int num_planning_attempts)
const moveit::core::RobotModelConstPtr & getRobotModel() const
void setGoalJointTolerance(double tolerance)
const geometry_msgs::msg::PoseStamped & getPoseTarget(const std::string &end_effector_link) const
void setTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints &constraint)
moveit::core::MoveItErrorCode move(bool wait)
void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest &request) const
void setPlanningTime(double seconds)
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::RobotState & getTargetRobotState()
void clearTrajectoryConstraints()
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)
const std::string & getPlanningPipelineId() const
bool detachObject(const std::string &name)
bool setJointValueTarget(const geometry_msgs::msg::Pose &eef_pose, const std::string &end_effector_link, const std::string &frame, bool approx)
moveit::core::RobotStatePtr getStartState()
const std::vector< geometry_msgs::msg::PoseStamped > & getPoseTargets(const std::string &end_effector_link) const
bool hasPoseTarget(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 > ¶ms, bool replace=false)
bool getInterfaceDescriptions(std::vector< moveit_msgs::msg::PlannerInterfaceDescription > &desc)
void setStartStateToCurrentState()
moveit::core::MoveItErrorCode plan(Plan &plan)
const std::string & getPoseReferenceFrame() const
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....
std::string getDefaultPlanningPipelineId() const
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.
void clearTrajectoryConstraints()
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.
void getJointValueTarget(std::vector< double > &group_variable_values) const
Get the current joint state goal in a form compatible to setJointValueTarget()
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.
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 > ¶ms, 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.
static const std::string EXECUTION_EVENT_TOPIC
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....
moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constraints &first, const moveit_msgs::msg::Constraints &second)
Merge two sets of constraints into one.
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_...
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::shared_ptr< planning_scene::PlanningScene > &planning_scene, 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.
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
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)
A set of options for the kinematics solver.
bool return_approximate_solution
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)