moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
move_group_sequence_action.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, Willow Garage, 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 Willow Garage 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: Ioan Sucan */
36
37// Modified by Pilz GmbH & Co. KG
38
40
41#include <time.h>
42
51
54
56{
57namespace
58{
59rclcpp::Logger getLogger()
60{
61 return moveit::getLogger("moveit.planners.pilz.move_group_sequence_action");
62}
63} // namespace
64
66 : MoveGroupCapability("SequenceAction")
67 , move_feedback_(std::make_shared<moveit_msgs::action::MoveGroupSequence::Feedback>())
68{
69}
70
72{
73 // start the move action server
74 RCLCPP_INFO_STREAM(getLogger(), "initialize move group sequence action");
75 // Use MutuallyExclusiveCallbackGroup to prevent race conditions in callbacks.
76 // See: https://github.com/moveit/moveit2/issues/3117 for details.
77 action_callback_group_ =
78 context_->moveit_cpp_->getNode()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
79 move_action_server_ = rclcpp_action::create_server<moveit_msgs::action::MoveGroupSequence>(
80 context_->moveit_cpp_->getNode(), "sequence_move_group",
81 [](const rclcpp_action::GoalUUID& /* unused */,
82 const std::shared_ptr<const moveit_msgs::action::MoveGroupSequence::Goal>& /* unused */) {
83 RCLCPP_DEBUG(getLogger(), "Received action goal");
84 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
85 },
86 [this](const std::shared_ptr<MoveGroupSequenceGoalHandle>& /* unused goal_handle */) {
87 RCLCPP_DEBUG(getLogger(), "Canceling action goal");
88 preemptMoveCallback();
89 return rclcpp_action::CancelResponse::ACCEPT;
90 },
91 [this](const std::shared_ptr<MoveGroupSequenceGoalHandle>& goal_handle) {
92 RCLCPP_DEBUG(getLogger(), "Accepting new action goal");
93 executeSequenceCallback(goal_handle);
94 },
95 rcl_action_server_get_default_options(), action_callback_group_);
96
97 command_list_manager_ = std::make_unique<pilz_industrial_motion_planner::CommandListManager>(
98 context_->moveit_cpp_->getNode(), context_->planning_scene_monitor_->getRobotModel());
99}
100
101void MoveGroupSequenceAction::executeSequenceCallback(const std::shared_ptr<MoveGroupSequenceGoalHandle>& goal_handle)
102{
103 // Notify that goal is being executed
104 goal_handle_ = goal_handle;
105 const auto goal = goal_handle->get_goal();
106
107 setMoveState(move_group::PLANNING);
108
109 // Handle empty requests
110 if (goal->request.items.empty())
111 {
112 RCLCPP_WARN(getLogger(), "Received empty request. That's ok but maybe not what you intended.");
113 setMoveState(move_group::IDLE);
114 const auto action_res = std::make_shared<moveit_msgs::action::MoveGroupSequence::Result>();
115 action_res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
116 goal_handle->succeed(action_res);
117 return;
118 }
119
120 // before we start planning, ensure that we have the latest robot state
121 // received...
122 auto node = context_->moveit_cpp_->getNode();
123 context_->planning_scene_monitor_->waitForCurrentRobotState(node->get_clock()->now());
124 context_->planning_scene_monitor_->updateFrameTransforms();
125
126 const auto action_res = std::make_shared<moveit_msgs::action::MoveGroupSequence::Result>();
127 if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_)
128 {
129 if (!goal->planning_options.plan_only)
130 {
131 RCLCPP_WARN(getLogger(), "Only plan will be calculated, although plan_only == false."); // LCOV_EXCL_LINE
132 }
133 executeMoveCallbackPlanOnly(goal, action_res);
134 }
135 else
136 {
137 executeSequenceCallbackPlanAndExecute(goal, action_res);
138 }
139
140 switch (action_res->response.error_code.val)
141 {
142 case moveit_msgs::msg::MoveItErrorCodes::SUCCESS:
143 goal_handle->succeed(action_res);
144 break;
145 case moveit_msgs::msg::MoveItErrorCodes::PREEMPTED:
146 goal_handle->canceled(action_res);
147 break;
148 default:
149 goal_handle->abort(action_res);
150 break;
151 }
152
153 setMoveState(move_group::IDLE);
154 goal_handle_.reset();
155}
156
157void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute(
158 const moveit_msgs::action::MoveGroupSequence::Goal::ConstSharedPtr& goal,
159 const moveit_msgs::action::MoveGroupSequence::Result::SharedPtr& action_res)
160{
161 RCLCPP_INFO(getLogger(), "Combined planning and execution request received for MoveGroupSequenceAction.");
162
164 const moveit_msgs::msg::PlanningScene& planning_scene_diff =
165 moveit::core::isEmpty(goal->planning_options.planning_scene_diff.robot_state) ?
166 goal->planning_options.planning_scene_diff :
167 clearSceneRobotState(goal->planning_options.planning_scene_diff);
168
169 opt.replan = goal->planning_options.replan;
170 opt.replan_attemps = goal->planning_options.replan_attempts;
171 opt.replan_delay = goal->planning_options.replan_delay;
172 opt.before_execution_callback_ = [this] { startMoveExecutionCallback(); };
173
174 opt.plan_callback = [this, &request = goal->request](plan_execution::ExecutableMotionPlan& plan) {
175 return planUsingSequenceManager(request, plan);
176 };
177
179 context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt);
180
181 StartStatesMsg start_states_msg;
182 convertToMsg(plan.plan_components, start_states_msg, action_res->response.planned_trajectories);
183 try
184 {
185 action_res->response.sequence_start = start_states_msg.at(0);
186 }
187 catch (std::out_of_range&)
188 {
189 RCLCPP_WARN(getLogger(), "Can not determine start state from empty sequence.");
190 }
191 action_res->response.error_code = plan.error_code;
192}
193
194void MoveGroupSequenceAction::convertToMsg(const ExecutableTrajs& trajs, StartStatesMsg& start_states_msg,
195 PlannedTrajMsgs& planned_trajs_msgs)
196{
197 start_states_msg.resize(trajs.size());
198 planned_trajs_msgs.resize(trajs.size());
199 for (size_t i = 0; i < trajs.size(); ++i)
200 {
201 moveit::core::robotStateToRobotStateMsg(trajs.at(i).trajectory->getFirstWayPoint(), start_states_msg.at(i));
202 trajs.at(i).trajectory->getRobotTrajectoryMsg(planned_trajs_msgs.at(i));
203 }
204}
205
206void MoveGroupSequenceAction::executeMoveCallbackPlanOnly(
207 const moveit_msgs::action::MoveGroupSequence::Goal::ConstSharedPtr& goal,
208 const moveit_msgs::action::MoveGroupSequence::Result::SharedPtr& action_res)
209{
210 RCLCPP_INFO(getLogger(), "Planning request received for MoveGroupSequenceAction action.");
211
212 // lock the scene so that it does not modify the world representation while
213 // diff() is called
214 planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
215
216 const planning_scene::PlanningSceneConstPtr& the_scene =
217 (moveit::core::isEmpty(goal->planning_options.planning_scene_diff)) ?
218 static_cast<const planning_scene::PlanningSceneConstPtr&>(lscene) :
219 lscene->diff(goal->planning_options.planning_scene_diff);
220
221 rclcpp::Time planning_start = context_->moveit_cpp_->getNode()->now();
222 RobotTrajCont traj_vec;
223 try
224 {
225 // Select planning_pipeline to handle request
226 // All motions in the SequenceRequest need to use the same planning pipeline (but can use different planners)
227 const planning_pipeline::PlanningPipelinePtr planning_pipeline =
228 resolvePlanningPipeline(goal->request.items[0].req.pipeline_id);
230 {
231 RCLCPP_ERROR_STREAM(getLogger(), "Could not load planning pipeline " << goal->request.items[0].req.pipeline_id);
232 action_res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
233 return;
234 }
235
236 traj_vec = command_list_manager_->solve(the_scene, planning_pipeline, goal->request);
237 }
238 catch (const MoveItErrorCodeException& ex)
239 {
240 RCLCPP_ERROR_STREAM(getLogger(), "> Planning pipeline threw an exception (error code: " << ex.getErrorCode()
241 << "): " << ex.what());
242 action_res->response.error_code.val = ex.getErrorCode();
243 return;
244 }
245 // LCOV_EXCL_START // Keep moveit up even if lower parts throw
246 catch (const std::exception& ex)
247 {
248 RCLCPP_ERROR(getLogger(), "Planning pipeline threw an exception: %s", ex.what());
249 action_res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
250 return;
251 }
252 // LCOV_EXCL_STOP
253
254 StartStatesMsg start_states_msg;
255 start_states_msg.resize(traj_vec.size());
256 action_res->response.planned_trajectories.resize(traj_vec.size());
257 for (RobotTrajCont::size_type i = 0; i < traj_vec.size(); ++i)
258 {
259 move_group::MoveGroupCapability::convertToMsg(traj_vec.at(i), start_states_msg.at(i),
260 action_res->response.planned_trajectories.at(i));
261 }
262 try
263 {
264 action_res->response.sequence_start = start_states_msg.at(0);
265 }
266 catch (std::out_of_range&)
267 {
268 RCLCPP_WARN(getLogger(), "Can not determine start state from empty sequence.");
269 }
270
271 action_res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
272 action_res->response.planning_time = (context_->moveit_cpp_->getNode()->now() - planning_start).seconds();
273}
274
275bool MoveGroupSequenceAction::planUsingSequenceManager(const moveit_msgs::msg::MotionSequenceRequest& req,
277{
278 setMoveState(move_group::PLANNING);
279
280 planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor);
281 RobotTrajCont traj_vec;
282 try
283 {
284 // Select planning_pipeline to handle request
285 // All motions in the SequenceRequest need to use the same planning pipeline (but can use different planners)
286 const planning_pipeline::PlanningPipelinePtr planning_pipeline =
287 resolvePlanningPipeline(req.items[0].req.pipeline_id);
289 {
290 RCLCPP_ERROR_STREAM(getLogger(), "Could not load planning pipeline " << req.items[0].req.pipeline_id);
291 return false;
292 }
293
294 traj_vec = command_list_manager_->solve(plan.planning_scene, planning_pipeline, req);
295 }
296 catch (const MoveItErrorCodeException& ex)
297 {
298 RCLCPP_ERROR_STREAM(getLogger(), "Planning pipeline threw an exception (error code: " << ex.getErrorCode()
299 << "): " << ex.what());
300 plan.error_code.val = ex.getErrorCode();
301 return false;
302 }
303 // LCOV_EXCL_START // Keep MoveIt up even if lower parts throw
304 catch (const std::exception& ex)
305 {
306 RCLCPP_ERROR_STREAM(getLogger(), "Planning pipeline threw an exception: " << ex.what());
307 plan.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
308 return false;
309 }
310 // LCOV_EXCL_STOP
311
312 if (!traj_vec.empty())
313 {
314 plan.plan_components.resize(traj_vec.size());
315 for (size_t i = 0; i < traj_vec.size(); ++i)
316 {
317 plan.plan_components.at(i).trajectory = traj_vec.at(i);
318 plan.plan_components.at(i).description = "plan";
319 }
320 }
321 plan.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
322 return true;
323}
324
325void MoveGroupSequenceAction::startMoveExecutionCallback()
326{
327 setMoveState(move_group::MONITOR);
328}
329
330void MoveGroupSequenceAction::preemptMoveCallback()
331{
332 context_->plan_execution_->stop();
333}
334
335void MoveGroupSequenceAction::setMoveState(move_group::MoveGroupState state)
336{
337 move_state_ = state;
338 move_feedback_->state = stateToStr(state);
339 goal_handle_->publish_feedback(move_feedback_);
340}
341
342} // namespace pilz_industrial_motion_planner
343
344#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin< kdl_kinematics_plugin::KDLKinematicsPlugin >, kinematics::KinematicsBase)
moveit_msgs::msg::PlanningScene clearSceneRobotState(const moveit_msgs::msg::PlanningScene &scene) const
void convertToMsg(const std::vector< plan_execution::ExecutableTrajectory > &trajectory, moveit_msgs::msg::RobotState &first_state_msg, std::vector< moveit_msgs::msg::RobotTrajectory > &trajectory_msg) const
std::string stateToStr(MoveGroupState state) const
planning_pipeline::PlanningPipelinePtr resolvePlanningPipeline(const std::string &pipeline_id) const
Provide action to handle multiple trajectories and execute the result in the form of a MoveGroup capa...
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
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.
bool isEmpty(const moveit_msgs::msg::PlanningScene &msg)
Check if a message includes any information about a planning scene, or whether it is empty.
planning_interface::MotionPlanResponse plan(std::shared_ptr< moveit_cpp::PlanningComponent > &planning_component, std::shared_ptr< moveit_cpp::PlanningComponent::PlanRequestParameters > &single_plan_parameters, std::shared_ptr< moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters > &multi_plan_parameters, std::shared_ptr< planning_scene::PlanningScene > &planning_scene, std::optional< const moveit::planning_pipeline_interfaces::SolutionSelectionFunction > solution_selection_function, std::optional< moveit::planning_pipeline_interfaces::StoppingCriterionFunction > stopping_criterion_callback)
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
Definition logger.cpp:79
std::vector< robot_trajectory::RobotTrajectoryPtr > RobotTrajCont
A generic representation on what a computed motion plan looks like.
double replan_delay
The amount of time to wait in between replanning attempts (in seconds)
ExecutableMotionPlanComputationFn plan_callback
Callback for computing motion plans. This callback must always be specified.
std::function< void()> before_execution_callback_
bool replan
Flag indicating whether replanning is allowed.
moveit::core::MoveItErrorCode error_code
robot_trajectory::RobotTrajectoryPtr trajectory