rotunit.h
Go to the documentation of this file.
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 */


uos_rotunit_driver
Author(s):
autogenerated on Fri Aug 28 2015 13:31:03