planning_component.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, PickNik LLC
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of PickNik LLC nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Henning Kayser */
36 
37 #include <stdexcept>
38 
44 #include <thread>
45 
46 namespace moveit_cpp
47 {
48 constexpr char LOGNAME[] = "planning_component";
49 
50 PlanningComponent::PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp)
51  : nh_(moveit_cpp->getNodeHandle()), moveit_cpp_(moveit_cpp), group_name_(group_name)
52 {
53  joint_model_group_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name);
54  if (!joint_model_group_)
55  {
56  std::string error = "Could not find joint model group '" + group_name + "'.";
58  throw std::runtime_error(error);
59  }
60  plan_request_parameters_.load(nh_);
62  LOGNAME, "Plan request parameters loaded with --"
63  << " planning_pipeline: " << plan_request_parameters_.planning_pipeline << ","
64  << " planner_id: " << plan_request_parameters_.planner_id << ","
65  << " planning_time: " << plan_request_parameters_.planning_time << ","
66  << " planning_attempts: " << plan_request_parameters_.planning_attempts << ","
67  << " max_velocity_scaling_factor: " << plan_request_parameters_.max_velocity_scaling_factor << ","
68  << " max_acceleration_scaling_factor: " << plan_request_parameters_.max_acceleration_scaling_factor);
69 }
70 
71 PlanningComponent::PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh)
72  : PlanningComponent(group_name, std::make_shared<MoveItCpp>(nh))
73 {
74 }
75 
76 PlanningComponent::~PlanningComponent()
77 {
78  ROS_INFO_NAMED(LOGNAME, "Deleting PlanningComponent '%s'", group_name_.c_str());
79  clearContents();
80 }
81 
82 const std::vector<std::string> PlanningComponent::getNamedTargetStates()
83 {
84  if (joint_model_group_)
85  {
86  return joint_model_group_->getDefaultStateNames();
87  }
88  else
89  {
90  ROS_WARN_NAMED(LOGNAME, "Unable to find joint group with name '%s'.", group_name_.c_str());
91  }
92 
93  std::vector<std::string> empty;
94  return empty;
95 }
96 
97 const std::string& PlanningComponent::getPlanningGroupName() const
98 {
99  return group_name_;
100 }
101 
102 bool PlanningComponent::setPathConstraints(const moveit_msgs::Constraints& path_constraints)
103 {
104  current_path_constraints_ = path_constraints;
105  return true;
106 }
107 
108 bool PlanningComponent::setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& trajectory_constraints)
109 {
110  current_trajectory_constraints_ = trajectory_constraints;
111  return true;
112 }
113 
114 planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequestParameters& parameters,
115  const bool store_solution)
116 {
117  auto plan_solution = planning_interface::MotionPlanResponse();
118  if (!joint_model_group_)
119  {
120  ROS_ERROR_NAMED(LOGNAME, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str());
121  plan_solution.error_code_ = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME;
122  if (store_solution)
123  {
124  last_plan_solution_ = plan_solution;
125  }
126  return plan_solution;
127  }
128 
129  // Clone current planning scene
130  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor =
131  moveit_cpp_->getPlanningSceneMonitorNonConst();
132  planning_scene_monitor->updateFrameTransforms();
133  planning_scene_monitor->lockSceneRead(); // LOCK planning scene
134  planning_scene::PlanningScenePtr planning_scene =
136  planning_scene_monitor->unlockSceneRead(); // UNLOCK planning scene
137  planning_scene_monitor.reset(); // release this pointer
138 
139  // Init MotionPlanRequest
141  req.group_name = group_name_;
142  req.planner_id = parameters.planner_id;
143  req.num_planning_attempts = std::max(1, parameters.planning_attempts);
144  req.allowed_planning_time = parameters.planning_time;
145  req.max_velocity_scaling_factor = parameters.max_velocity_scaling_factor;
146  req.max_acceleration_scaling_factor = parameters.max_acceleration_scaling_factor;
147  if (workspace_parameters_set_)
148  req.workspace_parameters = workspace_parameters_;
149 
150  // Set start state
151  moveit::core::RobotStatePtr start_state = considered_start_state_;
152  if (!start_state)
153  start_state = moveit_cpp_->getCurrentState();
154  start_state->update();
155  moveit::core::robotStateToRobotStateMsg(*start_state, req.start_state);
156  planning_scene->setCurrentState(*start_state);
157 
158  // Set goal constraints
159  if (current_goal_constraints_.empty())
160  {
161  ROS_ERROR_NAMED(LOGNAME, "No goal constraints set for planning request");
162  plan_solution.error_code_ = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS;
163  if (store_solution)
164  {
165  last_plan_solution_ = plan_solution;
166  }
167  return plan_solution;
168  }
169  req.goal_constraints = current_goal_constraints_;
170 
171  // Set path constraints
172  req.path_constraints = current_path_constraints_;
173 
174  // Set trajectory constraints
175  req.trajectory_constraints = current_trajectory_constraints_;
176 
177  // Run planning attempt
179  const auto& pipelines = moveit_cpp_->getPlanningPipelines();
180  auto it = pipelines.find(parameters.planning_pipeline);
181  if (it == pipelines.end())
182  {
183  ROS_ERROR_NAMED(LOGNAME, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str());
184  plan_solution.error_code_ = moveit::core::MoveItErrorCode::FAILURE;
185  if (store_solution)
186  {
187  last_plan_solution_ = plan_solution;
188  }
189  return plan_solution;
190  }
191  const planning_pipeline::PlanningPipelinePtr pipeline = it->second;
192  pipeline->generatePlan(planning_scene, req, res);
193  plan_solution.error_code_ = res.error_code_;
194  if (res.error_code_.val != res.error_code_.SUCCESS)
195  {
196  ROS_ERROR("Could not compute plan successfully");
197  if (store_solution)
198  {
199  last_plan_solution_ = plan_solution;
200  }
201  return plan_solution;
202  }
203  plan_solution.trajectory_ = res.trajectory_;
204  plan_solution.planning_time_ = res.planning_time_;
205  plan_solution.start_state_ = req.start_state;
206  plan_solution.error_code_ = res.error_code_.val;
207  // TODO(henningkayser): Visualize trajectory
208  // std::vector<const moveit::core::LinkModel*> eef_links;
209  // if (joint_model_group->getEndEffectorTips(eef_links))
210  //{
211  // for (const auto& eef_link : eef_links)
212  // {
213  // ROS_INFO_STREAM("Publishing trajectory for end effector " << eef_link->getName());
214  // visual_tools_->publishTrajectoryLine(last_solution_trajectory_, eef_link);
215  // visual_tools_->publishTrajectoryPath(last_solution_trajectory_, false);
216  // visual_tools_->publishRobotState(last_solution_trajectory_->getLastWayPoint(), rviz_visual_tools::TRANSLUCENT);
217  // }
218  //}
219 
220  if (store_solution)
221  {
222  last_plan_solution_ = plan_solution;
223  }
224  return plan_solution;
225 }
226 
228 PlanningComponent::plan(const MultiPipelinePlanRequestParameters& parameters,
229  const SolutionCallbackFunction& solution_selection_callback,
230  const StoppingCriterionFunction& stopping_criterion_callback)
231 {
232  // Create solutions container
233  PlanningComponent::PlanSolutions planning_solutions{ parameters.multi_plan_request_parameters.size() };
234  std::vector<std::thread> planning_threads;
235  planning_threads.reserve(parameters.multi_plan_request_parameters.size());
236 
237  // Print a warning if more parallel planning problems than available concurrent threads are defined. If
238  // std::thread::hardware_concurrency() is not defined, the command returns 0 so the check does not work
239  auto const hardware_concurrency = std::thread::hardware_concurrency();
240  if (parameters.multi_plan_request_parameters.size() > hardware_concurrency && hardware_concurrency != 0)
241  {
243  LOGNAME,
244  "More parallel planning problems defined ('%ld') than possible to solve concurrently with the hardware ('%d')",
245  parameters.multi_plan_request_parameters.size(), hardware_concurrency);
246  }
247 
248  // Launch planning threads
249  for (auto const& plan_request_parameter : parameters.multi_plan_request_parameters)
250  {
251  auto planning_thread = std::thread([&]() {
252  auto plan_solution = planning_interface::MotionPlanResponse();
253  try
254  {
255  plan_solution = plan(plan_request_parameter, false);
256  }
257  catch (const std::exception& e)
258  {
259  ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning pipeline '" << plan_request_parameter.planning_pipeline.c_str()
260  << "' threw exception '" << e.what() << "'");
261  plan_solution.error_code_ = moveit::core::MoveItErrorCode::FAILURE;
262  }
263  plan_solution.planner_id_ = plan_request_parameter.planner_id;
264  planning_solutions.pushBack(plan_solution);
265 
266  if (stopping_criterion_callback != nullptr)
267  {
268  if (stopping_criterion_callback(planning_solutions, parameters))
269  {
270  // Terminate planning pipelines
272  "Stopping criterion met: Terminating planning pipelines that are still active");
273  for (auto const& plan_request_parameter : parameters.multi_plan_request_parameters)
274  {
275  moveit_cpp_->terminatePlanningPipeline(plan_request_parameter.planning_pipeline);
276  }
277  }
278  }
279  });
280  planning_threads.push_back(std::move(planning_thread));
281  }
282 
283  // Wait for threads to finish
284  for (auto& planning_thread : planning_threads)
285  {
286  if (planning_thread.joinable())
287  {
288  planning_thread.join();
289  }
290  }
291 
292  // Return best solution determined by user defined callback (Default: Shortest path)
293  last_plan_solution_ = solution_selection_callback(planning_solutions.getSolutions());
294  return last_plan_solution_;
295 }
296 
297 planning_interface::MotionPlanResponse PlanningComponent::plan()
298 {
299  return plan(plan_request_parameters_);
300 }
301 
302 bool PlanningComponent::setStartState(const moveit::core::RobotState& start_state)
303 {
304  considered_start_state_ = std::make_shared<moveit::core::RobotState>(start_state);
305  return true;
306 }
307 
308 moveit::core::RobotStatePtr PlanningComponent::getStartState()
309 {
310  if (considered_start_state_)
311  return considered_start_state_;
312  else
313  {
314  moveit::core::RobotStatePtr s;
315  moveit_cpp_->getCurrentState(s, 1.0);
316  return s;
317  }
318 }
319 
320 bool PlanningComponent::setStartState(const std::string& start_state_name)
321 {
322  const auto& named_targets = getNamedTargetStates();
323  if (std::find(named_targets.begin(), named_targets.end(), start_state_name) == named_targets.end())
324  {
325  ROS_ERROR_NAMED(LOGNAME, "No predefined joint state found for target name '%s'", start_state_name.c_str());
326  return false;
327  }
328  moveit::core::RobotState start_state(moveit_cpp_->getRobotModel());
329  start_state.setToDefaultValues(joint_model_group_, start_state_name);
330  return setStartState(start_state);
331 }
332 
333 void PlanningComponent::setStartStateToCurrentState()
334 {
335  considered_start_state_.reset();
336 }
337 
338 std::map<std::string, double> PlanningComponent::getNamedTargetStateValues(const std::string& name)
339 {
340  // TODO(henningkayser): verify result
341  std::map<std::string, double> positions;
342  joint_model_group_->getVariableDefaultPositions(name, positions);
343  return positions;
344 }
345 
346 void PlanningComponent::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
347 {
348  workspace_parameters_.header.frame_id = moveit_cpp_->getRobotModel()->getModelFrame();
349  workspace_parameters_.header.stamp = ros::Time::now();
350  workspace_parameters_.min_corner.x = minx;
351  workspace_parameters_.min_corner.y = miny;
352  workspace_parameters_.min_corner.z = minz;
353  workspace_parameters_.max_corner.x = maxx;
354  workspace_parameters_.max_corner.y = maxy;
355  workspace_parameters_.max_corner.z = maxz;
356  workspace_parameters_set_ = true;
357 }
358 
359 void PlanningComponent::unsetWorkspace()
360 {
361  workspace_parameters_set_ = false;
362 }
363 
364 bool PlanningComponent::setGoal(const std::vector<moveit_msgs::Constraints>& goal_constraints)
365 {
366  current_goal_constraints_ = goal_constraints;
367  return true;
368 }
369 
370 bool PlanningComponent::setGoal(const moveit::core::RobotState& goal_state)
371 {
372  current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_) };
373  return true;
374 }
375 
376 bool PlanningComponent::setGoal(const geometry_msgs::PoseStamped& goal_pose, const std::string& link_name)
377 {
378  current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(link_name, goal_pose) };
379  return true;
380 }
381 
382 bool PlanningComponent::setGoal(const std::string& goal_state_name)
383 {
384  const auto& named_targets = getNamedTargetStates();
385  if (std::find(named_targets.begin(), named_targets.end(), goal_state_name) == named_targets.end())
386  {
387  ROS_ERROR_NAMED(LOGNAME, "No predefined joint state found for target name '%s'", goal_state_name.c_str());
388  return false;
389  }
390  moveit::core::RobotState goal_state(moveit_cpp_->getRobotModel());
391  goal_state.setToDefaultValues(joint_model_group_, goal_state_name);
392  return setGoal(goal_state);
393 }
394 
395 bool PlanningComponent::execute(bool blocking)
396 {
397  if (!last_plan_solution_)
398  {
399  ROS_ERROR_NAMED(LOGNAME, "There is no successful plan to execute");
400  return false;
401  }
402 
403  // TODO(henningkayser): parameterize timestamps if required
404  // trajectory_processing::TimeOptimalTrajectoryGeneration totg;
405  // if (!totg.computeTimeStamps(*last_solution_trajectory_, max_velocity_scaling_factor_,
406  // max_acceleration_scaling_factor_))
407  //{
408  // ROS_ERROR("Failed to parameterize trajectory");
409  // return false;
410  //}
411  return moveit_cpp_->execute(group_name_, last_plan_solution_.trajectory_, blocking);
412 }
413 
414 planning_interface::MotionPlanResponse const& PlanningComponent::getLastMotionPlanResponse()
415 {
416  return last_plan_solution_;
417 }
418 
420 getShortestSolution(std::vector<planning_interface::MotionPlanResponse> const& solutions)
421 {
422  // Find trajectory with minimal path
423  auto const shortest_trajectory = std::min_element(solutions.begin(), solutions.end(),
424  [](planning_interface::MotionPlanResponse const& solution_a,
425  planning_interface::MotionPlanResponse const& solution_b) {
426  // If both solutions were successful, check which path is shorter
427  if (solution_a && solution_b)
428  {
429  return robot_trajectory::path_length(*solution_a.trajectory_) <
430  robot_trajectory::path_length(*solution_b.trajectory_);
431  }
432  // If only solution a is successful, return a
433  else if (solution_a)
434  {
435  return true;
436  }
437  // Else return solution b, either because it is successful or not
438  return false;
439  });
440  if (shortest_trajectory->trajectory_ != nullptr)
441  {
442  ROS_INFO_NAMED(LOGNAME, "Chosen solution with shortest path length: '%f'",
443  robot_trajectory::path_length(*shortest_trajectory->trajectory_));
444  }
445  else
446  {
447  ROS_INFO_STREAM_NAMED(LOGNAME, "Could not determine shortest path");
448  }
449  return *shortest_trajectory;
450 }
451 
452 void PlanningComponent::clearContents()
453 {
454  considered_start_state_.reset();
455  last_plan_solution_ = planning_interface::MotionPlanResponse();
456  current_goal_constraints_.clear();
457  moveit_cpp_.reset();
458 }
459 } // namespace moveit_cpp
planning_scene_monitor
Definition: current_state_monitor.h:47
moveit_cpp::PlanningComponent::PlanRequestParameters::planning_attempts
int planning_attempts
Definition: planning_component.h:127
planning_interface::MotionPlanResponse
utils.h
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
s
XmlRpcServer s
moveit_cpp::PlanningComponent::PlanRequestParameters::max_acceleration_scaling_factor
double max_acceleration_scaling_factor
Definition: planning_component.h:130
planning_interface::MotionPlanResponse::error_code_
moveit::core::MoveItErrorCode error_code_
kinematic_constraints::constructGoalConstraints
moveit_msgs::Constraints constructGoalConstraints(const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance=std::numeric_limits< double >::epsilon())
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
robot_trajectory::path_length
double path_length(RobotTrajectory const &trajectory)
moveit_cpp::PlanningComponent::PlanRequestParameters::planning_pipeline
std::string planning_pipeline
Definition: planning_component.h:126
moveit_cpp
Definition: moveit_cpp.h:50
moveit_cpp::PlanningComponent::PlanRequestParameters::planner_id
std::string planner_id
Definition: planning_component.h:125
planning_interface::MotionPlanResponse::trajectory_
robot_trajectory::RobotTrajectoryPtr trajectory_
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
ROS_FATAL_STREAM_NAMED
#define ROS_FATAL_STREAM_NAMED(name, args)
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
ROS_ERROR
#define ROS_ERROR(...)
moveit_cpp::PlanningComponent::PlanRequestParameters
Planner parameters provided with the MotionPlanRequest.
Definition: planning_component.h:123
moveit_cpp::PlanningComponent::PlanRequestParameters::planning_time
double planning_time
Definition: planning_component.h:128
planning_scene_monitor.h
planning_interface::MotionPlanRequest
moveit_msgs::MotionPlanRequest MotionPlanRequest
planning_component.h
moveit_cpp::PlanningComponent::PlanRequestParameters::max_velocity_scaling_factor
double max_velocity_scaling_factor
Definition: planning_component.h:129
LOGNAME
static const std::string LOGNAME
Definition: collision_plugin_loader.cpp:37
moveit_cpp::getShortestSolution
planning_interface::MotionPlanResponse getShortestSolution(std::vector< planning_interface::MotionPlanResponse > const &solutions)
A function to choose the solution with the shortest path from a vector of solutions.
Definition: planning_component.cpp:452
planning_scene::PlanningScene::clone
static PlanningScenePtr clone(const PlanningSceneConstPtr &scene)
std
planning_pipeline.h
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
ROS_INFO_STREAM_NAMED
#define ROS_INFO_STREAM_NAMED(name, args)
conversions.h
moveit_cpp::PlanningComponent::PlanningComponent
PlanningComponent(const std::string &group_name, const ros::NodeHandle &nh)
Constructor.
Definition: planning_component.cpp:103
planning_interface::MotionPlanResponse::planning_time_
double planning_time_
planning_scene
moveit_cpp::LOGNAME
constexpr char LOGNAME[]
Definition: moveit_cpp.cpp:74
moveit::core::robotStateToRobotStateMsg
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
ros::NodeHandle
moveit::planning_interface::PlanningComponent
moveit_cpp::PlanningComponent PlanningComponent
Definition: planning_component.h:265
moveit::planning_interface::MoveItCpp
moveit_cpp::MoveItCpp MoveItCpp
Definition: moveit_cpp.h:192
ros::Time::now
static Time now()


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Mar 3 2024 03:24:16