publish motor error as diag

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 72c9340e169fbfe6ea29b95d33bb49c905b32d43..5084c1ae7dd7f119d1968b551ac3daa7d2426183 100644 (file)
--- 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
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
 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 49dcb69da4bad610ae5b59d72633de804c45ab62..1d5c369877b44b48298acfff68f7c884ecf16a3b 100644 (file)
--- 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 bb825be3fdb8c762843bb1115503e074f96234e6..113fc142d861199141aae89fbb2c15aead2997a1 100644 (file)
--- a/package.xml
+++ b/package.xml
@@ -17,6 +17,7 @@
   <depend>std_srvs</depend>
   <depend>nav_msgs</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>diagnostic_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

diff --git a/src/hw_node.cpp b/src/hw_node.cpp
index 0ac70c4cc19696eb892689071561bca30b693cd7..77343a1f603c3eadc8715e7439051da94f0fbf84 100644 (file)
--- a/src/hw_node.cpp
+++ b/src/hw_node.cpp
@@ -3,6 +3,7 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <format>
 #include <linux/i2c-dev.h>
 #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 <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
@@ -100,6 +102,7 @@ class A4wd3 : public rclcpp::Node {
                        pub_odom = this->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
                        pub_bat = this->create_publisher<sensor_msgs::msg::BatteryState>("battery", 10);
                        pub_light = this->create_publisher<sensor_msgs::msg::Illuminance>("light", 10);
+                       pub_diag = this->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("diagnostics", 10);
                        sub_cmd_vel = this->create_subscription<geometry_msgs::msg::Twist>("cmd_vel", 10, std::bind(&A4wd3::cmdvel_callback, this, std::placeholders::_1));
                        sub_imu = this->create_subscription<sensor_msgs::msg::Imu>("imu", 10, std::bind(&A4wd3::imu_callback, this, std::placeholders::_1));
                        sub_led = this->create_subscription<a4wd3::msg::LedStripe>("led_stripe", 10, std::bind(&A4wd3::led_stripe_callback, this, std::placeholders::_1));
@@ -122,6 +125,7 @@ class A4wd3 : public rclcpp::Node {
                rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
                rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr pub_bat;
                rclcpp::Publisher<sensor_msgs::msg::Illuminance>::SharedPtr pub_light;
+               rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr pub_diag;
                rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_cmd_vel;
                rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu;
                rclcpp::Subscription<a4wd3::msg::LedStripe>::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<uint8_t> 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]);