ric_node.cpp
Go to the documentation of this file.
00001 #include "ros/ros.h"
00002 #include <ric_robot/ric_raw.h>
00003 #include <ric_robot/ric_gps.h>
00004 #include <ric_robot/ric_pan_tilt.h>
00005 #include <ric_robot/ric_command.h>
00006 #include <ric_robot/set_odom.h>
00007 #include "sensor_msgs/Imu.h"
00008 #include "sensor_msgs/Range.h"
00009 #include "sensor_msgs/NavSatFix.h"
00010 #include "sensor_msgs/NavSatStatus.h"
00011 #include <sensor_msgs/JointState.h>
00012 #include "geometry_msgs/Twist.h"
00013 #include "geometry_msgs/Quaternion.h"
00014 #include "geometry_msgs/Vector3Stamped.h"
00015 #include "nav_msgs/Odometry.h"
00016 #include "tf/transform_datatypes.h"
00017 #include <tf/transform_broadcaster.h>
00018 #include <math.h>
00019 
00020 
00021 ros::Publisher left_urf_pub;
00022 ros::Publisher rear_urf_pub;
00023 ros::Publisher right_urf_pub;
00024 
00025 ros::Publisher odom_pub;
00026 ros::Publisher gps_pub;
00027 ros::Publisher imu_pub;
00028 ros::Publisher command_pub;
00029 ros::Publisher pan_tilt_pub;
00030 ros::Publisher wheels_pub;
00031 
00032 sensor_msgs::Range left_urf_msg;
00033 sensor_msgs::Range rear_urf_msg;
00034 sensor_msgs::Range right_urf_msg;
00035 
00036 //std::string ric_id;
00037 std::string map_frame_id;
00038 std::string odom_frame_id;
00039 std::string base_frame_id;
00040 std::string imu_frame_id;
00041 std::string cmd_vel_topic;
00042 std::string pan_tilt_topic;
00043 std::string ric_command_topic;
00044 std::string gps_pub_topic;
00045 std::string set_odom_srv;
00046 std::string odom_topic;
00047 std::string ric_raw_topic;
00048 std::string ric_gps_topic;
00049 std::string left_urf_frame_id;
00050 std::string rear_urf_frame_id;
00051 std::string right_urf_frame_id;
00052 std::string pan_joint_id;
00053 std::string tilt_joint_id;
00054 std::string FR_joint_id;
00055 std::string FL_joint_id;
00056 std::string RR_joint_id;
00057 std::string RL_joint_id;
00058 
00059 std::string joint_states_topic;
00060 
00061 sensor_msgs::JointState jointstate_msg;
00062 
00063 tf::TransformBroadcaster *imu_broadcaster;
00064 tf::TransformBroadcaster *odom_broadcaster;
00065 
00066 
00067 double wheel_base_length;
00068 double wheel_diameter;
00069 double encoder_cpr; //=ppr*4
00070 
00071 int l_encoder=0,pre_l_encoder=0;
00072 int r_encoder=0,pre_r_encoder=0;
00073 geometry_msgs::Quaternion q_imu;
00074 double xx = 0, yy = 0, tt = 0;
00075 bool first_enc_read = true;
00076 
00077 bool fuse_imu_roll_pitch;
00078 bool fuse_imu_yaw;
00079 double slip_factor;
00080 
00081 double rot_cov = 0.0;
00082 double pos_cov = 0.0;
00083 ros::Time prev_time;
00084 
00085 bool wd_on=false;
00086 ros::Time wd_time;
00087 
00088 double pan=0;
00089 double tilt=0;
00090 
00091 bool have_pan_tilt;
00092 
00093 
00094 void cmd_velCallback(const geometry_msgs::Twist::ConstPtr& msg) {
00095         
00096   if (wd_on) wd_on=false;
00097         
00098   wd_time = ros::Time::now();
00099         
00100         
00101   double Right_Motor_v = msg->linear.x + msg->angular.z * (wheel_base_length / 2.0);
00102   double Left_Motor_v =  msg->linear.x - msg->angular.z * (wheel_base_length / 2.0);
00103 
00104   ric_robot::ric_command command_msg;
00105   command_msg.left_wheel=(int)(Left_Motor_v*encoder_cpr/(M_PI * wheel_diameter)); //tick per sec
00106   command_msg.right_wheel=(int)(Right_Motor_v*encoder_cpr/(M_PI * wheel_diameter)); //tick per sec
00107   command_pub.publish(command_msg);
00108 
00109 
00110 
00111 }
00112 
00113 double wrapToPi(double angle) {
00114   angle += M_PI;
00115   bool is_neg = (angle < 0);
00116   angle = fmod(angle, (2.0 * M_PI));
00117   if (is_neg) {
00118     angle += (2.0 * M_PI);
00119   }
00120   angle -= M_PI;
00121   return angle;
00122 }
00123 
00124 void encodersCallback(int32_t left_ticks, int32_t right_ticks) {
00125         
00126   ros::Time now = ros::Time::now();
00127   double delta_time = (now - prev_time).toSec();
00128   prev_time = now;
00129         
00130   pre_l_encoder=l_encoder;
00131   pre_r_encoder=r_encoder;
00132                                 
00133   l_encoder = left_ticks;
00134   r_encoder = right_ticks;
00135         
00136 
00138   double l_tics_per_s=(double)(l_encoder-pre_l_encoder)/delta_time;
00139   double r_tics_per_s=(double)(r_encoder-pre_r_encoder)/delta_time;
00140                 
00141   double l_w = l_tics_per_s * 2.0 * M_PI / (double) encoder_cpr;
00142   double r_w = r_tics_per_s * 2.0 * M_PI / (double) encoder_cpr;
00143                         
00144                 
00145                 
00146   double l_v = l_w * wheel_diameter / 2.0;
00147   double r_v = r_w * wheel_diameter / 2.0;
00148 
00149   double v = (l_v + r_v) / 2.0;
00150 
00151   double w = (r_v - l_v) / wheel_base_length;
00152 
00153   if (first_enc_read) {
00154 
00155     xx = 0;
00156     yy = 0;
00157     tt = 0;
00158     first_enc_read = false;
00159   }
00160                 
00161   tt = tt + w * delta_time*slip_factor;
00162   tt = wrapToPi(tt);
00163   xx = xx + (v * cos(tt))*delta_time;
00164   yy = yy + (v * sin(tt))*delta_time;
00165 
00166                 
00167                 
00168   nav_msgs::Odometry odom_msg;
00169 
00170   geometry_msgs::Quaternion q_odom;
00171                 
00172   odom_msg.header.stamp = now;
00173   odom_msg.header.frame_id = odom_frame_id;
00174   odom_msg.pose.pose.position.x = xx;
00175   odom_msg.pose.pose.position.y = yy;
00176   odom_msg.pose.pose.position.z = 0;
00177                 
00178   if ((fuse_imu_roll_pitch)&&(fuse_imu_yaw)) {
00179     odom_msg.pose.pose.orientation = q_odom=q_imu;              
00180   }
00181   else if ((!fuse_imu_roll_pitch)&&(fuse_imu_yaw)) {
00182     double roll = 0, pitch = 0, yaw = 0;                                        
00183     tf::Quaternion q;           
00184     tf::quaternionMsgToTF(q_imu, q);                            
00185     tf::Matrix3x3(q).getRPY(roll, pitch, yaw);  
00186     q_odom =tf::createQuaternionMsgFromRollPitchYaw(0,0, yaw);
00187   }
00188   else if ((fuse_imu_roll_pitch)&&(!fuse_imu_yaw)) {
00189     double roll = 0, pitch = 0, yaw = 0;                                        
00190     tf::Quaternion q;           
00191     tf::quaternionMsgToTF(q_imu, q);                            
00192     tf::Matrix3x3(q).getRPY(roll, pitch, yaw);  
00193     q_odom =tf::createQuaternionMsgFromRollPitchYaw(roll,pitch, tt);
00194   }     
00195   else if ((!fuse_imu_roll_pitch)&&(!fuse_imu_yaw)) {
00196     q_odom =tf::createQuaternionMsgFromRollPitchYaw(0,0, tt);
00197   }
00198         
00199   odom_msg.pose.pose.orientation=q_odom;
00200                 
00201   odom_msg.child_frame_id = base_frame_id;
00202   odom_msg.twist.twist.linear.x = v;
00203   odom_msg.twist.twist.linear.y = 0;
00204   odom_msg.twist.twist.angular.z = w;
00205 
00206   odom_msg.pose.covariance[0] = pos_cov;
00207   odom_msg.pose.covariance[7] = pos_cov;
00208   odom_msg.pose.covariance[14] = 1e100;
00209   odom_msg.pose.covariance[21] = 1e100;
00210   odom_msg.pose.covariance[28] = 1e100;
00211   odom_msg.pose.covariance[35] = rot_cov;
00212   odom_msg.twist.covariance = odom_msg.pose.covariance;
00213 
00214   odom_pub.publish(odom_msg);
00215 
00216   //Add TF broadcaster
00217   geometry_msgs::TransformStamped odom_trans;
00218   odom_trans.header.stamp = now;
00219   odom_trans.header.frame_id = odom_frame_id;
00220   odom_trans.child_frame_id = base_frame_id;
00221   odom_trans.transform.translation.x = xx;
00222   odom_trans.transform.translation.y = yy;
00223   odom_trans.transform.translation.z = 0.0;
00224   odom_trans.transform.rotation = q_odom;
00225         
00226                         
00227   odom_broadcaster->sendTransform(odom_trans);
00228 
00229                 
00230 
00231         
00232   jointstate_msg.header.stamp = ros::Time::now();
00233 
00234   jointstate_msg.position[0]=r_w*delta_time;
00235   jointstate_msg.velocity[0]=r_w;
00236   jointstate_msg.position[1]=l_w*delta_time;
00237   jointstate_msg.velocity[1]=l_w;
00238   jointstate_msg.position[2]=r_w*delta_time;
00239   jointstate_msg.velocity[2]=r_w;
00240   jointstate_msg.position[3]=l_w*delta_time;
00241   jointstate_msg.velocity[3]=l_w;
00242 
00243   if (have_pan_tilt) {
00244     jointstate_msg.position[5]=pan;
00245     jointstate_msg.position[6]=tilt;
00246   }
00247 
00248         
00249 
00250   pan_tilt_pub.publish(jointstate_msg);
00251 
00252 }
00253 
00254 void gpsCallback(const ric_robot::ric_gps::ConstPtr& msg) {
00255 
00256         
00257   sensor_msgs::NavSatFix gps_msg;
00258   gps_msg.header.frame_id = map_frame_id;
00259   gps_msg.header.stamp = ros::Time::now();
00260   gps_msg.latitude = msg->Lat;
00261   gps_msg.longitude = msg->Lon;
00262   gps_msg.altitude = msg->Alt;
00263 
00264   sensor_msgs::NavSatStatus gps_status_msg;
00265 
00266   gps_status_msg.service = 1; //SERVICE_GPS;
00267   if (msg->Status == 0) {
00268     gps_status_msg.status = 1; //STATUS_NO_FIX;
00269   }
00270   else {//==1
00271     gps_status_msg.status = 0; //STATUS_FIX;
00272     //ROS_INFO("lat:%.7f  lon:%.7f",msg->Lat,msg->Lon  );
00273   }
00274 
00275   gps_msg.status = gps_status_msg;
00276 
00277   gps_msg.position_covariance_type = 1;
00278   //uint8 COVARIANCE_TYPE_UNKNOWN = 0
00279   //uint8 COVARIANCE_TYPE_APPROXIMATED = 1
00280   //uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2
00281   //uint8 COVARIANCE_TYPE_KNOWN = 3
00282   //float64[9] position_covariance;
00283         
00284   float cov =  (msg->HDOP) * (msg->HDOP);  //TODO: CHECK UNITS /100
00285   gps_msg.position_covariance[0] = cov;
00286   gps_msg.position_covariance[4] = cov;
00287   gps_msg.position_covariance[8] = cov;
00288 
00289   gps_pub.publish(gps_msg);
00290   //gps status:
00291   //NO_GPS = 0,             ///< No GPS connected/detected
00292   //  NO_FIX = 1,             ///< Receiving valid GPS messages but no lock
00293   //  GPS_OK = 2              ///< Receiving valid messages and locked
00294 }
00295 
00296 void urfCallback(float left_urf,float rear_urf,float right_urf) {
00297         
00298         
00299   ros::Time now = ros::Time::now();
00300 
00301   left_urf_msg.header.stamp = now;
00302   left_urf_msg.range = left_urf ;
00303 
00304   rear_urf_msg.header.stamp = now;
00305   rear_urf_msg.range = rear_urf ;
00306         
00307   right_urf_msg.header.stamp = now;
00308   right_urf_msg.range = right_urf ;
00309 
00310         
00311   left_urf_pub.publish(left_urf_msg);
00312   rear_urf_pub.publish(rear_urf_msg);
00313   right_urf_pub.publish(right_urf_msg);
00314         
00315 }
00316 
00317 
00318 
00319 void imuCallback(const ric_robot::ric_raw::ConstPtr& msg) {
00320 
00321 
00322   sensor_msgs::Imu imu_msg;
00323         
00324   imu_msg.header.frame_id = imu_frame_id;
00325   imu_msg.header.stamp = ros::Time::now();
00326   q_imu.x=msg->orientation[0];
00327   q_imu.y=msg->orientation[1];
00328   q_imu.z=msg->orientation[2];
00329   q_imu.w=msg->orientation[3];
00330         
00331   double roll = 0, pitch = 0, yaw = 0;                                                  
00332   tf::Quaternion q;                     
00333   tf::quaternionMsgToTF(q_imu, q);                                                      
00334   tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
00335         
00336   q_imu =tf::createQuaternionMsgFromRollPitchYaw(roll,-pitch, -yaw);
00337 
00338   imu_msg.orientation=q_imu;
00339 
00340   imu_msg.linear_acceleration.x=msg->linear_acceleration[0];
00341   imu_msg.linear_acceleration.y=msg->linear_acceleration[1];
00342   imu_msg.linear_acceleration.z=msg->linear_acceleration[2];
00343   
00344   imu_msg.angular_velocity.x=msg->angular_velocity[0];
00345   imu_msg.angular_velocity.y=msg->angular_velocity[1];
00346   imu_msg.angular_velocity.z=msg->angular_velocity[2];
00347 
00348   double angular_velocity_stdev_ = 0.012;
00349   double orientation_stdev_ = 0.035;
00350   double linear_acceleration_stdev_ = 0.098;
00351         
00352   double angular_velocity_covariance = angular_velocity_stdev_
00353     * angular_velocity_stdev_;
00354   double orientation_covariance = orientation_stdev_ * orientation_stdev_;
00355   double linear_acceleration_covariance = linear_acceleration_stdev_
00356     * linear_acceleration_stdev_;
00357 
00358   imu_msg.linear_acceleration_covariance[0] = linear_acceleration_covariance;
00359   imu_msg.linear_acceleration_covariance[4] = linear_acceleration_covariance;
00360   imu_msg.linear_acceleration_covariance[8] = linear_acceleration_covariance;
00361 
00362   imu_msg.angular_velocity_covariance[0] = angular_velocity_covariance;
00363   imu_msg.angular_velocity_covariance[4] = angular_velocity_covariance;
00364   imu_msg.angular_velocity_covariance[8] = angular_velocity_covariance;
00365 
00366   imu_msg.orientation_covariance[0] = orientation_covariance;
00367   imu_msg.orientation_covariance[4] = orientation_covariance;
00368   imu_msg.orientation_covariance[8] = orientation_covariance;
00369   imu_pub.publish(imu_msg);
00370         
00371   /*
00372     geometry_msgs::TransformStamped imu_trans;
00373     imu_trans.header.stamp = ros::Time::now();
00374     imu_trans.header.frame_id = map_frame_id;
00375     imu_trans.child_frame_id = base_frame_id;
00376     imu_trans.transform.rotation=q_imu;
00377     imu_broadcaster->sendTransform(imu_trans);
00378   */
00379 
00380 
00381 }
00382 void rawCallback(const ric_robot::ric_raw::ConstPtr& msg) {
00383 
00384   
00385   imuCallback(msg);
00386 
00387   int32_t left_ticks=msg->encoders[0]; // left_ticks;
00388   int32_t right_ticks=msg->encoders[1]; // right_ticks;
00389   encodersCallback( left_ticks,  right_ticks);
00390 
00391   float left_urf=msg->urf[0];// left_urf;
00392   float rear_urf=msg->urf[1];// rear_urf;
00393   float right_urf=msg->urf[2];// right_urf;
00394   urfCallback(left_urf, rear_urf, right_urf);
00395 
00396 }
00397 
00398 bool set_odom(ric_robot::set_odom::Request  &req, ric_robot::set_odom::Response &res)
00399 {
00400 
00401   xx=req.x;
00402   yy=req.y;
00403   tt=req.theta;
00404   ROS_INFO("Set odometry to: x=%.3f, y=%.3f, theta=%.3f", xx, yy,tt);
00405  
00406   return true;
00407 }
00408 
00409 
00410 void pan_tiltCallback(const ric_robot::ric_pan_tilt::ConstPtr& msg) {
00411 
00412         
00413         
00414   pan=-1.0*msg->pan_angle;
00415   tilt=-1.0*msg->tilt_angle;
00416   if (pan*180/M_PI>35) pan=35*M_PI/180;
00417   else if (pan*180/M_PI<-35) pan=-35*M_PI/180;
00418 
00419   if (tilt*180/M_PI>30) tilt=30*M_PI/180;
00420   else if (tilt*180/M_PI<-30) tilt=-30*M_PI/180;
00421 
00422 
00423 }
00424 int main(int argc, char **argv) {
00425 
00426 
00427   ros::init(argc, argv, "ric_node", ros::init_options::AnonymousName);
00428 
00429   ros::NodeHandle n;
00430 
00431   n.param("map_frame_id", map_frame_id, std::string("/map"));
00432 
00433   std::string pre = ros::this_node::getNamespace();
00434   std::string pre1 = pre.substr (2,pre.length()-2); 
00435 
00436   n.param("odom_frame_id", odom_frame_id, std::string(pre1+"_odom"));
00437   n.param("base_frame_id", base_frame_id, std::string(pre1+"_base_link"));
00438   n.param("imu_frame_id", imu_frame_id, std::string(pre1+"_imu_link"));
00439   n.param("left_urf_frame_id", left_urf_frame_id, std::string(pre1+"_Left_URF_link"));
00440   n.param("rear_urf_frame_id", rear_urf_frame_id, std::string(pre1+"_Rear_URF_link"));
00441   n.param("right_urf_frame_id", right_urf_frame_id, std::string(pre1+"_Right_URF_link"));
00442   n.param("pan_joint_id", pan_joint_id, std::string(pre1+"_Asus_Pan_Joint"));
00443   n.param("tilt_joint_id", tilt_joint_id, std::string(pre1+"_Asus_Tilt_Joint"));
00444   n.param("FR_joint_id", FR_joint_id, std::string(pre1+"_FR_Wheel_joint"));
00445   n.param("FL_joint_id", FL_joint_id, std::string(pre1+"_FL_Wheel_joint"));
00446   n.param("RR_joint_id", RR_joint_id, std::string(pre1+"_RR_Wheel_joint"));
00447   n.param("RL_joint_id", RL_joint_id, std::string(pre1+"_RL_Wheel_joint"));
00448 
00449   n.param("cmd_vel_sub_topic", cmd_vel_topic, std::string("cmd_vel"));
00450   n.param("pan_tilt_sub_topic", pan_tilt_topic, std::string("pan_tilt"));
00451         
00452   n.param("ric_raw_sub_topic", ric_raw_topic, std::string("ric_raw"));
00453   n.param("ric_gps_sub_topic", ric_gps_topic, std::string("raw_gps"));
00454 
00455   n.param("odom_pub_topic", odom_topic, std::string("odom_pub"));
00456   n.param("gps_pub_topic", gps_pub_topic, std::string("gps_pub"));      
00457 
00458   n.param("joint_states_topic", joint_states_topic, std::string("joint_states"));       
00459 
00460   n.param("ric_command_pub_topic", ric_command_topic, std::string("command"));
00461 
00462   n.param("set_odom_srv", set_odom_srv, std::string("set_odom"));
00463 
00464   n.param("wheel_diameter", wheel_diameter, 0.2622);
00465   n.param("wheel_base_length", wheel_base_length, 0.396);
00466   n.param("encoder_cpr", encoder_cpr, 2048.0);
00467   n.param("rotation_covariance", rot_cov, 1.0);
00468   n.param("position_covariance", pos_cov, 1.0);
00469   n.param("fuse_imu_roll_pitch", fuse_imu_roll_pitch, true);
00470   n.param("fuse_imu_yaw", fuse_imu_yaw, false);
00471   n.param("slip_factor", slip_factor, 0.65); 
00472   n.param("have_pan_tilt", have_pan_tilt, false);
00473         
00474   q_imu.x=0;
00475   q_imu.y=0;
00476   q_imu.z=1;
00477   q_imu.w=0;
00478 
00479 
00480   ros::ServiceServer service = n.advertiseService(set_odom_srv, set_odom);
00481                 
00482         
00483   ros::Subscriber raw_sub = n.subscribe(ric_raw_topic, 10, rawCallback);
00484   ros::Subscriber gps_sub = n.subscribe(ric_gps_topic, 10, gpsCallback);
00485   ros::Subscriber vel_sub = n.subscribe(cmd_vel_topic, 5, cmd_velCallback);
00486 
00487   ros::Subscriber pan_tilt_sub = n.subscribe(pan_tilt_topic, 1, pan_tiltCallback);
00488 
00489   imu_pub = n.advertise < sensor_msgs::Imu > ("imu_pub", 10);
00490 
00491 
00492   jointstate_msg.name.push_back(FR_joint_id);
00493   jointstate_msg.position.push_back(0);
00494   jointstate_msg.velocity.push_back(0);
00495   jointstate_msg.name.push_back(FL_joint_id);
00496   jointstate_msg.position.push_back(0);
00497   jointstate_msg.velocity.push_back(0);
00498   jointstate_msg.name.push_back(RR_joint_id);
00499   jointstate_msg.position.push_back(0);
00500   jointstate_msg.velocity.push_back(0);
00501   jointstate_msg.name.push_back(RL_joint_id);
00502   jointstate_msg.position.push_back(0);
00503   jointstate_msg.velocity.push_back(0);
00504 
00505   if (have_pan_tilt) {
00506     jointstate_msg.name.push_back(pan_joint_id);
00507     jointstate_msg.position.push_back(0);
00508     jointstate_msg.velocity.push_back(0);
00509     jointstate_msg.name.push_back(tilt_joint_id);
00510     jointstate_msg.position.push_back(0);
00511     jointstate_msg.velocity.push_back(0);
00512   }
00513 
00514   right_urf_msg.header.frame_id = right_urf_frame_id;
00515   right_urf_msg.field_of_view = 0.7;
00516   right_urf_msg.radiation_type = 0; //us
00517   right_urf_msg.min_range = 0.25;
00518   right_urf_msg.max_range = 6;
00519 
00520   rear_urf_msg.header.frame_id = rear_urf_frame_id;
00521   rear_urf_msg.field_of_view = 0.7;
00522   rear_urf_msg.radiation_type = 0; //us
00523   rear_urf_msg.min_range = 0.25;
00524   rear_urf_msg.max_range = 6;
00525 
00526   left_urf_msg.header.frame_id = left_urf_frame_id;
00527   left_urf_msg.field_of_view = 0.7;
00528   left_urf_msg.radiation_type = 0; //us
00529   left_urf_msg.min_range = 0.25;
00530   left_urf_msg.max_range = 6;
00531 
00532 
00533   right_urf_pub = n.advertise < sensor_msgs::Range > ("Rangers/Right_URF", 5);
00534   rear_urf_pub = n.advertise < sensor_msgs::Range > ("Rangers/Rear_URF", 5);
00535   left_urf_pub = n.advertise < sensor_msgs::Range > ("Rangers/Left_URF", 5);
00536         
00537   odom_pub = n.advertise < nav_msgs::Odometry > (odom_topic, 10);
00538 
00539 
00540   command_pub = n.advertise < ric_robot::ric_command > (ric_command_topic, 5);
00541   pan_tilt_pub = n.advertise < sensor_msgs::JointState > (joint_states_topic, 5); //
00542   wheels_pub = n.advertise < sensor_msgs::JointState > (joint_states_topic, 5); //
00543 
00544   //pan_tilt_pub.publish(jointstate_msg);
00545 
00546   gps_pub = n.advertise < sensor_msgs::NavSatFix > (gps_pub_topic, 5);
00547 
00548 
00549   imu_broadcaster = new tf::TransformBroadcaster;
00550 
00551   odom_broadcaster = new tf::TransformBroadcaster;
00552 
00553 
00554   wd_time=ros::Time::now();
00555   while (n.ok()) {
00556 
00557     /*   if (((ros::Time::now() - wd_time).toSec()>3)&&(!wd_on)) {          
00558          wd_on=true;
00559          ric_robot::ric_command command_msg;            
00560          command_msg.left_wheel=0;
00561          command_msg.right_wheel=0;             
00562          command_pub.publish(command_msg);
00563          }*/
00564     ros::spinOnce();
00565   }
00566                 
00567   //ros::spin();
00568 
00569   return 0;
00570 }
00571 


ric_robot
Author(s): RoboTiCan
autogenerated on Fri May 20 2016 03:50:58