From: Erik Andresen Date: Thu, 21 May 2026 19:28:32 +0000 (+0200) Subject: publish motor error as diag X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=refs%2Fheads%2Fmaster;p=a4wd3.git publish motor error as diag --- diff --git a/CMakeLists.txt b/CMakeLists.txt index 72c9340..5084c1a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(geometry_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(nav_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} @@ -26,13 +27,13 @@ rosidl_generate_interfaces(${PROJECT_NAME} ) add_executable(hw_node src/hw_node.cpp) -ament_target_dependencies(hw_node rclcpp std_msgs sensor_msgs geometry_msgs std_srvs nav_msgs tf2_geometry_msgs) +ament_target_dependencies(hw_node rclcpp std_msgs sensor_msgs geometry_msgs std_srvs nav_msgs tf2_geometry_msgs diagnostic_msgs) target_include_directories(hw_node PUBLIC $ $) rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") target_link_libraries(hw_node i2c "${cpp_typesupport_target}") -target_compile_features(hw_node PUBLIC c_std_99 cxx_std_17) +target_compile_features(hw_node PUBLIC c_std_99 cxx_std_20) add_executable(display src/display.cpp) ament_target_dependencies(display rclcpp sensor_msgs) diff --git a/launch/a4wd3_launch.py b/launch/a4wd3_launch.py index 49dcb69..1d5c369 100644 --- a/launch/a4wd3_launch.py +++ b/launch/a4wd3_launch.py @@ -143,8 +143,8 @@ def generate_launch_description(): XMLLaunchDescriptionSource([os.path.join(get_package_share_directory('rosbridge_server'), 'launch'),'/rosbridge_websocket_launch.xml']) ), Node( - package='bme680_driver', - executable='bme680_driver', + package='bme680_ros', + executable='bme680_ros', name='bme680', parameters=[{"i2c_bus": 7}], output="screen", diff --git a/package.xml b/package.xml index bb825be..113fc14 100644 --- a/package.xml +++ b/package.xml @@ -17,6 +17,7 @@ std_srvs nav_msgs tf2_geometry_msgs + diagnostic_msgs ament_lint_auto ament_lint_common diff --git a/src/hw_node.cpp b/src/hw_node.cpp index 0ac70c4..77343a1 100644 --- a/src/hw_node.cpp +++ b/src/hw_node.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -10,6 +11,7 @@ #include "sensor_msgs/msg/battery_state.hpp" #include "sensor_msgs/msg/imu.hpp" #include "sensor_msgs/msg/illuminance.hpp" +#include "diagnostic_msgs/msg/diagnostic_array.hpp" #include #include #include @@ -100,6 +102,7 @@ class A4wd3 : public rclcpp::Node { pub_odom = this->create_publisher("odom", 10); pub_bat = this->create_publisher("battery", 10); pub_light = this->create_publisher("light", 10); + pub_diag = this->create_publisher("diagnostics", 10); sub_cmd_vel = this->create_subscription("cmd_vel", 10, std::bind(&A4wd3::cmdvel_callback, this, std::placeholders::_1)); sub_imu = this->create_subscription("imu", 10, std::bind(&A4wd3::imu_callback, this, std::placeholders::_1)); sub_led = this->create_subscription("led_stripe", 10, std::bind(&A4wd3::led_stripe_callback, this, std::placeholders::_1)); @@ -122,6 +125,7 @@ class A4wd3 : public rclcpp::Node { rclcpp::Publisher::SharedPtr pub_odom; rclcpp::Publisher::SharedPtr pub_bat; rclcpp::Publisher::SharedPtr pub_light; + rclcpp::Publisher::SharedPtr pub_diag; rclcpp::Subscription::SharedPtr sub_cmd_vel; rclcpp::Subscription::SharedPtr sub_imu; rclcpp::Subscription::SharedPtr sub_led; @@ -269,6 +273,60 @@ class A4wd3 : public rclcpp::Node { pub_odom->publish(odom); } + void update_motor_err() { + uint8_t err; + const int ret = i2c_read_reg(0x50, 0xa1, 1, &err); + if (ret != 1) { + RCLCPP_ERROR(this->get_logger(), "Failed to read motor status err=%d", ret); + return; + } + + auto msg = diagnostic_msgs::msg::DiagnosticArray(); + msg.header.stamp = this->get_clock()->now(); + auto stat = diagnostic_msgs::msg::DiagnosticStatus(); + stat.name = "Motor: Error Status"; + stat.level = err ? diagnostic_msgs::msg::DiagnosticStatus::ERROR : diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = std::format("0x{:02x}", err); + + // Diag + auto kvAftLeft = diagnostic_msgs::msg::KeyValue(); + kvAftLeft.key = "aft right diag"; + kvAftLeft.value = std::format("{}", err & (1 << 0)); + stat.values.push_back(kvAftLeft); + auto kvFrontLeft = diagnostic_msgs::msg::KeyValue(); + kvFrontLeft.key = "front right diag"; + kvFrontLeft.value = std::format("{}", err & (1 << 1)); + stat.values.push_back(kvFrontLeft); + auto kvAftRight = diagnostic_msgs::msg::KeyValue(); + kvAftRight.key = "aft left diag"; + kvAftRight.value = std::format("{}", err & (1 << 2)); + stat.values.push_back(kvAftRight); + auto kvFrontRight = diagnostic_msgs::msg::KeyValue(); + kvFrontRight.key = "front left diag"; + kvFrontRight.value = std::format("{}", err & (1 << 3)); + stat.values.push_back(kvFrontRight); + // Stall + auto kvAftLeft2 = diagnostic_msgs::msg::KeyValue(); + auto kvFrontLeft2 = diagnostic_msgs::msg::KeyValue(); + auto kvAftRight2 = diagnostic_msgs::msg::KeyValue(); + auto kvFrontRight2 = diagnostic_msgs::msg::KeyValue(); + kvAftLeft2.key = "aft right stall"; + kvAftLeft2.value = std::format("{}", err & (1 << 4)); + stat.values.push_back(kvAftLeft2); + kvFrontLeft2.key = "front right stall"; + kvFrontLeft2.value = std::format("{}", err & (1 << 5)); + stat.values.push_back(kvFrontLeft2); + kvAftRight2.key = "aft left stall"; + kvAftRight2.value = std::format("{}", err & (1 << 6)); + stat.values.push_back(kvAftRight2); + kvFrontRight2.key = "front left stall"; + kvFrontRight2.value = std::format("{}", err & (1 << 7)); + stat.values.push_back(kvFrontRight2); + + msg.status.push_back(stat); + pub_diag->publish(msg); + } + void init_pwr() { std::vector buf; uint16_t values[1]; @@ -356,6 +414,7 @@ class A4wd3 : public rclcpp::Node { } update_pwr(); update_light(); + update_motor_err(); if (!cmd_vel.empty()) { set_speed(cmd_vel[0], cmd_vel[1]);