00001 00008 #ifndef JOINT_TRAJECTORY_FOLLOWER_H 00009 #define JOINT_TRAJECTORY_FOLLOWER_H 00010 00011 #include <vector> 00012 #include <trajectory_msgs/JointTrajectory.h> 00013 #include <sensor_msgs/JointState.h> 00014 #include "robodyn_controllers/GoalManager.h" 00015 #include "robodyn_controllers/RosMsgTrajectory.h" 00016 00021 class JointTrajectoryFollower 00022 { 00023 public: 00024 JointTrajectoryFollower(); 00025 virtual ~JointTrajectoryFollower() {} 00026 00027 virtual const std::vector<std::string>& getJointNames() const = 0; 00028 00029 virtual double getDuration() const = 0; 00030 00036 inline virtual bool isActive() const {return goalManager.isReady();} 00037 00041 virtual void abort(const std::string& msg = std::string()); 00045 virtual void preempt(const std::string& msg = std::string()); 00046 00053 virtual void getNextPoint(sensor_msgs::JointState& nextPoint, ros::Duration& timeFromStart) = 0; 00054 00055 GoalManager goalManager; 00056 double velocityFactor; 00057 }; 00058 00063 class PreplannedJointTrajectoryFollower : public JointTrajectoryFollower 00064 { 00065 public: 00066 PreplannedJointTrajectoryFollower(); 00067 virtual ~PreplannedJointTrajectoryFollower() {} 00068 00069 inline virtual const std::vector<std::string>& getJointNames() const {return trajectory.joint_names;} 00070 00071 inline virtual double getDuration() const {return trajectory.points.back().time_from_start.toSec();} 00072 00079 void setTrajectory(const trajectory_msgs::JointTrajectory& trajectory_in, double timestep_in = 0.03333, double velocityFactor_in = 1.); 00080 00087 virtual void getNextPoint(sensor_msgs::JointState& nextPoint, ros::Duration& timeFromStart); 00088 00089 private: 00090 trajectory_msgs::JointTrajectory trajectory; 00091 trajectory_msgs::JointTrajectory::_points_type::const_iterator trajIt; 00092 double currentTrajCount; 00093 double timestep; 00094 }; 00095 00100 class OnlineJointTrajectoryFollower : public JointTrajectoryFollower 00101 { 00102 public: 00103 OnlineJointTrajectoryFollower(); 00104 virtual ~OnlineJointTrajectoryFollower() {} 00105 00106 inline virtual const std::vector<std::string>& getJointNames() const {return trajectory->getJointNames();} 00107 00108 inline virtual double getDuration() const {return trajectory->getDuration();} 00109 00116 void setTrajectory(boost::shared_ptr<RosMsgJointTrajectory> trajectory_in, double timestep_in, double velocityFactor_in = 1.); 00117 00124 virtual void getNextPoint(sensor_msgs::JointState& nextPoint, ros::Duration& timeFromStart); 00125 00126 private: 00127 boost::shared_ptr<RosMsgJointTrajectory> trajectory; 00128 double time, timestep; 00129 }; 00130 00131 #endif