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