Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
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_);
00154
00155
00156
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)