asr_vosk: Allow to handle keyword and command in one sentence
[ros_wild_thumper.git] / 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 }