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>
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>
77 const rclcpp::Logger
LOGGER = rclcpp::get_logger(
"move_group_interface");
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), 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;
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(rclcpp::get_logger(
"move_group_interface"),
"Limiting max_%s (%.2f) to 1.0.", factor_name,
375 else if (target_value <= 0.0)
377 node_->get_parameter_or<
double>(std::string(
"robot_description_planning.default_") + factor_name, variable,
379 if (target_value < 0.0)
381 RCLCPP_WARN(rclcpp::get_logger(
"move_group_interface"),
"max_%s < 0.0! Setting to default: %.2f.", factor_name,
387 variable = target_value;
393 return *joint_state_target_;
398 return *joint_state_target_;
403 considered_start_state_ = std::make_shared<moveit::core::RobotState>(start_state);
408 considered_start_state_.reset();
413 if (considered_start_state_)
415 return considered_start_state_;
419 moveit::core::RobotStatePtr s;
426 const std::string& frame,
bool approx)
428 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
463 tf2::fromMsg(eef_pose, p);
469 RCLCPP_ERROR(
LOGGER,
"Unable to transform from frame '%s' to frame '%s'", frame.c_str(),
481 end_effector_link_ = end_effector;
486 pose_targets_.erase(end_effector_link);
491 pose_targets_.clear();
496 return end_effector_link_;
501 if (!end_effector_link_.empty())
503 const std::vector<std::string>& possible_eefs =
505 for (
const std::string& possible_eef : possible_eefs)
511 static std::string empty;
515 bool setPoseTargets(
const std::vector<geometry_msgs::msg::PoseStamped>& poses,
const std::string& end_effector_link)
517 const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
520 RCLCPP_ERROR(
LOGGER,
"No end-effector to set the pose for");
525 pose_targets_[eef] = poses;
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);
536 const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
537 return pose_targets_.find(eef) != pose_targets_.end();
540 const geometry_msgs::msg::PoseStamped&
getPoseTarget(
const std::string& end_effector_link)
const
542 const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
545 std::map<std::string, std::vector<geometry_msgs::msg::PoseStamped>>::const_iterator jt = pose_targets_.find(eef);
546 if (jt != pose_targets_.end())
548 if (!jt->second.empty())
549 return jt->second.at(0);
553 static const geometry_msgs::msg::PoseStamped UNKNOWN;
554 RCLCPP_ERROR(
LOGGER,
"Pose for end-effector '%s' not known.", eef.c_str());
558 const std::vector<geometry_msgs::msg::PoseStamped>&
getPoseTargets(
const std::string& end_effector_link)
const
560 const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
562 std::map<std::string, std::vector<geometry_msgs::msg::PoseStamped>>::const_iterator jt = pose_targets_.find(eef);
563 if (jt != pose_targets_.end())
565 if (!jt->second.empty())
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());
577 pose_reference_frame_ = pose_reference_frame;
582 support_surface_ = support_surface;
587 return pose_reference_frame_;
592 active_target_ = type;
597 return active_target_;
602 if (!current_state_monitor_)
604 RCLCPP_ERROR(
LOGGER,
"Unable to monitor current robot state");
609 if (!current_state_monitor_->isActive())
610 current_state_monitor_->startStateMonitor();
612 current_state_monitor_->waitForCompleteState(opt_.
group_name, wait);
616 bool getCurrentState(moveit::core::RobotStatePtr& current_state,
double wait_seconds = 1.0)
618 if (!current_state_monitor_)
620 RCLCPP_ERROR(
LOGGER,
"Unable to get current robot state");
625 if (!current_state_monitor_->isActive())
626 current_state_monitor_->startStateMonitor();
628 if (!current_state_monitor_->waitForCurrentState(node_->now(), wait_seconds))
630 RCLCPP_ERROR(
LOGGER,
"Failed to fetch current robot state");
634 current_state = current_state_monitor_->getCurrentState();
640 if (!move_action_client_ || !move_action_client_->action_server_is_ready())
642 RCLCPP_INFO_STREAM(
LOGGER,
"MoveGroup action client/server not ready");
643 return moveit::core::MoveItErrorCode::FAILURE;
645 RCLCPP_INFO_STREAM(
LOGGER,
"MoveGroup action client/server ready");
647 moveit_msgs::action::MoveGroup::Goal 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;
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();
660 send_goal_opts.goal_response_callback =
661 [&](
const rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::SharedPtr& goal_handle) {
665 RCLCPP_INFO(
LOGGER,
"Planning request rejected");
668 RCLCPP_INFO(
LOGGER,
"Planning request accepted");
670 send_goal_opts.result_callback =
671 [&](
const rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::WrappedResult& result) {
678 case rclcpp_action::ResultCode::SUCCEEDED:
679 RCLCPP_INFO(
LOGGER,
"Planning request complete!");
681 case rclcpp_action::ResultCode::ABORTED:
682 RCLCPP_INFO(
LOGGER,
"Planning request aborted");
684 case rclcpp_action::ResultCode::CANCELED:
685 RCLCPP_INFO(
LOGGER,
"Planning request canceled");
688 RCLCPP_INFO(
LOGGER,
"Planning request unknown result code");
693 auto goal_handle_future = move_action_client_->async_send_goal(goal, send_goal_opts);
698 std::this_thread::sleep_for(std::chrono::milliseconds(1));
701 if (code != rclcpp_action::ResultCode::SUCCEEDED)
703 RCLCPP_ERROR_STREAM(
LOGGER,
"MoveGroupInterface::plan() failed or timeout reached");
704 return res->error_code;
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);
712 return res->error_code;
717 if (!move_action_client_ || !move_action_client_->action_server_is_ready())
719 RCLCPP_INFO_STREAM(
LOGGER,
"MoveGroup action client/server not ready");
720 return moveit::core::MoveItErrorCode::FAILURE;
723 moveit_msgs::action::MoveGroup::Goal 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;
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();
737 send_goal_opts.goal_response_callback =
738 [&](
const rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::SharedPtr& goal_handle) {
742 RCLCPP_INFO(
LOGGER,
"Plan and Execute request rejected");
745 RCLCPP_INFO(
LOGGER,
"Plan and Execute request accepted");
747 send_goal_opts.result_callback =
748 [&](
const rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::WrappedResult& result) {
755 case rclcpp_action::ResultCode::SUCCEEDED:
756 RCLCPP_INFO(
LOGGER,
"Plan and Execute request complete!");
758 case rclcpp_action::ResultCode::ABORTED:
759 RCLCPP_INFO(
LOGGER,
"Plan and Execute request aborted");
761 case rclcpp_action::ResultCode::CANCELED:
762 RCLCPP_INFO(
LOGGER,
"Plan and Execute request canceled");
765 RCLCPP_INFO(
LOGGER,
"Plan and Execute request unknown result code");
769 auto goal_handle_future = move_action_client_->async_send_goal(goal, send_goal_opts);
771 return moveit::core::MoveItErrorCode::SUCCESS;
776 std::this_thread::sleep_for(std::chrono::milliseconds(1));
779 if (code != rclcpp_action::ResultCode::SUCCEEDED)
781 RCLCPP_ERROR_STREAM(
LOGGER,
"MoveGroupInterface::move() failed or timeout reached");
783 return res->error_code;
787 const std::vector<std::string>& controllers = std::vector<std::string>())
789 if (!execute_action_client_ || !execute_action_client_->action_server_is_ready())
791 RCLCPP_INFO_STREAM(
LOGGER,
"execute_action_client_ client/server not ready");
792 return moveit::core::MoveItErrorCode::FAILURE;
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();
800 send_goal_opts.goal_response_callback =
801 [&](
const rclcpp_action::ClientGoalHandle<moveit_msgs::action::ExecuteTrajectory>::SharedPtr& goal_handle) {
805 RCLCPP_INFO(
LOGGER,
"Execute request rejected");
808 RCLCPP_INFO(
LOGGER,
"Execute request accepted");
810 send_goal_opts.result_callback =
811 [&](
const rclcpp_action::ClientGoalHandle<moveit_msgs::action::ExecuteTrajectory>::WrappedResult& result) {
818 case rclcpp_action::ResultCode::SUCCEEDED:
819 RCLCPP_INFO(
LOGGER,
"Execute request success!");
821 case rclcpp_action::ResultCode::ABORTED:
822 RCLCPP_INFO(
LOGGER,
"Execute request aborted");
824 case rclcpp_action::ResultCode::CANCELED:
825 RCLCPP_INFO(
LOGGER,
"Execute request canceled");
828 RCLCPP_INFO(
LOGGER,
"Execute request unknown result code");
833 moveit_msgs::action::ExecuteTrajectory::Goal goal;
834 goal.trajectory = trajectory;
835 goal.controller_names = controllers;
837 auto goal_handle_future = execute_action_client_->async_send_goal(goal, send_goal_opts);
839 return moveit::core::MoveItErrorCode::SUCCESS;
844 std::this_thread::sleep_for(std::chrono::milliseconds(1));
847 if (code != rclcpp_action::ResultCode::SUCCEEDED)
849 RCLCPP_ERROR_STREAM(
LOGGER,
"MoveGroupInterface::execute() failed or timeout reached");
851 return res->error_code;
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)
859 auto req = std::make_shared<moveit_msgs::srv::GetCartesianPath::Request>();
860 moveit_msgs::srv::GetCartesianPath::Response::SharedPtr response;
862 if (considered_start_state_)
868 req->start_state.is_diff =
true;
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;
880 req->max_velocity_scaling_factor = max_velocity_scaling_factor_;
881 req->max_acceleration_scaling_factor = max_acceleration_scaling_factor_;
883 auto future_response = cartesian_path_service_->async_send_request(req);
884 if (future_response.valid())
886 response = future_response.get();
887 error_code = response->error_code;
888 if (response->error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
890 msg = response->solution;
891 return response->fraction;
898 error_code.val = error_code.FAILURE;
905 if (trajectory_event_publisher_)
907 std_msgs::msg::String event;
909 trajectory_event_publisher_->publish(event);
913 bool attachObject(
const std::string&
object,
const std::string& link,
const std::vector<std::string>& touch_links)
924 RCLCPP_ERROR(
LOGGER,
"No known link to attach object '%s' to",
object.c_str());
927 moveit_msgs::msg::AttachedCollisionObject aco;
928 aco.object.id = object;
929 aco.link_name.swap(l);
930 if (touch_links.empty())
932 aco.touch_links.push_back(aco.link_name);
936 aco.touch_links = touch_links;
938 aco.object.operation = moveit_msgs::msg::CollisionObject::ADD;
939 attached_object_publisher_->publish(aco);
945 moveit_msgs::msg::AttachedCollisionObject aco;
949 aco.link_name =
name;
953 aco.object.id =
name;
955 aco.object.operation = moveit_msgs::msg::CollisionObject::REMOVE;
956 if (aco.link_name.empty() && aco.object.id.empty())
959 const std::vector<std::string>& lnames = joint_model_group_->
getLinkModelNames();
960 for (
const std::string& lname : lnames)
962 aco.link_name = lname;
963 attached_object_publisher_->publish(aco);
968 attached_object_publisher_->publish(aco);
975 return goal_position_tolerance_;
980 return goal_orientation_tolerance_;
985 return goal_joint_tolerance_;
990 goal_joint_tolerance_ = tolerance;
995 goal_position_tolerance_ = tolerance;
1000 goal_orientation_tolerance_ = tolerance;
1006 allowed_planning_time_ = seconds;
1011 return allowed_planning_time_;
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_;
1025 if (considered_start_state_)
1031 request.start_state.is_diff =
true;
1034 if (active_target_ ==
JOINT)
1036 request.goal_constraints.resize(1);
1040 else if (active_target_ == POSE || active_target_ ==
POSITION || active_target_ ==
ORIENTATION)
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());
1051 request.goal_constraints.resize(goal_count);
1053 for (
const auto& pose_target : pose_targets_)
1055 for (std::size_t i = 0; i < pose_target.second.size(); ++i)
1058 pose_target.first, pose_target.second[i], goal_position_tolerance_, goal_orientation_tolerance_);
1060 c.position_constraints.clear();
1062 c.orientation_constraints.clear();
1068 RCLCPP_ERROR(
LOGGER,
"Unable to construct MotionPlanRequest representation");
1070 if (path_constraints_)
1071 request.path_constraints = *path_constraints_;
1072 if (trajectory_constraints_)
1073 request.trajectory_constraints = *trajectory_constraints_;
1138 path_constraints_ = std::make_unique<moveit_msgs::msg::Constraints>(constraint);
1143 if (constraints_storage_)
1146 if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.
group_name))
1149 std::make_unique<moveit_msgs::msg::Constraints>(
static_cast<moveit_msgs::msg::Constraints
>(*msg_m));
1161 path_constraints_.reset();
1166 trajectory_constraints_ = std::make_unique<moveit_msgs::msg::TrajectoryConstraints>(constraint);
1171 trajectory_constraints_.reset();
1176 while (initializing_constraints_)
1178 std::chrono::duration<double>
d(0.01);
1179 rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(
d), rclcpp::Context::SharedPtr(
nullptr));
1182 std::vector<std::string> c;
1183 if (constraints_storage_)
1184 constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.
group_name);
1191 if (path_constraints_)
1193 return *path_constraints_;
1197 return moveit_msgs::msg::Constraints();
1203 if (trajectory_constraints_)
1205 return *trajectory_constraints_;
1209 return moveit_msgs::msg::TrajectoryConstraints();
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); });
1222 void setWorkspace(
double minx,
double miny,
double minz,
double maxx,
double maxy,
double maxz)
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;
1236 return node_->get_clock();
1240 void initializeConstraintsStorageThread(
const std::string& host,
unsigned int port)
1246 conn->setParams(host, port);
1247 if (conn->connect())
1249 constraints_storage_ = std::make_unique<moveit_warehouse::ConstraintsStorage>(conn);
1252 catch (std::exception& ex)
1254 RCLCPP_ERROR(
LOGGER,
"%s", ex.what());
1256 initializing_constraints_ =
false;
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_;
1268 std::shared_ptr<rclcpp_action::Client<moveit_msgs::action::MoveGroup>> move_action_client_;
1271 std::shared_ptr<rclcpp_action::Client<moveit_msgs::action::ExecuteTrajectory>> execute_action_client_;
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_;
1286 int32_t look_around_attempts_;
1288 int32_t replan_attempts_;
1289 double replan_delay_;
1292 moveit::core::RobotStatePtr joint_state_target_;
1297 std::map<std::string, std::vector<geometry_msgs::msg::PoseStamped>> pose_targets_;
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_;
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_;
1315 std::unique_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
1316 std::unique_ptr<std::thread> constraints_init_thread_;
1317 bool initializing_constraints_;
1321 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1322 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)
1343 : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_)
1345 other.impl_ =
nullptr;
1353 impl_ = other.impl_;
1354 remembered_joint_values_ = std::move(other.remembered_joint_values_);
1355 other.impl_ =
nullptr;
1389 const std::string&
group)
const
1395 const std::map<std::string, std::string>& params,
bool replace)
1447 return impl_->
move(
false);
1457 return impl_->
move(
true);
1461 const std::vector<std::string>& controllers)
1463 return impl_->
execute(
plan.trajectory,
false, controllers);
1467 const std::vector<std::string>& controllers)
1469 return impl_->
execute(trajectory,
false, controllers);
1474 return impl_->
execute(
plan.trajectory,
true, controllers);
1478 const std::vector<std::string>& controllers)
1480 return impl_->
execute(trajectory,
true, controllers);
1529 double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory,
1530 bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code)
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);
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)
1544 return impl_->
computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1545 avoid_collisions, *error_code);
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);
1564 moveit::core::RobotStatePtr rs;
1565 if (start_state.is_diff)
1571 rs = std::make_shared<moveit::core::RobotState>(
getRobotModel());
1572 rs->setToDefaultValues();
1606 std::map<std::string, std::vector<double>>::const_iterator it = remembered_joint_values_.find(
name);
1607 std::map<std::string, double> positions;
1609 if (it != remembered_joint_values_.cend())
1612 for (
size_t x = 0; x < names.size(); ++x)
1614 positions[names[x]] = it->second[x];
1621 RCLCPP_ERROR(
LOGGER,
"The requested named target '%s' does not exist, returning empty positions.",
name.c_str());
1629 std::map<std::string, std::vector<double>>::const_iterator it = remembered_joint_values_.find(
name);
1630 if (it != remembered_joint_values_.end())
1641 RCLCPP_ERROR(
LOGGER,
"The requested named target '%s' does not exist",
name.c_str());
1654 if (joint_values.size() != n_group_joints)
1656 RCLCPP_DEBUG_STREAM(
LOGGER,
"Provided joint value list has length " << joint_values.size() <<
" but group "
1658 <<
" has " << n_group_joints <<
" joints");
1669 for (
const auto& pair : variable_values)
1671 if (std::find(allowed.begin(), allowed.end(), pair.first) == allowed.end())
1673 RCLCPP_ERROR_STREAM(
LOGGER,
"joint variable " << pair.first <<
" is not part of group "
1685 const std::vector<double>& variable_values)
1687 if (variable_names.size() != variable_values.size())
1689 RCLCPP_ERROR_STREAM(
LOGGER,
"sizes of name and position arrays do not match");
1693 for (
const auto& variable_name : variable_names)
1695 if (std::find(allowed.begin(), allowed.end(), variable_name) == allowed.end())
1697 RCLCPP_ERROR_STREAM(
LOGGER,
"joint variable " << variable_name <<
" is not part of group "
1717 std::vector<double> values(1, value);
1731 RCLCPP_ERROR_STREAM(
LOGGER,
1742 const std::string& end_effector_link)
1748 const std::string& end_effector_link)
1750 return impl_->
setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id,
false);
1755 geometry_msgs::msg::Pose msg = tf2::toMsg(eef_pose);
1760 const std::string& end_effector_link)
1766 const std::string& end_effector_link)
1768 return impl_->
setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id,
true);
1772 const std::string& end_effector_link)
1774 geometry_msgs::msg::Pose msg = tf2::toMsg(eef_pose);
1827 std::vector<geometry_msgs::msg::PoseStamped> pose_msg(1);
1828 pose_msg[0].pose = tf2::toMsg(pose);
1830 pose_msg[0].header.stamp = impl_->
getClock()->now();
1836 std::vector<geometry_msgs::msg::PoseStamped> pose_msg(1);
1837 pose_msg[0].pose = target;
1839 pose_msg[0].header.stamp = impl_->
getClock()->now();
1844 const std::string& end_effector_link)
1846 std::vector<geometry_msgs::msg::PoseStamped> targets(1, target);
1852 std::vector<geometry_msgs::msg::PoseStamped> pose_out(target.size());
1853 rclcpp::Time tm = impl_->
getClock()->now();
1855 for (std::size_t i = 0; i < target.size(); ++i)
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;
1865 const std::string& end_effector_link)
1867 std::vector<geometry_msgs::msg::PoseStamped> target_stamped(target.size());
1868 rclcpp::Time tm = impl_->
getClock()->now();
1870 for (std::size_t i = 0; i < target.size(); ++i)
1872 target_stamped[i].pose = target[i];
1873 target_stamped[i].header.stamp = tm;
1874 target_stamped[i].header.frame_id = frame_id;
1880 const std::string& end_effector_link)
1884 RCLCPP_ERROR(
LOGGER,
"No pose specified as goal target");
1899 const std::vector<geometry_msgs::msg::PoseStamped>&
1907 inline void transformPose(
const tf2_ros::Buffer& tf_buffer,
const std::string& desired_frame,
1908 geometry_msgs::msg::PoseStamped& target)
1910 if (desired_frame != target.header.frame_id)
1912 geometry_msgs::msg::PoseStamped target_in(target);
1913 tf_buffer.transform(target_in, target, desired_frame);
1915 target.header.stamp = rclcpp::Time(0);
1922 geometry_msgs::msg::PoseStamped target;
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;
1937 target.pose.position.x = x;
1938 target.pose.position.y = y;
1939 target.pose.position.z = z;
1947 geometry_msgs::msg::PoseStamped target;
1955 target.pose.position.x = 0.0;
1956 target.pose.position.y = 0.0;
1957 target.pose.position.z = 0.0;
1962 target.pose.orientation = tf2::toMsg(q);
1969 const std::string& end_effector_link)
1971 geometry_msgs::msg::PoseStamped target;
1979 target.pose.position.x = 0.0;
1980 target.pose.position.y = 0.0;
1981 target.pose.position.z = 0.0;
1985 target.pose.orientation.x = x;
1986 target.pose.orientation.y = y;
1987 target.pose.orientation.z = z;
1988 target.pose.orientation.w = w;
2053 moveit::core::RobotStatePtr current_state;
2054 std::vector<double> values;
2056 current_state->copyJointGroupPositions(
getName(), values);
2062 std::vector<double> r;
2069 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
2070 Eigen::Isometry3d pose;
2074 RCLCPP_ERROR(
LOGGER,
"No end-effector specified");
2078 moveit::core::RobotStatePtr current_state;
2084 pose = current_state->getGlobalLinkTransform(lm);
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);
2096 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
2097 Eigen::Isometry3d pose;
2101 RCLCPP_ERROR(
LOGGER,
"No end-effector specified");
2105 moveit::core::RobotStatePtr current_state;
2110 pose = current_state->getGlobalLinkTransform(lm);
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);
2122 std::vector<double> result;
2123 const std::string& eef = end_effector_link.empty() ?
getEndEffectorLink() : end_effector_link;
2126 RCLCPP_ERROR(
LOGGER,
"No end-effector specified");
2130 moveit::core::RobotStatePtr current_state;
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);
2166 moveit::core::RobotStatePtr current_state;
2168 return current_state;
2173 remembered_joint_values_[
name] = values;
2178 remembered_joint_values_.erase(
name);
2183 impl_->can_look_ = flag;
2184 RCLCPP_DEBUG(
LOGGER,
"Looking around: %s", flag ?
"yes" :
"no");
2191 RCLCPP_ERROR(
LOGGER,
"Tried to set negative number of look-around attempts");
2195 RCLCPP_DEBUG_STREAM(
LOGGER,
"Look around attempts: " << attempts);
2196 impl_->look_around_attempts_ = attempts;
2202 impl_->can_replan_ = flag;
2203 RCLCPP_DEBUG(
LOGGER,
"Replanning: %s", flag ?
"yes" :
"no");
2210 RCLCPP_ERROR(
LOGGER,
"Tried to set negative number of replan attempts");
2214 RCLCPP_DEBUG_STREAM(
LOGGER,
"Replan Attempts: " << attempts);
2215 impl_->replan_attempts_ = attempts;
2223 RCLCPP_ERROR(
LOGGER,
"Tried to set negative replan delay");
2227 RCLCPP_DEBUG_STREAM(
LOGGER,
"Replan Delay: " << delay);
2228 impl_->replan_delay_ = delay;
2279 impl_->
setWorkspace(minx, miny, minz, maxx, maxy, maxz);
2311 return attachObject(
object, link, std::vector<std::string>());
2315 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.
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 > ¶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()
const rclcpp::Logger LOGGER
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.
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)