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


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