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 <moveit/move_group/move_group_capability.h>
00038 #include <moveit/robot_state/conversions.h>
00039
00040 void move_group::MoveGroupCapability::setContext(const MoveGroupContextPtr &context)
00041 {
00042 context_ = context;
00043 }
00044
00045 void move_group::MoveGroupCapability::convertToMsg(const std::vector<plan_execution::ExecutableTrajectory> &trajectory,
00046 moveit_msgs::RobotState &first_state_msg, std::vector<moveit_msgs::RobotTrajectory> &trajectory_msg) const
00047 {
00048 if (!trajectory.empty())
00049 {
00050 bool first = true;
00051 trajectory_msg.resize(trajectory.size());
00052 for (std::size_t i = 0 ; i < trajectory.size() ; ++i)
00053 {
00054 if (trajectory[i].trajectory_)
00055 {
00056 if (first && !trajectory[i].trajectory_->empty())
00057 {
00058 robot_state::robotStateToRobotStateMsg(trajectory[i].trajectory_->getFirstWayPoint(), first_state_msg);
00059 first = false;
00060 }
00061 trajectory[i].trajectory_->getRobotTrajectoryMsg(trajectory_msg[i]);
00062 }
00063 }
00064 }
00065 }
00066
00067 void move_group::MoveGroupCapability::convertToMsg(const robot_trajectory::RobotTrajectoryPtr &trajectory,
00068 moveit_msgs::RobotState &first_state_msg, moveit_msgs::RobotTrajectory &trajectory_msg) const
00069 {
00070 if (trajectory && !trajectory->empty())
00071 {
00072 robot_state::robotStateToRobotStateMsg(trajectory->getFirstWayPoint(), first_state_msg);
00073 trajectory->getRobotTrajectoryMsg(trajectory_msg);
00074 }
00075 }
00076
00077 void move_group::MoveGroupCapability::convertToMsg(const std::vector<plan_execution::ExecutableTrajectory> &trajectory,
00078 moveit_msgs::RobotState &first_state_msg, moveit_msgs::RobotTrajectory &trajectory_msg) const
00079 {
00080 if (trajectory.size() > 1)
00081 ROS_ERROR_STREAM("Internal logic error: trajectory component ignored. !!! THIS IS A SERIOUS ERROR !!!");
00082 if (trajectory.size() > 0)
00083 convertToMsg(trajectory[0].trajectory_, first_state_msg,trajectory_msg);
00084 }
00085
00086 planning_interface::MotionPlanRequest move_group::MoveGroupCapability::clearRequestStartState(const planning_interface::MotionPlanRequest &request) const
00087 {
00088 planning_interface::MotionPlanRequest r = request;
00089 r.start_state = moveit_msgs::RobotState();
00090 r.start_state.is_diff = true;
00091 ROS_WARN("Execution of motions should always start at the robot's current state. Ignoring the state supplied as start state in the motion planning request");
00092 return r;
00093 }
00094
00095 moveit_msgs::PlanningScene move_group::MoveGroupCapability::clearSceneRobotState(const moveit_msgs::PlanningScene &scene) const
00096 {
00097 moveit_msgs::PlanningScene r = scene;
00098 r.robot_state = moveit_msgs::RobotState();
00099 r.robot_state.is_diff = true;
00100 ROS_WARN("Execution of motions should always start at the robot's current state. Ignoring the state supplied as difference in the planning scene diff");
00101 return r;
00102 }
00103
00104 std::string move_group::MoveGroupCapability::getActionResultString(const moveit_msgs::MoveItErrorCodes &error_code, bool planned_trajectory_empty, bool plan_only)
00105 {
00106 if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00107 {
00108 if (planned_trajectory_empty)
00109 return "Requested path and goal constraints are already met.";
00110 else
00111 {
00112 if (plan_only)
00113 return "Motion plan was computed succesfully.";
00114 else
00115 return "Solution was found and executed.";
00116 }
00117 }
00118 else
00119 if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME)
00120 return "Must specify group in motion plan request";
00121 else
00122 if (error_code.val == moveit_msgs::MoveItErrorCodes::PLANNING_FAILED || error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN)
00123 {
00124 if (planned_trajectory_empty)
00125 return "No motion plan found. No execution attempted.";
00126 else
00127 return "Motion plan was found but it seems to be invalid (possibly due to postprocessing). Not executing.";
00128 }
00129 else
00130 if (error_code.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA)
00131 return "Motion plan was found but it seems to be too costly and looking around did not help.";
00132 else
00133 if (error_code.val == moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE)
00134 return "Solution found but the environment changed during execution and the path was aborted";
00135 else
00136 if (error_code.val == moveit_msgs::MoveItErrorCodes::CONTROL_FAILED)
00137 return "Solution found but controller failed during execution";
00138 else
00139 if (error_code.val == moveit_msgs::MoveItErrorCodes::TIMED_OUT)
00140 return "Timeout reached";
00141 else
00142 if (error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
00143 return "Preempted";
00144 else
00145 if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS)
00146 return "Invalid goal constraints";
00147 else
00148 if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME)
00149 return "Invalid object name";
00150 else
00151 if (error_code.val == moveit_msgs::MoveItErrorCodes::FAILURE)
00152 return "Catastrophic failure";
00153 return "Unknown event";
00154 }
00155
00156 std::string move_group::MoveGroupCapability::stateToStr(MoveGroupState state) const
00157 {
00158 switch (state)
00159 {
00160 case IDLE:
00161 return "IDLE";
00162 case PLANNING:
00163 return "PLANNING";
00164 case MONITOR:
00165 return "MONITOR";
00166 case LOOK:
00167 return "LOOK";
00168 default:
00169 return "UNKNOWN";
00170 }
00171 }
00172
00173 bool move_group::MoveGroupCapability::performTransform(geometry_msgs::PoseStamped &pose_msg, const std::string &target_frame) const
00174 {
00175 if (!context_ || !context_->planning_scene_monitor_->getTFClient())
00176 return false;
00177 if (pose_msg.header.frame_id == target_frame)
00178 return true;
00179 if (pose_msg.header.frame_id.empty())
00180 {
00181 pose_msg.header.frame_id = target_frame;
00182 return true;
00183 }
00184
00185 try
00186 {
00187 std::string error;
00188 ros::Time common_time;
00189 context_->planning_scene_monitor_->getTFClient()->getLatestCommonTime(pose_msg.header.frame_id, target_frame, common_time, &error);
00190 if (!error.empty())
00191 ROS_ERROR("TF Problem: %s", error.c_str());
00192
00193 tf::Stamped<tf::Pose> pose_tf, pose_tf_out;
00194 tf::poseStampedMsgToTF(pose_msg, pose_tf);
00195 pose_tf.stamp_ = common_time;
00196 context_->planning_scene_monitor_->getTFClient()->transformPose(target_frame, pose_tf, pose_tf_out);
00197 tf::poseStampedTFToMsg(pose_tf_out, pose_msg);
00198 }
00199 catch (tf::TransformException &ex)
00200 {
00201 ROS_ERROR("TF Problem: %s", ex.what());
00202 return false;
00203 }
00204 return true;
00205 }