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


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Tue Feb 20 2018 03:45:55