00001 00031 #ifndef MOTORHARDWARE_H 00032 #define MOTORHARDWARE_H 00033 00034 #include "hardware_interface/joint_state_interface.h" 00035 #include "hardware_interface/joint_command_interface.h" 00036 #include "hardware_interface/robot_hw.h" 00037 #include "ros/ros.h" 00038 #include "sensor_msgs/JointState.h" 00039 00040 #include <ubiquity_motor/motor_serial.h> 00041 00042 class MotorHardware : public hardware_interface::RobotHW { 00043 public: 00044 MotorHardware(ros::NodeHandle nh); 00045 ~MotorHardware(); 00046 void readInputs(); 00047 void writeSpeeds(); 00048 void requestVersion(); 00049 void requestOdometry(); 00050 void requestVelocity(); 00051 void setPid(int32_t p, int32_t i, int32_t d, int32_t denominator); 00052 void sendPid(); 00053 private: 00054 hardware_interface::JointStateInterface joint_state_interface_; 00055 hardware_interface::VelocityJointInterface velocity_joint_interface_; 00056 00057 int32_t p_value; 00058 int32_t i_value; 00059 int32_t d_value; 00060 int32_t denominator_value; 00061 00062 struct Joint { 00063 double position; 00064 double velocity; 00065 double effort; 00066 double velocity_command; 00067 00068 Joint() : position(0), velocity(0), effort(0), velocity_command(0) { 00069 } 00070 } 00071 joints_[2]; 00072 00073 MotorSerial* motor_serial_; 00074 }; 00075 00076 #endif