driver.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <ros/console.h>
00003 #include <serial/serial.h>
00004 #include <signal.h>
00005 #include <string>
00006 #include <sstream>
00007 
00008 
00009 #define DELTAT(_nowtime,_thentime) ((_thentime>_nowtime)?((0xffffffff-_thentime)+_nowtime):(_nowtime-_thentime))
00010 
00011 
00012 //
00013 // cmd_vel subscriber
00014 //
00015 
00016 // Define following to enable cmdvel debug output
00017 #define _CMDVEL_DEBUG
00018 
00019 #include <geometry_msgs/Twist.h>
00020 #include <nav_msgs/Odometry.h>
00021 #include <tf/transform_broadcaster.h>
00022 
00023 
00024 //
00025 // odom publisher
00026 //
00027 
00028 // Define following to enable odom debug output
00029 #define _ODOM_DEBUG
00030 
00031 // Define following to publish additional sensor information
00032 #define _ODOM_SENSORS
00033 
00034 // Define following to enable service for returning covariance
00035 //#define _ODOM_COVAR_SERVER
00036 
00037 #define NORMALIZE(_z) atan2(sin(_z), cos(_z))
00038 
00039 #include <tf/tf.h>
00040 #include <geometry_msgs/Quaternion.h>
00041 #include <tf/transform_broadcaster.h>
00042 #include <geometry_msgs/TransformStamped.h>
00043 #include <nav_msgs/Odometry.h>
00044 #ifdef _ODOM_SENSORS
00045 #include <std_msgs/Float32.h>
00046 #include <roboteq_diff_msgs/Duplex.h>
00047 #endif
00048 #ifdef _ODOM_COVAR_SERVER
00049 #include "roboteq_diff_msgs/OdometryCovariances.h"
00050 #include "rogoteq_diff_msgs/RequestOdometryCovariances.h"
00051 #endif
00052 
00053 
00054 
00055 void mySigintHandler(int sig)
00056 {
00057   ROS_INFO("Received SIGINT signal, shutting down...");
00058   ros::shutdown();
00059 }
00060 
00061 
00062 uint32_t millis()
00063 {
00064         ros::WallTime walltime = ros::WallTime::now();
00065 //      return (uint32_t)((walltime._sec*1000 + walltime.nsec/1000000.0) + 0.5);
00066 //      return (uint32_t)(walltime.toNSec()/1000000.0+0.5);
00067         return (uint32_t)(walltime.toNSec()/1000000);
00068 }
00069 
00070 class MainNode
00071 {
00072 
00073 public:
00074   MainNode();
00075 
00076 public:
00077 
00078   //
00079   // cmd_vel subscriber
00080   //
00081   void cmdvel_callback( const geometry_msgs::Twist& twist_msg);
00082   void cmdvel_setup();
00083   void cmdvel_loop();
00084   void cmdvel_run();
00085 
00086   //
00087   // odom publisher
00088   //
00089   void odom_setup();
00090   void odom_stream();
00091   void odom_loop();
00092   //void odom_hs_run();
00093   void odom_ms_run();
00094   void odom_ls_run();
00095   void odom_publish();
00096 #ifdef _ODOM_COVAR_SERVER
00097   void odom_covar_callback(const roboteq_diff_msgs::RequestOdometryCovariancesRequest& req, roboteq_diff_msgs::RequestOdometryCovariancesResponse& res);
00098 #endif
00099 
00100   int run();
00101 
00102 protected:
00103   ros::NodeHandle nh;
00104 
00105   serial::Serial controller;
00106 
00107   uint32_t starttime;
00108   uint32_t hstimer;
00109   uint32_t mstimer;
00110   uint32_t lstimer;
00111 
00112   //
00113   // cmd_vel subscriber
00114   //
00115   ros::Subscriber cmdvel_sub;
00116 
00117   //
00118   // odom publisher
00119   //
00120   geometry_msgs::TransformStamped tf_msg;
00121   tf::TransformBroadcaster odom_broadcaster;
00122   nav_msgs::Odometry odom_msg;
00123   ros::Publisher odom_pub;
00124 
00125 #ifdef _ODOM_SENSORS
00126   std_msgs::Float32 voltage_msg;
00127   ros::Publisher voltage_pub;
00128   roboteq_diff_msgs::Duplex current_msg;
00129   ros::Publisher current_pub;
00130   std_msgs::Float32 energy_msg;
00131   ros::Publisher energy_pub;
00132   std_msgs::Float32 temperature_msg;
00133   ros::Publisher temperature_pub;
00134 #endif
00135 
00136   // buffer for reading encoder counts
00137   int odom_idx;
00138   char odom_buf[24];
00139 
00140   // toss out initial encoder readings
00141   char odom_encoder_toss;
00142 
00143   int32_t odom_encoder_left;
00144   int32_t odom_encoder_right;
00145 
00146   float odom_x;
00147   float odom_y;
00148   float odom_yaw;
00149   float odom_last_x;
00150   float odom_last_y;
00151   float odom_last_yaw;
00152 
00153   uint32_t odom_last_time;
00154 
00155 #ifdef _ODOM_SENSORS
00156   float voltage;
00157   float current_right;
00158   float current_left;
00159   float energy;
00160   float temperature;
00161   uint32_t current_last_time;
00162 #endif
00163 
00164   // settings
00165   bool pub_odom_tf;
00166   std::string odom_frame;
00167   std::string base_frame;
00168   std::string cmdvel_topic;
00169   std::string odom_topic;
00170   std::string port;
00171   int baud;
00172   bool open_loop;
00173   double wheel_circumference;
00174   double track_width;
00175   int encoder_ppr;
00176   int encoder_cpr;
00177 
00178 };
00179 
00180 MainNode::MainNode() : 
00181   starttime(0),
00182   hstimer(0),
00183   mstimer(0),
00184   odom_idx(0),
00185   odom_encoder_toss(5),
00186   odom_encoder_left(0),
00187   odom_encoder_right(0),
00188   odom_x(0.0),
00189   odom_y(0.0),
00190   odom_yaw(0.0),
00191   odom_last_x(0.0),
00192   odom_last_y(0.0),
00193   odom_last_yaw(0.0),
00194   odom_last_time(0),
00195 #ifdef _ODOM_SENSORS
00196   voltage(0.0),
00197   current_right(0.0),
00198   current_left(0.0),
00199   energy(0.0),
00200   temperature(0.0),
00201   current_last_time(0),
00202 #endif
00203   pub_odom_tf(true),
00204   open_loop(false),
00205   baud(115200),
00206   wheel_circumference(0),
00207   track_width(0),
00208   encoder_ppr(0),
00209   encoder_cpr(0)
00210 {
00211 
00212 
00213   // CBA Read local params (from launch file)
00214   ros::NodeHandle nhLocal("~");
00215   nhLocal.param("pub_odom_tf", pub_odom_tf, true);
00216   ROS_INFO_STREAM("pub_odom_tf: " << pub_odom_tf);
00217   nhLocal.param<std::string>("odom_frame", odom_frame, "odom");
00218   ROS_INFO_STREAM("odom_frame: " << odom_frame);
00219   nhLocal.param<std::string>("base_frame", base_frame, "base_link");
00220   ROS_INFO_STREAM("base_frame: " << base_frame);
00221   nhLocal.param<std::string>("cmdvel_topic", cmdvel_topic, "cmd_vel");
00222   ROS_INFO_STREAM("cmdvel_topic: " << cmdvel_topic);
00223   nhLocal.param<std::string>("odom_topic", odom_topic, "odom");
00224   ROS_INFO_STREAM("odom_topic: " << odom_topic);
00225   nhLocal.param<std::string>("port", port, "/dev/ttyACM0");
00226   ROS_INFO_STREAM("port: " << port);
00227   nhLocal.param("baud", baud, 115200);
00228   ROS_INFO_STREAM("baud: " << baud);
00229   nhLocal.param("open_loop", open_loop, false);
00230   ROS_INFO_STREAM("open_loop: " << open_loop);
00231   nhLocal.param("wheel_circumference", wheel_circumference, 0.3192);
00232   ROS_INFO_STREAM("wheel_circumference: " << wheel_circumference);
00233   nhLocal.param("track_width", track_width, 0.4318);
00234   ROS_INFO_STREAM("track_width: " << track_width);
00235   nhLocal.param("encoder_ppr", encoder_ppr, 900);
00236   ROS_INFO_STREAM("encoder_ppr: " << encoder_ppr);
00237   nhLocal.param("encoder_cpr", encoder_cpr, 3600);
00238   ROS_INFO_STREAM("encoder_cpr: " << encoder_cpr);
00239 
00240 }
00241 
00242 
00243 //
00244 // cmd_vel subscriber
00245 //
00246 
00247 void MainNode::cmdvel_callback( const geometry_msgs::Twist& twist_msg)
00248 {
00249 
00250   // wheel speed (m/s)
00251   float right_speed = twist_msg.linear.x + track_width * twist_msg.angular.z / 2.0;
00252   float left_speed = twist_msg.linear.x - track_width * twist_msg.angular.z / 2.0;
00253 #ifdef _CMDVEL_DEBUG
00254 ROS_DEBUG_STREAM("cmdvel speed right: " << right_speed << " left: " << left_speed);
00255 #endif
00256 
00257   std::stringstream right_cmd;
00258   std::stringstream left_cmd;
00259 
00260   if (open_loop)
00261   {
00262     // motor power (scale 0-1000)
00263     int32_t right_power = right_speed / wheel_circumference * 60.0 / 82.0 * 1000.0;
00264     int32_t left_power = left_speed / wheel_circumference * 60.0 / 82.0 * 1000.0;
00265 #ifdef _CMDVEL_DEBUG
00266 ROS_DEBUG_STREAM("cmdvel power right: " << right_power << " left: " << left_power);
00267 #endif
00268     right_cmd << "!G 1 " << right_power << "\r";
00269     left_cmd << "!G 2 " << left_power << "\r";
00270   }
00271   else
00272   {
00273     // motor speed (rpm)
00274     int32_t right_rpm = right_speed / wheel_circumference * 60.0;
00275     int32_t left_rpm = left_speed / wheel_circumference * 60.0;
00276 #ifdef _CMDVEL_DEBUG
00277 ROS_DEBUG_STREAM("cmdvel rpm right: " << right_rpm << " left: " << left_rpm);
00278 #endif
00279     right_cmd << "!S 1 " << right_rpm << "\r";
00280     left_cmd << "!S 2 " << left_rpm << "\r";
00281   }
00282 
00283   controller.write(right_cmd.str());
00284   controller.write(left_cmd.str());
00285   controller.flush();
00286 }
00287 
00288 void MainNode::cmdvel_setup()
00289 {
00290 
00291   // stop motors
00292   controller.write("!G 1 0\r");
00293   controller.write("!G 2 0\r");
00294   controller.write("!S 1 0\r");
00295   controller.write("!S 2 0\r");
00296   controller.flush();
00297 
00298   // disable echo
00299   controller.write("^ECHOF 1\r");
00300   controller.flush();
00301 
00302   // enable watchdog timer (1000 ms)
00303   controller.write("^RWD 1000\r");
00304 //  controller.write("^RWD 250\r");
00305 
00306   // set motor operating mode (1 for closed-loop speed)
00307   if (open_loop)
00308   {
00309     controller.write("^MMOD 1 0\r");
00310     controller.write("^MMOD 2 0\r");
00311   }
00312   else
00313   {
00314     controller.write("^MMOD 1 1\r");
00315     controller.write("^MMOD 2 1\r");
00316   }
00317 
00318   // set motor amps limit (5 A * 10)
00319   controller.write("^ALIM 1 50\r");
00320   controller.write("^ALIM 2 50\r");
00321 
00322   // set max speed (rpm) for relative speed commands
00323 //  controller.write("^MXRPM 1 82\r");
00324 //  controller.write("^MXRPM 2 82\r");
00325   controller.write("^MXRPM 1 100\r");
00326   controller.write("^MXRPM 2 100\r");
00327 
00328   // set max acceleration rate (200 rpm/s * 10)
00329 //  controller.write("^MAC 1 2000\r");
00330 //  controller.write("^MAC 2 2000\r");
00331   controller.write("^MAC 1 20000\r");
00332   controller.write("^MAC 2 20000\r");
00333 
00334   // set max deceleration rate (2000 rpm/s * 10)
00335   controller.write("^MDEC 1 20000\r");
00336   controller.write("^MDEC 2 20000\r");
00337 
00338   // set PID parameters (gain * 10)
00339   controller.write("^KP 1 10\r");
00340   controller.write("^KP 2 10\r");
00341   controller.write("^KI 1 80\r");
00342   controller.write("^KI 2 80\r");
00343   controller.write("^KD 1 0\r");
00344   controller.write("^KD 2 0\r");
00345 
00346   // set encoder mode (18 for feedback on motor1, 34 for feedback on motor2)
00347   controller.write("^EMOD 1 18\r");
00348   controller.write("^EMOD 2 34\r");
00349 
00350   // set encoder counts (ppr)
00351   std::stringstream right_enccmd;
00352   std::stringstream left_enccmd;
00353   right_enccmd << "^EPPR 1 " << encoder_ppr << "\r";
00354   left_enccmd << "^EPPR 2 " << encoder_ppr << "\r";
00355   controller.write(right_enccmd.str());
00356   controller.write(left_enccmd.str());
00357 
00358   controller.flush();
00359 
00360   ROS_INFO_STREAM("Subscribing to topic " << cmdvel_topic);
00361   cmdvel_sub = nh.subscribe(cmdvel_topic, 1000, &MainNode::cmdvel_callback, this);
00362 
00363 }
00364 
00365 void MainNode::cmdvel_loop()
00366 {
00367 }
00368 
00369 void MainNode::cmdvel_run()
00370 {
00371 #ifdef _CMDVEL_FORCE_RUN
00372 //  controller.write("!G 1 100\r");
00373 //  controller.write("!G 2 100\r");
00374   controller.write("!S 1 10\r");
00375   controller.write("!S 2 10\r");
00376 #endif
00377 }
00378 
00379 
00380 //
00381 // odom publisher
00382 //
00383 
00384 #ifdef _ODOM_COVAR_SERVER
00385 void MainNode::odom_covar_callback(const roboteq_diff_msgs::RequestOdometryCovariancesRequest& req, roboteq_diff_msgs::RequestOdometryCovariancesResponse& res)
00386 {
00387   res.odometry_covariances.pose.pose.covariance[0] = 0.001;
00388   res.odometry_covariances.pose.pose.covariance[7] = 0.001;
00389   res.odometry_covariances.pose.pose.covariance[14] = 1000000;
00390   res.odometry_covariances.pose.pose.covariance[21] = 1000000;
00391   res.odometry_covariances.pose.pose.covariance[28] = 1000000;
00392   res.odometry_covariances.pose.pose.covariance[35] = 1000;
00393 
00394   res.odometry_covariances.twist.twist.covariance[0] = 0.001;
00395   res.odometry_covariances.twist.twist.covariance[7] = 0.001;
00396   res.odometry_covariances.twist.twist.covariance[14] = 1000000;
00397   res.odometry_covariances.twist.twist.covariance[21] = 1000000;
00398   res.odometry_covariances.twist.twist.covariance[28] = 1000000;
00399   res.odometry_covariances.twist.twist.covariance[35] = 1000;
00400 }
00401 #endif
00402 
00403 /*
00404   position.pose.covariance =  boost::assign::list_of(1e-3) (0) (0)  (0)  (0)  (0)
00405                                                        (0) (1e-3)  (0)  (0)  (0)  (0)
00406                                                        (0)   (0)  (1e6) (0)  (0)  (0)
00407                                                        (0)   (0)   (0) (1e6) (0)  (0)
00408                                                        (0)   (0)   (0)  (0) (1e6) (0)
00409                                                        (0)   (0)   (0)  (0)  (0)  (1e3) ;
00410 
00411   position.twist.covariance =  boost::assign::list_of(1e-3) (0)   (0)  (0)  (0)  (0)
00412                                                       (0) (1e-3)  (0)  (0)  (0)  (0)
00413                                                       (0)   (0)  (1e6) (0)  (0)  (0)
00414                                                       (0)   (0)   (0) (1e6) (0)  (0)
00415                                                       (0)   (0)   (0)  (0) (1e6) (0)
00416                                                       (0)   (0)   (0)  (0)  (0)  (1e3) ;
00417 
00418 To estimate velocity covariance you should know TICKSMM (128) and SIPCYCLE (100) parameters of your robot (written in your robots flash memory and not accessible with Aria). First parameter tells you how many encoder impulses (count) gets generated by your robot's forward movement of 1 mm. Second parameter tells you number of milliseconds between two consecutive Server Information Packets from your robot. The values in the parentheses are for P3-DX (ARCOS).
00419 
00420 So an error in determining velocity could come from missing an encoder impulse in a cycle. This would result in 1/TICKSMM/SIPCYCLE velocity error (mm/ms or m/s) for one wheel. For P3-DX parameters above, this value is 7.8125e-05. Note that you would err by the same absolute amount of velocity in the next cycle. Gearbox also plays a role in velocity error, but you would need to measure to find the exact amount. As a rule of thumb, I would at least double the previous amount in order to include gearbox error.
00421 
00422 Now that we have determined maximum error of a single wheel's (transversal) velocity, i.e. we expect 99.7% of errors to be less than this number, we can determine sigma = max_err/3 and C = sigma^2. Translational and rotational velocities are determined from left and right wheel velocities like this:
00423 
00424 v = (v_R + v_L)/2
00425 
00426 w = (v_R - v_L)/(2d)
00427 
00428 So the covariance for transversal velocity would be (1/2)^2 2C and the covariance for rotational velocity would be (1/(2d))^2 2C. The d parameter is 1/DiffConvFactor and is accessible from Aria (ArRobot::getDiffConvFactor()).
00429 
00430 */
00431 
00432 void MainNode::odom_setup()
00433 {
00434 
00435   if ( pub_odom_tf )
00436   {
00437     ROS_INFO("Broadcasting odom tf");
00438 //    odom_broadcaster.init(nh);        // ???
00439   }
00440 
00441   ROS_INFO_STREAM("Publishing to topic " << odom_topic);
00442   odom_pub = nh.advertise<nav_msgs::Odometry>(odom_topic, 1000);
00443         
00444 #ifdef _ODOM_COVAR_SERVER
00445   ROS_INFO("Advertising service on roboteq/odom_covar_srv");
00446   odom_covar_server = nh.advertiseService("roboteq/odom_covar_srv", &MainNode::odom_covar_callback, this);
00447 #endif
00448 
00449 #ifdef _ODOM_SENSORS
00450   ROS_INFO("Publishing to topic roboteq/voltage");
00451   voltage_pub = nh.advertise<std_msgs::Float32>("roboteq/voltage", 1000);
00452   ROS_INFO("Publishing to topic roboteq/current");
00453   current_pub = nh.advertise<roboteq_diff_msgs::Duplex>("roboteq/current", 1000);
00454   ROS_INFO("Publishing to topic roboteq/energy");
00455   energy_pub = nh.advertise<std_msgs::Float32>("roboteq/energy", 1000);
00456   ROS_INFO("Publishing to topic roboteq/temperature");
00457   temperature_pub = nh.advertise<std_msgs::Float32>("roboteq/temperature", 1000);
00458 #endif
00459 
00460   tf_msg.header.seq = 0;
00461   tf_msg.header.frame_id = odom_frame;
00462   tf_msg.child_frame_id = base_frame;
00463 
00464   odom_msg.header.seq = 0;
00465   odom_msg.header.frame_id = odom_frame;
00466   odom_msg.child_frame_id = base_frame;
00467 
00468   odom_msg.pose.covariance.assign(0);
00469   odom_msg.pose.covariance[0] = 0.001;
00470   odom_msg.pose.covariance[7] = 0.001;
00471   odom_msg.pose.covariance[14] = 1000000;
00472   odom_msg.pose.covariance[21] = 1000000;
00473   odom_msg.pose.covariance[28] = 1000000;
00474   odom_msg.pose.covariance[35] = 1000;
00475 
00476   odom_msg.twist.covariance.assign(0);
00477   odom_msg.twist.covariance[0] = 0.001;
00478   odom_msg.twist.covariance[7] = 0.001;
00479   odom_msg.twist.covariance[14] = 1000000;
00480   odom_msg.twist.covariance[21] = 1000000;
00481   odom_msg.twist.covariance[28] = 1000000;
00482   odom_msg.twist.covariance[35] = 1000;
00483 
00484 #ifdef _ODOM_SENSORS
00485 //  voltage_msg.header.seq = 0;
00486 //  voltage_msg.header.frame_id = 0;
00487 //  current_msg.header.seq = 0;
00488 //  current_msg.header.frame_id = 0;
00489 //  energy_msg.header.seq = 0;
00490 //  energy_msg.header.frame_id = 0;
00491 //  temperature_msg.header.seq = 0;
00492 //  temperature_msg.header.frame_id = 0;
00493 #endif
00494 
00495   // start encoder streaming
00496   odom_stream();
00497 
00498   odom_last_time = millis();
00499 #ifdef _ODOM_SENSORS
00500   current_last_time = millis();
00501 #endif
00502 }
00503 
00504 void MainNode::odom_stream()
00505 {
00506   
00507 #ifdef _ODOM_SENSORS
00508   // start encoder and current output (30 hz)
00509   // doubling frequency since one value is output at each cycle
00510 //  controller.write("# C_?CR_?BA_# 17\r");
00511   // start encoder, current and voltage output (30 hz)
00512   // tripling frequency since one value is output at each cycle
00513   controller.write("# C_?CR_?BA_?V_# 11\r");
00514 #else
00515 //  // start encoder output (10 hz)
00516 //  controller.write("# C_?CR_# 100\r");
00517   // start encoder output (30 hz)
00518   controller.write("# C_?CR_# 33\r");
00519 //  // start encoder output (50 hz)
00520 //  controller.write("# C_?CR_# 20\r");
00521 #endif
00522   controller.flush();
00523 
00524 }
00525 
00526 void MainNode::odom_loop()
00527 {
00528 
00529   uint32_t nowtime = millis();
00530 
00531   // if we haven't received encoder counts in some time then restart streaming
00532   if( DELTAT(nowtime,odom_last_time) >= 1000 )
00533   {
00534     odom_stream();
00535     odom_last_time = nowtime;
00536   }
00537 
00538   // read sensor data stream from motor controller
00539   if (controller.available())
00540   {
00541     char ch = 0;
00542     if ( controller.read((uint8_t*)&ch, 1) == 0 )
00543       return;
00544     if (ch == '\r')
00545     {
00546       odom_buf[odom_idx] = 0;
00547 #ifdef _ODOM_DEBUG
00548 //ROS_DEBUG_STREAM( "line: " << odom_buf );
00549 #endif
00550       // CR= is encoder counts
00551       if ( odom_buf[0] == 'C' && odom_buf[1] == 'R' && odom_buf[2] == '=' )
00552       {
00553         int delim;
00554         for ( delim = 3; delim < odom_idx; delim++ )
00555         {
00556           if ( odom_encoder_toss > 0 )
00557           {
00558             --odom_encoder_toss;
00559             break;
00560           }
00561           if (odom_buf[delim] == ':')
00562           {
00563             odom_buf[delim] = 0;
00564             odom_encoder_right = (int32_t)strtol(odom_buf+3, NULL, 10);
00565             odom_encoder_left = (int32_t)strtol(odom_buf+delim+1, NULL, 10);
00566 #ifdef _ODOM_DEBUG
00567 ROS_DEBUG_STREAM("encoder right: " << odom_encoder_right << " left: " << odom_encoder_left);
00568 #endif
00569             odom_publish();
00570             break;
00571           }
00572         }
00573       }
00574 #ifdef _ODOM_SENSORS
00575       // V= is voltages
00576       else if ( odom_buf[0] == 'V' && odom_buf[1] == '=' )
00577       {
00578         int count = 0;
00579         int start = 2;
00580         for ( int delim = 2; delim <= odom_idx; delim++ )
00581         {
00582           if (odom_buf[delim] == ':' || odom_buf[delim] == 0)
00583           {
00584             odom_buf[delim] = 0;
00585 /*
00586             switch (count)
00587             {
00588             case 0:
00589 //              odom_internal_voltage = (float)strtol(odom_buf+start, NULL, 10) / 10.0;
00590               break;
00591             case 1:
00592               voltage = (float)strtol(odom_buf+start, NULL, 10) / 10.0;
00593               break;
00594             case 2:
00595 //              odom_aux_voltage = (float)strtol(odom_buf+start, NULL, 1000.0);
00596               break;
00597             }
00598 */
00599             if ( count == 1 )
00600             {
00601               voltage = (float)strtol(odom_buf+start, NULL, 10) / 10.0;
00602 #ifdef _ODOM_DEBUG
00603 //ROS_DEBUG_STREAM("voltage: " << voltage);
00604 #endif
00605               break;
00606             }
00607             start = delim + 1;
00608             count++;
00609           }
00610         }
00611       }
00612       // BA= is motor currents
00613       else if ( odom_buf[0] == 'B' && odom_buf[1] == 'A' && odom_buf[2] == '=' )
00614       {
00615         int delim;
00616         for ( delim = 3; delim < odom_idx; delim++ )
00617         {
00618           if (odom_buf[delim] == ':')
00619           {
00620             odom_buf[delim] = 0;
00621             current_right = (float)strtol(odom_buf+3, NULL, 10) / 10.0;
00622             current_left = (float)strtol(odom_buf+delim+1, NULL, 10) / 10.0;
00623 #ifdef _ODOM_DEBUG
00624 //ROS_DEBUG_STREAM("current right: " << current_right << " left: " << current_left);
00625 #endif
00626 
00627             // determine delta time in seconds
00628             float dt = (float)DELTAT(nowtime,current_last_time) / 1000.0;
00629             current_last_time = nowtime;
00630             energy += (current_right + current_left) * dt / 3600.0;
00631             break;
00632           }
00633         }
00634       }
00635 #endif
00636       odom_idx = 0;
00637     }
00638     else if ( odom_idx < (sizeof(odom_buf)-1) )
00639     {
00640       odom_buf[odom_idx++] = ch;
00641     }
00642   }
00643 }
00644 
00645 //void MainNode::odom_hs_run()
00646 //{
00647 //}
00648 
00649 void MainNode::odom_ms_run()
00650 {
00651 
00652 #ifdef _ODOM_SENSORS
00653 //  current_msg.header.seq++;
00654 //  current_msg.header.stamp = ros::Time::now();
00655   current_msg.a = current_right;
00656   current_msg.b = current_left;
00657   current_pub.publish(current_msg);
00658 #endif
00659 
00660 }
00661 
00662 void MainNode::odom_ls_run()
00663 {
00664 
00665 #ifdef _ODOM_SENSORS
00666 //  voltage_msg.header.seq++;
00667 //  voltage_msg.header.stamp = ros::Time::now();
00668   voltage_msg.data = voltage;
00669   voltage_pub.publish(voltage_msg);
00670 //  energy_msg.header.seq++;
00671 //  energy_msg.header.stamp = ros::Time::now();
00672   energy_msg.data = energy;
00673   energy_pub.publish(energy_msg);
00674 //  temperature_msg.header.seq++;
00675 //  temperature_msg.header.stamp = ros::Time::now();
00676   temperature_msg.data = temperature;
00677   temperature_pub.publish(temperature_msg);
00678 #endif
00679 
00680 }
00681 
00682 void MainNode::odom_publish()
00683 {
00684 
00685   // determine delta time in seconds
00686   uint32_t nowtime = millis();
00687   float dt = (float)DELTAT(nowtime,odom_last_time) / 1000.0;
00688   odom_last_time = nowtime;
00689 
00690 #ifdef _ODOM_DEBUG
00691 /*
00692 ROS_DEBUG("right: ");
00693 ROS_DEBUG(odom_encoder_right);
00694 ROS_DEBUG(" left: ");
00695 ROS_DEBUG(odom_encoder_left);
00696 ROS_DEBUG(" dt: ");
00697 ROS_DEBUG(dt);
00698 ROS_DEBUG("");
00699 */
00700 #endif
00701 
00702   // determine deltas of distance and angle
00703   float linear = ((float)odom_encoder_right / (float)encoder_cpr * wheel_circumference + (float)odom_encoder_left / (float)encoder_cpr * wheel_circumference) / 2.0;
00704 //  float angular = ((float)odom_encoder_right / (float)encoder_cpr * wheel_circumference - (float)odom_encoder_left / (float)encoder_cpr * wheel_circumference) / track_width * -1.0;
00705   float angular = ((float)odom_encoder_right / (float)encoder_cpr * wheel_circumference - (float)odom_encoder_left / (float)encoder_cpr * wheel_circumference) / track_width;
00706 #ifdef _ODOM_DEBUG
00707 /*
00708 ROS_DEBUG("linear: ");
00709 ROS_DEBUG(linear);
00710 ROS_DEBUG(" angular: ");
00711 ROS_DEBUG(angular);
00712 ROS_DEBUG("");
00713 */
00714 #endif
00715 
00716   // Update odometry
00717   odom_x += linear * cos(odom_yaw);        // m
00718   odom_y += linear * sin(odom_yaw);        // m
00719   odom_yaw = NORMALIZE(odom_yaw + angular);  // rad
00720 #ifdef _ODOM_DEBUG
00721 //ROS_DEBUG_STREAM( "odom x: " << odom_x << " y: " << odom_y << " yaw: " << odom_yaw );
00722 #endif
00723 
00724   // Calculate velocities
00725   float vx = (odom_x - odom_last_x) / dt;
00726   float vy = (odom_y - odom_last_y) / dt;
00727   float vyaw = (odom_yaw - odom_last_yaw) / dt;
00728 #ifdef _ODOM_DEBUG
00729 //ROS_DEBUG_STREAM( "velocity vx: " << odom_x << " vy: " << odom_y << " vyaw: " << odom_yaw );
00730 #endif
00731   odom_last_x = odom_x;
00732   odom_last_y = odom_y;
00733   odom_last_yaw = odom_yaw;
00734 #ifdef _ODOM_DEBUG
00735 /*
00736 ROS_DEBUG("vx: ");
00737 ROS_DEBUG(vx);
00738 ROS_DEBUG(" vy: ");
00739 ROS_DEBUG(vy);
00740 ROS_DEBUG(" vyaw: ");
00741 ROS_DEBUG(vyaw);
00742 ROS_DEBUG("");
00743 */
00744 #endif
00745 
00746   geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(odom_yaw);
00747 
00748   if ( pub_odom_tf )
00749   {
00750     tf_msg.header.seq++;
00751     tf_msg.header.stamp = ros::Time::now();
00752     tf_msg.transform.translation.x = odom_x;
00753     tf_msg.transform.translation.y = odom_y;
00754     tf_msg.transform.translation.z = 0.0;
00755     tf_msg.transform.rotation = quat;
00756     odom_broadcaster.sendTransform(tf_msg);
00757   }
00758 
00759   odom_msg.header.seq++;
00760   odom_msg.header.stamp = ros::Time::now();
00761   odom_msg.pose.pose.position.x = odom_x;
00762   odom_msg.pose.pose.position.y = odom_y;
00763   odom_msg.pose.pose.position.z = 0.0;
00764   odom_msg.pose.pose.orientation = quat;
00765   odom_msg.twist.twist.linear.x = vx;
00766   odom_msg.twist.twist.linear.y = vy;
00767   odom_msg.twist.twist.linear.z = 0.0;
00768   odom_msg.twist.twist.angular.x = 0.0;
00769   odom_msg.twist.twist.angular.y = 0.0;
00770   odom_msg.twist.twist.angular.z = vyaw;
00771   odom_pub.publish(odom_msg);
00772 
00773 }
00774 
00775 
00776 int MainNode::run()
00777 {
00778 
00779         ROS_INFO("Beginning setup...");
00780 
00781         serial::Timeout timeout = serial::Timeout::simpleTimeout(1000);
00782         controller.setPort(port);
00783         controller.setBaudrate(baud);
00784         controller.setTimeout(timeout);
00785 
00786         // TODO: support automatic re-opening of port after disconnection
00787         while ( ros::ok() )
00788         {
00789                 ROS_INFO_STREAM("Opening serial port on " << port << " at " << baud << "..." );
00790                 try
00791                 {
00792                         controller.open();
00793                         if ( controller.isOpen() )
00794                         {
00795                                 ROS_INFO("Successfully opened serial port");
00796                                 break;
00797                         }
00798                 }
00799                 catch (serial::IOException e)
00800                 {
00801                         ROS_WARN_STREAM("serial::IOException: " << e.what());
00802                 }
00803                 ROS_WARN("Failed to open serial port");
00804                 sleep( 5 );
00805         }
00806 
00807         cmdvel_setup();
00808         odom_setup();
00809 
00810   starttime = millis();
00811   hstimer = starttime;
00812   mstimer = starttime;
00813   lstimer = starttime;
00814 
00815 //  ros::Rate loop_rate(10);
00816 
00817   ROS_INFO("Beginning looping...");
00818         
00819   while (ros::ok())
00820   {
00821 
00822     cmdvel_loop();
00823     odom_loop();
00824 
00825     uint32_t nowtime = millis();
00826 //ROS_INFO_STREAM("loop nowtime: " << nowtime << " lstimer: " << lstimer << " delta: " << DELTAT(nowtime,lstimer) << " / " << (nowtime-lstimer));
00827 //uint32_t delta = DELTAT(nowtime,lstimer);
00828 //ROS_INFO_STREAM("loop nowtime: " << nowtime << " lstimer: " << lstimer << " delta: " << delta << " / " << (nowtime-lstimer));
00829 
00830 //    // Handle 50 Hz publishing
00831 //    if (DELTAT(nowtime,hstimer) >= 20)
00832     // Handle 30 Hz publishing
00833     if (DELTAT(nowtime,hstimer) >= 33)
00834     {
00835       hstimer = nowtime;
00836 //      odom_hs_run();
00837     }
00838 
00839     // Handle 10 Hz publishing
00840     if (DELTAT(nowtime,mstimer) >= 100)
00841     {
00842       mstimer = nowtime;
00843       cmdvel_run();
00844       odom_ms_run();
00845     }
00846 
00847     // Handle 1 Hz publishing
00848     if (DELTAT(nowtime,lstimer) >= 1000)
00849     {
00850       lstimer = nowtime;
00851       odom_ls_run();
00852     }
00853 
00854     ros::spinOnce();
00855 
00856 //    loop_rate.sleep();
00857   }
00858         
00859   if ( controller.isOpen() )
00860     controller.close();
00861 
00862   ROS_INFO("Exiting");
00863         
00864   return 0;
00865 }
00866 
00867 int main(int argc, char **argv)
00868 {
00869 
00870   ros::init(argc, argv, "main_node");
00871 
00872   MainNode node;
00873 
00874   // Override the default ros sigint handler.
00875   // This must be set after the first NodeHandle is created.
00876   signal(SIGINT, mySigintHandler);
00877 
00878   return node.run();
00879 }
00880 


roboteq_diff_driver
Author(s): Chad Attermann
autogenerated on Mon Apr 1 2019 10:03:18