R2RosTrajectoryManager.hpp
Go to the documentation of this file.
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 };


r2_controllers_ros
Author(s): Allison Thackston
autogenerated on Mon Oct 6 2014 02:46:50