41 #include <moveit_msgs/DisplayTrajectory.h>
42 #include <visualization_msgs/MarkerArray.h>
43 #include <boost/tokenizer.hpp>
44 #include <boost/algorithm/string/join.hpp>
53 const std::string& planner_plugin_param_name,
54 const std::string& adapter_plugins_param_name)
55 : active_{
false }, pipeline_nh_(pipeline_nh), private_nh_(
"~"), robot_model_(model)
58 if (pipeline_nh_.getParam(planner_plugin_param_name, planner))
59 planner_plugin_name_ = planner;
62 if (pipeline_nh_.getParam(adapter_plugins_param_name, adapters))
64 boost::char_separator<char> sep(
" ");
65 boost::tokenizer<boost::char_separator<char>> tok(adapters, sep);
66 for (boost::tokenizer<boost::char_separator<char>>::iterator beg = tok.begin(); beg != tok.end(); ++beg)
67 adapter_plugin_names_.push_back(*beg);
75 const std::string& planner_plugin_name,
76 const std::vector<std::string>& adapter_plugin_names)
78 , pipeline_nh_(pipeline_nh)
80 , planner_plugin_name_(planner_plugin_name)
81 , adapter_plugin_names_(adapter_plugin_names)
89 check_solution_paths_ =
false;
90 publish_received_requests_ =
false;
91 display_computed_motion_plans_ =
false;
96 planner_plugin_loader_ = std::make_unique<pluginlib::ClassLoader<planning_interface::PlannerManager>>(
97 "moveit_core",
"planning_interface::PlannerManager");
101 ROS_FATAL_STREAM(
"Exception while creating planning plugin loader " << ex.what());
104 std::vector<std::string> classes;
105 if (planner_plugin_loader_)
106 classes = planner_plugin_loader_->getDeclaredClasses();
107 if (planner_plugin_name_.empty() && classes.size() == 1)
109 planner_plugin_name_ = classes[0];
110 ROS_INFO(
"No '~planning_plugin' parameter specified, but only '%s' planning plugin is available. Using that one.",
111 planner_plugin_name_.c_str());
113 if (planner_plugin_name_.empty() && classes.size() > 1)
115 planner_plugin_name_ = classes[0];
116 ROS_INFO(
"Multiple planning plugins available. You should specify the '~planning_plugin' parameter. Using '%s' for "
118 planner_plugin_name_.c_str());
122 planner_instance_ = planner_plugin_loader_->createUniqueInstance(planner_plugin_name_);
123 if (!planner_instance_->initialize(robot_model_, pipeline_nh_.getNamespace()))
124 throw std::runtime_error(
"Unable to initialize planning plugin");
125 ROS_INFO_STREAM(
"Using planning interface '" << planner_instance_->getDescription() <<
"'");
130 << planner_plugin_name_ <<
"': " << ex.what() << std::endl
131 <<
"Available plugins: " << boost::algorithm::join(classes,
", "));
135 if (!adapter_plugin_names_.empty())
137 std::vector<planning_request_adapter::PlanningRequestAdapterConstPtr> ads;
140 adapter_plugin_loader_ =
141 std::make_unique<pluginlib::ClassLoader<planning_request_adapter::PlanningRequestAdapter>>(
142 "moveit_core",
"planning_request_adapter::PlanningRequestAdapter");
146 ROS_ERROR_STREAM(
"Exception while creating planning plugin loader " << ex.what());
149 if (adapter_plugin_loader_)
150 for (
const std::string& adapter_plugin_name : adapter_plugin_names_)
152 planning_request_adapter::PlanningRequestAdapterPtr ad;
155 ad = adapter_plugin_loader_->createUniqueInstance(adapter_plugin_name);
159 ROS_ERROR_STREAM(
"Exception while loading planning adapter plugin '" << adapter_plugin_name
160 <<
"': " << ex.what());
164 ad->initialize(pipeline_nh_);
165 ads.push_back(std::move(ad));
170 adapter_chain_ = std::make_unique<planning_request_adapter::PlanningRequestAdapterChain>();
171 for (planning_request_adapter::PlanningRequestAdapterConstPtr& ad : ads)
173 ROS_INFO_STREAM(
"Using planning request adapter '" << ad->getDescription() <<
"'");
174 adapter_chain_->addAdapter(ad);
178 displayComputedMotionPlans(
true);
179 checkSolutionPaths(
true);
184 if (display_computed_motion_plans_ && !flag)
185 display_path_publisher_.shutdown();
186 else if (!display_computed_motion_plans_ && flag)
187 display_path_publisher_ = private_nh_.advertise<moveit_msgs::DisplayTrajectory>(DISPLAY_PATH_TOPIC, 10,
true);
188 display_computed_motion_plans_ = flag;
193 if (publish_received_requests_ && !flag)
194 received_request_publisher_.shutdown();
195 else if (!publish_received_requests_ && flag)
196 received_request_publisher_ =
197 private_nh_.advertise<moveit_msgs::MotionPlanRequest>(MOTION_PLAN_REQUEST_TOPIC, 10,
true);
198 publish_received_requests_ = flag;
203 if (check_solution_paths_ && !flag)
204 contacts_publisher_.shutdown();
205 else if (!check_solution_paths_ && flag)
206 contacts_publisher_ = private_nh_.advertise<visualization_msgs::MarkerArray>(MOTION_CONTACTS_TOPIC, 100,
true);
207 check_solution_paths_ = flag;
214 std::vector<std::size_t> dummy;
221 std::vector<std::size_t>& adapter_added_state_index)
const
227 if (publish_received_requests_)
228 received_request_publisher_.publish(req);
229 adapter_added_state_index.clear();
231 if (!planner_instance_)
233 ROS_ERROR(
"No planning plugin loaded. Cannot plan.");
244 solved = adapter_chain_->adaptAndPlan(planner_instance_,
planning_scene, req, res, adapter_added_state_index);
245 if (!adapter_added_state_index.empty())
247 std::stringstream ss;
248 for (std::size_t added_index : adapter_added_state_index)
249 ss << added_index <<
" ";
250 ROS_INFO(
"Planning adapters have added states at index positions: [ %s]", ss.str().c_str());
255 planning_interface::PlanningContextPtr context =
257 solved = context ? context->solve(res) :
false;
260 catch (std::exception& ex)
262 ROS_ERROR(
"Exception caught: '%s'", ex.what());
271 std::size_t state_count = res.
trajectory_->getWayPointCount();
272 ROS_DEBUG_STREAM(
"Motion planner reported a solution path with " << state_count <<
" states");
273 if (check_solution_paths_)
275 visualization_msgs::MarkerArray arr;
276 visualization_msgs::Marker m;
277 m.action = visualization_msgs::Marker::DELETEALL;
278 arr.markers.push_back(m);
280 std::vector<std::size_t>
index;
285 bool problem =
false;
286 for (std::size_t i = 0; i <
index.size() && !problem; ++i)
289 for (std::size_t added_index : adapter_added_state_index)
290 if (
index[i] == added_index)
301 ROS_DEBUG(
"It appears the robot is starting at an invalid state, but that is ok.");
305 res.
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
308 std::stringstream ss;
309 for (std::size_t it :
index)
311 ROS_ERROR_STREAM(
"Computed path is not valid. Invalid states at index locations: [ "
312 << ss.str() <<
"] out of " << state_count
313 <<
". Explanations follow in command line. Contacts are published on "
314 << private_nh_.resolveName(MOTION_CONTACTS_TOPIC));
317 for (std::size_t it :
index)
321 planning_scene->isStateValid(robot_state, req.path_constraints, req.group_name,
true);
333 visualization_msgs::MarkerArray arr_i;
336 arr.markers.insert(arr.markers.end(), arr_i.markers.begin(), arr_i.markers.end());
343 ROS_DEBUG(
"Planned path was found to be valid, except for states that were added by planning request "
344 "adapters, but that is ok.");
347 ROS_DEBUG(
"Planned path was found to be valid when rechecked");
348 contacts_publisher_.publish(arr);
353 if (display_computed_motion_plans_ && solved)
355 moveit_msgs::DisplayTrajectory disp;
356 disp.model_id = robot_model_->getName();
357 disp.trajectory.resize(1);
358 res.
trajectory_->getRobotTrajectoryMsg(disp.trajectory[0]);
360 display_path_publisher_.publish(disp);
367 bool stacked_constraints =
false;
368 if (req.path_constraints.position_constraints.size() > 1 || req.path_constraints.orientation_constraints.size() > 1)
369 stacked_constraints =
true;
370 for (
const auto& constraint : req.goal_constraints)
372 if (constraint.position_constraints.size() > 1 || constraint.orientation_constraints.size() > 1)
373 stacked_constraints =
true;
375 if (stacked_constraints)
376 ROS_WARN(
"More than one constraint is set. If your move_group does not have multiple end effectors/arms, this is "
377 "unusual. Are you using a move_group_interface and forgetting to call clearPoseTargets() or "
382 return solved && valid;
387 if (planner_instance_)
388 planner_instance_->terminate();