rotunit.cpp
Go to the documentation of this file.
00001 #include "rotunit.h"
00002 
00003 Rotunit::Rotunit(ros::NodeHandle &nh)
00004   : nh_(nh)
00005 {
00006   rot_pub_ = nh_.advertise<sensor_msgs::JointState> ("rotunit_joint_states", 1);
00007   vel_pub_ = nh_.advertise<geometry_msgs::TwistStamped> ("rotunit_velocity", 1);
00008   sub_ = nh_.subscribe("rot_vel", 10, &Rotunit::rotunitCallback, this);
00009   rot_vel_srv_ = nh_.advertiseService("rot_vel", &Rotunit::rotunitRotVelSrv, this);
00010   double rotunit_speed;
00011   nh_.param("rotunit_speed", rotunit_speed, M_PI/6.0);
00012   can_rotunit_send(rotunit_speed);
00013 }
00014 
00015 Rotunit::~Rotunit()
00016 {
00017   can_rotunit_send(0.0);
00018 }
00019 
00020 void Rotunit::can_rotunit_send(double speed)
00021 {
00022   int ticks =  (int)(speed / (2.0 * M_PI) * 10240 / 20);
00023   can_frame frame;
00024   frame.can_id = CAN_SETROTUNT;
00025   frame.can_dlc = 8;
00026   frame.data[0] = (ticks >> 8); //high
00027   frame.data[1] = (ticks & 0xFF); //low
00028   frame.data[2] = 0;
00029   frame.data[3] = 0;
00030   frame.data[4] = 0;
00031   frame.data[5] = 0;
00032   frame.data[6] = 0;
00033   frame.data[7] = 0;
00034 
00035   if(!can_.send_frame(&frame))
00036     ROS_ERROR("can_rotunit_send: Error sending rotunit speed");
00037 }
00038 
00039 double Rotunit::normalize2PI (double angle)
00040 {
00041   while (angle > 2.0 * M_PI)
00042     angle -= 2.0 * M_PI;
00043   while (angle < 0)
00044     angle += 2.0 * M_PI;
00045   return angle;
00046 }
00047 
00048 
00049 bool Rotunit::rotunitRotVelSrv(
00050   uos_rotunit_driver::RotVelSrv::Request &req,
00051   uos_rotunit_driver::RotVelSrv::Response &res){
00052   can_rotunit_send(req.twist.angular.z);  
00053   return true;
00054 }
00055 
00056 void Rotunit::rotunitCallback(const geometry_msgs::Twist::ConstPtr& msg)
00057 {
00058   can_rotunit_send(msg->angular.z);
00059 }
00060 
00061 void Rotunit::can_rotunit(const can_frame &frame)
00062 {
00063   int rot = (frame.data[1] << 8) + frame.data[2];
00064   double rot2 = rot * 2 * M_PI / 10240;
00065   rot2 = rot2 - 6 * M_PI / 180.0; // 6 degree angle correction
00066   rot2 = normalize2PI(rot2);      // normalize to element of [0 2PI]
00067   
00068   ros::Time now = ros::Time::now();
00069 
00070   sensor_msgs::JointState joint_state;
00071   joint_state.header.stamp = now;
00072   joint_state.name.resize(1);
00073   joint_state.position.resize(1);
00074   joint_state.name[0] = "laser_rot_joint";
00075   joint_state.position[0] = rot2;
00076   
00077   rot_pub_.publish(joint_state);
00078 
00079   geometry_msgs::TwistStamped twist;
00080   if(previous_state.header.stamp.toSec() == 0){
00081     twist.twist.angular.z = 0;
00082   }
00083   else{
00084     twist.twist.angular.z = (joint_state.position[0] - previous_state.position[0])
00085       / (joint_state.header.stamp - previous_state.header.stamp).toSec();   
00086   }
00087   twist.header.stamp = now;
00088   vel_pub_.publish(twist);
00089   previous_state = joint_state;
00090 }
00091 
00092 int Rotunit::can_read_fifo()
00093 {
00094   can_frame frame;
00095   if(!can_.receive_frame(&frame))
00096     return -1;
00097 
00098   switch (frame.can_id) {
00099     case CAN_GETROTUNIT:
00100       can_rotunit(frame);
00101       break;
00102   }
00103   return frame.can_id;
00104 }
00105 
00106 int main(int argc, char** argv)
00107 {
00108   ros::init(argc, argv, "rotunit");
00109   ros::NodeHandle nh;
00110   Rotunit rotunit(nh);
00111 
00112   while (ros::ok())
00113   {
00114     rotunit.can_read_fifo();
00115     ros::spinOnce();
00116   }
00117   return 0;
00118 }
00119 


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