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 {
48 }
49 
51 {
52  // start the move action server
55  move_action_server_->registerPreemptCallback(boost::bind(&MoveGroupMoveAction::preemptMoveCallback, this));
56  move_action_server_->start();
57 }
58 
59 void move_group::MoveGroupMoveAction::executeMoveCallback(const moveit_msgs::MoveGroupGoalConstPtr& goal)
60 {
62  // before we start planning, ensure that we have the latest robot state received...
63  context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());
64  context_->planning_scene_monitor_->updateFrameTransforms();
65 
66  moveit_msgs::MoveGroupResult action_res;
67  if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_)
68  {
69  if (!goal->planning_options.plan_only)
70  ROS_WARN("This instance of MoveGroup is not allowed to execute trajectories but the goal request has plan_only "
71  "set to false. Only a motion plan will be computed anyway.");
72  executeMoveCallback_PlanOnly(goal, action_res);
73  }
74  else
75  executeMoveCallback_PlanAndExecute(goal, action_res);
76 
77  bool planned_trajectory_empty = trajectory_processing::isTrajectoryEmpty(action_res.planned_trajectory);
78  std::string response =
79  getActionResultString(action_res.error_code, planned_trajectory_empty, goal->planning_options.plan_only);
80  if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
81  move_action_server_->setSucceeded(action_res, response);
82  else
83  {
84  if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
85  move_action_server_->setPreempted(action_res, response);
86  else
87  move_action_server_->setAborted(action_res, response);
88  }
89 
91 }
92 
93 void move_group::MoveGroupMoveAction::executeMoveCallback_PlanAndExecute(const moveit_msgs::MoveGroupGoalConstPtr& goal,
94  moveit_msgs::MoveGroupResult& action_res)
95 {
96  ROS_INFO("Combined planning and execution request received for MoveGroup action. Forwarding to planning and "
97  "execution pipeline.");
98 
99  if (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff))
100  {
101  planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
102  const robot_state::RobotState& current_state = lscene->getCurrentState();
103 
104  // check to see if the desired constraints are already met
105  for (std::size_t i = 0; i < goal->request.goal_constraints.size(); ++i)
106  if (lscene->isStateConstrained(current_state,
107  kinematic_constraints::mergeConstraints(goal->request.goal_constraints[i],
108  goal->request.path_constraints)))
109  {
110  ROS_INFO("Goal constraints are already satisfied. No need to plan or execute any motions");
111  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
112  return;
113  }
114  }
115 
117 
118  const moveit_msgs::MotionPlanRequest& motion_plan_request =
119  planning_scene::PlanningScene::isEmpty(goal->request.start_state) ? goal->request :
120  clearRequestStartState(goal->request);
121  const moveit_msgs::PlanningScene& planning_scene_diff =
122  planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff.robot_state) ?
123  goal->planning_options.planning_scene_diff :
124  clearSceneRobotState(goal->planning_options.planning_scene_diff);
125 
126  opt.replan_ = goal->planning_options.replan;
127  opt.replan_attempts_ = goal->planning_options.replan_attempts;
128  opt.replan_delay_ = goal->planning_options.replan_delay;
130 
131  opt.plan_callback_ =
132  boost::bind(&MoveGroupMoveAction::planUsingPlanningPipeline, this, boost::cref(motion_plan_request), _1);
133  if (goal->planning_options.look_around && context_->plan_with_sensing_)
134  {
135  opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
136  _1, opt.plan_callback_, goal->planning_options.look_around_attempts,
137  goal->planning_options.max_safe_execution_cost);
138  context_->plan_with_sensing_->setBeforeLookCallback(boost::bind(&MoveGroupMoveAction::startMoveLookCallback, this));
139  }
140 
142  context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt);
143 
144  convertToMsg(plan.plan_components_, action_res.trajectory_start, action_res.planned_trajectory);
145  if (plan.executed_trajectory_)
146  plan.executed_trajectory_->getRobotTrajectoryMsg(action_res.executed_trajectory);
147  action_res.error_code = plan.error_code_;
148 }
149 
150 void move_group::MoveGroupMoveAction::executeMoveCallback_PlanOnly(const moveit_msgs::MoveGroupGoalConstPtr& goal,
151  moveit_msgs::MoveGroupResult& action_res)
152 {
153  ROS_INFO("Planning request received for MoveGroup action. Forwarding to planning pipeline.");
154 
155  planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_); // lock the scene so that it
156  // does not modify the world
157  // representation while
158  // diff() is called
159  const planning_scene::PlanningSceneConstPtr& the_scene =
160  (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff)) ?
161  static_cast<const planning_scene::PlanningSceneConstPtr&>(lscene) :
162  lscene->diff(goal->planning_options.planning_scene_diff);
164  try
165  {
166  context_->planning_pipeline_->generatePlan(the_scene, goal->request, res);
167  }
168  catch (std::exception& ex)
169  {
170  ROS_ERROR("Planning pipeline threw an exception: %s", ex.what());
171  res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
172  }
173 
174  convertToMsg(res.trajectory_, action_res.trajectory_start, action_res.planned_trajectory);
175  action_res.error_code = res.error_code_;
176  action_res.planning_time = res.planning_time_;
177 }
178 
181 {
183 
185  bool solved = false;
187  try
188  {
189  solved = context_->planning_pipeline_->generatePlan(plan.planning_scene_, req, res);
190  }
191  catch (std::exception& ex)
192  {
193  ROS_ERROR("Planning pipeline threw an exception: %s", ex.what());
194  res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
195  }
196  if (res.trajectory_)
197  {
198  plan.plan_components_.resize(1);
199  plan.plan_components_[0].trajectory_ = res.trajectory_;
200  plan.plan_components_[0].description_ = "plan";
201  }
202  plan.error_code_ = res.error_code_;
203  return solved;
204 }
205 
207 {
209 }
210 
212 {
214 }
215 
217 {
218  context_->plan_execution_->stop();
219 }
220 
222 {
223  move_state_ = state;
224  move_feedback_.state = stateToStr(state);
225  move_action_server_->publishFeedback(move_feedback_);
226 }
227 
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 Mon Jan 15 2018 03:51:53