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 #ifndef MOVEIT_MOVE_GROUP_CAPABILITY_
00038 #define MOVEIT_MOVE_GROUP_CAPABILITY_
00039
00040 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00041 #include <moveit/planning_interface/planning_interface.h>
00042 #include <moveit/plan_execution/plan_representation.h>
00043 #include <moveit/move_group/move_group_context.h>
00044
00045 namespace move_group
00046 {
00047
00048 enum MoveGroupState
00049 {
00050 IDLE,
00051 PLANNING,
00052 MONITOR,
00053 LOOK
00054 };
00055
00056 class MoveGroupCapability
00057 {
00058 public:
00059
00060 MoveGroupCapability(const std::string &capability_name) :
00061 node_handle_("~"),
00062 capability_name_(capability_name)
00063 {
00064 }
00065
00066 virtual ~MoveGroupCapability()
00067 {
00068 }
00069
00070 void setContext(const MoveGroupContextPtr &context);
00071
00072 virtual void initialize() = 0;
00073
00074 const std::string& getName() const
00075 {
00076 return capability_name_;
00077 }
00078
00079 protected:
00080
00081 std::string getActionResultString(const moveit_msgs::MoveItErrorCodes &error_code, bool planned_trajectory_empty, bool plan_only);
00082 std::string stateToStr(MoveGroupState state) const;
00083
00084 void convertToMsg(const std::vector<plan_execution::ExecutableTrajectory> &trajectory,
00085 moveit_msgs::RobotState &first_state_msg, std::vector<moveit_msgs::RobotTrajectory> &trajectory_msg) const;
00086 void convertToMsg(const robot_trajectory::RobotTrajectoryPtr &trajectory,
00087 moveit_msgs::RobotState &first_state_msg, moveit_msgs::RobotTrajectory &trajectory_msg) const;
00088 void convertToMsg(const std::vector<plan_execution::ExecutableTrajectory> &trajectory,
00089 moveit_msgs::RobotState &first_state_msg, moveit_msgs::RobotTrajectory &trajectory_msg) const;
00090
00091 planning_interface::MotionPlanRequest clearRequestStartState(const planning_interface::MotionPlanRequest &request) const;
00092 moveit_msgs::PlanningScene clearSceneRobotState(const moveit_msgs::PlanningScene &scene) const;
00093 bool performTransform(geometry_msgs::PoseStamped &pose_msg, const std::string &target_frame) const;
00094
00095 ros::NodeHandle root_node_handle_;
00096 ros::NodeHandle node_handle_;
00097 std::string capability_name_;
00098 MoveGroupContextPtr context_;
00099 };
00100
00101 }
00102
00103 #endif