move_action_capability.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include "move_action_capability.h"
00038 
00039 #include <moveit/planning_pipeline/planning_pipeline.h>
00040 #include <moveit/plan_execution/plan_execution.h>
00041 #include <moveit/plan_execution/plan_with_sensing.h>
00042 #include <moveit/trajectory_processing/trajectory_tools.h>
00043 #include <moveit/kinematic_constraints/utils.h>
00044 #include <moveit/move_group/capability_names.h>
00045 
00046 move_group::MoveGroupMoveAction::MoveGroupMoveAction() :
00047   MoveGroupCapability("MoveAction"),
00048   move_state_(IDLE)
00049 {
00050 }
00051 
00052 void move_group::MoveGroupMoveAction::initialize()
00053 {
00054   // start the move action server
00055   move_action_server_.reset(new actionlib::SimpleActionServer<moveit_msgs::MoveGroupAction>(root_node_handle_, MOVE_ACTION,
00056                                                                                             boost::bind(&MoveGroupMoveAction::executeMoveCallback, this, _1), false));
00057   move_action_server_->registerPreemptCallback(boost::bind(&MoveGroupMoveAction::preemptMoveCallback, this));
00058   move_action_server_->start();
00059 }
00060 
00061 void move_group::MoveGroupMoveAction::executeMoveCallback(const moveit_msgs::MoveGroupGoalConstPtr& goal)
00062 {
00063   setMoveState(PLANNING);
00064   context_->planning_scene_monitor_->updateFrameTransforms();
00065 
00066   moveit_msgs::MoveGroupResult action_res;
00067   if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_)
00068   {
00069     if (!goal->planning_options.plan_only)
00070       ROS_WARN("This instance of MoveGroup is not allowed to execute trajectories but the goal request has plan_only set to false. Only a motion plan will be computed anyway.");
00071     executeMoveCallback_PlanOnly(goal, action_res);
00072   }
00073   else
00074     executeMoveCallback_PlanAndExecute(goal, action_res);
00075 
00076   bool planned_trajectory_empty = trajectory_processing::isTrajectoryEmpty(action_res.planned_trajectory);
00077   std::string response = getActionResultString(action_res.error_code, planned_trajectory_empty, goal->planning_options.plan_only);
00078   if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00079     move_action_server_->setSucceeded(action_res, response);
00080   else
00081   {
00082     if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
00083       move_action_server_->setPreempted(action_res, response);
00084     else
00085       move_action_server_->setAborted(action_res, response);
00086   }
00087 
00088   setMoveState(IDLE);
00089 }
00090 
00091 void move_group::MoveGroupMoveAction::executeMoveCallback_PlanAndExecute(const moveit_msgs::MoveGroupGoalConstPtr& goal, moveit_msgs::MoveGroupResult &action_res)
00092 {
00093   ROS_INFO("Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.");
00094 
00095   if (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff))
00096   {
00097     planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
00098     const robot_state::RobotState &current_state = lscene->getCurrentState();
00099 
00100     // check to see if the desired constraints are already met
00101     for (std::size_t i = 0 ; i < goal->request.goal_constraints.size() ; ++i)
00102       if (lscene->isStateConstrained(current_state, kinematic_constraints::mergeConstraints(goal->request.goal_constraints[i],
00103                                                                                             goal->request.path_constraints)))
00104       {
00105         ROS_INFO("Goal constraints are already satisfied. No need to plan or execute any motions");
00106         action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00107         return;
00108       }
00109   }
00110 
00111   plan_execution::PlanExecution::Options opt;
00112 
00113   const moveit_msgs::MotionPlanRequest &motion_plan_request = planning_scene::PlanningScene::isEmpty(goal->request.start_state) ?
00114     goal->request : clearRequestStartState(goal->request);
00115   const moveit_msgs::PlanningScene &planning_scene_diff = planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff.robot_state) ?
00116     goal->planning_options.planning_scene_diff : clearSceneRobotState(goal->planning_options.planning_scene_diff);
00117 
00118   opt.replan_ = goal->planning_options.replan;
00119   opt.replan_attempts_ = goal->planning_options.replan_attempts;
00120   opt.replan_delay_ = goal->planning_options.replan_delay;
00121   opt.before_execution_callback_ = boost::bind(&MoveGroupMoveAction::startMoveExecutionCallback, this);
00122 
00123   opt.plan_callback_ = boost::bind(&MoveGroupMoveAction::planUsingPlanningPipeline, this, boost::cref(motion_plan_request), _1);
00124   if (goal->planning_options.look_around && context_->plan_with_sensing_)
00125   {
00126     opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(), _1, opt.plan_callback_,
00127                                      goal->planning_options.look_around_attempts, goal->planning_options.max_safe_execution_cost);
00128     context_->plan_with_sensing_->setBeforeLookCallback(boost::bind(&MoveGroupMoveAction::startMoveLookCallback, this));
00129   }
00130 
00131   plan_execution::ExecutableMotionPlan plan;
00132   context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt);
00133 
00134   convertToMsg(plan.plan_components_, action_res.trajectory_start, action_res.planned_trajectory);
00135   if (plan.executed_trajectory_)
00136     plan.executed_trajectory_->getRobotTrajectoryMsg(action_res.executed_trajectory);
00137   action_res.error_code = plan.error_code_;
00138 }
00139 
00140 void move_group::MoveGroupMoveAction::executeMoveCallback_PlanOnly(const moveit_msgs::MoveGroupGoalConstPtr& goal, moveit_msgs::MoveGroupResult &action_res)
00141 {
00142   ROS_INFO("Planning request received for MoveGroup action. Forwarding to planning pipeline.");
00143 
00144   planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_); // lock the scene so that it does not modify the world representation while diff() is called
00145   const planning_scene::PlanningSceneConstPtr &the_scene = (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff)) ?
00146     static_cast<const planning_scene::PlanningSceneConstPtr&>(lscene) : lscene->diff(goal->planning_options.planning_scene_diff);
00147   planning_interface::MotionPlanResponse res;
00148   try
00149   {
00150     context_->planning_pipeline_->generatePlan(the_scene, goal->request, res);
00151   }
00152   catch(std::runtime_error &ex)
00153   {
00154     ROS_ERROR("Planning pipeline threw an exception: %s", ex.what());
00155     res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
00156   }
00157   catch(...)
00158   {
00159     ROS_ERROR("Planning pipeline threw an exception");
00160     res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
00161   }
00162 
00163   convertToMsg(res.trajectory_, action_res.trajectory_start, action_res.planned_trajectory);
00164   action_res.error_code = res.error_code_;
00165   action_res.planning_time = res.planning_time_;
00166 }
00167 
00168 bool move_group::MoveGroupMoveAction::planUsingPlanningPipeline(const planning_interface::MotionPlanRequest &req, plan_execution::ExecutableMotionPlan &plan)
00169 {
00170   setMoveState(PLANNING);
00171 
00172   planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_);
00173   bool solved = false;
00174   planning_interface::MotionPlanResponse res;
00175   try
00176   {
00177     solved = context_->planning_pipeline_->generatePlan(plan.planning_scene_, req, res);
00178   }
00179   catch(std::runtime_error &ex)
00180   {
00181     ROS_ERROR("Planning pipeline threw an exception: %s", ex.what());
00182     res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
00183   }
00184   catch(...)
00185   {
00186     ROS_ERROR("Planning pipeline threw an exception");
00187     res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
00188   }
00189   if (res.trajectory_)
00190   {
00191     plan.plan_components_.resize(1);
00192     plan.plan_components_[0].trajectory_ = res.trajectory_;
00193     plan.plan_components_[0].description_ = "plan";
00194   }
00195   plan.error_code_ = res.error_code_;
00196   return solved;
00197 }
00198 
00199 void move_group::MoveGroupMoveAction::startMoveExecutionCallback()
00200 {
00201   setMoveState(MONITOR);
00202 }
00203 
00204 void move_group::MoveGroupMoveAction::startMoveLookCallback()
00205 {
00206   setMoveState(LOOK);
00207 }
00208 
00209 void move_group::MoveGroupMoveAction::preemptMoveCallback()
00210 {
00211   context_->plan_execution_->stop();
00212 }
00213 
00214 void move_group::MoveGroupMoveAction::setMoveState(MoveGroupState state)
00215 {
00216   move_state_ = state;
00217   move_feedback_.state = stateToStr(state);
00218   move_action_server_->publishFeedback(move_feedback_);
00219 }
00220 
00221 #include <class_loader/class_loader.h>
00222 CLASS_LOADER_REGISTER_CLASS(move_group::MoveGroupMoveAction, move_group::MoveGroupCapability)


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Aug 26 2015 12:43:56