37 #ifndef MOVEIT_MOVE_GROUP_CAPABILITY_ 38 #define MOVEIT_MOVE_GROUP_CAPABILITY_ 69 void setContext(
const MoveGroupContextPtr& context);
79 std::string
getActionResultString(
const moveit_msgs::MoveItErrorCodes& error_code,
bool planned_trajectory_empty,
84 moveit_msgs::RobotState& first_state_msg,
85 std::vector<moveit_msgs::RobotTrajectory>& trajectory_msg)
const;
86 void convertToMsg(
const robot_trajectory::RobotTrajectoryPtr& trajectory, moveit_msgs::RobotState& first_state_msg,
87 moveit_msgs::RobotTrajectory& trajectory_msg)
const;
88 void convertToMsg(
const std::vector<plan_execution::ExecutableTrajectory>& trajectory,
89 moveit_msgs::RobotState& first_state_msg, moveit_msgs::RobotTrajectory& trajectory_msg)
const;
94 bool performTransform(geometry_msgs::PoseStamped& pose_msg,
const std::string& target_frame)
const;
virtual ~MoveGroupCapability()
std::string stateToStr(MoveGroupState state) const
ros::NodeHandle node_handle_
MoveGroupContextPtr context_
planning_interface::MotionPlanRequest clearRequestStartState(const planning_interface::MotionPlanRequest &request) const
std::string capability_name_
robot_trajectory::RobotTrajectory trajectory(rmodel,"right_arm")
std::string getActionResultString(const moveit_msgs::MoveItErrorCodes &error_code, bool planned_trajectory_empty, bool plan_only)
moveit_msgs::MotionPlanRequest MotionPlanRequest
void setContext(const MoveGroupContextPtr &context)
MoveGroupCapability(const std::string &capability_name)
bool performTransform(geometry_msgs::PoseStamped &pose_msg, const std::string &target_frame) const
void convertToMsg(const std::vector< plan_execution::ExecutableTrajectory > &trajectory, moveit_msgs::RobotState &first_state_msg, std::vector< moveit_msgs::RobotTrajectory > &trajectory_msg) const
virtual void initialize()=0
MOVEIT_CLASS_FORWARD(MoveGroupCapability)
ros::NodeHandle root_node_handle_
const std::string & getName() const
moveit_msgs::PlanningScene clearSceneRobotState(const moveit_msgs::PlanningScene &scene) const