src/wt_node.cpp

   1 #include <stdio.h>
   2 #include <math.h>
   3 #include <unistd.h>
   4 #include <errno.h>
   5 #include <sys/types.h>
   6 #include <sys/stat.h>
   7 #include <fcntl.h>
   8 #include <sys/ioctl.h>
   9 #include <linux/i2c-dev.h>
  10 #include <vector>
  11 #include <ros/ros.h>
  12 #include <tf/transform_broadcaster.h>
  13 #include <dynamic_reconfigure/server.h>
  14 #include <wild_thumper/WildThumperConfig.h>
  15 #include <nav_msgs/Odometry.h>
  16 #include <sensor_msgs/Range.h>
  17 #include <sensor_msgs/BatteryState.h>
  18 #include <sensor_msgs/Imu.h>
  19 #include <diagnostic_msgs/DiagnosticArray.h>
  20 #include <geometry_msgs/Twist.h>
  21 #include <std_srvs/SetBool.h>
  22 #include <diagnostic_msgs/DiagnosticArray.h>
  23 #include <diagnostic_msgs/DiagnosticStatus.h>
  24 #include <boost/format.hpp>
  25 extern "C" {
  26 #include <i2c/smbus.h>
  27 }
  28
  29 #define I2C_FILE "/dev/i2c-2"
  30 #define WHEEL_DIST 0.248
  31 #define BATTERY_CAPACITY_FULL 2.750 // Ah, NiMH=2.750, LiFePO4=3.100
  32 #define UPDATE_RATE 20.0
  33 #define BOOL_TO_S(b) (b ? "True": "False")
  34 #define PTR_TO_FLOAT(x) ({ \
  35         uint32_t tmp = __bswap_32(*reinterpret_cast<uint32_t*>(x));     \
  36         *reinterpret_cast<float*>(&tmp);        \
  37 })
  38
  39
  40 int i2c_write_reg(const uint8_t addr, const uint8_t command, const std::vector<uint8_t> values) {
  41         int ret, file=-1;
  42
  43         if ((file = open(I2C_FILE, O_RDWR)) < 0) {
  44                 perror("open");
  45                 goto error;
  46         }
  47         if (ioctl(file, I2C_SLAVE, (addr>>1)) < 0) {
  48                 perror("ioctl");
  49                 goto error;
  50         }
  51
  52         if ((ret = i2c_smbus_write_i2c_block_data(file, command, values.size(), &values[0])) != 0) {
  53                 perror("i2c_smbus_write_block_data");
  54                 goto error;
  55         }
  56
  57         close(file);
  58         return ret;
  59
  60 error:
  61         if (file > 0) {
  62                 close(file);
  63         }
  64         throw -1;
  65 }
  66
  67 int i2c_read_reg(const uint8_t addr, const uint8_t command, const uint8_t length, uint8_t *values) {
  68         int ret, file=-1;
  69
  70         if ((file = open(I2C_FILE, O_RDWR)) < 0) {
  71                 perror("open");
  72                 goto error;
  73         }
  74         if (ioctl(file, I2C_SLAVE, (addr>>1)) < 0) {
  75                 perror("ioctl");
  76                 goto error;
  77         }
  78
  79         if ((ret = i2c_smbus_read_i2c_block_data(file, command, length, values)) != length) {
  80                 perror("i2c_smbus_read_block_data");
  81                 goto error;
  82         }
  83
  84         close(file);
  85         return ret;
  86
  87 error:
  88         if (file > 0) {
  89                 close(file);
  90         }
  91         throw -1;
  92 }
  93
  94
  95 class WTBase {
  96         public:
  97                 WTBase(ros::NodeHandle nh, ros::NodeHandle pnh) {
  98                         this->nh = nh;
  99                         this->pnh = pnh;
 100                         enable_odom_tf = pnh.param("enable_odom_tf", true);
 101
 102                         dynamic_reconfigure::Server<wild_thumper::WildThumperConfig>::CallbackType f;
 103                         f = boost::bind(&WTBase::execute_dyn_reconf, this, _1, _2);
 104                         dyn_conf.setCallback(f);
 105
 106                         pub_odom = nh.advertise<nav_msgs::Odometry>("odom", 16);
 107                         pub_diag = nh.advertise<diagnostic_msgs::DiagnosticArray>("diagnostics", 16);
 108                         pub_range_fwd_left = nh.advertise<sensor_msgs::Range>("range_forward_left", 16);
 109                         pub_range_fwd_right = nh.advertise<sensor_msgs::Range>("range_forward_right", 16);
 110                         pub_range_bwd = nh.advertise<sensor_msgs::Range>("range_backward", 16);
 111                         pub_battery = nh.advertise<sensor_msgs::BatteryState>("battery", 16);
 112                         set_speed(0, 0);
 113                         i2c_write_reg(0x50, 0x90, {1, 1}); // switch direction
 114                         sub_cmd_vel = nh.subscribe("cmd_vel_out", 10, &WTBase::cmdVelReceived, this);
 115                         sub_imu = nh.subscribe("imu", 10, &WTBase::imuReceived, this);
 116                         srv_manipulator_enable = pnh.advertiseService("manipulator_enable", &WTBase::enableManipulator, this);
 117                         ROS_INFO("Init done");
 118                 }
 119
 120                 void run() {
 121                         ros::Rate loop_rate(UPDATE_RATE);
 122                         sleep(3); // wait 3s for ros to register and establish all subscriber connections before sending reset diag
 123                         uint8_t reset_val = get_reset();
 124                         ROS_INFO("Reset Status: 0x%x", reset_val);
 125                         i2c_write_reg(0x50, 0xA4, {1}); // enable Watchdog
 126                         uint32_t sonar_count = 0;
 127                         while (ros::ok()) {
 128                                 ROS_DEBUG("Loop alive");
 129                                 //print "Watchdog", struct.unpack(">B", i2c_read_reg(0x50, 0xA4, 1))[0]
 130                                 //print struct.unpack(">B", i2c_read_reg(0x50, 0xA2, 1))[0] # count test
 131                                 get_motor_err();
 132                                 get_odom();
 133                                 get_power();
 134
 135                                 if (sonar_count == 0) {
 136                                         get_dist_forward_left();
 137                                         update_dist_backward();
 138                                         sonar_count+=1;
 139                                 } else if (sonar_count == 1) {
 140                                         get_dist_backward();
 141                                         update_dist_forward_right();
 142                                         sonar_count+=1;
 143                                 } else if (sonar_count == 2) {
 144                                         get_dist_forward_right();
 145                                         update_dist_forward_left();
 146                                         sonar_count=0;
 147                                 }
 148
 149                                 if (!cmd_vel.empty()) {
 150                                         set_speed(cmd_vel[0], cmd_vel[1]);
 151                                         cur_vel[0] = cmd_vel[0]; cur_vel[1] = cmd_vel[1];
 152                                         cmd_vel.clear();
 153                                 }
 154
 155                                 check_docked();
 156
 157                                 ros::spinOnce();
 158                                 loop_rate.sleep();
 159                         }
 160                 }
 161
 162                 bool isDocked() {
 163                         return bDocked;
 164                 }
 165
 166                 void setVelocity(float trans, float rot) {
 167                         cmd_vel.resize(2);
 168                         cmd_vel[0] = trans;
 169                         cmd_vel[1] = rot;
 170                 }
 171
 172                 bool enableManipulator(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) {
 173                         ROS_INFO("Set Manipulator: %d", req.data);
 174                         i2c_write_reg(0x52, 0x0f, {req.data});
 175                         res.success = true;
 176                         return true;
 177                 }
 178
 179         private:
 180                 ros::NodeHandle nh;
 181                 ros::NodeHandle pnh;
 182                 tf::TransformBroadcaster tf_broadcaster;
 183                 dynamic_reconfigure::Server<wild_thumper::WildThumperConfig> dyn_conf;
 184                 ros::Publisher pub_odom;
 185                 ros::Publisher pub_diag;
 186                 ros::Publisher pub_range_fwd_left;
 187                 ros::Publisher pub_range_fwd_right;
 188                 ros::Publisher pub_range_bwd;
 189                 ros::Publisher pub_battery;
 190                 std::vector<float> cmd_vel;
 191                 float cur_vel[2] = {0, 0};
 192                 bool bMotorManual = false;
 193                 ros::Subscriber sub_cmd_vel;
 194                 ros::Subscriber sub_imu;
 195                 ros::ServiceServer srv_manipulator_enable;
 196                 bool bDocked = false;
 197                 bool bDocked_last = false;
 198                 double battery_capacity = BATTERY_CAPACITY_FULL*3600; // unit=As
 199                 bool enable_odom_tf;
 200                 bool bClipRangeSensor;
 201                 double range_sensor_max;
 202                 double range_sensor_fov;
 203                 double odom_covar_xy;
 204                 double odom_covar_angle;
 205                 bool rollover_protect;
 206                 double rollover_protect_limit;
 207                 int16_t rollover_protect_pwm;
 208                 bool bStayDocked;
 209
 210                 void execute_dyn_reconf(wild_thumper::WildThumperConfig &config, uint32_t level) {
 211                         bClipRangeSensor = config.range_sensor_clip;
 212                         range_sensor_max = config.range_sensor_max;
 213                         range_sensor_fov = config.range_sensor_fov;
 214                         odom_covar_xy = config.odom_covar_xy;
 215                         odom_covar_angle = config.odom_covar_angle;
 216                         rollover_protect = config.rollover_protect;
 217                         rollover_protect_limit = config.rollover_protect_limit;
 218                         rollover_protect_pwm = config.rollover_protect_pwm;
 219                         bStayDocked = config.stay_docked;
 220                 }
 221
 222                 void imuReceived(const sensor_msgs::Imu::ConstPtr& msg) {
 223                         bool isDriving = fabs(cur_vel[0]) > 0 || fabs(cur_vel[1]) > 0;
 224                         if (rollover_protect && (isDriving || bMotorManual)) {
 225                                 double roll, pitch, yaw;
 226                                 tf::Quaternion quat;
 227                                 tf::quaternionMsgToTF(msg->orientation, quat);
 228                                 tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
 229                                 if (pitch > rollover_protect_limit*M_PI/180) {
 230                                         bMotorManual = true;
 231                                         i2c_write_reg(0x50, 0x1, {0, 0, (uint8_t)(rollover_protect_pwm>>8), (uint8_t)rollover_protect_pwm, 0, 0, (uint8_t)(rollover_protect_pwm>>8), (uint8_t)rollover_protect_pwm});
 232                                         ROS_WARN("Running forward rollver protection");
 233                                 } else if (pitch < -rollover_protect_limit*M_PI/180) {
 234                                         bMotorManual = true;
 235                                         i2c_write_reg(0x50, 0x1, {(uint8_t)(-rollover_protect_pwm>>8), (uint8_t)-rollover_protect_pwm, 0, 0, (uint8_t)(-rollover_protect_pwm>>8), (uint8_t)-rollover_protect_pwm, 0, 0});
 236                                         ROS_WARN("Running backward rollver protection");
 237                                 } else if (bMotorManual) {
 238                                         i2c_write_reg(0x50, 0x1, {0, 0, 0, 0, 0, 0, 0, 0});
 239                                         bMotorManual = false;
 240                                         setVelocity(0, 0);
 241                                         ROS_WARN("Rollver protection done");
 242                                 }
 243                         }
 244                 }
 245
 246                 uint8_t get_reset() {
 247                         uint8_t buf[1];
 248                         i2c_read_reg(0x50, 0xA0, 1, buf);
 249                         uint8_t reset = buf[0];
 250
 251                         diagnostic_msgs::DiagnosticArray msg;
 252                         msg.header.stamp = ros::Time::now();
 253                         diagnostic_msgs::DiagnosticStatus stat;
 254                         stat.name = "Reset reason";
 255                         stat.level = reset & 0x0c ? diagnostic_msgs::DiagnosticStatus::ERROR : diagnostic_msgs::DiagnosticStatus::OK;
 256                         stat.message = str(boost::format("0x%02x") % reset);
 257
 258                         bool wdrf = reset & (1 << 3);
 259                         if (wdrf) ROS_INFO("Watchdog Reset");
 260                         bool borf = reset & (1 << 2);
 261                         if (borf) ROS_INFO("Brown-out Reset Flag");
 262                         bool extrf = reset & (1 << 1);
 263                         if (extrf) ROS_INFO("External Reset Flag");
 264                         bool porf = reset & (1 << 0);
 265                         if (porf) ROS_INFO("Power-on Reset Flag");
 266
 267                         diagnostic_msgs::KeyValue kvWdrf, kvBorf, kvExtrf, kvPorf;
 268                         kvWdrf.key = "Watchdog Reset Flag";
 269                         kvWdrf.value = BOOL_TO_S(wdrf);
 270                         stat.values.push_back(kvWdrf);
 271                         kvBorf.key = "Brown-out Reset Flag";
 272                         kvBorf.value = BOOL_TO_S(borf);
 273                         stat.values.push_back(kvBorf);
 274                         kvExtrf.key = "External Reset Flag";
 275                         kvExtrf.value = BOOL_TO_S(extrf);
 276                         stat.values.push_back(kvExtrf);
 277                         kvPorf.key = "Power-on Reset Flag";
 278                         kvPorf.value = BOOL_TO_S(porf);
 279                         stat.values.push_back(kvPorf);
 280
 281                         msg.status.push_back(stat);
 282                         pub_diag.publish(msg);
 283                         return reset;
 284                 }
 285
 286                 void get_motor_err() {
 287                         uint8_t buf[1];
 288                         i2c_read_reg(0x50, 0xA1, 1, buf);
 289                         uint8_t err = buf[0];
 290
 291                         diagnostic_msgs::DiagnosticArray msg;
 292                         msg.header.stamp = ros::Time::now();
 293                         diagnostic_msgs::DiagnosticStatus stat;
 294                         stat.name = "Motor: Error Status";
 295                         stat.level = err ? diagnostic_msgs::DiagnosticStatus::ERROR : diagnostic_msgs::DiagnosticStatus::OK;
 296                         stat.message = str(boost::format("0x%02x") % err);
 297
 298                         // Diag
 299                         diagnostic_msgs::KeyValue kvAftLeft, kvFrontLeft, kvAftRight, kvFrontRight;
 300                         kvAftLeft.key = "aft left diag";
 301                         kvAftLeft.value = BOOL_TO_S((bool)(err & (1 << 0)));
 302                         stat.values.push_back(kvAftLeft);
 303                         kvFrontLeft.key = "front left diag";
 304                         kvFrontLeft.value = BOOL_TO_S((bool)(err & (1 << 1)));
 305                         stat.values.push_back(kvFrontLeft);
 306                         kvAftRight.key = "aft right diag";
 307                         kvAftRight.value = BOOL_TO_S((bool)(err & (1 << 2)));
 308                         stat.values.push_back(kvAftRight);
 309                         kvFrontRight.key = "front right diag";
 310                         kvFrontRight.value = BOOL_TO_S((bool)(err & (1 << 3)));
 311                         stat.values.push_back(kvFrontRight);
 312                         // Stall
 313                         diagnostic_msgs::KeyValue kvAftLeft2, kvFrontLeft2, kvAftRight2, kvFrontRight2;
 314                         kvAftLeft2.key = "aft left stall";
 315                         kvAftLeft2.value = BOOL_TO_S((bool)(err & (1 << 4)));
 316                         stat.values.push_back(kvAftLeft2);
 317                         kvFrontLeft2.key = "front left stall";
 318                         kvFrontLeft2.value = BOOL_TO_S((bool)(err & (1 << 5)));
 319                         stat.values.push_back(kvFrontLeft2);
 320                         kvAftRight2.key = "aft right stall";
 321                         kvAftRight2.value = BOOL_TO_S((bool)(err & (1 << 6)));
 322                         stat.values.push_back(kvAftRight2);
 323                         kvFrontRight2.key = "front right stall";
 324                         kvFrontRight2.value = BOOL_TO_S((bool)(err & (1 << 7)));
 325                         stat.values.push_back(kvFrontRight2);
 326
 327                         msg.status.push_back(stat);
 328                         pub_diag.publish(msg);
 329                 }
 330
 331                 void get_power() {
 332                         uint8_t buf[2];
 333                         i2c_read_reg(0x52, 0x09, 2, buf);
 334                         double volt = __bswap_16(*(int16_t*)buf)/100.0;
 335                         i2c_read_reg(0x52, 0x0D, 2, buf);
 336                         double current = __bswap_16(*(int16_t*)buf)/1000.0;
 337
 338                         diagnostic_msgs::DiagnosticArray msg;
 339                         msg.header.stamp = ros::Time::now();
 340                         diagnostic_msgs::DiagnosticStatus stat;
 341                         stat.name = "Power";
 342                         stat.level = volt < 7 || current > 5 ? diagnostic_msgs::DiagnosticStatus::ERROR : diagnostic_msgs::DiagnosticStatus::OK;
 343                         stat.message = str(boost::format("%.2fV, %.2fA") % volt % current);
 344
 345                         msg.status.push_back(stat);
 346                         pub_diag.publish(msg);
 347
 348                         if (volt < 7) {
 349                                 ROS_ERROR_THROTTLE(10, "Voltage critical: %.2fV", volt);
 350                         }
 351
 352                         bDocked = volt > 10.1;
 353                         update_capacity(volt, current);
 354
 355                         if (pub_battery.getNumSubscribers()) {
 356                                 sensor_msgs::BatteryState batmsg;
 357                                 batmsg.header.stamp = ros::Time::now();
 358                                 batmsg.voltage = volt;
 359                                 batmsg.current = current;
 360                                 batmsg.charge = battery_capacity/3600.0;
 361                                 batmsg.capacity = NAN;
 362                                 batmsg.design_capacity = BATTERY_CAPACITY_FULL;
 363                                 batmsg.percentage = batmsg.charge/batmsg.design_capacity;
 364                                 batmsg.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
 365                                 batmsg.power_supply_health = sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN;
 366                                 batmsg.power_supply_technology = sensor_msgs::BatteryState::POWER_SUPPLY_TECHNOLOGY_NIMH;
 367                                 batmsg.present = true;
 368                                 pub_battery.publish(batmsg);
 369                         }
 370                 }
 371
 372                 void update_capacity(double volt, double current) {
 373                         if (bDocked)
 374                                 battery_capacity = BATTERY_CAPACITY_FULL*3600;
 375                         else
 376                                 battery_capacity -= current/UPDATE_RATE;
 377                 }
 378
 379                 void get_odom() {
 380                         uint8_t buf[20];
 381                         i2c_read_reg(0x50, 0x38, 20, buf);
 382                         float speed_trans = PTR_TO_FLOAT(buf+0);
 383                         float speed_rot = PTR_TO_FLOAT(buf+4);
 384                         float posx = PTR_TO_FLOAT(buf+8);
 385                         float posy = PTR_TO_FLOAT(buf+12);
 386                         float angle = PTR_TO_FLOAT(buf+16);
 387                         ros::Time current_time = ros::Time::now();
 388
 389                         // since all odometry is 6DOF we'll need a quaternion created from yaw
 390                         geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(angle);
 391
 392                         // first, we'll publish the transform over tf
 393                         if (enable_odom_tf) {
 394                                 geometry_msgs::TransformStamped odom_trans;
 395                                 odom_trans.header.stamp = current_time;
 396                                 odom_trans.header.frame_id = "odom";
 397                                 odom_trans.child_frame_id = "base_footprint";
 398                                 odom_trans.transform.translation.x = posx;
 399                                 odom_trans.transform.translation.y = posy;
 400                                 odom_trans.transform.translation.z = 0.0;
 401                                 odom_trans.transform.rotation = odom_quat;
 402                                 tf_broadcaster.sendTransform(odom_trans);
 403                         }
 404
 405                         // next, we'll publish the odometry message over ROS
 406                         nav_msgs::Odometry odom;
 407                         odom.header.stamp = current_time;
 408                         odom.header.frame_id = "odom";
 409
 410                         // set the position
 411                         odom.pose.pose.position.x = posx;
 412                         odom.pose.pose.position.y = posy;
 413                         odom.pose.pose.position.z = 0.0;
 414                         odom.pose.pose.orientation = odom_quat;
 415                         odom.pose.covariance[0] = odom_covar_xy; // x
 416                         odom.pose.covariance[7] = odom_covar_xy; // y
 417                         odom.pose.covariance[14] = odom_covar_xy; // z
 418                         odom.pose.covariance[21] = odom_covar_angle; // rotation about X axis
 419                         odom.pose.covariance[28] = odom_covar_angle; // rotation about Y axis
 420                         odom.pose.covariance[35] = odom_covar_angle; // rotation about Z axis
 421
 422                         // set the velocity
 423                         odom.child_frame_id = "base_footprint";
 424                         odom.twist.twist.linear.x = speed_trans;
 425                         odom.twist.twist.linear.y = 0.0;
 426                         odom.twist.twist.linear.z = 0.0;
 427                         odom.twist.twist.angular.z = speed_rot;
 428                         odom.twist.covariance[0] = odom_covar_xy; // x
 429                         odom.twist.covariance[7] = odom_covar_xy; // y
 430                         odom.twist.covariance[14] = odom_covar_xy; // z
 431                         odom.twist.covariance[21] = odom_covar_angle; // rotation about X axis
 432                         odom.twist.covariance[28] = odom_covar_angle; // rotation about Y axis
 433                         odom.twist.covariance[35] = odom_covar_angle; // rotation about Z axis
 434
 435                         // publish the message
 436                         pub_odom.publish(odom);
 437                 }
 438
 439                 void set_speed(float trans, float rot) {
 440                         std::vector<uint8_t> buf;
 441                         buf.push_back((*(uint32_t*)&trans)>>24);
 442                         buf.push_back((*(uint32_t*)&trans)>>16);
 443                         buf.push_back((*(uint32_t*)&trans)>>8);
 444                         buf.push_back((*(uint32_t*)&trans)>>0);
 445                         buf.push_back((*(uint32_t*)&rot)>>24);
 446                         buf.push_back((*(uint32_t*)&rot)>>16);
 447                         buf.push_back((*(uint32_t*)&rot)>>8);
 448                         buf.push_back((*(uint32_t*)&rot)>>0);
 449                         i2c_write_reg(0x50, 0x50, buf);
 450                 }
 451
 452                 void cmdVelReceived(const geometry_msgs::Twist::ConstPtr& msg) {
 453                         if (!bMotorManual) {
 454                                 ROS_DEBUG("Set new cmd_vel: %.2f %.2f", msg->linear.x, msg->angular.z);
 455                                 setVelocity(msg->linear.x, msg->angular.z); // commit speed on next update cycle
 456                                 ROS_DEBUG("Set new cmd_vel done");
 457                         }
 458                 }
 459
 460                 void start_dist_srf(uint8_t num) {
 461                         i2c_write_reg(0x52, num, {});
 462                 }
 463
 464                 float read_dist_srf(uint8_t num) {
 465                         uint8_t buf[2];
 466                         i2c_read_reg(0x52, num, 2, buf);
 467                         return __bswap_16(*(uint16_t*)buf)/1000.0;
 468                 }
 469
 470                 void send_range(ros::Publisher &pub, std::string frame_id, uint8_t typ, double dist, double min_range, double max_range, double fov_deg) {
 471                         if (bClipRangeSensor && dist > max_range) {
 472                                 dist = max_range;
 473                         }
 474                         sensor_msgs::Range msg;
 475                         msg.header.stamp = ros::Time::now();
 476                         msg.header.frame_id = frame_id;
 477                         msg.radiation_type = typ;
 478                         msg.field_of_view = fov_deg*M_PI/180;
 479                         msg.min_range = min_range;
 480                         msg.max_range = max_range;
 481                         msg.range = dist;
 482                         pub.publish(msg);
 483                 }
 484
 485                 void get_dist_forward_left() {
 486                         if (pub_range_fwd_left.getNumSubscribers() > 0) {
 487                                 float dist = read_dist_srf(0x15);
 488                                 send_range(pub_range_fwd_left, "sonar_forward_left", sensor_msgs::Range::ULTRASOUND, dist, 0.04, range_sensor_max, range_sensor_fov);
 489                         }
 490                 }
 491
 492                 void update_dist_forward_left() {
 493                         if (pub_range_fwd_left.getNumSubscribers() > 0) {
 494                                 start_dist_srf(0x5);
 495                         }
 496                 }
 497
 498                 void get_dist_backward() {
 499                         if (pub_range_bwd.getNumSubscribers() > 0) {
 500                                 float dist = read_dist_srf(0x17);
 501                                 send_range(pub_range_bwd, "sonar_backward", sensor_msgs::Range::ULTRASOUND, dist, 0.04, range_sensor_max, range_sensor_fov);
 502                         }
 503                 }
 504
 505                 void update_dist_backward() {
 506                         if (pub_range_bwd.getNumSubscribers() > 0) {
 507                                 start_dist_srf(0x7);
 508                         }
 509                 }
 510
 511                 void get_dist_forward_right() {
 512                         if (pub_range_fwd_right.getNumSubscribers() > 0) {
 513                                 float dist = read_dist_srf(0x19);
 514                                 send_range(pub_range_fwd_right, "sonar_forward_right", sensor_msgs::Range::ULTRASOUND, dist, 0.04, range_sensor_max, range_sensor_fov);
 515                         }
 516                 }
 517
 518                 void update_dist_forward_right() {
 519                         if (pub_range_fwd_right.getNumSubscribers() > 0) {
 520                                 start_dist_srf(0xb);
 521                         }
 522                 }
 523
 524                 void check_docked() {
 525                         if (bDocked && !bDocked_last) {
 526                                 ROS_INFO("Docking event");
 527                         } else if (!bDocked && bDocked_last) {
 528                                 bool stopped = cur_vel[0] == 0 && cur_vel[1] == 0;
 529                                 if (!bStayDocked || !stopped) {
 530                                         ROS_INFO("Undocking event");
 531                                 } else {
 532                                         ROS_INFO("Undocking event..redocking");
 533                                         pthread_t tid;
 534                                         pthread_create(&tid, NULL, &WTBase::redock, this);
 535                                 }
 536                         }
 537
 538                         bDocked_last = bDocked;
 539                 }
 540
 541                 static void *redock(void *context) {
 542                         WTBase *This = (WTBase *)context;
 543                         This->setVelocity(-0.1, 0);
 544                         for (int i=0; i<100; i++) {
 545                                 if (This->isDocked())
 546                                         break;
 547                                 usleep(10*1000);
 548                         }
 549                         This->setVelocity(0, 0);
 550                         if (This->isDocked()) {
 551                                 ROS_INFO("Redocking done");
 552                         } else {
 553                                 ROS_ERROR("Redocking failed");
 554                         }
 555
 556                         pthread_exit((void *) 0);
 557                 }
 558 };
 559
 560 int main(int argc, char **argv) {
 561         ros::init(argc, argv, "wild_thumper");
 562         ros::NodeHandle nh;
 563         ros::NodeHandle pnh("~");
 564         WTBase robot(nh, pnh);
 565         robot.run();
 566         return 0;
 567 }