Go to the documentation of this file.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
00030 private:
00031 ros::NodeHandle nh, nh_tilde;
00032 ros::Subscriber command_sub;
00033 ros::Publisher sr_arm_target_pub;
00034 ros::Publisher sr_hand_target_pub;
00035 ros::Publisher desired_joint_state_pusblisher;
00036 std::vector<std::string> joint_names_;
00037 ros::ServiceClient joint_state_client;
00038 std::map<std::string,double> joint_state_map;
00039
00040 std::vector< ros::Publisher > controller_publishers;
00041 std::map<std::string,std::string> jointControllerMap;
00042 std::map<std::string,unsigned int> jointPubIdxMap;
00043 std::map<std::string,unsigned int> joint_state_idx_map;
00044 bool use_sendupdate;
00045
00046 ros::Time last_time_;
00047 boost::shared_ptr<JTAS> action_server;
00048
00049
00050
00051
00052
00053
00054
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;
00071
00072
00073
00074
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
00088
00089
00090
00091
00092
00093 #endif
00094
00095