From 58e6047a99365abcd393c951c6fb59f74d52b09c Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Mon, 3 Aug 2020 12:08:30 +0200 Subject: [PATCH] convert base node to c++ --- CMakeLists.txt | 7 +- launch/wild_thumper.launch | 9 +- src/wt_node.cpp | 567 +++++++++++++++++++++++++++++++++++++ 3 files changed, 578 insertions(+), 5 deletions(-) create mode 100644 src/wt_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5dfe0e2..965b4ee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 ## diff --git a/launch/wild_thumper.launch b/launch/wild_thumper.launch index 1e1bf66..c721dcc 100644 --- a/launch/wild_thumper.launch +++ b/launch/wild_thumper.launch @@ -7,22 +7,22 @@ - + - + - + @@ -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 --> + diff --git a/src/wt_node.cpp b/src/wt_node.cpp new file mode 100644 index 0000000..e8303a6 --- /dev/null +++ b/src/wt_node.cpp @@ -0,0 +1,567 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +extern "C" { +#include +} + +#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(x)); \ + *reinterpret_cast(&tmp); \ +}) + + +int i2c_write_reg(const uint8_t addr, const uint8_t command, const std::vector 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::CallbackType f; + f = boost::bind(&WTBase::execute_dyn_reconf, this, _1, _2); + dyn_conf.setCallback(f); + + pub_odom = nh.advertise("odom", 16); + pub_diag = nh.advertise("diagnostics", 16); + pub_range_fwd_left = nh.advertise("range_forward_left", 16); + pub_range_fwd_right = nh.advertise("range_forward_right", 16); + pub_range_bwd = nh.advertise("range_backward", 16); + pub_battery = nh.advertise("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 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 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 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; +} -- 2.39.5