00001 #ifndef _ROTUNIT_H_ 00002 #define _ROTUNIT_H_ 00003 00004 00005 #include <net/if.h> 00006 #include <sys/ioctl.h> 00007 #include <linux/can.h> 00008 #include <ros/ros.h> 00009 00010 #include <sensor_msgs/JointState.h> 00011 #include <geometry_msgs/Twist.h> 00012 #include <geometry_msgs/TwistStamped.h> 00013 #include <uos_rotunit_driver/RotVelSrv.h> 00014 00015 #include <cmath> 00016 #include <cstdio> 00017 00018 #include "can.h" 00019 00020 //CAN IDs 00021 #define CAN_GETROTUNIT 0x00000010 // current rotunit angle 00022 #define CAN_SETROTUNT 0x00000080 // send rotunit speed 00023 00024 class Rotunit 00025 { 00026 public: 00027 Rotunit(ros::NodeHandle &nh); 00028 ~Rotunit(); 00029 void can_rotunit_send(double speed); 00030 void can_rotunit(const can_frame &frame); 00031 int can_read_fifo(); 00032 void rotunitCallback( 00033 const geometry_msgs::Twist::ConstPtr& msg); 00034 bool rotunitRotVelSrv( 00035 uos_rotunit_driver::RotVelSrv::Request &req, 00036 uos_rotunit_driver::RotVelSrv::Response &res); 00037 00038 private: 00039 sensor_msgs::JointState previous_state; 00040 ros::ServiceServer rot_vel_srv_; 00041 double normalize2PI(double angle); 00042 ros::Subscriber sub_; 00043 ros::Publisher rot_pub_; 00044 ros::Publisher vel_pub_; 00045 CAN can_; 00046 ros::NodeHandle nh_; 00047 }; 00048 00049 #endif /* rotunit.h */