stingbot_sensors_node.cpp
Go to the documentation of this file.
00001 #include <stdlib.h>
00002 #include <stdio.h>
00003 #include <string>
00004 #include <vector>
00005 
00006 #include <ros/ros.h>
00007 #include <tf/transform_broadcaster.h>
00008 #include <nav_msgs/Odometry.h>                  // odom
00009 #include <geometry_msgs/Twist.h>                // cmd_vel
00010 
00011 #include <cereal_port/CerealPort.h>
00012 #include <geometry_msgs/Vector3.h>
00013 #include <std_msgs/String.h>
00014 #include <std_msgs/Float32.h>
00015 
00016 
00017 #define BAUD_RATE 115200
00018 #define MAX_ENCODER_COUNT 32768
00019 
00020 #define TRAXBOT_AXLE_LENGTH     0.225
00021 #define TRAXBOT_PULSES_TO_M     0.000073529
00022                         
00023 #ifndef NORMALIZE
00024     #define NORMALIZE(z) atan2(sin(z), cos(z))  // Normalize angle to domain -pi, pi 
00025 #endif
00026 
00027 
00028 ros::Publisher * odom_pub_ptr;
00029 ros::Publisher pub_sensors;
00030 ros::Publisher pub_LDRsensor;
00031 ros::Subscriber sub_class;
00032 tf::TransformBroadcaster * odom_broadcaster_ptr;
00033 
00034 cereal::CerealPort serial_port;
00035 ros::Time current_time, last_time;
00036 double last_time_pub = 0.0;
00037 
00038 bool signof (int n) { return n >= 0; }
00039 bool confirm_communication = true;
00040 int ID_Robot = 0;
00041 
00042 double odometry_x_ = 0.0;
00043 double odometry_y_ = 0.0;
00044 double odometry_yaw_ = 0.0;
00045 int right_encoder_prev = 0;
00046 int left_encoder_prev = 0;
00047 
00048 
00049 //Receive encoder ticks and send 'odom' and 'tf'
00050 void robotDataCallback(std::string * data){ 
00051    
00052     if (confirm_communication){
00053       //ROS_INFO("Robot -- Communication OK! Received: \"%s\"", data->c_str());
00054       ROS_INFO("Traxbot is Streaming Data.");
00055       confirm_communication = false;
00056     }
00057     
00058     int first_at = data->find_first_of("@", 0);
00059     int second_at = data->find_first_of("@", first_at+1);    
00060     int first_comma = data->find_first_of(",", 0);
00061     int second_comma = data->find_first_of(",", first_comma+1);    
00062 
00063     //protection against broken msgs from the buffer (e.g., '@6,425@6,4250,6430e')
00064     if ( second_at > -1 || second_comma == -1){
00065       ROS_WARN("%s ::: ENCODER MSG IGNORED", data->c_str());
00066       return; 
00067     }    
00068     
00069     int left_encoder_count, right_encoder_count, alcohol_sensor, dust_sensor, temp_max, ldr;
00070     sscanf(data->c_str(), "@25,%d,%d,%d,%d,%d,%de", &right_encoder_count, &left_encoder_count, &alcohol_sensor, &dust_sensor, &temp_max, &ldr);   //encoder msg parsing
00071 
00072     //protection against broken msgs from the buffer (e.g., '@6,425@6,4250,6430e')
00073     if ( abs(right_encoder_count) > MAX_ENCODER_COUNT || abs(left_encoder_count) > MAX_ENCODER_COUNT){
00074       ROS_WARN("Encoders > MAX_ENCODER_COUNT");
00075       return; 
00076     }
00077         
00078 
00079     double last_x = odometry_x_;
00080     double last_y = odometry_y_;
00081     double last_yaw = odometry_yaw_;           
00082     
00083     // It is not necessary to reset the encoders:    
00084     int right_enc_dif = 0, left_enc_dif = 0;    
00085     
00086     if ( right_encoder_prev >= (0.75*MAX_ENCODER_COUNT) && signof (right_encoder_count) == 0 && signof(right_encoder_prev) == 1 ){
00087       right_enc_dif = (MAX_ENCODER_COUNT-right_encoder_prev) + (MAX_ENCODER_COUNT+right_encoder_count);
00088           
00089     }else if (right_encoder_prev <= (-0.75*MAX_ENCODER_COUNT) && signof (right_encoder_count) == 1 && signof(right_encoder_prev) == 0){
00090       right_enc_dif = -(MAX_ENCODER_COUNT+right_encoder_prev) + (right_encoder_count-MAX_ENCODER_COUNT);
00091       
00092     }else{
00093       right_enc_dif = right_encoder_count - right_encoder_prev;
00094     }
00095     
00096     
00097     if ( left_encoder_prev >= (0.75*MAX_ENCODER_COUNT) && signof (left_encoder_count) == 0 && signof(left_encoder_prev) == 1){
00098       left_enc_dif = (MAX_ENCODER_COUNT-left_encoder_prev) + (MAX_ENCODER_COUNT+left_encoder_count);
00099       
00100     }else if (left_encoder_prev <= (-0.75*MAX_ENCODER_COUNT) && signof (left_encoder_count) == 1 && signof(left_encoder_prev) == 0){
00101       left_enc_dif = -(MAX_ENCODER_COUNT+left_encoder_prev) + (left_encoder_count-MAX_ENCODER_COUNT);
00102       
00103     }else{
00104       left_enc_dif = left_encoder_count - left_encoder_prev;
00105     }      
00106   
00107     //calulate Odometry: 
00108     double dist = (right_enc_dif*PULSES_TO_M + left_enc_dif*PULSES_TO_M) / 2.0; 
00109     double ang = (right_enc_dif*PULSES_TO_M - left_enc_dif*PULSES_TO_M) / AXLE_LENGTH;
00110     bool publish_info = true;
00111 
00112     if (right_encoder_prev == right_encoder_count && left_encoder_prev == left_encoder_count){
00113         publish_info = false;
00114     }
00115     
00116     // update previous encoder counts:
00117     left_encoder_prev = left_encoder_count;
00118     right_encoder_prev = right_encoder_count;
00119 
00120     // Update odometry
00121     odometry_yaw_ = NORMALIZE(odometry_yaw_ + ang);             // rad
00122     odometry_x_ = odometry_x_ + dist*cos(odometry_yaw_);        // m
00123     odometry_y_ = odometry_y_ + dist*sin(odometry_yaw_);        // m   
00124     
00125     last_time = current_time;
00126     current_time = ros::Time::now();      
00127 
00128     // Calculate the robot speed 
00129     double dt = (current_time - last_time).toSec();
00130     double vel_x = (odometry_x_ - last_x)/dt;
00131     double vel_y = (odometry_y_ - last_y)/dt;
00132     double vel_yaw = (odometry_yaw_ - last_yaw)/dt;    
00133 
00134     //Publish at least at most at 75Hz
00135     if (current_time.toSec() - 0.099 >= last_time_pub){
00136         publish_info = true;
00137     }
00138     
00139     if(publish_info){
00140 
00141         last_time_pub = current_time.toSec();
00142     
00143         // Since all odometry is 6DOF we'll need a quaternion created from yaw    
00144         geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(odometry_yaw_);
00145 
00146         // First, we'll publish the transform over tf
00147         geometry_msgs::TransformStamped odom_trans;
00148         odom_trans.header.stamp = current_time;
00149         odom_trans.header.frame_id = "odom";
00150         odom_trans.child_frame_id = "base_link";
00151             
00152         odom_trans.transform.translation.x = odometry_x_;
00153         odom_trans.transform.translation.y = odometry_y_;
00154         odom_trans.transform.translation.z = 0.0;
00155         odom_trans.transform.rotation = odom_quat;
00156             
00157         // Send the transform
00158         odom_broadcaster_ptr->sendTransform(odom_trans);                // odom->base_link transform
00159                 
00160             
00161         // Next, we'll publish the odometry message over ROS
00162         nav_msgs::Odometry odom;
00163         odom.header.stamp = current_time;
00164         odom.header.frame_id = "odom";
00165             
00166         // Set the position
00167         odom.pose.pose.position.x = odometry_x_;
00168         odom.pose.pose.position.y = odometry_y_;
00169         odom.pose.pose.position.z = 0.0;
00170         odom.pose.pose.orientation = odom_quat;
00171             
00172         // Set the velocity
00173         odom.child_frame_id = "base_link";
00174         odom.twist.twist.linear.x = vel_x;
00175         odom.twist.twist.linear.y = vel_y;
00176         odom.twist.twist.angular.z = vel_yaw;
00177         
00178  
00179         geometry_msgs::Vector3 msg;
00180         msg.x=alcohol_sensor;
00181         msg.y=dust_sensor;
00182         msg.z=temp_max;
00183         
00184         std_msgs::Float32 msg_ldr;
00185         msg_ldr.data=ldr;
00186         
00187         // Publish the message
00188         pub_LDRsensor.publish(msg_ldr);
00189         pub_sensors.publish(msg);
00190         odom_pub_ptr->publish(odom);            // odometry publisher 
00191         ros::spinOnce();
00192     } 
00193 }
00194 
00195 std::string drive(double linear_speed, double angular_speed, int ID_Robot) {
00196   
00197   //truncate to 3 decimals:
00198   int linear_speed_3decimals = floorf(linear_speed * 1000 + 0.5);
00199   int angular_speed_3decimals = floorf(angular_speed * 1000 + 0.5);
00200   
00201   std::string vel_lin_str, vel_ang_str;
00202   std::stringstream out_lin, out_ang;
00203   
00204   //int to string:
00205   out_lin << linear_speed_3decimals;
00206   vel_lin_str = out_lin.str();
00207   
00208   out_ang << angular_speed_3decimals;
00209   vel_ang_str = out_ang.str();
00210   
00211   std::string result = "@11," + vel_lin_str + "," + vel_ang_str + "e";
00212      
00213   return result;
00214 }
00215 
00216 //receive cmds_vel from nav_stack
00217 void cmdVelReceived(const geometry_msgs::Twist::ConstPtr& cmd_vel){
00218   
00219         std::string result = drive(cmd_vel->linear.x, cmd_vel->angular.z, ID_Robot);
00220         ROS_INFO("[cmd_vel]: Vlinear = %f, Vangular = %f",cmd_vel->linear.x, cmd_vel->angular.z);
00221         ROS_INFO("Sending: %s\n", result.c_str());
00222         serial_port.write( result.c_str() ); 
00223 }
00224 
00225 void classifficationCB(const std_msgs::String::ConstPtr& msg){
00226 
00227 //  FILE *file;
00228 //  file = fopen ("/home/mrl/Desktop/sensores.m","a");
00229 //  fprintf(file, "%s %f %f;\n",msg->data.c_str(),odometry_x_,odometry_y_);
00230 //  fclose(file);
00231 
00232 }
00233 
00234 int main(int argc, char** argv){ //typical usage: "./traxbot_node /dev/ttyACMx"
00235   
00236         ros::init(argc, argv, "traxbot_node");
00237         ros::NodeHandle n;
00238         ros::NodeHandle pn("~");
00239         std::string port;
00240         
00241         if (argc<2){
00242          port="/dev/ttyACM0";
00243          ROS_WARN("No Serial Port defined, defaulting to \"%s\"",port.c_str());
00244          ROS_WARN("Usage: \"rosrun [pkg] robot_node /serial_port\"");
00245         }else{
00246           port=(std::string)argv[1];
00247           ROS_INFO("Serial port: %s",port.c_str());
00248         }       
00249         
00250         // ROS publishers and subscribers
00251         ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("/odom", 100);
00252         tf::TransformBroadcaster odom_broadcaster;
00253         ros::Subscriber cmd_vel_sub  = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 10, cmdVelReceived);
00254     
00255         odom_pub_ptr = &odom_pub;
00256         odom_broadcaster_ptr = &odom_broadcaster;
00257         pub_sensors = n.advertise<geometry_msgs::Vector3>("robot_sensors", 1);
00258         pub_LDRsensor = n.advertise<std_msgs::Float32>("/robot_ldr_sensor", 1);
00259         sub_class = n.subscribe("/sensors_classiffication", 1, classifficationCB);   
00260         
00261         
00262         // baud_rate and serial port:   
00263         int baudrate;
00264         pn.param<std::string>("port", port, port.c_str()); 
00265         pn.param("baudrate", baudrate, 115200); 
00266 
00267         // Open the serial port to the robot
00268         try{ serial_port.open((char*)port.c_str(), baudrate); }
00269         catch(cereal::Exception& e){
00270                 ROS_FATAL("Robot -- Failed to open serial port!");
00271                 ROS_BREAK();
00272         }
00273     
00274         //wait (2.5 seconds) until serial port gets ready
00275         ros::Duration(2.5).sleep();     
00276                 
00277         // Ask Robot ID from the Arduino board (stored in the EEPROM)
00278         ROS_INFO("Starting Traxbot...");
00279         serial_port.write("@5e");
00280         std::string reply;
00281 
00282         try{ serial_port.readBetween(&reply,'@','e'); } 
00283         catch(cereal::TimeoutException& e){
00284           ROS_ERROR("Initial Read Timeout!");
00285         }
00286         
00287         int VDriver, Temperature, OMNI_Firmware, Battery;
00288         sscanf(reply.c_str(), "@5,%d,%d,%d,%d,%de", &Temperature, &OMNI_Firmware, &Battery, &VDriver, &ID_Robot);   //encoder msg parsing
00289 
00290         ROS_INFO("Traxbot ID = %d", ID_Robot);
00291         if (ID_Robot < 1 || ID_Robot > 3){
00292                 ROS_WARN("Attention! Unexpected Traxbot ID!");
00293         }
00294         ROS_INFO("OMNI Board Temperature = %.2f C", Temperature*0.01);
00295         ROS_INFO("OMNI Firmware = %.2f", OMNI_Firmware*0.01);
00296         ROS_INFO("Arduino Firmware Version = %d.00", VDriver);
00297 
00298         if (VDriver > 1500){
00299                 ROS_ERROR("Reset Robot Connection and try again.");
00300                 return(0);
00301         }
00302 
00303         ROS_INFO("Battery = %.2f V", Battery*0.01 );
00304         if (Battery < 100){
00305                 ROS_ERROR("Robot is off. Check power source!");
00306                 return(0);
00307         }
00308         if (Battery < 850 && Battery > 100){
00309                 ROS_WARN("Warning! Low Battery!");
00310         }
00311         
00312         // Start receiving streaming data
00313         if( !serial_port.startReadBetweenStream(boost::bind(&robotDataCallback, _1), '@', 'e') ){
00314                   ROS_FATAL("Robot -- Failed to start streaming data!");
00315                   ROS_BREAK();
00316         }
00317         serial_port.write("@25e");
00318     
00319         ros::spin(); //trigger callbacks and prevents exiting
00320         return(0);
00321 
00322 }
00323 
00324 
00325 
00326 
00327 
00328 
00329 
00330 


mrl_robots_sensors
Author(s): André Araújo and Nuno Ferreira
autogenerated on Mon Jan 6 2014 11:27:36