00001 #include <ros/ros.h> 00002 #include <queue> 00003 00004 #include "robodyn_controllers/TrajectoryManager.h" 00005 #include "robodyn_controllers/RosMsgTreeFk.h" 00006 00007 #include "sensor_msgs/JointState.h" 00008 #include "r2_msgs/JointCapability.h" 00009 #include "r2_msgs/JointCommand.h" 00010 #include "r2_msgs/JointControlDataArray.h" 00011 #include "r2_msgs/JointTrajectoryReplan.h" 00012 #include "r2_msgs/PoseTrajectoryReplan.h" 00013 #include "r2_msgs/ForceControlAxisArray.h" 00014 #include "r2_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 r2_msgs::ControllerJointSettings& msg); 00025 void jointCommandCallback(const r2_msgs::JointCommand& msg); 00026 void jointStatesCallback(const sensor_msgs::JointState& msg); 00027 void jointCapabilitiesCallback(const r2_msgs::JointCapability& msg); 00028 void jointRefsCallback(const r2_msgs::JointTrajectoryReplan& msg); 00029 void jointTrajCallback(const trajectory_msgs::JointTrajectory& msg); 00030 00031 void basesCallback(const r2_msgs::StringArray& msg); 00032 void inertiaCallback(const sensor_msgs::JointState& msg); 00033 void poseSettingsCallback(const r2_msgs::ControllerPoseSettings& msg); 00034 void poseStatesCallback(const r2_msgs::PoseState& msg); 00035 void poseCommandCallback(const r2_msgs::PoseState& msg); 00036 void poseRefsCallback(const r2_msgs::PoseTrajectoryReplan& msg); 00037 void poseWaitCallback(const ros::TimerEvent& event); 00038 00039 void update(); 00040 sensor_msgs::JointState fromJointCommandToJointState(const r2_msgs::JointCommand& jointCommandMsg); 00041 r2_msgs::JointControlDataArray fromJointCommandToJointControl(const r2_msgs::JointCommand& jointCommandMsg); 00042 00043 protected: 00044 virtual void writeStatus(const actionlib_msgs::GoalStatusArray& goalStatusMsg_out); 00045 00046 virtual void writeJointState(const sensor_msgs::JointState& jointState_out); 00047 virtual void writeJointControl(const r2_msgs::JointControlDataArray& jointControl_out); 00048 00049 private: 00050 ros::NodeHandle nh; 00052 ros::Publisher jointCommandOut; 00053 ros::Publisher poseCommandOut; 00054 ros::Publisher poseStatesOut; 00055 ros::Publisher goalStatusOut; 00056 ros::Publisher poseRefsOut; 00057 ros::Publisher jointSettingsOut; 00058 00059 ros::Subscriber jointSettingsIn; 00060 ros::Subscriber jointCommandRefsIn; 00061 ros::Subscriber jointStatesIn; 00062 ros::Subscriber jointCapabilitiesIn; 00063 ros::Subscriber jointRefsIn; 00064 ros::Subscriber jointTrajIn; 00065 00066 ros::Subscriber basesIn; 00067 ros::Subscriber inertiaIn; 00068 ros::Subscriber poseSettingsIn; 00069 ros::Subscriber poseStatesIn; 00070 ros::Subscriber poseCommandRefIn; 00071 ros::Subscriber poseRefsIn; 00072 00074 r2_msgs::ControllerJointSettings jointSettingsMsg; 00075 r2_msgs::JointTrajectoryReplan jointRefsMsg; 00076 sensor_msgs::JointState prevJointStatesMsg; 00077 sensor_msgs::JointState jointStatesMsg; 00078 r2_msgs::JointCommand jointCommandsMsg; 00079 r2_msgs::JointCommand prevJointCommandsMsg; 00080 r2_msgs::JointControlDataArray jointStatusMsg; 00081 trajectory_msgs::JointTrajectory trajInMsg; 00082 sensor_msgs::JointState jointCommandOutMsg; 00083 00084 r2_msgs::PoseState poseCommandsMsg; 00085 r2_msgs::PoseTrajectoryReplan poseRefsMsg; 00086 std::queue<r2_msgs::PoseTrajectoryReplan> poseRefsQueue; 00087 r2_msgs::PoseState poseStatesOutMsg; 00088 r2_msgs::PoseState poseCommandOutMsg; 00089 00090 std::string urdfFile; 00091 double timeStepProp; 00092 double waitForKasquish; 00093 00094 r2_msgs::ControllerJointSettings replanJointSettings; 00095 double replanHoldVelocityLimit; 00096 double replanHoldAccelerationLimit; 00097 double replanStopVelocityLimit; 00098 double replanStopAccelerationLimit; 00099 double replanSettingsVelocityCutoff; 00100 00101 RosMsgTreeFk treeFk; 00102 00103 };