00001 00008 #ifndef ROS_MSG_TRAJECTORY_H 00009 #define ROS_MSG_TRAJECTORY_H 00010 00011 #include <robodyn_controllers/Trajectory.h> 00012 #include <actionlib_msgs/GoalID.h> 00013 #include <trajectory_msgs/JointTrajectoryPoint.h> 00014 #include <r2_msgs/PoseTrajectoryPoint.h> 00015 #include <r2_msgs/PriorityArray.h> 00016 00017 template <class P, class outType> 00018 class RosMsgTrajectory : public Trajectory<outType> 00019 { 00020 public: 00021 RosMsgTrajectory() {} 00022 virtual ~RosMsgTrajectory() {} 00023 00024 void setTrajectory(boost::shared_ptr<Trajectory<P> > traj_in) 00025 { 00026 Trajectory<outType>::setDuration(traj_in->getDuration()); 00027 trajectory = traj_in; 00028 } 00029 00030 inline const actionlib_msgs::GoalID& getGoalId() {return goalId;} 00031 00032 void setGoalId(const actionlib_msgs::GoalID& goalId_in) {goalId = goalId_in;} 00033 void setGoalId(const ros::Time& stamp, const std::string& id) 00034 { 00035 goalId.stamp = stamp; 00036 goalId.id = id; 00037 } 00038 00039 protected: 00040 boost::shared_ptr<Trajectory<P> > trajectory; 00041 00042 private: 00043 actionlib_msgs::GoalID goalId; 00044 }; 00045 00050 class RosMsgJointTrajectory : public RosMsgTrajectory<JointTrajectory::poseType, trajectory_msgs::JointTrajectoryPoint> 00051 { 00052 public: 00053 RosMsgJointTrajectory() {} 00054 virtual ~RosMsgJointTrajectory() {} 00055 00056 void setJointNames(const std::vector<std::string>& jointNames_in) {jointNames = jointNames_in;} 00057 00058 inline const std::vector<std::string>& getJointNames() {return jointNames;} 00059 00060 void getPose(double time, trajectory_msgs::JointTrajectoryPoint& pose); 00061 00062 private: 00063 std::vector<std::string> jointNames; 00064 }; 00065 00070 class RosMsgSynchedCartesianTrajectory : public RosMsgTrajectory<SynchedCartesianTrajectory::poseType, r2_msgs::PoseTrajectoryPoint> 00071 { 00072 public: 00073 RosMsgSynchedCartesianTrajectory() {} 00074 virtual ~RosMsgSynchedCartesianTrajectory() {} 00075 00076 void setNodeNames(const std::vector<std::string>& nodeNames_in) {nodeNames = nodeNames_in;} 00077 void setRefFrames(const std::vector<std::string>& refFrames_in) {refFrames = refFrames_in;} 00078 void setPriorities(const std::vector<r2_msgs::PriorityArray>& priorities_in) {priorities = priorities_in;} 00079 00080 inline const std::vector<std::string>& getNodeNames() {return nodeNames;} 00081 inline const std::vector<std::string>& getRefFrames() {return refFrames;} 00082 inline const std::vector<r2_msgs::PriorityArray>& getPriorities() {return priorities;} 00083 00084 virtual void getPose(double time, r2_msgs::PoseTrajectoryPoint& stepPose); 00085 00086 private: 00087 std::vector<std::string> nodeNames; 00088 std::vector<std::string> refFrames; 00089 std::vector<r2_msgs::PriorityArray> priorities; 00090 }; 00091 00092 #endif