move_action_capability.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 #include "move_action_capability.h"
38 
45 
47  : MoveGroupCapability("MoveAction"), move_state_(IDLE), preempt_requested_{ false }
48 {
49 }
50 
52 {
53  // start the move action server
56  move_action_server_->registerPreemptCallback(boost::bind(&MoveGroupMoveAction::preemptMoveCallback, this));
57  move_action_server_->start();
58 }
59 
60 void move_group::MoveGroupMoveAction::executeMoveCallback(const moveit_msgs::MoveGroupGoalConstPtr& goal)
61 {
63  // before we start planning, ensure that we have the latest robot state received...
64  context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());
65  context_->planning_scene_monitor_->updateFrameTransforms();
66 
67  moveit_msgs::MoveGroupResult action_res;
68  if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_)
69  {
70  if (!goal->planning_options.plan_only)
71  ROS_WARN("This instance of MoveGroup is not allowed to execute trajectories but the goal request has plan_only "
72  "set to false. Only a motion plan will be computed anyway.");
73  executeMoveCallback_PlanOnly(goal, action_res);
74  }
75  else
76  executeMoveCallback_PlanAndExecute(goal, action_res);
77 
78  bool planned_trajectory_empty = trajectory_processing::isTrajectoryEmpty(action_res.planned_trajectory);
79  std::string response =
80  getActionResultString(action_res.error_code, planned_trajectory_empty, goal->planning_options.plan_only);
81  if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
82  move_action_server_->setSucceeded(action_res, response);
83  else
84  {
85  if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
86  move_action_server_->setPreempted(action_res, response);
87  else
88  move_action_server_->setAborted(action_res, response);
89  }
90 
92 
93  preempt_requested_ = false;
94 }
95 
96 void move_group::MoveGroupMoveAction::executeMoveCallback_PlanAndExecute(const moveit_msgs::MoveGroupGoalConstPtr& goal,
97  moveit_msgs::MoveGroupResult& action_res)
98 {
99  ROS_INFO("Combined planning and execution request received for MoveGroup action. Forwarding to planning and "
100  "execution pipeline.");
101 
102  if (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff))
103  {
104  planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
105  const robot_state::RobotState& current_state = lscene->getCurrentState();
106 
107  // check to see if the desired constraints are already met
108  for (std::size_t i = 0; i < goal->request.goal_constraints.size(); ++i)
109  if (lscene->isStateConstrained(current_state,
110  kinematic_constraints::mergeConstraints(goal->request.goal_constraints[i],
111  goal->request.path_constraints)))
112  {
113  ROS_INFO("Goal constraints are already satisfied. No need to plan or execute any motions");
114  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
115  return;
116  }
117  }
118 
120 
121  const moveit_msgs::MotionPlanRequest& motion_plan_request =
122  planning_scene::PlanningScene::isEmpty(goal->request.start_state) ? goal->request :
123  clearRequestStartState(goal->request);
124  const moveit_msgs::PlanningScene& planning_scene_diff =
125  planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff.robot_state) ?
126  goal->planning_options.planning_scene_diff :
127  clearSceneRobotState(goal->planning_options.planning_scene_diff);
128 
129  opt.replan_ = goal->planning_options.replan;
130  opt.replan_attempts_ = goal->planning_options.replan_attempts;
131  opt.replan_delay_ = goal->planning_options.replan_delay;
133 
134  opt.plan_callback_ =
135  boost::bind(&MoveGroupMoveAction::planUsingPlanningPipeline, this, boost::cref(motion_plan_request), _1);
136  if (goal->planning_options.look_around && context_->plan_with_sensing_)
137  {
138  opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
139  _1, opt.plan_callback_, goal->planning_options.look_around_attempts,
140  goal->planning_options.max_safe_execution_cost);
141  context_->plan_with_sensing_->setBeforeLookCallback(boost::bind(&MoveGroupMoveAction::startMoveLookCallback, this));
142  }
143 
145  if (preempt_requested_)
146  {
147  ROS_INFO("Preempt requested before the goal is planned and executed.");
148  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
149  return;
150  }
151 
152  context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt);
153 
154  convertToMsg(plan.plan_components_, action_res.trajectory_start, action_res.planned_trajectory);
155  if (plan.executed_trajectory_)
156  plan.executed_trajectory_->getRobotTrajectoryMsg(action_res.executed_trajectory);
157  action_res.error_code = plan.error_code_;
158 }
159 
160 void move_group::MoveGroupMoveAction::executeMoveCallback_PlanOnly(const moveit_msgs::MoveGroupGoalConstPtr& goal,
161  moveit_msgs::MoveGroupResult& action_res)
162 {
163  ROS_INFO("Planning request received for MoveGroup action. Forwarding to planning pipeline.");
164 
165  planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_); // lock the scene so that it
166  // does not modify the world
167  // representation while
168  // diff() is called
169  const planning_scene::PlanningSceneConstPtr& the_scene =
170  (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff)) ?
171  static_cast<const planning_scene::PlanningSceneConstPtr&>(lscene) :
172  lscene->diff(goal->planning_options.planning_scene_diff);
174 
175  if (preempt_requested_)
176  {
177  ROS_INFO("Preempt requested before the goal is planned.");
178  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
179  return;
180  }
181 
182  try
183  {
184  context_->planning_pipeline_->generatePlan(the_scene, goal->request, res);
185  }
186  catch (std::exception& ex)
187  {
188  ROS_ERROR("Planning pipeline threw an exception: %s", ex.what());
189  res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
190  }
191 
192  convertToMsg(res.trajectory_, action_res.trajectory_start, action_res.planned_trajectory);
193  action_res.error_code = res.error_code_;
194  action_res.planning_time = res.planning_time_;
195 }
196 
199 {
201 
203  bool solved = false;
205  try
206  {
207  solved = context_->planning_pipeline_->generatePlan(plan.planning_scene_, req, res);
208  }
209  catch (std::exception& ex)
210  {
211  ROS_ERROR("Planning pipeline threw an exception: %s", ex.what());
212  res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
213  }
214  if (res.trajectory_)
215  {
216  plan.plan_components_.resize(1);
217  plan.plan_components_[0].trajectory_ = res.trajectory_;
218  plan.plan_components_[0].description_ = "plan";
219  }
220  plan.error_code_ = res.error_code_;
221  return solved;
222 }
223 
225 {
227 }
228 
230 {
232 }
233 
235 {
236  preempt_requested_ = true;
237  context_->plan_execution_->stop();
238 }
239 
241 {
242  move_state_ = state;
243  move_feedback_.state = stateToStr(state);
244  move_action_server_->publishFeedback(move_feedback_);
245 }
246 
robot_trajectory::RobotTrajectoryPtr trajectory_
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
static const std::string MOVE_ACTION
std::string stateToStr(MoveGroupState state) const
moveit_msgs::MoveItErrorCodes error_code_
#define ROS_WARN(...)
void executeMoveCallback_PlanAndExecute(const moveit_msgs::MoveGroupGoalConstPtr &goal, moveit_msgs::MoveGroupResult &action_res)
moveit_msgs::MoveItErrorCodes error_code_
planning_interface::MotionPlanRequest clearRequestStartState(const planning_interface::MotionPlanRequest &request) const
planning_scene::PlanningSceneConstPtr planning_scene_
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
moveit_msgs::MoveGroupFeedback move_feedback_
bool computePlan(ExecutableMotionPlan &plan, const ExecutableMotionPlanComputationFn &motion_planner, unsigned int max_look_attempts, double max_safe_path_cost)
#define ROS_INFO(...)
void executeMoveCallback_PlanOnly(const moveit_msgs::MoveGroupGoalConstPtr &goal, moveit_msgs::MoveGroupResult &action_res)
ExecutableMotionPlanComputationFn plan_callback_
robot_trajectory::RobotTrajectoryPtr executed_trajectory_
std::string getActionResultString(const moveit_msgs::MoveItErrorCodes &error_code, bool planned_trajectory_empty, bool plan_only)
bool isTrajectoryEmpty(const moveit_msgs::RobotTrajectory &trajectory)
void executeMoveCallback(const moveit_msgs::MoveGroupGoalConstPtr &goal)
moveit_msgs::MotionPlanRequest MotionPlanRequest
void convertToMsg(const std::vector< plan_execution::ExecutableTrajectory > &trajectory, moveit_msgs::RobotState &first_state_msg, std::vector< moveit_msgs::RobotTrajectory > &trajectory_msg) const
static Time now()
std::unique_ptr< actionlib::SimpleActionServer< moveit_msgs::MoveGroupAction > > move_action_server_
moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
void setMoveState(MoveGroupState state)
boost::function< void()> before_execution_callback_
moveit_msgs::PlanningScene clearSceneRobotState(const moveit_msgs::PlanningScene &scene) const
#define ROS_ERROR(...)
bool planUsingPlanningPipeline(const planning_interface::MotionPlanRequest &req, plan_execution::ExecutableMotionPlan &plan)
std::vector< ExecutableTrajectory > plan_components_
CLASS_LOADER_REGISTER_CLASS(Dog, Base)


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:18:14