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() : MoveGroupCapability("MoveAction"), move_state_(IDLE)
00047 {
00048 }
00049 
00050 void move_group::MoveGroupMoveAction::initialize()
00051 {
00052   // start the move action server
00053   move_action_server_.reset(new actionlib::SimpleActionServer<moveit_msgs::MoveGroupAction>(
00054       root_node_handle_, MOVE_ACTION, boost::bind(&MoveGroupMoveAction::executeMoveCallback, this, _1), false));
00055   move_action_server_->registerPreemptCallback(boost::bind(&MoveGroupMoveAction::preemptMoveCallback, this));
00056   move_action_server_->start();
00057 }
00058 
00059 void move_group::MoveGroupMoveAction::executeMoveCallback(const moveit_msgs::MoveGroupGoalConstPtr& goal)
00060 {
00061   setMoveState(PLANNING);
00062   context_->planning_scene_monitor_->updateFrameTransforms();
00063 
00064   moveit_msgs::MoveGroupResult action_res;
00065   if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_)
00066   {
00067     if (!goal->planning_options.plan_only)
00068       ROS_WARN("This instance of MoveGroup is not allowed to execute trajectories but the goal request has plan_only "
00069                "set to false. Only a motion plan will be computed anyway.");
00070     executeMoveCallback_PlanOnly(goal, action_res);
00071   }
00072   else
00073     executeMoveCallback_PlanAndExecute(goal, action_res);
00074 
00075   bool planned_trajectory_empty = trajectory_processing::isTrajectoryEmpty(action_res.planned_trajectory);
00076   std::string response =
00077       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,
00092                                                                          moveit_msgs::MoveGroupResult& action_res)
00093 {
00094   ROS_INFO("Combined planning and execution request received for MoveGroup action. Forwarding to planning and "
00095            "execution pipeline.");
00096 
00097   if (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff))
00098   {
00099     planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
00100     const robot_state::RobotState& current_state = lscene->getCurrentState();
00101 
00102     // check to see if the desired constraints are already met
00103     for (std::size_t i = 0; i < goal->request.goal_constraints.size(); ++i)
00104       if (lscene->isStateConstrained(current_state,
00105                                      kinematic_constraints::mergeConstraints(goal->request.goal_constraints[i],
00106                                                                              goal->request.path_constraints)))
00107       {
00108         ROS_INFO("Goal constraints are already satisfied. No need to plan or execute any motions");
00109         action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00110         return;
00111       }
00112   }
00113 
00114   plan_execution::PlanExecution::Options opt;
00115 
00116   const moveit_msgs::MotionPlanRequest& motion_plan_request =
00117       planning_scene::PlanningScene::isEmpty(goal->request.start_state) ? goal->request :
00118                                                                           clearRequestStartState(goal->request);
00119   const moveit_msgs::PlanningScene& planning_scene_diff =
00120       planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff.robot_state) ?
00121           goal->planning_options.planning_scene_diff :
00122           clearSceneRobotState(goal->planning_options.planning_scene_diff);
00123 
00124   opt.replan_ = goal->planning_options.replan;
00125   opt.replan_attempts_ = goal->planning_options.replan_attempts;
00126   opt.replan_delay_ = goal->planning_options.replan_delay;
00127   opt.before_execution_callback_ = boost::bind(&MoveGroupMoveAction::startMoveExecutionCallback, this);
00128 
00129   opt.plan_callback_ =
00130       boost::bind(&MoveGroupMoveAction::planUsingPlanningPipeline, this, boost::cref(motion_plan_request), _1);
00131   if (goal->planning_options.look_around && context_->plan_with_sensing_)
00132   {
00133     opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
00134                                      _1, opt.plan_callback_, goal->planning_options.look_around_attempts,
00135                                      goal->planning_options.max_safe_execution_cost);
00136     context_->plan_with_sensing_->setBeforeLookCallback(boost::bind(&MoveGroupMoveAction::startMoveLookCallback, this));
00137   }
00138 
00139   plan_execution::ExecutableMotionPlan plan;
00140   context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt);
00141 
00142   convertToMsg(plan.plan_components_, action_res.trajectory_start, action_res.planned_trajectory);
00143   if (plan.executed_trajectory_)
00144     plan.executed_trajectory_->getRobotTrajectoryMsg(action_res.executed_trajectory);
00145   action_res.error_code = plan.error_code_;
00146 }
00147 
00148 void move_group::MoveGroupMoveAction::executeMoveCallback_PlanOnly(const moveit_msgs::MoveGroupGoalConstPtr& goal,
00149                                                                    moveit_msgs::MoveGroupResult& action_res)
00150 {
00151   ROS_INFO("Planning request received for MoveGroup action. Forwarding to planning pipeline.");
00152 
00153   planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);  // lock the scene so that it
00154                                                                                             // does not modify the world
00155                                                                                             // representation while
00156                                                                                             // diff() is called
00157   const planning_scene::PlanningSceneConstPtr& the_scene =
00158       (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff)) ?
00159           static_cast<const planning_scene::PlanningSceneConstPtr&>(lscene) :
00160           lscene->diff(goal->planning_options.planning_scene_diff);
00161   planning_interface::MotionPlanResponse res;
00162   try
00163   {
00164     context_->planning_pipeline_->generatePlan(the_scene, goal->request, res);
00165   }
00166   catch (std::runtime_error& ex)
00167   {
00168     ROS_ERROR("Planning pipeline threw an exception: %s", ex.what());
00169     res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
00170   }
00171   catch (...)
00172   {
00173     ROS_ERROR("Planning pipeline threw an exception");
00174     res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
00175   }
00176 
00177   convertToMsg(res.trajectory_, action_res.trajectory_start, action_res.planned_trajectory);
00178   action_res.error_code = res.error_code_;
00179   action_res.planning_time = res.planning_time_;
00180 }
00181 
00182 bool move_group::MoveGroupMoveAction::planUsingPlanningPipeline(const planning_interface::MotionPlanRequest& req,
00183                                                                 plan_execution::ExecutableMotionPlan& plan)
00184 {
00185   setMoveState(PLANNING);
00186 
00187   planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_);
00188   bool solved = false;
00189   planning_interface::MotionPlanResponse res;
00190   try
00191   {
00192     solved = context_->planning_pipeline_->generatePlan(plan.planning_scene_, req, res);
00193   }
00194   catch (std::runtime_error& ex)
00195   {
00196     ROS_ERROR("Planning pipeline threw an exception: %s", ex.what());
00197     res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
00198   }
00199   catch (...)
00200   {
00201     ROS_ERROR("Planning pipeline threw an exception");
00202     res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
00203   }
00204   if (res.trajectory_)
00205   {
00206     plan.plan_components_.resize(1);
00207     plan.plan_components_[0].trajectory_ = res.trajectory_;
00208     plan.plan_components_[0].description_ = "plan";
00209   }
00210   plan.error_code_ = res.error_code_;
00211   return solved;
00212 }
00213 
00214 void move_group::MoveGroupMoveAction::startMoveExecutionCallback()
00215 {
00216   setMoveState(MONITOR);
00217 }
00218 
00219 void move_group::MoveGroupMoveAction::startMoveLookCallback()
00220 {
00221   setMoveState(LOOK);
00222 }
00223 
00224 void move_group::MoveGroupMoveAction::preemptMoveCallback()
00225 {
00226   context_->plan_execution_->stop();
00227 }
00228 
00229 void move_group::MoveGroupMoveAction::setMoveState(MoveGroupState state)
00230 {
00231   move_state_ = state;
00232   move_feedback_.state = stateToStr(state);
00233   move_action_server_->publishFeedback(move_feedback_);
00234 }
00235 
00236 #include <class_loader/class_loader.h>
00237 CLASS_LOADER_REGISTER_CLASS(move_group::MoveGroupMoveAction, move_group::MoveGroupCapability)


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:21:44