00001 00002 #ifndef DYNAMIXEL_HARDWARE_INTERFACE_JOINT_TRAJECTORY_ACTION_CONTROLLER_H 00003 #define DYNAMIXEL_HARDWARE_INTERFACE_JOINT_TRAJECTORY_ACTION_CONTROLLER_H 00004 00005 #include <vector> 00006 #include <string> 00007 00008 #include <boost/scoped_ptr.hpp> 00009 #include <boost/thread.hpp> 00010 00011 #include <dynamixel_hardware_interface/single_joint_controller.h> 00012 #include <dynamixel_hardware_interface/multi_joint_controller.h> 00013 00014 #include <ros/ros.h> 00015 #include <actionlib/server/simple_action_server.h> 00016 #include <control_msgs/FollowJointTrajectoryAction.h> 00017 #include <trajectory_msgs/JointTrajectory.h> 00018 00019 namespace controller 00020 { 00021 00022 struct Segment 00023 { 00024 double start_time; 00025 double duration; 00026 std::vector<double> positions; 00027 std::vector<double> velocities; 00028 }; 00029 00030 class JointTrajectoryActionController : public MultiJointController 00031 { 00032 public: 00033 JointTrajectoryActionController(); 00034 virtual ~JointTrajectoryActionController(); 00035 00036 bool initialize(std::string name, std::vector<SingleJointController*> deps); 00037 00038 void start(); 00039 void stop(); 00040 00041 void processCommand(const trajectory_msgs::JointTrajectoryConstPtr& msg); 00042 void processFollowTrajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal); 00043 void updateState(); 00044 void processTrajectory(const trajectory_msgs::JointTrajectory& traj, bool is_action); 00045 00046 private: 00047 int update_rate_; 00048 int state_update_rate_; 00049 std::vector<Segment> trajectory_; 00050 00051 double goal_time_constraint_; 00052 double stopped_velocity_tolerance_; 00053 double min_velocity_; 00054 std::vector<double> goal_constraints_; 00055 std::vector<double> trajectory_constraints_; 00056 00057 control_msgs::FollowJointTrajectoryFeedback msg_; 00058 00059 ros::Subscriber command_sub_; 00060 ros::Publisher state_pub_; 00061 00062 typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> FJTAS; 00063 boost::scoped_ptr<FJTAS> action_server_; 00064 00065 boost::thread* feedback_thread_; 00066 boost::mutex terminate_mutex_; 00067 bool terminate_; 00068 }; 00069 00070 } 00071 00072 #endif // DYNAMIXEL_HARDWARE_INTERFACE_JOINT_TRAJECTORY_ACTION_CONTROLLER_H