00001 #include <ros/ros.h> 00002 #include <queue> 00003 00004 #include "nasa_robodyn_controllers_core/TrajectoryManager.h" 00005 #include "nasa_robodyn_controllers_core/RosMsgTreeFk.h" 00006 00007 #include "sensor_msgs/JointState.h" 00008 #include "nasa_r2_common_msgs/JointCapability.h" 00009 #include "nasa_r2_common_msgs/JointCommand.h" 00010 #include "nasa_r2_common_msgs/JointControlDataArray.h" 00011 #include "nasa_r2_common_msgs/JointTrajectoryReplan.h" 00012 #include "nasa_r2_common_msgs/PoseTrajectoryReplan.h" 00013 #include "nasa_r2_common_msgs/ForceControlAxisArray.h" 00014 #include "nasa_r2_common_msgs/StringArray.h" 00015 00016 class R2RosTrajectoryManager : public TrajectoryManager 00017 { 00018 public: 00019 R2RosTrajectoryManager(const std::string name, const std::string& urdf, const double& timestep, ros::NodeHandle &nh); 00020 virtual ~R2RosTrajectoryManager() {} 00021 00022 void setCartesianParameters(std::vector<double> priorityNum, std::vector<double> linearTol, std::vector<double> angularTol, double momLimit, double taskGain, int ikMaxItr, double ikMaxTwist); 00023 00024 void jointSettingsCallback(const nasa_r2_common_msgs::ControllerJointSettings& msg); 00025 void jointCommandCallback(const nasa_r2_common_msgs::JointCommand& msg); 00026 void jointStatesCallback(const sensor_msgs::JointState& msg); 00027 void jointCapabilitiesCallback(const nasa_r2_common_msgs::JointCapability& msg); 00028 void jointRefsCallback(const nasa_r2_common_msgs::JointTrajectoryReplan& msg); 00029 void jointTrajCallback(const trajectory_msgs::JointTrajectory& msg); 00030 00031 void basesCallback(const nasa_r2_common_msgs::StringArray& msg); 00032 void inertiaCallback(const sensor_msgs::JointState& msg); 00033 void poseSettingsCallback(const nasa_r2_common_msgs::ControllerPoseSettings& msg); 00034 void poseStatesCallback(const nasa_r2_common_msgs::PoseState& msg); 00035 void poseCommandCallback(const nasa_r2_common_msgs::PoseState& msg); 00036 void poseRefsCallback(const nasa_r2_common_msgs::PoseTrajectoryReplan& msg); 00037 00038 void update(); 00039 sensor_msgs::JointState fromJointCommandToJointState(const nasa_r2_common_msgs::JointCommand& jointCommandMsg); 00040 nasa_r2_common_msgs::JointControlDataArray fromJointCommandToJointControl(const nasa_r2_common_msgs::JointCommand& jointCommandMsg); 00041 00042 protected: 00043 virtual void writeStatus(const actionlib_msgs::GoalStatusArray& goalStatusMsg_out); 00044 00045 virtual void writeJointState(const sensor_msgs::JointState& jointState_out); 00046 virtual void writeJointControl(const nasa_r2_common_msgs::JointControlDataArray& jointControl_out); 00047 00048 private: 00050 ros::Publisher jointCommandOut; 00051 ros::Publisher poseCommandOut; 00052 ros::Publisher poseStatesOut; 00053 ros::Publisher goalStatusOut; 00054 00055 ros::Subscriber jointSettingsIn; 00056 ros::Subscriber jointCommandRefsIn; 00057 ros::Subscriber jointStatesIn; 00058 ros::Subscriber jointCapabilitiesIn; 00059 ros::Subscriber jointRefsIn; 00060 ros::Subscriber jointTrajIn; 00061 00062 ros::Subscriber basesIn; 00063 ros::Subscriber inertiaIn; 00064 ros::Subscriber poseSettingsIn; 00065 ros::Subscriber poseStatesIn; 00066 ros::Subscriber poseCommandRefIn; 00067 ros::Subscriber poseRefsIn; 00068 00070 nasa_r2_common_msgs::ControllerJointSettings jointSettingsMsg; 00071 nasa_r2_common_msgs::JointTrajectoryReplan jointRefsMsg; 00072 sensor_msgs::JointState prevJointStatesMsg; 00073 sensor_msgs::JointState jointStatesMsg; 00074 nasa_r2_common_msgs::JointCommand jointCommandsMsg; 00075 nasa_r2_common_msgs::JointCommand prevJointCommandsMsg; 00076 nasa_r2_common_msgs::JointControlDataArray jointStatusMsg; 00077 trajectory_msgs::JointTrajectory trajInMsg; 00078 sensor_msgs::JointState jointCommandOutMsg; 00079 00080 nasa_r2_common_msgs::PoseState poseCommandsMsg; 00081 nasa_r2_common_msgs::PoseTrajectoryReplan poseRefsMsg; 00082 std::queue<nasa_r2_common_msgs::PoseTrajectoryReplan> poseRefsQueue; 00083 nasa_r2_common_msgs::PoseState poseStatesOutMsg; 00084 nasa_r2_common_msgs::PoseState poseCommandOutMsg; 00085 00086 std::string urdfFile; 00087 double timeStepProp; 00088 double waitForKasquish; 00089 00090 bool newJointCommands; 00091 bool newPoseCommands; 00092 00093 nasa_r2_common_msgs::ControllerJointSettings replanJointSettings; 00094 double replanHoldVelocityLimit; 00095 double replanHoldAccelerationLimit; 00096 double replanStopVelocityLimit; 00097 double replanStopAccelerationLimit; 00098 double replanSettingsVelocityCutoff; 00099 00100 RosMsgTreeFk treeFk; 00101 00102 };