00001 #ifndef MOVEIT_CONTROLLER_MULTIDOF_FORWARDINGCONTROLLERMANAGER_H 00002 #define MOVEIT_CONTROLLER_MULTIDOF_FORWARDINGCONTROLLERMANAGER_H 00003 00004 #include <map> 00005 00006 #include <ros/ros.h> 00007 00008 #include <moveit/controller_manager/controller_manager.h> 00009 #include <moveit_controller_multidof/RobotTrajectoryExecutor.h> 00010 00011 namespace moveit_controller_multidof 00012 { 00013 00014 MOVEIT_CLASS_FORWARD(ForwardingControllerHandle); 00015 00022 class ForwardingControllerHandle: public ActionBasedControllerJointsHandle 00023 { 00024 public: 00025 00026 ForwardingControllerHandle(const std::string &joint_action_topic, 00027 const std::string& path_action_topic, 00028 const std::string& virtual_joint_name) : 00029 ActionBasedControllerJointsHandle("no-name"), 00030 trajectory_executor(virtual_joint_name, joint_action_topic, path_action_topic), 00031 last_exec(moveit_controller_manager::ExecutionStatus::SUCCEEDED) 00032 { 00033 } 00034 00035 00036 virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory &t) { 00037 return trajectory_executor.sendTrajectory(t); 00038 } 00039 00040 virtual bool cancelExecution() { 00041 return trajectory_executor.cancelExecution(); 00042 } 00043 00044 virtual bool waitForExecution(const ros::Duration & timeout) 00045 { 00046 return trajectory_executor.waitForExecution(timeout); 00047 } 00048 00049 virtual moveit_controller_manager::ExecutionStatus getLastExecutionStatus() 00050 { 00051 moveit_controller_multidof::RobotTrajectoryExecutor::ExecutionStatus stat=trajectory_executor.getLastExecutionStatus(); 00052 moveit_controller_manager::ExecutionStatus exec=moveit_controller_manager::ExecutionStatus::UNKNOWN; 00053 if (stat==moveit_controller_multidof::RobotTrajectoryExecutor::RUNNING) exec=moveit_controller_manager::ExecutionStatus::RUNNING; 00054 else if (stat==moveit_controller_multidof::RobotTrajectoryExecutor::SUCCEEDED) exec=moveit_controller_manager::ExecutionStatus::SUCCEEDED; 00055 else if (stat==moveit_controller_multidof::RobotTrajectoryExecutor::PREEMPTED) exec=moveit_controller_manager::ExecutionStatus::PREEMPTED; 00056 else if (stat==moveit_controller_multidof::RobotTrajectoryExecutor::TIMED_OUT) exec=moveit_controller_manager::ExecutionStatus::TIMED_OUT; 00057 else if (stat==moveit_controller_multidof::RobotTrajectoryExecutor::ABORTED) exec=moveit_controller_manager::ExecutionStatus::ABORTED; 00058 else if (stat==moveit_controller_multidof::RobotTrajectoryExecutor::FAILED) exec=moveit_controller_manager::ExecutionStatus::FAILED; 00059 return exec; 00060 } 00061 private: 00062 00063 RobotTrajectoryExecutor trajectory_executor; 00064 moveit_controller_manager::ExecutionStatus last_exec; 00065 00066 }; 00067 00068 00069 00070 } // end namespace 00071 00072 00073 #endif // MOVEIT_CONTROLLER_MULTIDOF_FORWARDINGCONTROLLERMANAGER_H