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() :
00047 MoveGroupCapability("MoveAction"),
00048 move_state_(IDLE)
00049 {
00050 }
00051
00052 void move_group::MoveGroupMoveAction::initialize()
00053 {
00054
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 ¤t_state = lscene->getCurrentState();
00099
00100
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_);
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)