00001 00034 #ifndef JACKAL_BASE_JACKAL_HARDWARE_H 00035 #define JACKAL_BASE_JACKAL_HARDWARE_H 00036 00037 #include "boost/thread.hpp" 00038 #include "hardware_interface/joint_state_interface.h" 00039 #include "hardware_interface/joint_command_interface.h" 00040 #include "hardware_interface/robot_hw.h" 00041 #include "jackal_msgs/Drive.h" 00042 #include "jackal_msgs/Feedback.h" 00043 #include "realtime_tools/realtime_publisher.h" 00044 #include "ros/ros.h" 00045 #include "sensor_msgs/JointState.h" 00046 00047 00048 namespace jackal_base 00049 { 00050 00051 class JackalHardware : public hardware_interface::RobotHW 00052 { 00053 public: 00054 JackalHardware(); 00055 void copyJointsFromHardware(); 00056 void publishDriveFromController(); 00057 00058 private: 00059 void feedbackCallback(const jackal_msgs::Feedback::ConstPtr& msg); 00060 00061 ros::NodeHandle nh_; 00062 ros::Subscriber feedback_sub_; 00063 realtime_tools::RealtimePublisher<jackal_msgs::Drive> cmd_drive_pub_; 00064 00065 hardware_interface::JointStateInterface joint_state_interface_; 00066 hardware_interface::VelocityJointInterface velocity_joint_interface_; 00067 00068 // These are mutated on the controls thread only. 00069 struct Joint 00070 { 00071 double position; 00072 double velocity; 00073 double effort; 00074 double velocity_command; 00075 00076 Joint() : position(0), velocity(0), effort(0), velocity_command(0) 00077 { 00078 } 00079 } 00080 joints_[4]; 00081 00082 // This pointer is set from the ROS thread. 00083 jackal_msgs::Feedback::ConstPtr feedback_msg_; 00084 boost::mutex feedback_msg_mutex_; 00085 }; 00086 00087 } // namespace jackal_base 00088 00089 #endif // JACKAL_BASE_JACKAL_HARDWARE_H