9 #include <linux/i2c-dev.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>
26 #include <i2c/smbus.h>
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); \
40 int i2c_write_reg(const uint8_t addr, const uint8_t command, const std::vector<uint8_t> values) {
43 if ((file = open(I2C_FILE, O_RDWR)) < 0) {
47 if (ioctl(file, I2C_SLAVE, (addr>>1)) < 0) {
52 if ((ret = i2c_smbus_write_i2c_block_data(file, command, values.size(), &values[0])) != 0) {
53 perror("i2c_smbus_write_block_data");
67 int i2c_read_reg(const uint8_t addr, const uint8_t command, const uint8_t length, uint8_t *values) {
70 if ((file = open(I2C_FILE, O_RDWR)) < 0) {
74 if (ioctl(file, I2C_SLAVE, (addr>>1)) < 0) {
79 if ((ret = i2c_smbus_read_i2c_block_data(file, command, length, values)) != length) {
80 perror("i2c_smbus_read_block_data");
97 WTBase(ros::NodeHandle nh, ros::NodeHandle pnh) {
100 enable_odom_tf = pnh.param("enable_odom_tf", true);
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);
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);
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");
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;
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
135 if (sonar_count == 0) {
136 get_dist_forward_left();
137 update_dist_backward();
139 } else if (sonar_count == 1) {
141 update_dist_forward_right();
143 } else if (sonar_count == 2) {
144 get_dist_forward_right();
145 update_dist_forward_left();
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];
166 void setVelocity(float trans, float rot) {
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});
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
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;
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;
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;
227 tf::quaternionMsgToTF(msg->orientation, quat);
228 tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
229 if (pitch > rollover_protect_limit*M_PI/180) {
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) {
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;
241 ROS_WARN("Rollver protection done");
246 uint8_t get_reset() {
248 i2c_read_reg(0x50, 0xA0, 1, buf);
249 uint8_t reset = buf[0];
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);
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");
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);
281 msg.status.push_back(stat);
282 pub_diag.publish(msg);
286 void get_motor_err() {
288 i2c_read_reg(0x50, 0xA1, 1, buf);
289 uint8_t err = buf[0];
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);
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);
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);
327 msg.status.push_back(stat);
328 pub_diag.publish(msg);
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;
338 diagnostic_msgs::DiagnosticArray msg;
339 msg.header.stamp = ros::Time::now();
340 diagnostic_msgs::DiagnosticStatus stat;
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);
345 msg.status.push_back(stat);
346 pub_diag.publish(msg);
349 ROS_ERROR_THROTTLE(10, "Voltage critical: %.2fV", volt);
352 bDocked = volt > 10.1;
353 update_capacity(volt, current);
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);
372 void update_capacity(double volt, double current) {
374 battery_capacity = BATTERY_CAPACITY_FULL*3600;
376 battery_capacity -= current/UPDATE_RATE;
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();
389 // since all odometry is 6DOF we'll need a quaternion created from yaw
390 geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(angle);
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);
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";
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
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
435 // publish the message
436 pub_odom.publish(odom);
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);
452 void cmdVelReceived(const geometry_msgs::Twist::ConstPtr& msg) {
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");
460 void start_dist_srf(uint8_t num) {
461 i2c_write_reg(0x52, num, {});
464 float read_dist_srf(uint8_t num) {
466 i2c_read_reg(0x52, num, 2, buf);
467 return __bswap_16(*(uint16_t*)buf)/1000.0;
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) {
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;
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);
492 void update_dist_forward_left() {
493 if (pub_range_fwd_left.getNumSubscribers() > 0) {
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);
505 void update_dist_backward() {
506 if (pub_range_bwd.getNumSubscribers() > 0) {
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);
518 void update_dist_forward_right() {
519 if (pub_range_fwd_right.getNumSubscribers() > 0) {
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");
532 ROS_INFO("Undocking event..redocking");
534 pthread_create(&tid, NULL, &WTBase::redock, this);
538 bDocked_last = bDocked;
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())
549 This->setVelocity(0, 0);
550 if (This->isDocked()) {
551 ROS_INFO("Redocking done");
553 ROS_ERROR("Redocking failed");
556 pthread_exit((void *) 0);
560 int main(int argc, char **argv) {
561 ros::init(argc, argv, "wild_thumper");
563 ros::NodeHandle pnh("~");
564 WTBase robot(nh, pnh);