$search
00001 00013 #ifndef _SR_JOINT_TRAJECTORY_ACTION_CONTROLLER_H_ 00014 #define _SR_JOINT_TRAJECTORY_ACTION_CONTROLLER_H_ 00015 #include <ros/ros.h> 00016 00017 #include <actionlib/server/simple_action_server.h> 00018 #include <control_msgs/FollowJointTrajectoryAction.h> 00019 00020 00021 00022 namespace shadowrobot 00023 { 00024 class JointTrajectoryActionController 00025 { 00026 typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> JTAS; 00027 public: 00028 JointTrajectoryActionController(); 00029 ~JointTrajectoryActionController(); 00030 00031 private: 00032 ros::NodeHandle nh, nh_tilde; 00033 ros::Subscriber command_sub; 00034 ros::Publisher sr_arm_target_pub; 00035 ros::Publisher sr_hand_target_pub; 00036 ros::Publisher desired_joint_state_pusblisher; 00037 std::vector<std::string> joint_names_; 00038 ros::ServiceClient joint_state_client; 00039 std::map<std::string,double> joint_state_map; 00040 00041 std::vector< ros::Publisher > controller_publishers; 00042 std::map<std::string,std::string> jointControllerMap; 00043 std::map<std::string,unsigned int> jointPubIdxMap; 00044 bool use_sendupdate; 00045 00046 ros::Time last_time_; 00047 boost::shared_ptr<JTAS> action_server; 00048 00049 /* bool queryStateService(pr2_controllers_msgs::QueryTrajectoryState::Request &req, 00050 pr2_controllers_msgs::QueryTrajectoryState::Response &resp); 00051 ros::ServiceServer serve_query_state_;*/ 00052 00053 00054 // coef[0] + coef[1]*t + ... + coef[5]*t^5 00055 struct Spline 00056 { 00057 std::vector<double> coef; 00058 00059 Spline() : coef(6, 0.0) {} 00060 }; 00061 00062 struct Segment 00063 { 00064 double start_time; 00065 double duration; 00066 std::vector<Spline> splines; 00067 }; 00068 typedef std::vector<Segment> SpecifiedTrajectory; 00069 00070 std::vector<double> q, qd, qdd; // Preallocated in init 00071 00072 // Samples, but handling time bounds. When the time is past the end 00073 // of the spline duration, the position is the last valid position, 00074 // and the derivatives are all 0. 00075 static void sampleSplineWithTimeBounds(const std::vector<double>& coefficients, double duration, double time, 00076 double& position, double& velocity, double& acceleration); 00077 00078 00079 void execute_trajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal); 00080 void commandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg); 00081 void updateJointState(); 00082 bool getPosition(std::string joint_name, double &position); 00083 00084 }; 00085 } 00086 00087 /* For the emacs weenies in the crowd. 00088 Local Variables: 00089 c-basic-offset: 2 00090 End: 00091 */ 00092 00093 #endif