moveit2
The MoveIt Motion Planning Framework for ROS 2.
planning_component.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, PickNik Inc.
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 Inc. 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 
45 namespace moveit_cpp
46 {
47 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning_interface.planning_component");
48 
49 PlanningComponent::PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp)
50  : node_(moveit_cpp->getNode()), moveit_cpp_(moveit_cpp), group_name_(group_name)
51 {
52  joint_model_group_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name);
53  if (!joint_model_group_)
54  {
55  std::string error = "Could not find joint model group '" + group_name + "'.";
56  RCLCPP_FATAL_STREAM(LOGGER, error);
57  throw std::runtime_error(error);
58  }
59  planning_pipeline_names_ = moveit_cpp_->getPlanningPipelineNames(group_name);
60  plan_request_parameters_.load(node_);
61  RCLCPP_DEBUG_STREAM(
62  LOGGER, "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 rclcpp::Node::SharedPtr& node)
72  : PlanningComponent(group_name, std::make_shared<MoveItCpp>(node))
73 {
74  joint_model_group_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name);
75  if (!joint_model_group_)
76  {
77  std::string error = "Could not find joint model group '" + group_name + "'.";
78  RCLCPP_FATAL_STREAM(LOGGER, error);
79  throw std::runtime_error(error);
80  }
81  planning_pipeline_names_ = moveit_cpp_->getPlanningPipelineNames(group_name);
82 }
83 
84 const std::vector<std::string> PlanningComponent::getNamedTargetStates()
85 {
86  if (joint_model_group_)
87  {
88  return joint_model_group_->getDefaultStateNames();
89  }
90  else
91  {
92  RCLCPP_WARN(LOGGER, "Unable to find joint group with name '%s'.", group_name_.c_str());
93  }
94 
95  std::vector<std::string> empty;
96  return empty;
97 }
98 
99 const std::string& PlanningComponent::getPlanningGroupName() const
100 {
101  return group_name_;
102 }
103 
104 bool PlanningComponent::setPathConstraints(const moveit_msgs::msg::Constraints& path_constraints)
105 {
106  current_path_constraints_ = path_constraints;
107  return true;
108 }
109 
111 {
112  last_plan_solution_ = std::make_shared<PlanSolution>();
113  if (!joint_model_group_)
114  {
115  RCLCPP_ERROR(LOGGER, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str());
116  last_plan_solution_->error_code = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME;
117  return *last_plan_solution_;
118  }
119 
120  // Clone current planning scene
121  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor =
122  moveit_cpp_->getPlanningSceneMonitorNonConst();
123  planning_scene_monitor->updateFrameTransforms();
124  const planning_scene::PlanningScenePtr planning_scene = planning_scene_monitor->copyPlanningScene();
125  planning_scene_monitor.reset(); // release this pointer
126 
127  // Init MotionPlanRequest
129  req.group_name = group_name_;
130  req.planner_id = parameters.planner_id;
131  req.num_planning_attempts = std::max(1, parameters.planning_attempts);
132  req.allowed_planning_time = parameters.planning_time;
133  req.max_velocity_scaling_factor = parameters.max_velocity_scaling_factor;
134  req.max_acceleration_scaling_factor = parameters.max_acceleration_scaling_factor;
135  if (workspace_parameters_set_)
136  req.workspace_parameters = workspace_parameters_;
137 
138  // Set start state
139  moveit::core::RobotStatePtr start_state = considered_start_state_;
140  if (!start_state)
141  start_state = moveit_cpp_->getCurrentState();
142  start_state->update();
143  moveit::core::robotStateToRobotStateMsg(*start_state, req.start_state);
144  planning_scene->setCurrentState(*start_state);
145 
146  // Set goal constraints
147  if (current_goal_constraints_.empty())
148  {
149  RCLCPP_ERROR(LOGGER, "No goal constraints set for planning request");
150  last_plan_solution_->error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS;
151  return *last_plan_solution_;
152  }
153  req.goal_constraints = current_goal_constraints_;
154 
155  // Set path constraints
156  req.path_constraints = current_path_constraints_;
157 
158  // Run planning attempt
160  if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end())
161  {
162  RCLCPP_ERROR(LOGGER, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str());
163  last_plan_solution_->error_code = moveit::core::MoveItErrorCode::FAILURE;
164  return *last_plan_solution_;
165  }
166  const planning_pipeline::PlanningPipelinePtr pipeline =
167  moveit_cpp_->getPlanningPipelines().at(parameters.planning_pipeline);
168  pipeline->generatePlan(planning_scene, req, res);
169  last_plan_solution_->error_code = res.error_code_.val;
170  if (res.error_code_.val != res.error_code_.SUCCESS)
171  {
172  RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
173  return *last_plan_solution_;
174  }
175  last_plan_solution_->start_state = req.start_state;
176  last_plan_solution_->trajectory = res.trajectory_;
177  // TODO(henningkayser): Visualize trajectory
178  // std::vector<const moveit::core::LinkModel*> eef_links;
179  // if (joint_model_group->getEndEffectorTips(eef_links))
180  //{
181  // for (const auto& eef_link : eef_links)
182  // {
183  // RCLCPP_INFO_STREAM("Publishing trajectory for end effector " << eef_link->getName());
184  // visual_tools_->publishTrajectoryLine(last_solution_trajectory_, eef_link);
185  // visual_tools_->publishTrajectoryPath(last_solution_trajectory_, false);
186  // visual_tools_->publishRobotState(last_solution_trajectory_->getLastWayPoint(), rviz_visual_tools::TRANSLUCENT);
187  // }
188  //}
189  return *last_plan_solution_;
190 }
191 
193 {
194  return plan(plan_request_parameters_);
195 }
196 
198 {
199  considered_start_state_ = std::make_shared<moveit::core::RobotState>(start_state);
200  return true;
201 }
202 
203 moveit::core::RobotStatePtr PlanningComponent::getStartState()
204 {
205  if (considered_start_state_)
206  return considered_start_state_;
207  else
208  {
209  moveit::core::RobotStatePtr s;
210  moveit_cpp_->getCurrentState(s, 1.0);
211  return s;
212  }
213 }
214 
215 bool PlanningComponent::setStartState(const std::string& start_state_name)
216 {
217  const auto& named_targets = getNamedTargetStates();
218  if (std::find(named_targets.begin(), named_targets.end(), start_state_name) == named_targets.end())
219  {
220  RCLCPP_ERROR(LOGGER, "No predefined joint state found for target name '%s'", start_state_name.c_str());
221  return false;
222  }
223  moveit::core::RobotState start_state(moveit_cpp_->getRobotModel());
224  start_state.setToDefaultValues(joint_model_group_, start_state_name);
225  return setStartState(start_state);
226 }
227 
229 {
230  considered_start_state_.reset();
231 }
232 
233 std::map<std::string, double> PlanningComponent::getNamedTargetStateValues(const std::string& name)
234 {
235  // TODO(henningkayser): verify result
236  std::map<std::string, double> positions;
237  joint_model_group_->getVariableDefaultPositions(name, positions);
238  return positions;
239 }
240 
241 void PlanningComponent::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
242 {
243  workspace_parameters_.header.frame_id = moveit_cpp_->getRobotModel()->getModelFrame();
244  workspace_parameters_.header.stamp = node_->now();
245  workspace_parameters_.min_corner.x = minx;
246  workspace_parameters_.min_corner.y = miny;
247  workspace_parameters_.min_corner.z = minz;
248  workspace_parameters_.max_corner.x = maxx;
249  workspace_parameters_.max_corner.y = maxy;
250  workspace_parameters_.max_corner.z = maxz;
251  workspace_parameters_set_ = true;
252 }
253 
255 {
256  workspace_parameters_set_ = false;
257 }
258 
259 bool PlanningComponent::setGoal(const std::vector<moveit_msgs::msg::Constraints>& goal_constraints)
260 {
261  current_goal_constraints_ = goal_constraints;
262  return true;
263 }
264 
266 {
267  current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_) };
268  return true;
269 }
270 
271 bool PlanningComponent::setGoal(const geometry_msgs::msg::PoseStamped& goal_pose, const std::string& link_name)
272 {
273  current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(link_name, goal_pose) };
274  return true;
275 }
276 
277 bool PlanningComponent::setGoal(const std::string& goal_state_name)
278 {
279  const auto& named_targets = getNamedTargetStates();
280  if (std::find(named_targets.begin(), named_targets.end(), goal_state_name) == named_targets.end())
281  {
282  RCLCPP_ERROR(LOGGER, "No predefined joint state found for target name '%s'", goal_state_name.c_str());
283  return false;
284  }
285  moveit::core::RobotState goal_state(moveit_cpp_->getRobotModel());
286  goal_state.setToDefaultValues(joint_model_group_, goal_state_name);
287  return setGoal(goal_state);
288 }
289 
290 bool PlanningComponent::execute(bool blocking)
291 {
292  if (!last_plan_solution_)
293  {
294  RCLCPP_ERROR(LOGGER, "There is no successful plan to execute");
295  return false;
296  }
297 
298  // TODO(henningkayser): parameterize timestamps if required
299  // trajectory_processing::TimeOptimalTrajectoryGeneration totg;
300  // if (!totg.computeTimeStamps(*last_solution_trajectory_, max_velocity_scaling_factor_,
301  // max_acceleration_scaling_factor_))
302  //{
303  // RCLCPP_ERROR("Failed to parameterize trajectory");
304  // return false;
305  //}
306  return moveit_cpp_->execute(group_name_, last_plan_solution_->trajectory, blocking);
307 }
308 
309 const PlanningComponent::PlanSolutionPtr PlanningComponent::getLastPlanSolution()
310 {
311  return last_plan_solution_;
312 }
313 } // namespace moveit_cpp
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)
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
void setStartStateToCurrentState()
Set the start state for planning to be the current state of the robot.
bool setStartState(const moveit::core::RobotState &start_state)
Set the robot state that should be considered as start state for planning.
std::map< std::string, double > getNamedTargetStateValues(const std::string &name)
Get the joint values for targets specified by name.
void unsetWorkspace()
Remove the workspace bounding box from planning.
bool setGoal(const std::vector< moveit_msgs::msg::Constraints > &goal_constraints)
Set the goal constraints used for planning.
PlanSolution plan()
Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() usi...
const std::string & getPlanningGroupName() const
Get the name of the planning group.
const PlanSolutionPtr getLastPlanSolution()
Return the last plan solution.
PlanningComponent(const std::string &group_name, const rclcpp::Node::SharedPtr &node)
Constructor.
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....
bool setPathConstraints(const moveit_msgs::msg::Constraints &path_constraints)
Set the path constraints generated from a moveit msg Constraints.
const std::vector< std::string > getNamedTargetStates()
Get the names of the named robot states available as targets.
bool execute(bool blocking=true)
Execute the latest computed solution trajectory computed by plan(). By default this function terminat...
moveit::core::RobotStatePtr getStartState()
Get the considered start state if defined, otherwise get the current state.
moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
Generates a constraint message intended to be used as a goal constraint for a joint group....
Definition: utils.cpp:144
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.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
This namespace includes the central class for representing planning contexts.
name
Definition: setup.py:7
const rclcpp::Logger LOGGER
Definition: async_test.h:31
Planner parameters provided with the MotionPlanRequest.
void load(const rclcpp::Node::SharedPtr &node)
The representation of a plan solution.
robot_trajectory::RobotTrajectoryPtr trajectory_
moveit_msgs::msg::MoveItErrorCodes error_code_