convert base node to c++
authorErik Andresen <erik@vontaene.de>
Mon, 3 Aug 2020 10:08:30 +0000 (12:08 +0200)
committerErik Andresen <erik@vontaene.de>
Mon, 3 Aug 2020 10:08:30 +0000 (12:08 +0200)
CMakeLists.txt
launch/wild_thumper.launch
src/wt_node.cpp [new file with mode: 0644]

index 5dfe0e2..965b4ee 100644 (file)
@@ -4,7 +4,7 @@ project(wild_thumper)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED sensor_msgs cv_bridge roscpp rospy std_msgs image_transport dynamic_reconfigure message_generation rosruby)
+find_package(catkin REQUIRED sensor_msgs cv_bridge roscpp rospy std_msgs image_transport dynamic_reconfigure message_generation rosruby tf)
 find_package(OpenCV)
 
 ## System dependencies are found with CMake's conventions
@@ -109,6 +109,7 @@ include_directories(
 
 ## Declare a cpp executable
 add_executable(path_following src/path_following.cpp)
+add_executable(wt_node src/wt_node.cpp)
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
@@ -119,6 +120,10 @@ target_link_libraries(path_following
    ${catkin_LIBRARIES}
    ${OpenCV_LIBS}
 )
+target_link_libraries(wt_node
+   ${catkin_LIBRARIES}
+   i2c
+)
 
 #############
 ## Install ##
index 1e1bf66..c721dcc 100644 (file)
@@ -7,22 +7,22 @@
        <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
 
        <!--
-       <param name="robot_description" command="$(find xacro)/xacro.py $(find wild_thumper)/urdf/wild_thumper.urdf.xacro"/>
+       <param name="robot_description" command="$(find xacro)/xacro $(find wild_thumper)/urdf/wild_thumper.urdf.xacro"/>
        -->
-       <param name="robot_description" command="$(find xacro)/xacro.py $(find wt_open_manipulator)/urdf/wild_thumper_with_manipulator.urdf.xacro use_nominal_extrinsics:=false"/>
+       <param name="robot_description" command="$(find xacro)/xacro $(find wt_open_manipulator)/urdf/wild_thumper_with_manipulator.urdf.xacro use_nominal_extrinsics:=false"/>
 
        <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
                <param name="publish_frequency" value="20.0" />
        </node>
        <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen">
-               <param name="robot_description" command="$(find xacro)/xacro.py $(find wild_thumper)/urdf/wild_thumper.urdf.xacro" />
+               <param name="robot_description" command="$(find xacro)/xacro $(find wild_thumper)/urdf/wild_thumper.urdf.xacro" />
        </node>
 
        <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator" output="screen">
                <rosparam command="load" file="$(find wild_thumper)/config/analyzers.yaml"/>
        </node>
 
-       <node pkg="wild_thumper" type="wt_node.py" name="wild_thumper" output="screen" respawn="false" required="true">
+       <node pkg="wild_thumper" type="wt_node" name="wild_thumper" output="screen" respawn="false" required="true">
                <param name="enable_odom_tf" value="true" unless="$(arg use_imu)"/>
                <param name="enable_odom_tf" value="false" if="$(arg use_imu)"/>
        </node>
@@ -35,6 +35,7 @@
                     Magnometer heading accuracy is +-2.5 deg => 0.088 rad
                     With heading accuracy as std dev, variance = 0.088^2 = 0.008 -->
                <param name="variance_orientation" value="0.008"/>
+               <param name="disable_leds" value="true"/>
                <remap from="/tinkerforge_imu_brick2/imu" to="imu"/>
        </node>
 
diff --git a/src/wt_node.cpp b/src/wt_node.cpp
new file mode 100644 (file)
index 0000000..e8303a6
--- /dev/null
@@ -0,0 +1,567 @@
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <errno.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <linux/i2c-dev.h>
+#include <vector>
+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+#include <dynamic_reconfigure/server.h>
+#include <wild_thumper/WildThumperConfig.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/Range.h>
+#include <sensor_msgs/BatteryState.h>
+#include <sensor_msgs/Imu.h>
+#include <diagnostic_msgs/DiagnosticArray.h>
+#include <geometry_msgs/Twist.h>
+#include <std_srvs/SetBool.h>
+#include <diagnostic_msgs/DiagnosticArray.h>
+#include <diagnostic_msgs/DiagnosticStatus.h>
+#include <boost/format.hpp>
+extern "C" {
+#include <i2c/smbus.h>
+}
+
+#define I2C_FILE "/dev/i2c-2"
+#define WHEEL_DIST 0.248
+#define BATTERY_CAPACITY_FULL 2.750 // Ah, NiMH=2.750, LiFePO4=3.100
+#define UPDATE_RATE 20.0
+#define BOOL_TO_S(b) (b ? "True": "False")
+#define PTR_TO_FLOAT(x) ({ \
+       uint32_t tmp = __bswap_32(*reinterpret_cast<uint32_t*>(x));     \
+       *reinterpret_cast<float*>(&tmp);        \
+})
+
+
+int i2c_write_reg(const uint8_t addr, const uint8_t command, const std::vector<uint8_t> values) {
+       int ret, file=-1;
+
+       if ((file = open(I2C_FILE, O_RDWR)) < 0) {
+               perror("open");
+               goto error;
+       }
+       if (ioctl(file, I2C_SLAVE, (addr>>1)) < 0) {
+               perror("ioctl");
+               goto error;
+       }
+
+       if ((ret = i2c_smbus_write_i2c_block_data(file, command, values.size(), &values[0])) != 0) {
+               perror("i2c_smbus_write_block_data");
+               goto error;
+       }
+
+       close(file);
+       return ret;
+
+error:
+       if (file > 0) {
+               close(file);
+       }
+       throw -1;
+}
+
+int i2c_read_reg(const uint8_t addr, const uint8_t command, const uint8_t length, uint8_t *values) {
+       int ret, file=-1;
+
+       if ((file = open(I2C_FILE, O_RDWR)) < 0) {
+               perror("open");
+               goto error;
+       }
+       if (ioctl(file, I2C_SLAVE, (addr>>1)) < 0) {
+               perror("ioctl");
+               goto error;
+       }
+
+       if ((ret = i2c_smbus_read_i2c_block_data(file, command, length, values)) != length) {
+               perror("i2c_smbus_read_block_data");
+               goto error;
+       }
+
+       close(file);
+       return ret;
+
+error:
+       if (file > 0) {
+               close(file);
+       }
+       throw -1;
+}
+
+
+class WTBase {
+       public:
+               WTBase(ros::NodeHandle nh, ros::NodeHandle pnh) {
+                       this->nh = nh;
+                       this->pnh = pnh;
+                       enable_odom_tf = pnh.param("enable_odom_tf", true);
+
+                       dynamic_reconfigure::Server<wild_thumper::WildThumperConfig>::CallbackType f;
+                       f = boost::bind(&WTBase::execute_dyn_reconf, this, _1, _2);
+                       dyn_conf.setCallback(f);
+
+                       pub_odom = nh.advertise<nav_msgs::Odometry>("odom", 16);
+                       pub_diag = nh.advertise<diagnostic_msgs::DiagnosticArray>("diagnostics", 16);
+                       pub_range_fwd_left = nh.advertise<sensor_msgs::Range>("range_forward_left", 16);
+                       pub_range_fwd_right = nh.advertise<sensor_msgs::Range>("range_forward_right", 16);
+                       pub_range_bwd = nh.advertise<sensor_msgs::Range>("range_backward", 16);
+                       pub_battery = nh.advertise<sensor_msgs::BatteryState>("battery", 16);
+                       set_speed(0, 0);
+                       i2c_write_reg(0x50, 0x90, {1, 1}); // switch direction
+                       sub_cmd_vel = nh.subscribe("cmd_vel_out", 10, &WTBase::cmdVelReceived, this);
+                       sub_imu = nh.subscribe("imu", 10, &WTBase::imuReceived, this);
+                       srv_manipulator_enable = pnh.advertiseService("manipulator_enable", &WTBase::enableManipulator, this);
+                       ROS_INFO("Init done");
+               }
+
+               void run() {
+                       ros::Rate loop_rate(UPDATE_RATE);
+                       sleep(3); // wait 3s for ros to register and establish all subscriber connections before sending reset diag
+                       uint8_t reset_val = get_reset();
+                       ROS_INFO("Reset Status: 0x%x", reset_val);
+                       i2c_write_reg(0x50, 0xA4, {1}); // enable Watchdog
+                       uint32_t sonar_count = 0;
+                       while (ros::ok()) {
+                               ROS_DEBUG("Loop alive");
+                               //print "Watchdog", struct.unpack(">B", i2c_read_reg(0x50, 0xA4, 1))[0]
+                               //print struct.unpack(">B", i2c_read_reg(0x50, 0xA2, 1))[0] # count test
+                               get_motor_err();
+                               get_odom();
+                               get_power();
+
+                               if (sonar_count == 0) {
+                                       get_dist_forward_left();
+                                       update_dist_backward();
+                                       sonar_count+=1;
+                               } else if (sonar_count == 1) {
+                                       get_dist_backward();
+                                       update_dist_forward_right();
+                                       sonar_count+=1;
+                               } else if (sonar_count == 2) {
+                                       get_dist_forward_right();
+                                       update_dist_forward_left();
+                                       sonar_count=0;
+                               }
+
+                               if (!cmd_vel.empty()) {
+                                       set_speed(cmd_vel[0], cmd_vel[1]);
+                                       cur_vel[0] = cmd_vel[0]; cur_vel[1] = cmd_vel[1];
+                                       cmd_vel.clear();
+                               }
+
+                               check_docked();
+
+                               ros::spinOnce();
+                               loop_rate.sleep();
+                       }
+               }
+
+               bool isDocked() {
+                       return bDocked;
+               }
+
+               void setVelocity(float trans, float rot) {
+                       cmd_vel.resize(2);
+                       cmd_vel[0] = trans;
+                       cmd_vel[1] = rot;
+               }
+
+               bool enableManipulator(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) {
+                       ROS_INFO("Set Manipulator: %d", req.data);
+                       i2c_write_reg(0x52, 0x0f, {req.data});
+                       res.success = true;
+                       return true;
+               }
+
+       private:
+               ros::NodeHandle nh;
+               ros::NodeHandle pnh;
+               tf::TransformBroadcaster tf_broadcaster;
+               dynamic_reconfigure::Server<wild_thumper::WildThumperConfig> dyn_conf;
+               ros::Publisher pub_odom;
+               ros::Publisher pub_diag;
+               ros::Publisher pub_range_fwd_left;
+               ros::Publisher pub_range_fwd_right;
+               ros::Publisher pub_range_bwd;
+               ros::Publisher pub_battery;
+               std::vector<float> cmd_vel;
+               float cur_vel[2] = {0, 0};
+               bool bMotorManual = false;
+               ros::Subscriber sub_cmd_vel;
+               ros::Subscriber sub_imu;
+               ros::ServiceServer srv_manipulator_enable;
+               bool bDocked = false;
+               bool bDocked_last = false;
+               double battery_capacity = BATTERY_CAPACITY_FULL*3600; // unit=As
+               bool enable_odom_tf;
+               bool bClipRangeSensor;
+               double range_sensor_max;
+               double range_sensor_fov;
+               double odom_covar_xy;
+               double odom_covar_angle;
+               bool rollover_protect;
+               double rollover_protect_limit;
+               int16_t rollover_protect_pwm;
+               bool bStayDocked;
+
+               void execute_dyn_reconf(wild_thumper::WildThumperConfig &config, uint32_t level) {
+                       bClipRangeSensor = config.range_sensor_clip;
+                       range_sensor_max = config.range_sensor_max;
+                       range_sensor_fov = config.range_sensor_fov;
+                       odom_covar_xy = config.odom_covar_xy;
+                       odom_covar_angle = config.odom_covar_angle;
+                       rollover_protect = config.rollover_protect;
+                       rollover_protect_limit = config.rollover_protect_limit;
+                       rollover_protect_pwm = config.rollover_protect_pwm;
+                       bStayDocked = config.stay_docked;
+               }
+
+               void imuReceived(const sensor_msgs::Imu::ConstPtr& msg) {
+                       bool isDriving = fabs(cur_vel[0]) > 0 || fabs(cur_vel[1]) > 0;
+                       if (rollover_protect && (isDriving || bMotorManual)) {
+                               double roll, pitch, yaw;
+                               tf::Quaternion quat;
+                               tf::quaternionMsgToTF(msg->orientation, quat);
+                               tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
+                               if (pitch > rollover_protect_limit*M_PI/180) {
+                                       bMotorManual = true;
+                                       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});
+                                       ROS_WARN("Running forward rollver protection");
+                               } else if (pitch < -rollover_protect_limit*M_PI/180) {
+                                       bMotorManual = true;
+                                       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});
+                                       ROS_WARN("Running backward rollver protection");
+                               } else if (bMotorManual) {
+                                       i2c_write_reg(0x50, 0x1, {0, 0, 0, 0, 0, 0, 0, 0});
+                                       bMotorManual = false;
+                                       setVelocity(0, 0);
+                                       ROS_WARN("Rollver protection done");
+                               }
+                       }
+               }
+
+               uint8_t get_reset() {
+                       uint8_t buf[1];
+                       i2c_read_reg(0x50, 0xA0, 1, buf);
+                       uint8_t reset = buf[0];
+
+                       diagnostic_msgs::DiagnosticArray msg;
+                       msg.header.stamp = ros::Time::now();
+                       diagnostic_msgs::DiagnosticStatus stat;
+                       stat.name = "Reset reason";
+                       stat.level = reset & 0x0c ? diagnostic_msgs::DiagnosticStatus::ERROR : diagnostic_msgs::DiagnosticStatus::OK;
+                       stat.message = str(boost::format("0x%02x") % reset);
+
+                       bool wdrf = reset & (1 << 3);
+                       if (wdrf) ROS_INFO("Watchdog Reset");
+                       bool borf = reset & (1 << 2);
+                       if (borf) ROS_INFO("Brown-out Reset Flag");
+                       bool extrf = reset & (1 << 1);
+                       if (extrf) ROS_INFO("External Reset Flag");
+                       bool porf = reset & (1 << 0);
+                       if (porf) ROS_INFO("Power-on Reset Flag");
+
+                       diagnostic_msgs::KeyValue kvWdrf, kvBorf, kvExtrf, kvPorf;
+                       kvWdrf.key = "Watchdog Reset Flag";
+                       kvWdrf.value = BOOL_TO_S(wdrf);
+                       stat.values.push_back(kvWdrf);
+                       kvBorf.key = "Brown-out Reset Flag";
+                       kvBorf.value = BOOL_TO_S(borf);
+                       stat.values.push_back(kvBorf);
+                       kvExtrf.key = "External Reset Flag";
+                       kvExtrf.value = BOOL_TO_S(extrf);
+                       stat.values.push_back(kvExtrf);
+                       kvPorf.key = "Power-on Reset Flag";
+                       kvPorf.value = BOOL_TO_S(porf);
+                       stat.values.push_back(kvPorf);
+
+                       msg.status.push_back(stat);
+                       pub_diag.publish(msg);
+                       return reset;
+               }
+
+               void get_motor_err() {
+                       uint8_t buf[1];
+                       i2c_read_reg(0x50, 0xA1, 1, buf);
+                       uint8_t err = buf[0];
+
+                       diagnostic_msgs::DiagnosticArray msg;
+                       msg.header.stamp = ros::Time::now();
+                       diagnostic_msgs::DiagnosticStatus stat;
+                       stat.name = "Motor: Error Status";
+                       stat.level = err ? diagnostic_msgs::DiagnosticStatus::ERROR : diagnostic_msgs::DiagnosticStatus::OK;
+                       stat.message = str(boost::format("0x%02x") % err);
+
+                       // Diag
+                       diagnostic_msgs::KeyValue kvAftLeft, kvFrontLeft, kvAftRight, kvFrontRight;
+                       kvAftLeft.key = "aft left diag";
+                       kvAftLeft.value = BOOL_TO_S((bool)(err & (1 << 0)));
+                       stat.values.push_back(kvAftLeft);
+                       kvFrontLeft.key = "front left diag";
+                       kvFrontLeft.value = BOOL_TO_S((bool)(err & (1 << 1)));
+                       stat.values.push_back(kvFrontLeft);
+                       kvAftRight.key = "aft right diag";
+                       kvAftRight.value = BOOL_TO_S((bool)(err & (1 << 2)));
+                       stat.values.push_back(kvAftRight);
+                       kvFrontRight.key = "front right diag";
+                       kvFrontRight.value = BOOL_TO_S((bool)(err & (1 << 3)));
+                       stat.values.push_back(kvFrontRight);
+                       // Stall
+                       diagnostic_msgs::KeyValue kvAftLeft2, kvFrontLeft2, kvAftRight2, kvFrontRight2;
+                       kvAftLeft2.key = "aft left stall";
+                       kvAftLeft2.value = BOOL_TO_S((bool)(err & (1 << 4)));
+                       stat.values.push_back(kvAftLeft2);
+                       kvFrontLeft2.key = "front left stall";
+                       kvFrontLeft2.value = BOOL_TO_S((bool)(err & (1 << 5)));
+                       stat.values.push_back(kvFrontLeft2);
+                       kvAftRight2.key = "aft right stall";
+                       kvAftRight2.value = BOOL_TO_S((bool)(err & (1 << 6)));
+                       stat.values.push_back(kvAftRight2);
+                       kvFrontRight2.key = "front right stall";
+                       kvFrontRight2.value = BOOL_TO_S((bool)(err & (1 << 7)));
+                       stat.values.push_back(kvFrontRight2);
+
+                       msg.status.push_back(stat);
+                       pub_diag.publish(msg);
+               }
+
+               void get_power() {
+                       uint8_t buf[2];
+                       i2c_read_reg(0x52, 0x09, 2, buf);
+                       double volt = __bswap_16(*(int16_t*)buf)/100.0;
+                       i2c_read_reg(0x52, 0x0D, 2, buf);
+                       double current = __bswap_16(*(int16_t*)buf)/1000.0;
+
+                       diagnostic_msgs::DiagnosticArray msg;
+                       msg.header.stamp = ros::Time::now();
+                       diagnostic_msgs::DiagnosticStatus stat;
+                       stat.name = "Power";
+                       stat.level = volt < 7 || current > 5 ? diagnostic_msgs::DiagnosticStatus::ERROR : diagnostic_msgs::DiagnosticStatus::OK;
+                       stat.message = str(boost::format("%.2fV, %.2fA") % volt % current);
+
+                       msg.status.push_back(stat);
+                       pub_diag.publish(msg);
+
+                       if (volt < 7) {
+                               ROS_ERROR_THROTTLE(10, "Voltage critical: %.2fV", volt);
+                       }
+
+                       bDocked = volt > 10.1;
+                       update_capacity(volt, current);
+
+                       if (pub_battery.getNumSubscribers()) {
+                               sensor_msgs::BatteryState batmsg;
+                               batmsg.header.stamp = ros::Time::now();
+                               batmsg.voltage = volt;
+                               batmsg.current = current;
+                               batmsg.charge = battery_capacity/3600.0;
+                               batmsg.capacity = NAN;
+                               batmsg.design_capacity = BATTERY_CAPACITY_FULL;
+                               batmsg.percentage = batmsg.charge/batmsg.design_capacity;
+                               batmsg.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
+                               batmsg.power_supply_health = sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN;
+                               batmsg.power_supply_technology = sensor_msgs::BatteryState::POWER_SUPPLY_TECHNOLOGY_NIMH;
+                               batmsg.present = true;
+                               pub_battery.publish(batmsg);
+                       }
+               }
+
+               void update_capacity(double volt, double current) {
+                       if (bDocked)
+                               battery_capacity = BATTERY_CAPACITY_FULL*3600;
+                       else
+                               battery_capacity -= current/UPDATE_RATE;
+               }
+
+               void get_odom() {
+                       uint8_t buf[20];
+                       i2c_read_reg(0x50, 0x38, 20, buf);
+                       float speed_trans = PTR_TO_FLOAT(buf+0);
+                       float speed_rot = PTR_TO_FLOAT(buf+4);
+                       float posx = PTR_TO_FLOAT(buf+8);
+                       float posy = PTR_TO_FLOAT(buf+12);
+                       float angle = PTR_TO_FLOAT(buf+16);
+                       ros::Time current_time = ros::Time::now();
+
+                       // since all odometry is 6DOF we'll need a quaternion created from yaw
+                       geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(angle);
+
+                       // first, we'll publish the transform over tf
+                       if (enable_odom_tf) {
+                               geometry_msgs::TransformStamped odom_trans;
+                               odom_trans.header.stamp = current_time;
+                               odom_trans.header.frame_id = "odom";
+                               odom_trans.child_frame_id = "base_footprint";
+                               odom_trans.transform.translation.x = posx;
+                               odom_trans.transform.translation.y = posy;
+                               odom_trans.transform.translation.z = 0.0;
+                               odom_trans.transform.rotation = odom_quat;
+                               tf_broadcaster.sendTransform(odom_trans);
+                       }
+
+                       // next, we'll publish the odometry message over ROS
+                       nav_msgs::Odometry odom;
+                       odom.header.stamp = current_time;
+                       odom.header.frame_id = "odom";
+
+                       // set the position
+                       odom.pose.pose.position.x = posx;
+                       odom.pose.pose.position.y = posy;
+                       odom.pose.pose.position.z = 0.0;
+                       odom.pose.pose.orientation = odom_quat;
+                       odom.pose.covariance[0] = odom_covar_xy; // x
+                       odom.pose.covariance[7] = odom_covar_xy; // y
+                       odom.pose.covariance[14] = odom_covar_xy; // z
+                       odom.pose.covariance[21] = odom_covar_angle; // rotation about X axis
+                       odom.pose.covariance[28] = odom_covar_angle; // rotation about Y axis
+                       odom.pose.covariance[35] = odom_covar_angle; // rotation about Z axis
+
+                       // set the velocity
+                       odom.child_frame_id = "base_footprint";
+                       odom.twist.twist.linear.x = speed_trans;
+                       odom.twist.twist.linear.y = 0.0;
+                       odom.twist.twist.linear.z = 0.0;
+                       odom.twist.twist.angular.z = speed_rot;
+                       odom.twist.covariance[0] = odom_covar_xy; // x
+                       odom.twist.covariance[7] = odom_covar_xy; // y
+                       odom.twist.covariance[14] = odom_covar_xy; // z
+                       odom.twist.covariance[21] = odom_covar_angle; // rotation about X axis
+                       odom.twist.covariance[28] = odom_covar_angle; // rotation about Y axis
+                       odom.twist.covariance[35] = odom_covar_angle; // rotation about Z axis
+
+                       // publish the message
+                       pub_odom.publish(odom);
+               }
+
+               void set_speed(float trans, float rot) {
+                       std::vector<uint8_t> buf;
+                       buf.push_back((*(uint32_t*)&trans)>>24);
+                       buf.push_back((*(uint32_t*)&trans)>>16);
+                       buf.push_back((*(uint32_t*)&trans)>>8);
+                       buf.push_back((*(uint32_t*)&trans)>>0);
+                       buf.push_back((*(uint32_t*)&rot)>>24);
+                       buf.push_back((*(uint32_t*)&rot)>>16);
+                       buf.push_back((*(uint32_t*)&rot)>>8);
+                       buf.push_back((*(uint32_t*)&rot)>>0);
+                       i2c_write_reg(0x50, 0x50, buf);
+               }
+
+               void cmdVelReceived(const geometry_msgs::Twist::ConstPtr& msg) {
+                       if (!bMotorManual) {
+                               ROS_DEBUG("Set new cmd_vel: %.2f %.2f", msg->linear.x, msg->angular.z);
+                               setVelocity(msg->linear.x, msg->angular.z); // commit speed on next update cycle
+                               ROS_DEBUG("Set new cmd_vel done");
+                       }
+               }
+
+               void start_dist_srf(uint8_t num) {
+                       i2c_write_reg(0x52, num, {});
+               }
+
+               float read_dist_srf(uint8_t num) {
+                       uint8_t buf[2];
+                       i2c_read_reg(0x52, num, 2, buf);
+                       return __bswap_16(*(uint16_t*)buf)/1000.0;
+               }
+
+               void send_range(ros::Publisher &pub, std::string frame_id, uint8_t typ, double dist, double min_range, double max_range, double fov_deg) {
+                       if (bClipRangeSensor && dist > max_range) {
+                               dist = max_range;
+                       }
+                       sensor_msgs::Range msg;
+                       msg.header.stamp = ros::Time::now();
+                       msg.header.frame_id = frame_id;
+                       msg.radiation_type = typ;
+                       msg.field_of_view = fov_deg*M_PI/180;
+                       msg.min_range = min_range;
+                       msg.max_range = max_range;
+                       msg.range = dist;
+                       pub.publish(msg);
+               }
+
+               void get_dist_forward_left() {
+                       if (pub_range_fwd_left.getNumSubscribers() > 0) {
+                               float dist = read_dist_srf(0x15);
+                               send_range(pub_range_fwd_left, "sonar_forward_left", sensor_msgs::Range::ULTRASOUND, dist, 0.04, range_sensor_max, range_sensor_fov);
+                       }
+               }
+
+               void update_dist_forward_left() {
+                       if (pub_range_fwd_left.getNumSubscribers() > 0) {
+                               start_dist_srf(0x5);
+                       }
+               }
+
+               void get_dist_backward() {
+                       if (pub_range_bwd.getNumSubscribers() > 0) {
+                               float dist = read_dist_srf(0x17);
+                               send_range(pub_range_bwd, "sonar_backward", sensor_msgs::Range::ULTRASOUND, dist, 0.04, range_sensor_max, range_sensor_fov);
+                       }
+               }
+
+               void update_dist_backward() {
+                       if (pub_range_bwd.getNumSubscribers() > 0) {
+                               start_dist_srf(0x7);
+                       }
+               }
+
+               void get_dist_forward_right() {
+                       if (pub_range_fwd_right.getNumSubscribers() > 0) {
+                               float dist = read_dist_srf(0x19);
+                               send_range(pub_range_fwd_right, "sonar_forward_right", sensor_msgs::Range::ULTRASOUND, dist, 0.04, range_sensor_max, range_sensor_fov);
+                       }
+               }
+
+               void update_dist_forward_right() {
+                       if (pub_range_fwd_right.getNumSubscribers() > 0) {
+                               start_dist_srf(0xb);
+                       }
+               }
+
+               void check_docked() {
+                       if (bDocked && !bDocked_last) {
+                               ROS_INFO("Docking event");
+                       } else if (!bDocked && bDocked_last) {
+                               bool stopped = cur_vel[0] == 0 && cur_vel[1] == 0;
+                               if (!bStayDocked || !stopped) {
+                                       ROS_INFO("Undocking event");
+                               } else {
+                                       ROS_INFO("Undocking event..redocking");
+                                       pthread_t tid;
+                                       pthread_create(&tid, NULL, &WTBase::redock, this);
+                               }
+                       }
+
+                       bDocked_last = bDocked;
+               }
+
+               static void *redock(void *context) {
+                       WTBase *This = (WTBase *)context;
+                       This->setVelocity(-0.1, 0);
+                       for (int i=0; i<100; i++) {
+                               if (This->isDocked())
+                                       break;
+                               usleep(10*1000);
+                       }
+                       This->setVelocity(0, 0);
+                       if (This->isDocked()) {
+                               ROS_INFO("Redocking done");
+                       } else {
+                               ROS_ERROR("Redocking failed");
+                       }
+
+                       pthread_exit((void *) 0);
+               }
+};
+
+int main(int argc, char **argv) {
+       ros::init(argc, argv, "wild_thumper");
+       ros::NodeHandle nh;
+       ros::NodeHandle pnh("~");
+       WTBase robot(nh, pnh);
+       robot.run();
+       return 0;
+}