From c048c5dff3f3f9568c0d7a6e9cbeb8ccf4c460d8 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Wed, 16 Apr 2025 12:25:24 -0700 Subject: [PATCH 01/38] Publish OpenCR pins A0-A5 raw values to /analog_pins --- turtlebot3_node/CMakeLists.txt | 1 + .../include/turtlebot3_node/control_table.hpp | 7 ++ .../turtlebot3_node/sensors/analog_pins.hpp | 50 +++++++++++ .../include/turtlebot3_node/turtlebot3.hpp | 1 + turtlebot3_node/src/sensors/analog_pins.cpp | 85 +++++++++++++++++++ turtlebot3_node/src/turtlebot3.cpp | 17 +++- 6 files changed, 160 insertions(+), 1 deletion(-) create mode 100644 turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp create mode 100644 turtlebot3_node/src/sensors/analog_pins.cpp diff --git a/turtlebot3_node/CMakeLists.txt b/turtlebot3_node/CMakeLists.txt index d81d81276..fd8ee3e81 100644 --- a/turtlebot3_node/CMakeLists.txt +++ b/turtlebot3_node/CMakeLists.txt @@ -50,6 +50,7 @@ add_library(${PROJECT_NAME}_lib "src/sensors/imu.cpp" "src/sensors/joint_state.cpp" "src/sensors/sensor_state.cpp" + "src/sensors/analog_pins.cpp" ) set(DEPENDENCIES diff --git a/turtlebot3_node/include/turtlebot3_node/control_table.hpp b/turtlebot3_node/include/turtlebot3_node/control_table.hpp index 35e7e42c9..3155c6e66 100644 --- a/turtlebot3_node/include/turtlebot3_node/control_table.hpp +++ b/turtlebot3_node/include/turtlebot3_node/control_table.hpp @@ -105,6 +105,13 @@ typedef struct ControlItem profile_acceleration_left = {174, RAM, 4, READ_WRITE}; ControlItem profile_acceleration_right = {178, RAM, 4, READ_WRITE}; + + ControlItem analog_a0 = {350, RAM, 2, READ}; + ControlItem analog_a1 = {352, RAM, 2, READ}; + ControlItem analog_a2 = {354, RAM, 2, READ}; + ControlItem analog_a3 = {356, RAM, 2, READ}; + ControlItem analog_a4 = {358, RAM, 2, READ}; + ControlItem analog_a5 = {360, RAM, 2, READ}; } ControlTable; const ControlTable extern_control_table; diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp new file mode 100644 index 000000000..b0c89d769 --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp @@ -0,0 +1,50 @@ +// Copyright 2025 Travis Mendoza +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TURTLEBOT3_NODE__SENSORS__ANALOG_PINS_HPP_ +#define TURTLEBOT3_NODE__SENSORS__ANALOG_PINS_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/u_int16_multi_array.hpp" + +#include "turtlebot3_node/sensors/sensors.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +namespace sensors +{ +class AnalogPins : public Sensors +{ +public: + explicit AnalogPins( + std::shared_ptr & nh, + const std::string & topic_name); + + void publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) override; + +private: + rclcpp::Publisher::SharedPtr analog_publisher_; +}; +} // namespace sensors +} // namespace turtlebot3 +} // namespace robotis + +#endif // TURTLEBOT3_NODE__SENSORS__ANALOG_PINS_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp index fdc70e472..425904f04 100644 --- a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp +++ b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp @@ -43,6 +43,7 @@ #include "turtlebot3_node/devices/sound.hpp" #include "turtlebot3_node/dynamixel_sdk_wrapper.hpp" #include "turtlebot3_node/odometry.hpp" +#include "turtlebot3_node/sensors/analog_pins.hpp" #include "turtlebot3_node/sensors/battery_state.hpp" #include "turtlebot3_node/sensors/imu.hpp" #include "turtlebot3_node/sensors/joint_state.hpp" diff --git a/turtlebot3_node/src/sensors/analog_pins.cpp b/turtlebot3_node/src/sensors/analog_pins.cpp new file mode 100644 index 000000000..88b7314be --- /dev/null +++ b/turtlebot3_node/src/sensors/analog_pins.cpp @@ -0,0 +1,85 @@ +// Copyright 2025 Travis Mendoza +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "turtlebot3_node/sensors/analog_pins.hpp" + +#include +#include +#include + +using robotis::turtlebot3::sensors::AnalogPins; + +AnalogPins::AnalogPins( + std::shared_ptr & nh, + const std::string & topic_name) +: Sensors(nh, "") // Call parent constructor with node handle and empty frame_id +{ + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); + + analog_publisher_ = nh->create_publisher(topic_name, qos); + + RCLCPP_INFO(nh->get_logger(), "Succeeded to create analog pins publisher"); +} + +void AnalogPins::publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) +{ + (void)now; // Mark as unused intentionally to suppress warning + + try { + auto analog_msg = std::make_unique(); + + // Set up dimensions for the message + analog_msg->layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); + analog_msg->layout.dim[0].label = "analog_pins"; + analog_msg->layout.dim[0].size = 6; + analog_msg->layout.dim[0].stride = 6; + analog_msg->layout.data_offset = 0; + + // Initialize data array to hold 6 pin values + analog_msg->data.resize(6); + + // Read values from all 6 analog pins (A0-A5) + analog_msg->data[0] = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.analog_a0.addr, + extern_control_table.analog_a0.length); + analog_msg->data[1] = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.analog_a1.addr, + extern_control_table.analog_a1.length); + analog_msg->data[2] = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.analog_a2.addr, + extern_control_table.analog_a2.length); + analog_msg->data[3] = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.analog_a3.addr, + extern_control_table.analog_a3.length); + analog_msg->data[4] = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.analog_a4.addr, + extern_control_table.analog_a4.length); + analog_msg->data[5] = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.analog_a5.addr, + extern_control_table.analog_a5.length); + + // Publish the message + analog_publisher_->publish(std::move(analog_msg)); + } catch (const std::exception & e) { + RCLCPP_ERROR( + rclcpp::get_logger("analog_pins"), + "Exception in analog_pins publish: %s", e.what()); + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("analog_pins"), + "Unknown exception in analog_pins publish"); + } +} diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 55bb2e31b..c9cd33d28 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -56,7 +56,6 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) this->declare_parameter("opencr.id"); this->declare_parameter("opencr.baud_rate"); this->declare_parameter("opencr.protocol_version"); - this->declare_parameter("namespace"); this->get_parameter_or("opencr.id", opencr.id, 200); this->get_parameter_or("opencr.baud_rate", opencr.baud_rate, 1000000); @@ -66,11 +65,19 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) dxl_sdk_wrapper_ = std::make_shared(opencr); + // Initialize the main control table range (original range) dxl_sdk_wrapper_->init_read_memory( extern_control_table.millis.addr, (extern_control_table.profile_acceleration_right.addr - extern_control_table.millis.addr) + extern_control_table.profile_acceleration_right.length ); + + // Add a separate initialization for the analog pins range + dxl_sdk_wrapper_->init_read_memory( + extern_control_table.analog_a0.addr, + (extern_control_table.analog_a5.addr - extern_control_table.analog_a0.addr) + + extern_control_table.analog_a5.length + ); } void TurtleBot3::check_device_status() @@ -198,6 +205,14 @@ void TurtleBot3::add_sensors() is_connected_ir, is_connected_sonar)); + sensors_.push_back( + new sensors::AnalogPins( + node_handle_, + "analog_pins")); + + + RCLCPP_INFO(this->get_logger(), "Successfully added all sensors"); + dxl_sdk_wrapper_->read_data_set(); sensors_.push_back( new sensors::JointState( From d0402266090433a753e5ad7be19c2995b62cc438 Mon Sep 17 00:00:00 2001 From: travis-mendoza Date: Wed, 16 Apr 2025 21:37:56 -0700 Subject: [PATCH 02/38] assign copyright of analog pins files to ROBOTIS --- turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp | 2 +- turtlebot3_node/src/sensors/analog_pins.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp index b0c89d769..4a53fbe02 100644 --- a/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp +++ b/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp @@ -1,4 +1,4 @@ -// Copyright 2025 Travis Mendoza +// Copyright 2025 ROBOTIS CO., LTD. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/turtlebot3_node/src/sensors/analog_pins.cpp b/turtlebot3_node/src/sensors/analog_pins.cpp index 88b7314be..ace67e332 100644 --- a/turtlebot3_node/src/sensors/analog_pins.cpp +++ b/turtlebot3_node/src/sensors/analog_pins.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 Travis Mendoza +// Copyright 2025 ROBOTIS CO., LTD. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 79479c3c560980bd92499a5e9d4d80422cb1e8cd Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Mon, 12 May 2025 12:11:05 -0700 Subject: [PATCH 03/38] Add error messages and retry logic to check_device_status --- turtlebot3_node/src/turtlebot3.cpp | 126 +++++++++++++++++++++-------- 1 file changed, 93 insertions(+), 33 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index c9cd33d28..2244ec77c 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -18,6 +18,7 @@ #include #include +#include using robotis::turtlebot3::TurtleBot3; using namespace std::chrono_literals; @@ -28,15 +29,44 @@ TurtleBot3::TurtleBot3(const std::string & usb_port) RCLCPP_INFO(get_logger(), "Init TurtleBot3 Node Main"); node_handle_ = std::shared_ptr<::rclcpp::Node>(this, [](::rclcpp::Node *) {}); - init_dynamixel_sdk_wrapper(usb_port); - check_device_status(); + try { + init_dynamixel_sdk_wrapper(usb_port); + check_device_status(); - add_motors(); - add_wheels(); - add_sensors(); - add_devices(); + // Add stabilization delay after calibration + RCLCPP_INFO(this->get_logger(), "Waiting for IMU to stabilize after calibration..."); + rclcpp::sleep_for(std::chrono::milliseconds(500)); - run(); + // Verify device connectivity before proceeding + if (!dxl_sdk_wrapper_->is_connected_to_device()) { + RCLCPP_ERROR(this->get_logger(), "Lost connection to device after calibration"); + rclcpp::shutdown(); + return; + } + + try { + add_motors(); + add_wheels(); + add_sensors(); + add_devices(); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "Exception during initialization: %s", e.what()); + rclcpp::shutdown(); + return; + } catch (...) { + RCLCPP_ERROR(this->get_logger(), "Unknown exception during initialization"); + rclcpp::shutdown(); + return; + } + + run(); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "Critical exception in TurtleBot3 initialization: %s", e.what()); + rclcpp::shutdown(); + } catch (...) { + RCLCPP_ERROR(this->get_logger(), "Unknown critical exception in TurtleBot3 initialization"); + rclcpp::shutdown(); + } } TurtleBot3::Wheels * TurtleBot3::get_wheels() @@ -86,11 +116,30 @@ void TurtleBot3::check_device_status() std::string sdk_msg; uint8_t reset = 1; - dxl_sdk_wrapper_->set_data_to_device( - extern_control_table.imu_re_calibration.addr, - extern_control_table.imu_re_calibration.length, - &reset, - &sdk_msg); + // Add retry logic for setting calibration data + bool calibration_set = false; + for (int retry = 0; retry < 3 && !calibration_set; retry++) { + if (retry > 0) { + RCLCPP_WARN(this->get_logger(), "Retrying IMU calibration setup (attempt %d of 3)", retry + 1); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + sdk_msg.clear(); + if (dxl_sdk_wrapper_->set_data_to_device( + extern_control_table.imu_re_calibration.addr, + extern_control_table.imu_re_calibration.length, + &reset, + &sdk_msg)) { + calibration_set = true; + } else { + RCLCPP_WARN(this->get_logger(), "Failed to set IMU calibration: %s", sdk_msg.c_str()); + } + } + + if (!calibration_set) { + RCLCPP_ERROR(this->get_logger(), "Failed to set IMU calibration after multiple attempts"); + throw std::runtime_error("IMU calibration setup failed"); + } RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); rclcpp::sleep_for(std::chrono::seconds(5)); @@ -103,17 +152,23 @@ void TurtleBot3::check_device_status() const int8_t NOT_CONNECTED_MOTOR = -1; - int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( - extern_control_table.device_status.addr, - extern_control_table.device_status.length); + // Add error handling for device status check + try { + int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( + extern_control_table.device_status.addr, + extern_control_table.device_status.length); - switch (device_status) { - case NOT_CONNECTED_MOTOR: - RCLCPP_WARN(this->get_logger(), "Please double check your Dynamixels and Power"); - break; + switch (device_status) { + case NOT_CONNECTED_MOTOR: + RCLCPP_WARN(this->get_logger(), "Please double check your Dynamixels and Power"); + break; - default: - break; + default: + break; + } + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "Failed to get device status: %s", e.what()); + throw std::runtime_error("Device status check failed"); } } @@ -121,18 +176,23 @@ void TurtleBot3::add_motors() { RCLCPP_INFO(this->get_logger(), "Add Motors"); - this->declare_parameter("motors.profile_acceleration_constant"); - this->declare_parameter("motors.profile_acceleration"); - - this->get_parameter_or( - "motors.profile_acceleration_constant", - motors_.profile_acceleration_constant, - 214.577); - - this->get_parameter_or( - "motors.profile_acceleration", - motors_.profile_acceleration, - 0.0); + try { + this->declare_parameter("motors.profile_acceleration_constant"); + this->declare_parameter("motors.profile_acceleration"); + + this->get_parameter_or( + "motors.profile_acceleration_constant", + motors_.profile_acceleration_constant, + 214.577); + + this->get_parameter_or( + "motors.profile_acceleration", + motors_.profile_acceleration, + 0.0); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "Failed to add motors: %s", e.what()); + throw; + } } void TurtleBot3::add_wheels() From 32b985382556f9f863f8c9ef7bb4af9656756a5d Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Mon, 12 May 2025 12:57:33 -0700 Subject: [PATCH 04/38] Fix error handling for IMU calibration failures --- turtlebot3_node/src/turtlebot3.cpp | 114 ++++++++++++++++------------- 1 file changed, 62 insertions(+), 52 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 2244ec77c..1217853bc 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -29,9 +29,17 @@ TurtleBot3::TurtleBot3(const std::string & usb_port) RCLCPP_INFO(get_logger(), "Init TurtleBot3 Node Main"); node_handle_ = std::shared_ptr<::rclcpp::Node>(this, [](::rclcpp::Node *) {}); + bool initialization_successful = true; + try { init_dynamixel_sdk_wrapper(usb_port); - check_device_status(); + + // Check device status with error handling but without exceptions + if (!check_device_status()) { + RCLCPP_ERROR(this->get_logger(), "Device initialization failed, shutting down"); + initialization_successful = false; + return; // Exit constructor early + } // Add stabilization delay after calibration RCLCPP_INFO(this->get_logger(), "Waiting for IMU to stabilize after calibration..."); @@ -40,32 +48,33 @@ TurtleBot3::TurtleBot3(const std::string & usb_port) // Verify device connectivity before proceeding if (!dxl_sdk_wrapper_->is_connected_to_device()) { RCLCPP_ERROR(this->get_logger(), "Lost connection to device after calibration"); - rclcpp::shutdown(); - return; + initialization_successful = false; + return; // Exit constructor early } - try { - add_motors(); - add_wheels(); - add_sensors(); - add_devices(); - } catch (const std::exception & e) { - RCLCPP_ERROR(this->get_logger(), "Exception during initialization: %s", e.what()); - rclcpp::shutdown(); - return; - } catch (...) { - RCLCPP_ERROR(this->get_logger(), "Unknown exception during initialization"); - rclcpp::shutdown(); - return; - } + add_motors(); + add_wheels(); + add_sensors(); + add_devices(); run(); } catch (const std::exception & e) { RCLCPP_ERROR(this->get_logger(), "Critical exception in TurtleBot3 initialization: %s", e.what()); - rclcpp::shutdown(); + initialization_successful = false; } catch (...) { RCLCPP_ERROR(this->get_logger(), "Unknown critical exception in TurtleBot3 initialization"); - rclcpp::shutdown(); + initialization_successful = false; + } + + // Handle shutdown outside of the try-catch if initialization failed + if (!initialization_successful) { + RCLCPP_ERROR(this->get_logger(), "Initialization failed, requesting shutdown"); + // Schedule shutdown instead of doing it immediately + auto shutdown_timer = this->create_wall_timer( + std::chrono::milliseconds(100), + []() { + rclcpp::shutdown(); + }); } } @@ -110,46 +119,45 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) ); } -void TurtleBot3::check_device_status() +bool TurtleBot3::check_device_status() { - if (dxl_sdk_wrapper_->is_connected_to_device()) { - std::string sdk_msg; - uint8_t reset = 1; - - // Add retry logic for setting calibration data - bool calibration_set = false; - for (int retry = 0; retry < 3 && !calibration_set; retry++) { - if (retry > 0) { - RCLCPP_WARN(this->get_logger(), "Retrying IMU calibration setup (attempt %d of 3)", retry + 1); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } + if (!dxl_sdk_wrapper_->is_connected_to_device()) { + RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices"); + return false; + } - sdk_msg.clear(); - if (dxl_sdk_wrapper_->set_data_to_device( - extern_control_table.imu_re_calibration.addr, - extern_control_table.imu_re_calibration.length, - &reset, - &sdk_msg)) { - calibration_set = true; - } else { - RCLCPP_WARN(this->get_logger(), "Failed to set IMU calibration: %s", sdk_msg.c_str()); - } + std::string sdk_msg; + uint8_t reset = 1; + + // Add retry logic for setting calibration data + bool calibration_set = false; + for (int retry = 0; retry < 3 && !calibration_set; retry++) { + if (retry > 0) { + RCLCPP_WARN(this->get_logger(), "Retrying IMU calibration setup (attempt %d of 3)", retry + 1); + rclcpp::sleep_for(std::chrono::milliseconds(100)); } - if (!calibration_set) { - RCLCPP_ERROR(this->get_logger(), "Failed to set IMU calibration after multiple attempts"); - throw std::runtime_error("IMU calibration setup failed"); + sdk_msg.clear(); + if (dxl_sdk_wrapper_->set_data_to_device( + extern_control_table.imu_re_calibration.addr, + extern_control_table.imu_re_calibration.length, + &reset, + &sdk_msg)) { + calibration_set = true; + } else { + RCLCPP_WARN(this->get_logger(), "Failed to set IMU calibration: %s", sdk_msg.c_str()); } + } - RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); - rclcpp::sleep_for(std::chrono::seconds(5)); - RCLCPP_INFO(this->get_logger(), "Calibration End"); - } else { - RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices"); - rclcpp::shutdown(); - return; + if (!calibration_set) { + RCLCPP_ERROR(this->get_logger(), "Failed to set IMU calibration after multiple attempts"); + return false; } + RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); + rclcpp::sleep_for(std::chrono::seconds(5)); + RCLCPP_INFO(this->get_logger(), "Calibration End"); + const int8_t NOT_CONNECTED_MOTOR = -1; // Add error handling for device status check @@ -168,8 +176,10 @@ void TurtleBot3::check_device_status() } } catch (const std::exception & e) { RCLCPP_ERROR(this->get_logger(), "Failed to get device status: %s", e.what()); - throw std::runtime_error("Device status check failed"); + return false; } + + return true; } void TurtleBot3::add_motors() From 206c253aec7926c2acb8915dd4404bcd0fe6f4a6 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Mon, 12 May 2025 13:04:26 -0700 Subject: [PATCH 05/38] Update check_device_status declaration in header file --- turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp index 425904f04..821b7829c 100644 --- a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp +++ b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp @@ -79,7 +79,7 @@ class TurtleBot3 : public rclcpp::Node private: void init_dynamixel_sdk_wrapper(const std::string & usb_port); - void check_device_status(); + bool check_device_status(); void add_sensors(); void add_devices(); From 8b6cd9e219a5f0ba359557baaea2bc702604c46f Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Mon, 12 May 2025 13:20:04 -0700 Subject: [PATCH 06/38] Increase time between IMU calibration retries --- turtlebot3_node/src/turtlebot3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 1217853bc..d40ce3980 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -134,7 +134,7 @@ bool TurtleBot3::check_device_status() for (int retry = 0; retry < 3 && !calibration_set; retry++) { if (retry > 0) { RCLCPP_WARN(this->get_logger(), "Retrying IMU calibration setup (attempt %d of 3)", retry + 1); - rclcpp::sleep_for(std::chrono::milliseconds(100)); + rclcpp::sleep_for(std::chrono::milliseconds(1000)); } sdk_msg.clear(); From 78ad7d0024f865f1d113b7f0664a02cef176809a Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Mon, 12 May 2025 13:35:57 -0700 Subject: [PATCH 07/38] Add moar cowbell (delay) --- turtlebot3_node/src/turtlebot3.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index d40ce3980..96a31a239 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -129,12 +129,13 @@ bool TurtleBot3::check_device_status() std::string sdk_msg; uint8_t reset = 1; - // Add retry logic for setting calibration data + // Add retry logic for setting calibration data with longer delays bool calibration_set = false; for (int retry = 0; retry < 3 && !calibration_set; retry++) { if (retry > 0) { RCLCPP_WARN(this->get_logger(), "Retrying IMU calibration setup (attempt %d of 3)", retry + 1); - rclcpp::sleep_for(std::chrono::milliseconds(1000)); + // Increase delay to 2 seconds between attempts to give more recovery time + rclcpp::sleep_for(std::chrono::seconds(2)); } sdk_msg.clear(); @@ -144,6 +145,14 @@ bool TurtleBot3::check_device_status() &reset, &sdk_msg)) { calibration_set = true; + + // Add a brief pause after successful command before starting calibration process + // This helps ensure the OpenCR has fully processed the command + rclcpp::sleep_for(std::chrono::milliseconds(500)); + + RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); + rclcpp::sleep_for(std::chrono::seconds(5)); + RCLCPP_INFO(this->get_logger(), "Calibration End"); } else { RCLCPP_WARN(this->get_logger(), "Failed to set IMU calibration: %s", sdk_msg.c_str()); } @@ -154,10 +163,6 @@ bool TurtleBot3::check_device_status() return false; } - RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); - rclcpp::sleep_for(std::chrono::seconds(5)); - RCLCPP_INFO(this->get_logger(), "Calibration End"); - const int8_t NOT_CONNECTED_MOTOR = -1; // Add error handling for device status check From 28a48c04c6dbdf271a41841157ed15792ead2f7e Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Mon, 12 May 2025 14:42:29 -0700 Subject: [PATCH 08/38] Remove error handling and logging from last few commits to declutter --- .../include/turtlebot3_node/turtlebot3.hpp | 2 +- turtlebot3_node/src/turtlebot3.cpp | 138 ++++-------------- 2 files changed, 33 insertions(+), 107 deletions(-) diff --git a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp index 821b7829c..425904f04 100644 --- a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp +++ b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp @@ -79,7 +79,7 @@ class TurtleBot3 : public rclcpp::Node private: void init_dynamixel_sdk_wrapper(const std::string & usb_port); - bool check_device_status(); + void check_device_status(); void add_sensors(); void add_devices(); diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 96a31a239..d10705a3a 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -18,7 +18,6 @@ #include #include -#include using robotis::turtlebot3::TurtleBot3; using namespace std::chrono_literals; @@ -29,28 +28,8 @@ TurtleBot3::TurtleBot3(const std::string & usb_port) RCLCPP_INFO(get_logger(), "Init TurtleBot3 Node Main"); node_handle_ = std::shared_ptr<::rclcpp::Node>(this, [](::rclcpp::Node *) {}); - bool initialization_successful = true; - - try { init_dynamixel_sdk_wrapper(usb_port); - - // Check device status with error handling but without exceptions - if (!check_device_status()) { - RCLCPP_ERROR(this->get_logger(), "Device initialization failed, shutting down"); - initialization_successful = false; - return; // Exit constructor early - } - - // Add stabilization delay after calibration - RCLCPP_INFO(this->get_logger(), "Waiting for IMU to stabilize after calibration..."); - rclcpp::sleep_for(std::chrono::milliseconds(500)); - - // Verify device connectivity before proceeding - if (!dxl_sdk_wrapper_->is_connected_to_device()) { - RCLCPP_ERROR(this->get_logger(), "Lost connection to device after calibration"); - initialization_successful = false; - return; // Exit constructor early - } + check_device_status(); add_motors(); add_wheels(); @@ -58,24 +37,6 @@ TurtleBot3::TurtleBot3(const std::string & usb_port) add_devices(); run(); - } catch (const std::exception & e) { - RCLCPP_ERROR(this->get_logger(), "Critical exception in TurtleBot3 initialization: %s", e.what()); - initialization_successful = false; - } catch (...) { - RCLCPP_ERROR(this->get_logger(), "Unknown critical exception in TurtleBot3 initialization"); - initialization_successful = false; - } - - // Handle shutdown outside of the try-catch if initialization failed - if (!initialization_successful) { - RCLCPP_ERROR(this->get_logger(), "Initialization failed, requesting shutdown"); - // Schedule shutdown instead of doing it immediately - auto shutdown_timer = this->create_wall_timer( - std::chrono::milliseconds(100), - []() { - rclcpp::shutdown(); - }); - } } TurtleBot3::Wheels * TurtleBot3::get_wheels() @@ -95,6 +56,7 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) this->declare_parameter("opencr.id"); this->declare_parameter("opencr.baud_rate"); this->declare_parameter("opencr.protocol_version"); + this->declare_parameter("namespace"); this->get_parameter_or("opencr.id", opencr.id, 200); this->get_parameter_or("opencr.baud_rate", opencr.baud_rate, 1000000); @@ -119,95 +81,59 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) ); } -bool TurtleBot3::check_device_status() +void TurtleBot3::check_device_status() { - if (!dxl_sdk_wrapper_->is_connected_to_device()) { - RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices"); - return false; - } - - std::string sdk_msg; - uint8_t reset = 1; - - // Add retry logic for setting calibration data with longer delays - bool calibration_set = false; - for (int retry = 0; retry < 3 && !calibration_set; retry++) { - if (retry > 0) { - RCLCPP_WARN(this->get_logger(), "Retrying IMU calibration setup (attempt %d of 3)", retry + 1); - // Increase delay to 2 seconds between attempts to give more recovery time - rclcpp::sleep_for(std::chrono::seconds(2)); - } + if (dxl_sdk_wrapper_->is_connected_to_device()) { + std::string sdk_msg; + uint8_t reset = 1; - sdk_msg.clear(); - if (dxl_sdk_wrapper_->set_data_to_device( + dxl_sdk_wrapper_->set_data_to_device( extern_control_table.imu_re_calibration.addr, extern_control_table.imu_re_calibration.length, &reset, - &sdk_msg)) { - calibration_set = true; - - // Add a brief pause after successful command before starting calibration process - // This helps ensure the OpenCR has fully processed the command - rclcpp::sleep_for(std::chrono::milliseconds(500)); + &sdk_msg); RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); rclcpp::sleep_for(std::chrono::seconds(5)); RCLCPP_INFO(this->get_logger(), "Calibration End"); } else { - RCLCPP_WARN(this->get_logger(), "Failed to set IMU calibration: %s", sdk_msg.c_str()); - } - } - - if (!calibration_set) { - RCLCPP_ERROR(this->get_logger(), "Failed to set IMU calibration after multiple attempts"); - return false; + RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices"); + rclcpp::shutdown(); + return; } const int8_t NOT_CONNECTED_MOTOR = -1; - // Add error handling for device status check - try { - int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( - extern_control_table.device_status.addr, - extern_control_table.device_status.length); + int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( + extern_control_table.device_status.addr, + extern_control_table.device_status.length); - switch (device_status) { - case NOT_CONNECTED_MOTOR: - RCLCPP_WARN(this->get_logger(), "Please double check your Dynamixels and Power"); - break; + switch (device_status) { + case NOT_CONNECTED_MOTOR: + RCLCPP_WARN(this->get_logger(), "Please double check your Dynamixels and Power"); + break; - default: - break; - } - } catch (const std::exception & e) { - RCLCPP_ERROR(this->get_logger(), "Failed to get device status: %s", e.what()); - return false; + default: + break; } - - return true; } void TurtleBot3::add_motors() { RCLCPP_INFO(this->get_logger(), "Add Motors"); - try { - this->declare_parameter("motors.profile_acceleration_constant"); - this->declare_parameter("motors.profile_acceleration"); - - this->get_parameter_or( - "motors.profile_acceleration_constant", - motors_.profile_acceleration_constant, - 214.577); - - this->get_parameter_or( - "motors.profile_acceleration", - motors_.profile_acceleration, - 0.0); - } catch (const std::exception & e) { - RCLCPP_ERROR(this->get_logger(), "Failed to add motors: %s", e.what()); - throw; - } + this->declare_parameter("motors.profile_acceleration_constant"); + this->declare_parameter("motors.profile_acceleration"); + + this->get_parameter_or( + "motors.profile_acceleration_constant", + motors_.profile_acceleration_constant, + 214.577); + + this->get_parameter_or( + "motors.profile_acceleration", + motors_.profile_acceleration, + 0.0); } void TurtleBot3::add_wheels() From 050835f641c16cc9ce4ff22688d43c9ca3bcd097 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Mon, 12 May 2025 15:41:11 -0700 Subject: [PATCH 09/38] Address launch file substitution error --- turtlebot3_bringup/launch/robot.launch.py | 5 ++++- turtlebot3_node/src/turtlebot3.cpp | 2 ++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/turtlebot3_bringup/launch/robot.launch.py b/turtlebot3_bringup/launch/robot.launch.py index fbb819a06..f819cdf72 100644 --- a/turtlebot3_bringup/launch/robot.launch.py +++ b/turtlebot3_bringup/launch/robot.launch.py @@ -96,7 +96,10 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource( - [ThisLaunchFileDir(), '/turtlebot3_state_publisher.launch.py']), + [os.path.join( + get_package_share_directory('turtlebot3_bringup'), + 'launch', + 'turtlebot3_state_publisher.launch.py')]), launch_arguments={'use_sim_time': use_sim_time, 'namespace': namespace}.items(), ), diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index d10705a3a..ea9f2624a 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -107,6 +107,8 @@ void TurtleBot3::check_device_status() int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( extern_control_table.device_status.addr, extern_control_table.device_status.length); + + RCLCPP_INFO(this->get_logger(), "Device Status integer: %d", device_status); switch (device_status) { case NOT_CONNECTED_MOTOR: From e3e7fe7f42b90e260dee33ee810346e7fe038764 Mon Sep 17 00:00:00 2001 From: Travis Date: Mon, 12 May 2025 16:14:48 -0700 Subject: [PATCH 10/38] Sleep 5 seconds after IMU calibration for consistent bringup success --- turtlebot3_node/src/turtlebot3.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index ea9f2624a..14012ace1 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -92,7 +92,7 @@ void TurtleBot3::check_device_status() extern_control_table.imu_re_calibration.length, &reset, &sdk_msg); - + RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); rclcpp::sleep_for(std::chrono::seconds(5)); RCLCPP_INFO(this->get_logger(), "Calibration End"); @@ -104,6 +104,11 @@ void TurtleBot3::check_device_status() const int8_t NOT_CONNECTED_MOTOR = -1; + // Add a sleep to allow device to fully initialize + rclcpp::sleep_for(std::chrono::seconds(5)); + + RCLCPP_INFO(this->get_logger(), "Retrieving device status integer"); + int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( extern_control_table.device_status.addr, extern_control_table.device_status.length); From 227e36f1dd28f5d188f595083ceb9fc3407e03da Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Wed, 14 May 2025 14:18:14 -0700 Subject: [PATCH 11/38] Test OpenCR heartbeat before reading device status --- turtlebot3_node/src/turtlebot3.cpp | 59 ++++++++++++++++++++++++------ 1 file changed, 48 insertions(+), 11 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 14012ace1..160a9bead 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -81,8 +81,20 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) ); } +/** + * @brief Checks the status of connected devices and performs device initialization. + * + * This method verifies the connection to devices, performs IMU gyro calibration, + * and checks the device status. If no connection is established, it logs an error + * and shuts down the ROS node. The method includes a device status check and + * provides a warning if motors are not connected. + * + * @note Includes a 5-second sleep to allow device initialization and calibration. + * @throws Shuts down ROS node if device connection fails. + */ void TurtleBot3::check_device_status() { + // Check if device is connected and perform IMU gyroscope calibration if (dxl_sdk_wrapper_->is_connected_to_device()) { std::string sdk_msg; uint8_t reset = 1; @@ -103,18 +115,43 @@ void TurtleBot3::check_device_status() } const int8_t NOT_CONNECTED_MOTOR = -1; - - // Add a sleep to allow device to fully initialize - rclcpp::sleep_for(std::chrono::seconds(5)); - - RCLCPP_INFO(this->get_logger(), "Retrieving device status integer"); - - int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( - extern_control_table.device_status.addr, - extern_control_table.device_status.length); + const int8_t TIME_TO_INITIALIZE = 5; // seconds + + // Perform heartbeat check to verify basic device communication + bool device_responding = false; + for (int attempt = 0; attempt < 3 && !device_responding; attempt++) { + uint8_t test_value = 42; // Distinctive test value + std::string sdk_msg; + + if (dxl_sdk_wrapper_->set_data_to_device( + extern_control_table.heartbeat.addr, + extern_control_table.heartbeat.length, + &test_value, + &sdk_msg)) { + RCLCPP_INFO(this->get_logger(), "Device responded to heartbeat: %s", sdk_msg.c_str()); + device_responding = true; + } else { + RCLCPP_WARN(this->get_logger(), "Device failed to respond to heartbeat on attempt %d, retrying in %d seconds", attempt + 1, TIME_TO_INITIALIZE); + rclcpp::sleep_for(std::chrono::seconds(TIME_TO_INITIALIZE)); + } + } + + // Only attempt to read device status if heartbeat succeeded + int8_t device_status = 0; + if (device_responding) { + try { + device_status = dxl_sdk_wrapper_->get_data_from_device( + extern_control_table.device_status.addr, + extern_control_table.device_status.length); + } catch (...) { + RCLCPP_WARN(this->get_logger(), "Failed to read device status despite successful heartbeat"); + device_status = 0; // Default to a safe value + } + } else { + RCLCPP_WARN(this->get_logger(), "Skipping device status read due to heartbeat failure"); + device_status = 0; // Default to a safe value + } - RCLCPP_INFO(this->get_logger(), "Device Status integer: %d", device_status); - switch (device_status) { case NOT_CONNECTED_MOTOR: RCLCPP_WARN(this->get_logger(), "Please double check your Dynamixels and Power"); From 888a3a75d5e5a0a43fb22bb6b7e5da76e4be3f55 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Thu, 15 May 2025 13:28:44 -0700 Subject: [PATCH 12/38] Retry motor connection check if device status shows motor not connected --- turtlebot3_node/src/turtlebot3.cpp | 56 +++++++----------------------- 1 file changed, 13 insertions(+), 43 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 160a9bead..38e731b26 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -114,51 +114,21 @@ void TurtleBot3::check_device_status() return; } + // Check if motors are connected, retry after longer interval if not const int8_t NOT_CONNECTED_MOTOR = -1; - const int8_t TIME_TO_INITIALIZE = 5; // seconds - - // Perform heartbeat check to verify basic device communication - bool device_responding = false; - for (int attempt = 0; attempt < 3 && !device_responding; attempt++) { - uint8_t test_value = 42; // Distinctive test value - std::string sdk_msg; + int8_t device_status = NOT_CONNECTED_MOTOR; + for (int i = 0; i < 4; i++) { + device_status = dxl_sdk_wrapper_->get_data_from_device( + extern_control_table.device_status.addr, + extern_control_table.device_status.length); - if (dxl_sdk_wrapper_->set_data_to_device( - extern_control_table.heartbeat.addr, - extern_control_table.heartbeat.length, - &test_value, - &sdk_msg)) { - RCLCPP_INFO(this->get_logger(), "Device responded to heartbeat: %s", sdk_msg.c_str()); - device_responding = true; - } else { - RCLCPP_WARN(this->get_logger(), "Device failed to respond to heartbeat on attempt %d, retrying in %d seconds", attempt + 1, TIME_TO_INITIALIZE); - rclcpp::sleep_for(std::chrono::seconds(TIME_TO_INITIALIZE)); - } - } - - // Only attempt to read device status if heartbeat succeeded - int8_t device_status = 0; - if (device_responding) { - try { - device_status = dxl_sdk_wrapper_->get_data_from_device( - extern_control_table.device_status.addr, - extern_control_table.device_status.length); - } catch (...) { - RCLCPP_WARN(this->get_logger(), "Failed to read device status despite successful heartbeat"); - device_status = 0; // Default to a safe value - } - } else { - RCLCPP_WARN(this->get_logger(), "Skipping device status read due to heartbeat failure"); - device_status = 0; // Default to a safe value - } - - switch (device_status) { - case NOT_CONNECTED_MOTOR: - RCLCPP_WARN(this->get_logger(), "Please double check your Dynamixels and Power"); - break; - - default: - break; + if (device_status == NOT_CONNECTED_MOTOR) { + RCLCPP_INFO(this->get_logger(), "Motors not yet initialized, retrying in %d seconds", i+1); + rclcpp::sleep_for(std::chrono::seconds(i+1)); + } else { + break; + } + RCLCPP_WARN(this->get_logger(), "Motors not initialized, please double check your Dynamixels and Power"); } } From d30d71a2dea0c9a7416097e9fdd6e58dec044c90 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Thu, 15 May 2025 13:32:37 -0700 Subject: [PATCH 13/38] Move motors warning out of for loop --- turtlebot3_node/src/turtlebot3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 38e731b26..5aef013a8 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -128,8 +128,8 @@ void TurtleBot3::check_device_status() } else { break; } - RCLCPP_WARN(this->get_logger(), "Motors not initialized, please double check your Dynamixels and Power"); } + RCLCPP_WARN(this->get_logger(), "Motors not initialized, please double check your Dynamixels and Power"); } void TurtleBot3::add_motors() From 7f530fb98a61c2b47a789b1e77b60933725f379b Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Thu, 15 May 2025 13:55:47 -0700 Subject: [PATCH 14/38] Only show motor warning when motors do not initialize --- turtlebot3_node/src/turtlebot3.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 5aef013a8..f16bb0363 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -126,10 +126,13 @@ void TurtleBot3::check_device_status() RCLCPP_INFO(this->get_logger(), "Motors not yet initialized, retrying in %d seconds", i+1); rclcpp::sleep_for(std::chrono::seconds(i+1)); } else { + RCLCPP_INFO(this->get_logger(), "Motors successfully initialized"); break; } } - RCLCPP_WARN(this->get_logger(), "Motors not initialized, please double check your Dynamixels and Power"); + if (device_status == NOT_CONNECTED_MOTOR) { + RCLCPP_WARN(this->get_logger(), "Motors not initialized, please double check your Dynamixels and Power"); + } } void TurtleBot3::add_motors() From 046bffade9bee93c5a4c6470852f5207a235e096 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Tue, 20 May 2025 12:27:42 -0700 Subject: [PATCH 15/38] Create and check fw device_ready flag before checking device_status --- .../include/turtlebot3_node/control_table.hpp | 3 +- turtlebot3_node/src/turtlebot3.cpp | 55 +++++++++++++------ 2 files changed, 41 insertions(+), 17 deletions(-) diff --git a/turtlebot3_node/include/turtlebot3_node/control_table.hpp b/turtlebot3_node/include/turtlebot3_node/control_table.hpp index 3155c6e66..53fdb61b5 100644 --- a/turtlebot3_node/include/turtlebot3_node/control_table.hpp +++ b/turtlebot3_node/include/turtlebot3_node/control_table.hpp @@ -46,8 +46,9 @@ typedef struct ControlItem baud_rate = {8, EEPROM, 1, READ}; ControlItem millis = {10, RAM, 4, READ}; - ControlItem micros = {14, RAM, 4, READ}; + // ControlItem micros = {14, RAM, 4, READ}; // Does not match firmware + ControlItem device_ready = {17, RAM, 1, READ}; ControlItem device_status = {18, RAM, 1, READ}; ControlItem heartbeat = {19, RAM, 1, READ_WRITE}; diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index f16bb0363..d04c7f644 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -114,24 +114,47 @@ void TurtleBot3::check_device_status() return; } - // Check if motors are connected, retry after longer interval if not - const int8_t NOT_CONNECTED_MOTOR = -1; - int8_t device_status = NOT_CONNECTED_MOTOR; - for (int i = 0; i < 4; i++) { - device_status = dxl_sdk_wrapper_->get_data_from_device( - extern_control_table.device_status.addr, - extern_control_table.device_status.length); - - if (device_status == NOT_CONNECTED_MOTOR) { - RCLCPP_INFO(this->get_logger(), "Motors not yet initialized, retrying in %d seconds", i+1); - rclcpp::sleep_for(std::chrono::seconds(i+1)); - } else { - RCLCPP_INFO(this->get_logger(), "Motors successfully initialized"); - break; + // Wait for device to be fully ready before checking status + bool device_ready = false; + const int MAX_RETRIES = 4; + int retries = 0; + + while (!device_ready && retries < MAX_RETRIES) { + try { + device_ready = dxl_sdk_wrapper_->get_data_from_device( + extern_control_table.device_ready.addr, + extern_control_table.device_ready.length); + + if (!device_ready) { + RCLCPP_INFO(this->get_logger(), "Waiting %d seconds for device to become ready... (%d/%d)", + retries+1, retries+1, MAX_RETRIES); + rclcpp::sleep_for(std::chrono::seconds(retries+1)); + retries++; } + } catch (...) { + RCLCPP_WARN(this->get_logger(), "Failed to read device_ready flag, retrying in %d seconds... (%d/%d)", + retries+1, retries+1, MAX_RETRIES); + rclcpp::sleep_for(std::chrono::seconds(retries+1)); + retries++; + } } - if (device_status == NOT_CONNECTED_MOTOR) { - RCLCPP_WARN(this->get_logger(), "Motors not initialized, please double check your Dynamixels and Power"); + + // Only check device_status if device is ready + if (device_ready) { + RCLCPP_INFO(this->get_logger(), "Device is ready, retrieving device status"); + int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( + extern_control_table.device_status.addr, + extern_control_table.device_status.length); + + // Check if motors are connected, retry after longer interval if not + const int8_t NOT_CONNECTED_MOTOR = -1; + if (device_status == NOT_CONNECTED_MOTOR) { + RCLCPP_INFO(this->get_logger(), "Motors not initialized"); + } else { + RCLCPP_INFO(this->get_logger(), "Motors successfully initialized"); + } + } else { + RCLCPP_WARN(this->get_logger(), "Device did not become ready in time, skipping device status check"); } } From 8cb70655c8d586ed233775cda167456d12dfaaad Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Sat, 31 May 2025 09:27:08 -0700 Subject: [PATCH 16/38] Slap on a hot messy fix to address bringup segfault --- turtlebot3_node/src/turtlebot3.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index d04c7f644..d78770370 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -98,6 +98,7 @@ void TurtleBot3::check_device_status() if (dxl_sdk_wrapper_->is_connected_to_device()) { std::string sdk_msg; uint8_t reset = 1; + uint8_t calibration_wait_time = 5; // seconds dxl_sdk_wrapper_->set_data_to_device( extern_control_table.imu_re_calibration.addr, @@ -105,9 +106,10 @@ void TurtleBot3::check_device_status() &reset, &sdk_msg); - RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); - rclcpp::sleep_for(std::chrono::seconds(5)); - RCLCPP_INFO(this->get_logger(), "Calibration End"); + RCLCPP_INFO(this->get_logger(), "Start %d seconds calibration of Gyro ", calibration_wait_time); + rclcpp::sleep_for(std::chrono::seconds(calibration_wait_time)); + RCLCPP_INFO(this->get_logger(), "Calibration End, waiting %d seconds for device to be ready", calibration_wait_time); + rclcpp::sleep_for(std::chrono::seconds(5)); // HOT MESS FIX } else { RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices"); rclcpp::shutdown(); From f0179c8506b63b963f695eb9e06d5965b89bb301 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Sat, 31 May 2025 09:34:29 -0700 Subject: [PATCH 17/38] Revert "Slap on a hot messy fix to address bringup segfault" This reverts commit 8cb70655c8d586ed233775cda167456d12dfaaad. --- turtlebot3_node/src/turtlebot3.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index d78770370..d04c7f644 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -98,7 +98,6 @@ void TurtleBot3::check_device_status() if (dxl_sdk_wrapper_->is_connected_to_device()) { std::string sdk_msg; uint8_t reset = 1; - uint8_t calibration_wait_time = 5; // seconds dxl_sdk_wrapper_->set_data_to_device( extern_control_table.imu_re_calibration.addr, @@ -106,10 +105,9 @@ void TurtleBot3::check_device_status() &reset, &sdk_msg); - RCLCPP_INFO(this->get_logger(), "Start %d seconds calibration of Gyro ", calibration_wait_time); - rclcpp::sleep_for(std::chrono::seconds(calibration_wait_time)); - RCLCPP_INFO(this->get_logger(), "Calibration End, waiting %d seconds for device to be ready", calibration_wait_time); - rclcpp::sleep_for(std::chrono::seconds(5)); // HOT MESS FIX + RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); + rclcpp::sleep_for(std::chrono::seconds(5)); + RCLCPP_INFO(this->get_logger(), "Calibration End"); } else { RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices"); rclcpp::shutdown(); From 789b9efdc0eac4357a209794480137078d156ebf Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Wed, 4 Jun 2025 16:04:03 -0700 Subject: [PATCH 18/38] Separate analog pins update rate from other sensors --- turtlebot3_node/src/turtlebot3.cpp | 27 +++++++++++++++++++++------ 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index d04c7f644..4df399545 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -246,11 +246,11 @@ void TurtleBot3::add_sensors() is_connected_ir, is_connected_sonar)); - sensors_.push_back( - new sensors::AnalogPins( - node_handle_, - "analog_pins")); - + // Create and store pointer to analog pins sensor + analog_pins_sensor_ = new sensors::AnalogPins( + node_handle_, + "analog_pins"); + sensors_.push_back(analog_pins_sensor_); RCLCPP_INFO(this->get_logger(), "Successfully added all sensors"); @@ -278,7 +278,8 @@ void TurtleBot3::run() { RCLCPP_INFO(this->get_logger(), "Run!"); - publish_timer(std::chrono::milliseconds(50)); + publish_timer(std::chrono::milliseconds(50)); // 20 Hz for other sensors + analog_pins_timer(std::chrono::milliseconds(250)); // 4 Hz for analog pins heartbeat_timer(std::chrono::milliseconds(100)); parameter_event_callback(); @@ -302,6 +303,20 @@ void TurtleBot3::publish_timer(const std::chrono::milliseconds timeout) ); } +void TurtleBot3::analog_pins_timer(const std::chrono::milliseconds timeout) +{ + analog_pins_timer_ = this->create_wall_timer( + timeout, + [this]() -> void + { + rclcpp::Time now = this->now(); + if (analog_pins_sensor_) { + analog_pins_sensor_->publish(now, dxl_sdk_wrapper_); + } + } + ); +} + void TurtleBot3::heartbeat_timer(const std::chrono::milliseconds timeout) { heartbeat_timer_ = this->create_wall_timer( From 344bd83898f5db84b07c9eef69865370a762f99e Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Wed, 4 Jun 2025 16:08:28 -0700 Subject: [PATCH 19/38] Add pointer and timer for analog pins to header file --- turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp index 425904f04..f4d0b77db 100644 --- a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp +++ b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp @@ -90,6 +90,7 @@ class TurtleBot3 : public rclcpp::Node void publish_timer(const std::chrono::milliseconds timeout); void heartbeat_timer(const std::chrono::milliseconds timeout); + void analog_pins_timer(const std::chrono::milliseconds timeout); void cmd_vel_callback(); void parameter_event_callback(); @@ -108,11 +109,14 @@ class TurtleBot3 : public rclcpp::Node rclcpp::TimerBase::SharedPtr publish_timer_; rclcpp::TimerBase::SharedPtr heartbeat_timer_; + rclcpp::TimerBase::SharedPtr analog_pins_timer_; std::unique_ptr cmd_vel_sub_; rclcpp::AsyncParametersClient::SharedPtr priv_parameters_client_; rclcpp::Subscription::SharedPtr parameter_event_sub_; + + sensors::AnalogPins * analog_pins_sensor_; }; } // namespace turtlebot3 } // namespace robotis From 98ff228ec3c40d930c72970bb0ab529f400ce6be Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Tue, 17 Jun 2025 11:54:48 -0700 Subject: [PATCH 20/38] Write ez-turtlebot3 README --- README.md | 57 +++++++++++++++++++++++++------------------------------ 1 file changed, 26 insertions(+), 31 deletions(-) diff --git a/README.md b/README.md index 011fb0f3b..0ef976be5 100644 --- a/README.md +++ b/README.md @@ -1,41 +1,36 @@ -# TurtleBot3 - +# Overview +This is a fork of [ROBOTIS-GIT/turtlebot3](https://github.com/ROBOTIS-GIT/turtlebot3). This `publish-analog-pins` branch extends the `turtlebot3_node` to broadcast data collected by the OpenCR analog pins A0-A5 to a ROS 2 topic `/analog_pins`. The `main` branch is kept in sync with the ROBOTIS repository. -- Active Branches: noetic, humble, jazzy, main(rolling) -- Legacy Branches: *-devel +This repo is part of the larger project [ez-turtlebot3](https://github.com/ez-turtlebot3/ez-turtlebot3), which combines this repo with analog-enabled [OpenCR](https://github.com/ez-turtlebot3/OpenCR) firmware and a [ROS 2 analog processor package](https://github.com/ez-turtlebot3/ez_analog_processor/branches) to read, process, and publish analog data while operating a TurtleBot3. + +# Requirements +* [ROS 2 Humble](https://docs.ros.org/en/humble/index.html) +* [Analog-enabled TurtleBot3 ROS 2 OpenCR firmware](https://github.com/ez-turtlebot3/OpenCR) -## Open Source Projects Related to TurtleBot3 -- [turtlebot3](https://github.com/ROBOTIS-GIT/turtlebot3) -- [turtlebot3_msgs](https://github.com/ROBOTIS-GIT/turtlebot3_msgs) -- [turtlebot3_simulations](https://github.com/ROBOTIS-GIT/turtlebot3_simulations) -- [turtlebot3_manipulation](https://github.com/ROBOTIS-GIT/turtlebot3_manipulation) -- [turtlebot3_manipulation_simulations](https://github.com/ROBOTIS-GIT/turtlebot3_manipulation_simulations) -- [turtlebot3_applications](https://github.com/ROBOTIS-GIT/turtlebot3_applications) -- [turtlebot3_applications_msgs](https://github.com/ROBOTIS-GIT/turtlebot3_applications_msgs) -- [turtlebot3_machine_learning](https://github.com/ROBOTIS-GIT/turtlebot3_machine_learning) -- [turtlebot3_autorace](https://github.com/ROBOTIS-GIT/turtlebot3_autorace) -- [turtlebot3_home_service_challenge](https://github.com/ROBOTIS-GIT/turtlebot3_home_service_challenge) -- [hls_lfcd_lds_driver](https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver) -- [ld08_driver](https://github.com/ROBOTIS-GIT/ld08_driver) -- [open_manipulator](https://github.com/ROBOTIS-GIT/open_manipulator) -- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) -- [OpenCR-Hardware](https://github.com/ROBOTIS-GIT/OpenCR-Hardware) -- [OpenCR](https://github.com/ROBOTIS-GIT/OpenCR) +# Installation +1. If you haven't already, follow the Humble instructions for the [TurtleBot3 SBC setup](https://emanual.robotis.com/docs/en/platform/turtlebot3/sbc_setup/#sbc-setup) +2. Replace the turtlebot3 repo with the analog-enabled fork + * `cd ~/turtlebot3_ws/src` + * `rm -r turtlebot3` + * `git clone https://github.com/ez-turtlebot3/turtlebot3` + * `cd turtlebot3` +3. Rebuild the turtlebot3_node + * `colcon build --symlink-install --packages-select turtlebot3_node --allow-overriding turtlebot3_node` -## Documentation, Videos, and Community +# Use +1. Launch the TurtleBot with the same bringup command as before + * `ros2 launch turtlebot3_bringup robot.launch.py` +2. View the /analog_pins topic from your remote pc with + * `ros2 topic echo /analog_pins` +3. Optionally, install the [ROS 2 analog processor package](https://github.com/ez-turtlebot3/ez_analog_processor/branches) to process the raw data from `/analog_pins`. +4. Recommendation: view real-time data in plots with [PlotJuggler](https://github.com/facontidavide/PlotJuggler) -### Official Documentation +# ROBOTIS links +## Official Documentation - ⚙️ **[ROBOTIS DYNAMIXEL](https://dynamixel.com/)** - 📚 **[ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/)** - 📚 **[ROBOTIS e-Manual for TurtleBot3](http://turtlebot3.robotis.com/)** -- 📚 **[ROBOTIS e-Manual for OpenMANIPULATOR-X](https://emanual.robotis.com/docs/en/platform/openmanipulator_x/overview/)** -### Learning Resources -- 🎥 **[ROBOTIS YouTube Channel](https://www.youtube.com/@ROBOTISCHANNEL)** -- 🎥 **[ROBOTIS Open Source YouTube Channel](https://www.youtube.com/@ROBOTISOpenSourceTeam)** -- 🎥 **[ROBOTIS TurtleBot3 YouTube Playlist](https://www.youtube.com/playlist?list=PLRG6WP3c31_XI3wlvHlx2Mp8BYqgqDURU)** -- 🎥 **[ROBOTIS OpenMANIPULATOR YouTube Playlist](https://www.youtube.com/playlist?list=PLRG6WP3c31_WpEsB6_Rdt3KhiopXQlUkb)** - -### Community & Support +## Community & Support - 💬 **[ROBOTIS Community Forum](https://forum.robotis.com/)** - 💬 **[TurtleBot category from ROS Community](https://discourse.ros.org/c/turtlebot/)** From b87f432897c563e7062ff89b827efe2c44cf8cdf Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Tue, 17 Jun 2025 19:05:00 -0700 Subject: [PATCH 21/38] Fix rebuild instructions Signed-off-by: Travis Mendoza --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 0ef976be5..d9d074ef3 100644 --- a/README.md +++ b/README.md @@ -11,11 +11,11 @@ This repo is part of the larger project [ez-turtlebot3](https://github.com/ez-tu 1. If you haven't already, follow the Humble instructions for the [TurtleBot3 SBC setup](https://emanual.robotis.com/docs/en/platform/turtlebot3/sbc_setup/#sbc-setup) 2. Replace the turtlebot3 repo with the analog-enabled fork * `cd ~/turtlebot3_ws/src` - * `rm -r turtlebot3` + * `rm -rf turtlebot3` * `git clone https://github.com/ez-turtlebot3/turtlebot3` - * `cd turtlebot3` 3. Rebuild the turtlebot3_node - * `colcon build --symlink-install --packages-select turtlebot3_node --allow-overriding turtlebot3_node` + * `cd ~/turtlebot3_ws` + * `colcon build --packages-select turtlebot3_node` # Use 1. Launch the TurtleBot with the same bringup command as before From c299617f3ad13eba6769774798d5c37f715ec7b3 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Thu, 19 Jun 2025 15:55:52 -0700 Subject: [PATCH 22/38] Invoke MPPI controller and SMAC Hybrid planner in nav2 burger params --- turtlebot3_navigation2/param/burger.yaml | 565 ++++++++++++----------- 1 file changed, 288 insertions(+), 277 deletions(-) diff --git a/turtlebot3_navigation2/param/burger.yaml b/turtlebot3_navigation2/param/burger.yaml index 711d09584..759f57d4f 100644 --- a/turtlebot3_navigation2/param/burger.yaml +++ b/turtlebot3_navigation2/param/burger.yaml @@ -1,5 +1,6 @@ amcl: ros__parameters: + use_sim_time: False alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 @@ -29,7 +30,7 @@ amcl: save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: true - transform_tolerance: 0.5 + transform_tolerance: 1.0 update_min_a: 0.2 update_min_d: 0.25 z_hit: 0.5 @@ -37,128 +38,173 @@ amcl: z_rand: 0.5 z_short: 0.05 scan_topic: scan - map_topic: map - set_initial_pose: false - always_reset_initial_pose: false - first_map_only: false - initial_pose: - x: 0.0 - y: 0.0 - z: 0.0 - yaw: 0.0 + +amcl_map_client: + ros__parameters: + use_sim_time: False + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: False bt_navigator: ros__parameters: + use_sim_time: False global_frame: map robot_base_frame: base_link - transform_tolerance: 0.5 - filter_duration: 0.3 - default_nav_to_pose_bt_xml: "$(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml" # behavior trees location. - default_nav_through_poses_bt_xml: "$(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml" - always_reload_bt_xml: false - goal_blackboard_id: goal - goals_blackboard_id: goals - path_blackboard_id: path - navigators: ['navigate_to_pose', 'navigate_through_poses'] - navigate_to_pose: - plugin: "nav2_bt_navigator::NavigateToPoseNavigator" - navigate_through_poses: - plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" - error_code_name_prefixes: - - assisted_teleop - - backup - - compute_path - - dock_robot - - drive_on_heading - - follow_path - - nav_thru_poses - - nav_to_pose - - spin - - route - - undock_robot - - wait + odom_topic: /odom + default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" + bt_loop_duration: 10 + default_server_timeout: 20 + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node -docking_server: +bt_navigator_rclcpp_node: ros__parameters: - dock_plugins: ['nova_carter_dock'] - nova_carter_dock: - plugin: 'opennav_docking::SimpleChargingDock' - docks: ['home_dock','flex_dock1', 'flex_dock2'] - home_dock: - type: 'nova_carter_dock' - frame: map - pose: [0.0, 0.0, 0.0] - flex_dock1: - type: 'nova_carter_dock' - frame: map - pose: [10.0, 10.0, 0.0] - flex_dock2: - type: 'nova_carter_dock' - frame: map - pose: [30.0, 30.0, 0.0] - enable_stamped_cmd_vel: true + use_sim_time: False controller_server: ros__parameters: - controller_frequency: 10.0 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.001 - min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.3 - progress_checker_plugins: ["progress_checker"] - goal_checker_plugins: ["goal_checker"] - controller_plugins: ["FollowPath"] - progress_checker: - plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.1 - movement_time_allowance: 10.0 - goal_checker: - stateful: true - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 + controller_frequency: 20.0 FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: true - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.3 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.3 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 3.0 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - vx_samples: 20 - vy_samples: 0 - vtheta_samples: 40 - sim_time: 1.5 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.05 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: true - stateful: true - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 - enable_stamped_cmd_vel: true + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.2 # OOB: 0.5 + vx_min: -0.35 + vy_max: 0.2 # OOB: 0.5 + wz_max: 1.9 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + az_max: 3.5 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: false + reset_period: 1.0 # (only in Humble) + regenerate_noises: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + # ObstaclesCritic: + # enabled: true + # cost_power: 1 + # repulsion_weight: 1.5 + # critical_weight: 20.0 + # consider_footprint: false + # collision_cost: 10000.0 + # collision_margin_distance: 0.1 + # near_goal_distance: 0.5 + # inflation_radius: 0.55 # (only in Humble) + # cost_scaling_factor: 10.0 # (only in Humble) + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: true + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + # VelocityDeadbandCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 35.0 + # deadband_velocities: [0.05, 0.05, 0.05] + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: False local_costmap: local_costmap: @@ -167,30 +213,31 @@ local_costmap: publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link + use_sim_time: False rolling_window: true width: 3 height: 3 resolution: 0.05 - robot_radius: 0.1 + robot_radius: 0.105 plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - inflation_radius: 0.5 - cost_scaling_factor: 5.0 + inflation_radius: 1.0 + cost_scaling_factor: 3.0 obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: true + enabled: True observation_sources: scan scan: topic: /scan max_obstacle_height: 2.0 - clearing: true - marking: true + clearing: True + marking: True data_type: "LaserScan" voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" - enabled: true - publish_voxel_map: true + enabled: True + publish_voxel_map: True origin_z: 0.0 z_resolution: 0.05 z_voxels: 16 @@ -200,16 +247,22 @@ local_costmap: scan: topic: /scan max_obstacle_height: 2.0 - clearing: true - marking: true + clearing: True + marking: True data_type: "LaserScan" raytrace_max_range: 3.0 raytrace_min_range: 0.0 obstacle_max_range: 2.5 obstacle_min_range: 0.0 static_layer: - map_subscribe_transient_local: true - always_send_full_costmap: true + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False global_costmap: global_costmap: @@ -218,20 +271,20 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - transform_tolerance: 0.5 - robot_radius: 0.1 + use_sim_time: True + robot_radius: 0.105 resolution: 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: true + enabled: True observation_sources: scan scan: topic: /scan max_obstacle_height: 2.0 - clearing: true - marking: true + clearing: True + marking: True data_type: "LaserScan" raytrace_max_range: 3.0 raytrace_min_range: 0.0 @@ -239,8 +292,8 @@ global_costmap: obstacle_min_range: 0.0 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" - enabled: true - publish_voxel_map: true + enabled: True + publish_voxel_map: True origin_z: 0.0 z_resolution: 0.05 z_voxels: 16 @@ -250,8 +303,8 @@ global_costmap: scan: topic: /scan max_obstacle_height: 2.0 - clearing: true - marking: true + clearing: True + marking: True data_type: "LaserScan" raytrace_max_range: 3.0 raytrace_min_range: 0.0 @@ -259,175 +312,133 @@ global_costmap: obstacle_min_range: 0.0 static_layer: plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: true - transform_tolerance: 0.1 + map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - inflation_radius: 0.5 - cost_scaling_factor: 5.0 - always_send_full_costmap: true + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False map_server: ros__parameters: + use_sim_time: False yaml_filename: "map.yaml" map_saver: ros__parameters: + use_sim_time: False save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 - map_subscribe_transient_local: true + map_subscribe_transient_local: True planner_server: ros__parameters: - expected_planner_frequency: 10.0 planner_plugins: ["GridBased"] + use_sim_time: True + GridBased: - plugin: "nav2_navfn_planner::NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true + plugin: "nav2_smac_planner/SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::" + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # max time in s for planner to plan, smooth + motion_model_for_search: "REEDS_SHEPP" # Hybrid-A* Dubin, Redds-Shepp + angle_quantization_bins: 72 # Number of angle bins for search + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal + analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + minimum_turning_radius: 0.2 # minimum turning radius in m of path / vehicle + reverse_penalty: 1.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + retrospective_penalty: 0.015 + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. + use_quadratic_cost_penalty: False + downsample_obstacle_heuristic: True + allow_primitive_interpolation: False + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path -behavior_server: + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +recoveries_server: ros__parameters: - local_costmap_topic: local_costmap/costmap_raw - local_footprint_topic: local_costmap/published_footprint - global_costmap_topic: global_costmap/costmap_raw - global_footprint_topic: global_costmap/published_footprint + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "wait", "assisted_teleop"] - spin: - plugin: "nav2_behaviors::Spin" # In Iron and older versions, "/" was used instead of "::" - backup: - plugin: "nav2_behaviors::BackUp" # In Iron and older versions, "/" was used instead of "::" - drive_on_heading: - plugin: "nav2_behaviors::DriveOnHeading" # In Iron and older versions, "/" was used instead of "::" - wait: - plugin: "nav2_behaviors::Wait" # In Iron and older versions, "/" was used instead of "::" - assisted_teleop: - plugin: "nav2_behaviors::AssistedTeleop" # In Iron and older versions, "/" was used instead of "::" - local_frame: odom - global_frame: map + recovery_plugins: ["wait", "backup", "rotate1", "rotate2", "rotate3"] # Reordered and added multiple rotates + global_frame: odom robot_base_frame: base_link transform_timeout: 0.1 - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - enable_stamped_cmd_vel: true + use_sim_time: false + # simulate_ahead_time: 2.0 # This parameter seems to be misplaced, it belongs to backup/spin, but backup/spin do not use simulate_ahead_time + # # max_rotational_vel: 1.0 # Not needed here, as set in each spin config + # # min_rotational_vel: 0.4 + # # rotational_acc_lim: 3.2 + + wait: + plugin: "nav2_recoveries/Wait" + ros__parameters: + wait_duration: 5.0 # Wait for 5 seconds + + backup: + plugin: "nav2_recoveries/BackUp" + # ros__parameters: + # backup_dist: 0.15 # Backup distance in meters (adjust as needed) + # backup_speed: 0.1 # Backup speed in m/s (adjust as needed) + # #simulate_ahead_time: 2.0 #Not used in BackUp plugin + + rotate1: # First 90-degree rotation + plugin: "nav2_recoveries/Spin" + ros__parameters: + yaw_rot_vel: 1.0 # Desired rotational speed + target_yaw: 1.57 # Target angle (90 degrees in radians) + + rotate2: # Second 90-degree rotation + plugin: "nav2_recoveries/Spin" + ros__parameters: + yaw_rot_vel: 1.0 + target_yaw: 3.14 # Target angle (180 degrees in radians) + + rotate3: # Third 90 degree rotation + plugin: "nav2_recoveries/Spin" + ros__parameters: + yaw_rot_vel: 1.0 + target_yaw: 4.71 # Target angle (270 degrees in radians) + +robot_state_publisher: + ros__parameters: + use_sim_time: False waypoint_follower: ros__parameters: - loop_rate: 20 + loop_rate: 2000 stop_on_failure: false - waypoint_task_executor_plugin: "wait_at_waypoint" + waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: true - waypoint_pause_duration: 200 - -collision_monitor: - ros__parameters: - base_frame_id: "base_footprint" - odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_smoothed" - cmd_vel_out_topic: "cmd_vel" - state_topic: "collision_monitor_state" - transform_tolerance: 0.5 - source_timeout: 5.0 - base_shift_correction: true - stop_pub_timeout: 2.0 - enable_stamped_cmd_vel: true - use_realtime_priority: false - polygons: ["PolygonStop", "PolygonSlow", "FootprintApproach"] - PolygonStop: - type: "circle" - radius: 0.1 - action_type: "stop" - min_points: 4 - visualize: true - polygon_pub_topic: "polygon_stop" - enabled: true - PolygonSlow: - type: "polygon" - points: "[[0.1, 0.1], [0.1, -0.1], [-0.1, -0.1], [-0.1, 0.1]]" - action_type: "slowdown" - min_points: 4 - slowdown_ratio: 0.3 - visualize: true - polygon_pub_topic: "polygon_slowdown" - enabled: true - PolygonLimit: - type: "polygon" - points: "[[0.1, 0.1], [0.1, -0.1], [-0.1, -0.1], [-0.1, 0.1]]" - action_type: "limit" - min_points: 4 - linear_limit: 0.4 - angular_limit: 0.5 - visualize: true - polygon_pub_topic: "polygon_limit" - enabled: true - FootprintApproach: - type: "polygon" - action_type: "approach" - footprint_topic: "/local_costmap/published_footprint" - time_before_collision: 2.0 - simulation_time_step: 0.02 - min_points: 6 - visualize: False - enabled: true - VelocityPolygonStop: - type: "velocity_polygon" - action_type: "stop" - min_points: 6 - visualize: true - enabled: False - polygon_pub_topic: "velocity_polygon_stop" - velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stopped"] - holonomic: false - rotation: - points: "[[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]" - linear_min: 0.0 - linear_max: 0.05 - theta_min: -1.0 - theta_max: 1.0 - translation_forward: - points: "[[0.35, 0.3], [0.35, -0.3], [-0.2, -0.3], [-0.2, 0.3]]" - linear_min: 0.0 - linear_max: 1.0 - theta_min: -1.0 - theta_max: 1.0 - translation_backward: - points: "[[0.2, 0.3], [0.2, -0.3], [-0.35, -0.3], [-0.35, 0.3]]" - linear_min: -1.0 - linear_max: 0.0 - theta_min: -1.0 - theta_max: 1.0 - stopped: - points: "[[0.25, 0.25], [0.25, -0.25], [-0.25, -0.25], [-0.25, 0.25]]" - linear_min: -1.0 - linear_max: 1.0 - theta_min: -1.0 - theta_max: 1.0 - observation_sources: ["scan"] - scan: - source_timeout: 0.2 - type: "scan" - topic: "/scan" - enabled: true - -velocity_smoother: - ros__parameters: - smoothing_frequency: 20.0 - scale_velocities: false - feedback: "OPEN_LOOP" - max_velocity: [0.5, 0.0, 2.5] - min_velocity: [-0.5, 0.0, -2.5] - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] - odom_topic: "odom" - odom_duration: 0.1 - use_realtime_priority: false - enable_stamped_cmd_vel: true + enabled: True + waypoint_pause_duration: 200 \ No newline at end of file From 41c9ce9cc19abfff7752c8ec3aef772daf3546e6 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Tue, 15 Jul 2025 17:45:38 -0700 Subject: [PATCH 23/38] add .vscode to gitignore --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 979cf4c2a..e2b42f231 100644 --- a/.gitignore +++ b/.gitignore @@ -59,3 +59,6 @@ build-*/ *.txt.user *.gch /.project + +# IDE specific files +.vscode/ From ac70b2d6e7038d4b44030414c0545b0696ccf024 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Tue, 15 Jul 2025 18:02:07 -0700 Subject: [PATCH 24/38] Add AGENT.md to work with Sourcegraph Amp --- AGENT.md | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 AGENT.md diff --git a/AGENT.md b/AGENT.md new file mode 100644 index 000000000..bd554e6a5 --- /dev/null +++ b/AGENT.md @@ -0,0 +1,23 @@ +# TurtleBot3 ROS 2 Codebase Guide + +## Build Commands +- **Build single package**: `colcon build --packages-select turtlebot3_node` +- **Build all**: `colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release` +- **Source workspace**: `source install/setup.bash` +- **Launch robot**: Launching the robot is only possible from the turtlebot's raspberry pi. You, the agentic coding tool, only operate on the codebase from the remote PC. Therefore launching is not something you can do. +- **View analog data**: `ros2 topic echo /analog_pins` + +## Architecture +- **Main packages**: `turtlebot3_node` (core driver), `turtlebot3_bringup` (launch files), `turtlebot3_description` (URDF), `turtlebot3_navigation2`, `turtlebot3_teleop` +- **Core class**: `robotis::turtlebot3::TurtleBot3` - main node managing sensors, devices, motors +- **New feature**: Analog pins A0-A5 published to `/analog_pins` topic via `AnalogPins` sensor class +- **Communication**: Uses DynamixelSDKWrapper to read from OpenCR control table addresses 350-360 for analog pins +- **Dependencies**: ROS 2 Humble, Dynamixel SDK, custom OpenCR firmware with analog support + +## Code Style +- **Standard**: C++17, follows ROS 2 conventions +- **Naming**: snake_case for variables/functions, PascalCase for classes, namespaces: `robotis::turtlebot3::` +- **Includes**: System includes first, then ROS msgs, then local headers with relative paths +- **Error handling**: try-catch blocks with RCLCPP_ERROR logging +- **Memory**: Smart pointers (std::shared_ptr, std::unique_ptr), RAII patterns +- **Formatting**: 2-space indentation, header guards with full path, Apache 2.0 license headers From 807ee2125ef30f5e760c3d4b658f15c5e7d0c8ed Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Tue, 15 Jul 2025 18:13:47 -0700 Subject: [PATCH 25/38] Comment out duplicate init_read_memory call for analog pins Testing hypothesis that dual init_read_memory calls are causing bringup and SLAM issues. This removes the second call while keeping analog pins control table entries for future re-enablement. Amp-Thread: https://ampcode.com/threads/T-a05ec5f0-de5e-4ac9-a1b6-20e7ee67bf93 Co-authored-by: Amp --- turtlebot3_node/src/turtlebot3.cpp | 46 +++++++++++++++--------------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 4df399545..c2575c876 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -74,11 +74,11 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) ); // Add a separate initialization for the analog pins range - dxl_sdk_wrapper_->init_read_memory( - extern_control_table.analog_a0.addr, - (extern_control_table.analog_a5.addr - extern_control_table.analog_a0.addr) + - extern_control_table.analog_a5.length - ); + // dxl_sdk_wrapper_->init_read_memory( + // extern_control_table.analog_a0.addr, + // (extern_control_table.analog_a5.addr - extern_control_table.analog_a0.addr) + + // extern_control_table.analog_a5.length + // ); } /** @@ -247,10 +247,10 @@ void TurtleBot3::add_sensors() is_connected_sonar)); // Create and store pointer to analog pins sensor - analog_pins_sensor_ = new sensors::AnalogPins( - node_handle_, - "analog_pins"); - sensors_.push_back(analog_pins_sensor_); + // analog_pins_sensor_ = new sensors::AnalogPins( + // node_handle_, + // "analog_pins"); + // sensors_.push_back(analog_pins_sensor_); RCLCPP_INFO(this->get_logger(), "Successfully added all sensors"); @@ -279,7 +279,7 @@ void TurtleBot3::run() RCLCPP_INFO(this->get_logger(), "Run!"); publish_timer(std::chrono::milliseconds(50)); // 20 Hz for other sensors - analog_pins_timer(std::chrono::milliseconds(250)); // 4 Hz for analog pins + // analog_pins_timer(std::chrono::milliseconds(250)); // 4 Hz for analog pins heartbeat_timer(std::chrono::milliseconds(100)); parameter_event_callback(); @@ -303,19 +303,19 @@ void TurtleBot3::publish_timer(const std::chrono::milliseconds timeout) ); } -void TurtleBot3::analog_pins_timer(const std::chrono::milliseconds timeout) -{ - analog_pins_timer_ = this->create_wall_timer( - timeout, - [this]() -> void - { - rclcpp::Time now = this->now(); - if (analog_pins_sensor_) { - analog_pins_sensor_->publish(now, dxl_sdk_wrapper_); - } - } - ); -} +// void TurtleBot3::analog_pins_timer(const std::chrono::milliseconds timeout) +// { +// analog_pins_timer_ = this->create_wall_timer( +// timeout, +// [this]() -> void +// { +// rclcpp::Time now = this->now(); +// if (analog_pins_sensor_) { +// analog_pins_sensor_->publish(now, dxl_sdk_wrapper_); +// } +// } +// ); +// } void TurtleBot3::heartbeat_timer(const std::chrono::milliseconds timeout) { From 084be224b6720a672d371c509aeae6fcabde6d6e Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Wed, 16 Jul 2025 10:02:03 -0700 Subject: [PATCH 26/38] Checkout turtlebot3.cpp from 'humble' branch for baseline --- turtlebot3_node/src/turtlebot3.cpp | 127 +++++++---------------------- 1 file changed, 28 insertions(+), 99 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index c2575c876..55bb2e31b 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -28,15 +28,15 @@ TurtleBot3::TurtleBot3(const std::string & usb_port) RCLCPP_INFO(get_logger(), "Init TurtleBot3 Node Main"); node_handle_ = std::shared_ptr<::rclcpp::Node>(this, [](::rclcpp::Node *) {}); - init_dynamixel_sdk_wrapper(usb_port); - check_device_status(); + init_dynamixel_sdk_wrapper(usb_port); + check_device_status(); - add_motors(); - add_wheels(); - add_sensors(); - add_devices(); + add_motors(); + add_wheels(); + add_sensors(); + add_devices(); - run(); + run(); } TurtleBot3::Wheels * TurtleBot3::get_wheels() @@ -66,35 +66,15 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) dxl_sdk_wrapper_ = std::make_shared(opencr); - // Initialize the main control table range (original range) dxl_sdk_wrapper_->init_read_memory( extern_control_table.millis.addr, (extern_control_table.profile_acceleration_right.addr - extern_control_table.millis.addr) + extern_control_table.profile_acceleration_right.length ); - - // Add a separate initialization for the analog pins range - // dxl_sdk_wrapper_->init_read_memory( - // extern_control_table.analog_a0.addr, - // (extern_control_table.analog_a5.addr - extern_control_table.analog_a0.addr) + - // extern_control_table.analog_a5.length - // ); } -/** - * @brief Checks the status of connected devices and performs device initialization. - * - * This method verifies the connection to devices, performs IMU gyro calibration, - * and checks the device status. If no connection is established, it logs an error - * and shuts down the ROS node. The method includes a device status check and - * provides a warning if motors are not connected. - * - * @note Includes a 5-second sleep to allow device initialization and calibration. - * @throws Shuts down ROS node if device connection fails. - */ void TurtleBot3::check_device_status() { - // Check if device is connected and perform IMU gyroscope calibration if (dxl_sdk_wrapper_->is_connected_to_device()) { std::string sdk_msg; uint8_t reset = 1; @@ -105,56 +85,28 @@ void TurtleBot3::check_device_status() &reset, &sdk_msg); - RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); - rclcpp::sleep_for(std::chrono::seconds(5)); - RCLCPP_INFO(this->get_logger(), "Calibration End"); - } else { - RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices"); - rclcpp::shutdown(); - return; + RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); + rclcpp::sleep_for(std::chrono::seconds(5)); + RCLCPP_INFO(this->get_logger(), "Calibration End"); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices"); + rclcpp::shutdown(); + return; } - // Wait for device to be fully ready before checking status - bool device_ready = false; - const int MAX_RETRIES = 4; - int retries = 0; - - while (!device_ready && retries < MAX_RETRIES) { - try { - device_ready = dxl_sdk_wrapper_->get_data_from_device( - extern_control_table.device_ready.addr, - extern_control_table.device_ready.length); - - if (!device_ready) { - RCLCPP_INFO(this->get_logger(), "Waiting %d seconds for device to become ready... (%d/%d)", - retries+1, retries+1, MAX_RETRIES); - rclcpp::sleep_for(std::chrono::seconds(retries+1)); - retries++; - } - } catch (...) { - RCLCPP_WARN(this->get_logger(), "Failed to read device_ready flag, retrying in %d seconds... (%d/%d)", - retries+1, retries+1, MAX_RETRIES); - rclcpp::sleep_for(std::chrono::seconds(retries+1)); - retries++; - } - } - - // Only check device_status if device is ready - if (device_ready) { - RCLCPP_INFO(this->get_logger(), "Device is ready, retrieving device status"); - int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( - extern_control_table.device_status.addr, - extern_control_table.device_status.length); - - // Check if motors are connected, retry after longer interval if not - const int8_t NOT_CONNECTED_MOTOR = -1; - if (device_status == NOT_CONNECTED_MOTOR) { - RCLCPP_INFO(this->get_logger(), "Motors not initialized"); - } else { - RCLCPP_INFO(this->get_logger(), "Motors successfully initialized"); - } - } else { - RCLCPP_WARN(this->get_logger(), "Device did not become ready in time, skipping device status check"); + const int8_t NOT_CONNECTED_MOTOR = -1; + + int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( + extern_control_table.device_status.addr, + extern_control_table.device_status.length); + + switch (device_status) { + case NOT_CONNECTED_MOTOR: + RCLCPP_WARN(this->get_logger(), "Please double check your Dynamixels and Power"); + break; + + default: + break; } } @@ -246,14 +198,6 @@ void TurtleBot3::add_sensors() is_connected_ir, is_connected_sonar)); - // Create and store pointer to analog pins sensor - // analog_pins_sensor_ = new sensors::AnalogPins( - // node_handle_, - // "analog_pins"); - // sensors_.push_back(analog_pins_sensor_); - - RCLCPP_INFO(this->get_logger(), "Successfully added all sensors"); - dxl_sdk_wrapper_->read_data_set(); sensors_.push_back( new sensors::JointState( @@ -278,8 +222,7 @@ void TurtleBot3::run() { RCLCPP_INFO(this->get_logger(), "Run!"); - publish_timer(std::chrono::milliseconds(50)); // 20 Hz for other sensors - // analog_pins_timer(std::chrono::milliseconds(250)); // 4 Hz for analog pins + publish_timer(std::chrono::milliseconds(50)); heartbeat_timer(std::chrono::milliseconds(100)); parameter_event_callback(); @@ -303,20 +246,6 @@ void TurtleBot3::publish_timer(const std::chrono::milliseconds timeout) ); } -// void TurtleBot3::analog_pins_timer(const std::chrono::milliseconds timeout) -// { -// analog_pins_timer_ = this->create_wall_timer( -// timeout, -// [this]() -> void -// { -// rclcpp::Time now = this->now(); -// if (analog_pins_sensor_) { -// analog_pins_sensor_->publish(now, dxl_sdk_wrapper_); -// } -// } -// ); -// } - void TurtleBot3::heartbeat_timer(const std::chrono::milliseconds timeout) { heartbeat_timer_ = this->create_wall_timer( From 1aec572bec711b8cc72344945359ee6f008284e4 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Wed, 16 Jul 2025 10:36:50 -0700 Subject: [PATCH 27/38] Expand memory range in init_read_memory --- turtlebot3_node/src/turtlebot3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 55bb2e31b..f0edef581 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -68,8 +68,8 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) dxl_sdk_wrapper_->init_read_memory( extern_control_table.millis.addr, - (extern_control_table.profile_acceleration_right.addr - extern_control_table.millis.addr) + - extern_control_table.profile_acceleration_right.length + (extern_control_table.analog_a5.addr - extern_control_table.millis.addr) + + extern_control_table.analog_a5.length ); } From 402b765141300b4f09301bb7ed4a7627ae66d288 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Wed, 16 Jul 2025 11:37:26 -0700 Subject: [PATCH 28/38] Replace analog sensors with analog pins --- .../include/turtlebot3_node/control_table.hpp | 30 +++-- turtlebot3_node/src/turtlebot3.cpp | 124 +++++++++++------- 2 files changed, 96 insertions(+), 58 deletions(-) diff --git a/turtlebot3_node/include/turtlebot3_node/control_table.hpp b/turtlebot3_node/include/turtlebot3_node/control_table.hpp index 53fdb61b5..03d24fe4f 100644 --- a/turtlebot3_node/include/turtlebot3_node/control_table.hpp +++ b/turtlebot3_node/include/turtlebot3_node/control_table.hpp @@ -52,10 +52,10 @@ typedef struct ControlItem device_status = {18, RAM, 1, READ}; ControlItem heartbeat = {19, RAM, 1, READ_WRITE}; - ControlItem external_led_1 = {20, RAM, 1, READ_WRITE}; - ControlItem external_led_2 = {21, RAM, 1, READ_WRITE}; - ControlItem external_led_3 = {22, RAM, 1, READ_WRITE}; - ControlItem external_led_4 = {23, RAM, 1, READ_WRITE}; + ControlItem external_led_1 = {20, RAM, 1, READ_WRITE}; // FW: ADDR_USER_LED_1 + ControlItem external_led_2 = {21, RAM, 1, READ_WRITE}; // FW: ADDR_USER_LED_2 + ControlItem external_led_3 = {22, RAM, 1, READ_WRITE}; // FW: ADDR_USER_LED_3 + ControlItem external_led_4 = {23, RAM, 1, READ_WRITE}; // FW: ADDR_USER_LED_4 ControlItem button_1 = {26, RAM, 1, READ}; ControlItem button_2 = {27, RAM, 1, READ}; @@ -63,9 +63,16 @@ typedef struct ControlItem bumper_1 = {28, RAM, 1, READ}; ControlItem bumper_2 = {29, RAM, 1, READ}; - ControlItem illumination = {30, RAM, 4, READ}; - ControlItem ir = {34, RAM, 4, READ}; - ControlItem sonar = {38, RAM, 4, READ}; + // ControlItem illumination = {30, RAM, 4, READ}; + // ControlItem ir = {34, RAM, 4, READ}; + // ControlItem sonar = {38, RAM, 4, READ}; + + ControlItem analog_a0 = {30, RAM, 2, READ}; + ControlItem analog_a1 = {32, RAM, 2, READ}; + ControlItem analog_a2 = {34, RAM, 2, READ}; + ControlItem analog_a3 = {36, RAM, 2, READ}; + ControlItem analog_a4 = {38, RAM, 2, READ}; + ControlItem analog_a5 = {40, RAM, 2, READ}; ControlItem battery_voltage = {42, RAM, 4, READ}; ControlItem battery_percentage = {46, RAM, 4, READ}; @@ -95,7 +102,8 @@ typedef struct ControlItem present_position_left = {136, RAM, 4, READ}; ControlItem present_position_right = {140, RAM, 4, READ}; - ControlItem motor_torque_enable = {149, RAM, 1, READ_WRITE}; + // Missing: ADDR_MOTOR_CONNECT = 148 (FW has this but control table doesn't) + ControlItem motor_torque_enable = {149, RAM, 1, READ_WRITE}; // FW: ADDR_MOTOR_TORQUE ControlItem cmd_velocity_linear_x = {150, RAM, 4, READ_WRITE}; ControlItem cmd_velocity_linear_y = {154, RAM, 4, READ_WRITE}; @@ -107,12 +115,6 @@ typedef struct ControlItem profile_acceleration_left = {174, RAM, 4, READ_WRITE}; ControlItem profile_acceleration_right = {178, RAM, 4, READ_WRITE}; - ControlItem analog_a0 = {350, RAM, 2, READ}; - ControlItem analog_a1 = {352, RAM, 2, READ}; - ControlItem analog_a2 = {354, RAM, 2, READ}; - ControlItem analog_a3 = {356, RAM, 2, READ}; - ControlItem analog_a4 = {358, RAM, 2, READ}; - ControlItem analog_a5 = {360, RAM, 2, READ}; } ControlTable; const ControlTable extern_control_table; diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index f0edef581..7a190b8b2 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -68,8 +68,8 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) dxl_sdk_wrapper_->init_read_memory( extern_control_table.millis.addr, - (extern_control_table.analog_a5.addr - extern_control_table.millis.addr) + - extern_control_table.analog_a5.length + (extern_control_table.profile_acceleration_right.addr - extern_control_table.millis.addr) + + extern_control_table.profile_acceleration_right.length ); } @@ -139,43 +139,81 @@ void TurtleBot3::add_wheels() this->get_parameter_or("wheels.radius", wheels_.radius, 0.033); } +// Original add_sensors function - commented out for analog pins implementation +// void TurtleBot3::add_sensors() +// { +// RCLCPP_INFO(this->get_logger(), "Add Sensors"); + +// uint8_t is_connected_bumper_1 = 0; +// uint8_t is_connected_bumper_2 = 0; +// uint8_t is_connected_illumination = 0; +// uint8_t is_connected_ir = 0; +// uint8_t is_connected_sonar = 0; + +// this->declare_parameter("sensors.bumper_1"); +// this->declare_parameter("sensors.bumper_2"); +// this->declare_parameter("sensors.illumination"); +// this->declare_parameter("sensors.ir"); +// this->declare_parameter("sensors.sonar"); + +// this->get_parameter_or( +// "sensors.bumper_1", +// is_connected_bumper_1, +// 0); +// this->get_parameter_or( +// "sensors.bumper_2", +// is_connected_bumper_2, +// 0); +// this->get_parameter_or( +// "sensors.illumination", +// is_connected_illumination, +// 0); +// this->get_parameter_or( +// "sensors.ir", +// is_connected_ir, +// 0); +// this->get_parameter_or( +// "sensors.sonar", +// is_connected_sonar, +// 0); + +// sensors_.push_back( +// new sensors::BatteryState( +// node_handle_, +// "battery_state")); + +// sensors_.push_back( +// new sensors::Imu( +// node_handle_, +// "imu", +// "magnetic_field", +// "imu_link")); + +// sensors_.push_back( +// new sensors::SensorState( +// node_handle_, +// "sensor_state", +// is_connected_bumper_1, +// is_connected_bumper_2, +// is_connected_illumination, +// is_connected_ir, +// is_connected_sonar)); + +// dxl_sdk_wrapper_->read_data_set(); +// sensors_.push_back( +// new sensors::JointState( +// node_handle_, +// dxl_sdk_wrapper_, +// "joint_states", +// "base_link")); +// } + +// New add_sensors function with analog pins support void TurtleBot3::add_sensors() { RCLCPP_INFO(this->get_logger(), "Add Sensors"); - uint8_t is_connected_bumper_1 = 0; - uint8_t is_connected_bumper_2 = 0; - uint8_t is_connected_illumination = 0; - uint8_t is_connected_ir = 0; - uint8_t is_connected_sonar = 0; - - this->declare_parameter("sensors.bumper_1"); - this->declare_parameter("sensors.bumper_2"); - this->declare_parameter("sensors.illumination"); - this->declare_parameter("sensors.ir"); - this->declare_parameter("sensors.sonar"); - - this->get_parameter_or( - "sensors.bumper_1", - is_connected_bumper_1, - 0); - this->get_parameter_or( - "sensors.bumper_2", - is_connected_bumper_2, - 0); - this->get_parameter_or( - "sensors.illumination", - is_connected_illumination, - 0); - this->get_parameter_or( - "sensors.ir", - is_connected_ir, - 0); - this->get_parameter_or( - "sensors.sonar", - is_connected_sonar, - 0); - + // Keep essential sensors for basic TurtleBot3 functionality sensors_.push_back( new sensors::BatteryState( node_handle_, @@ -188,15 +226,13 @@ void TurtleBot3::add_sensors() "magnetic_field", "imu_link")); - sensors_.push_back( - new sensors::SensorState( - node_handle_, - "sensor_state", - is_connected_bumper_1, - is_connected_bumper_2, - is_connected_illumination, - is_connected_ir, - is_connected_sonar)); + // Add analog pins sensor + analog_pins_sensor_ = new sensors::AnalogPins( + node_handle_, + "analog_pins"); + sensors_.push_back(analog_pins_sensor_); + + RCLCPP_INFO(this->get_logger(), "Successfully added all sensors"); dxl_sdk_wrapper_->read_data_set(); sensors_.push_back( From 898bb8f5638aad44a2b5dfad160a70fffef9c6f2 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Wed, 16 Jul 2025 11:56:10 -0700 Subject: [PATCH 29/38] Remove sensor_state from tb3_node CMakeLists --- turtlebot3_node/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/turtlebot3_node/CMakeLists.txt b/turtlebot3_node/CMakeLists.txt index fd8ee3e81..5d8a766e0 100644 --- a/turtlebot3_node/CMakeLists.txt +++ b/turtlebot3_node/CMakeLists.txt @@ -49,7 +49,6 @@ add_library(${PROJECT_NAME}_lib "src/sensors/battery_state.cpp" "src/sensors/imu.cpp" "src/sensors/joint_state.cpp" - "src/sensors/sensor_state.cpp" "src/sensors/analog_pins.cpp" ) From 0f774ae1229c8c91794287ac1ff6cf6fc7e1cd86 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Mon, 25 Aug 2025 16:49:53 -0700 Subject: [PATCH 30/38] Specify which analog pins to read --- .../turtlebot3_node/sensors/analog_pins.hpp | 2 + turtlebot3_node/param/burger.yaml | 3 + turtlebot3_node/src/sensors/analog_pins.cpp | 71 ++++++++++++------- 3 files changed, 52 insertions(+), 24 deletions(-) diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp index 4a53fbe02..433b6e2ef 100644 --- a/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp +++ b/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/u_int16_multi_array.hpp" @@ -42,6 +43,7 @@ class AnalogPins : public Sensors private: rclcpp::Publisher::SharedPtr analog_publisher_; + std::vector configured_pins_; // Which pins to read from }; } // namespace sensors } // namespace turtlebot3 diff --git a/turtlebot3_node/param/burger.yaml b/turtlebot3_node/param/burger.yaml index 5263900a1..d7f6bd891 100644 --- a/turtlebot3_node/param/burger.yaml +++ b/turtlebot3_node/param/burger.yaml @@ -23,6 +23,9 @@ turtlebot3_node: illumination: 0 ir: 0 sonar: 0 + + # Analog pins to read from (0-5). Must match firmware CONNECTED_ANALOG_PINS configuration + analog_pins: [0, 1, 2, 3, 4, 5] diff_drive_controller: ros__parameters: diff --git a/turtlebot3_node/src/sensors/analog_pins.cpp b/turtlebot3_node/src/sensors/analog_pins.cpp index ace67e332..8b077a563 100644 --- a/turtlebot3_node/src/sensors/analog_pins.cpp +++ b/turtlebot3_node/src/sensors/analog_pins.cpp @@ -29,6 +29,27 @@ AnalogPins::AnalogPins( analog_publisher_ = nh->create_publisher(topic_name, qos); + // Read analog pin configuration from parameters + nh->declare_parameter>("sensors.analog_pins"); + nh->get_parameter_or>("sensors.analog_pins", configured_pins_, {0, 1, 2, 3, 4, 5}); + + // Validate pin numbers (must be 0-5) + for (auto pin : configured_pins_) { + if (pin < 0 || pin > 5) { + RCLCPP_WARN(nh->get_logger(), "Invalid analog pin %d, must be 0-5", pin); + } + } + + RCLCPP_INFO(nh->get_logger(), "Analog pins configured for pins: [%s]", + [this]() { + std::string pins_str; + for (size_t i = 0; i < configured_pins_.size(); ++i) { + pins_str += std::to_string(configured_pins_[i]); + if (i < configured_pins_.size() - 1) pins_str += ", "; + } + return pins_str; + }().c_str()); + RCLCPP_INFO(nh->get_logger(), "Succeeded to create analog pins publisher"); } @@ -41,35 +62,37 @@ void AnalogPins::publish( try { auto analog_msg = std::make_unique(); - // Set up dimensions for the message + // Set up dimensions for the message based on configured pins analog_msg->layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); analog_msg->layout.dim[0].label = "analog_pins"; - analog_msg->layout.dim[0].size = 6; - analog_msg->layout.dim[0].stride = 6; + analog_msg->layout.dim[0].size = configured_pins_.size(); + analog_msg->layout.dim[0].stride = configured_pins_.size(); analog_msg->layout.data_offset = 0; - // Initialize data array to hold 6 pin values - analog_msg->data.resize(6); + // Initialize data array to hold configured pin values + analog_msg->data.resize(configured_pins_.size()); + + // Array of control table entries for easy access + const auto* analog_entries[] = { + &extern_control_table.analog_a0, + &extern_control_table.analog_a1, + &extern_control_table.analog_a2, + &extern_control_table.analog_a3, + &extern_control_table.analog_a4, + &extern_control_table.analog_a5 + }; - // Read values from all 6 analog pins (A0-A5) - analog_msg->data[0] = dxl_sdk_wrapper->get_data_from_device( - extern_control_table.analog_a0.addr, - extern_control_table.analog_a0.length); - analog_msg->data[1] = dxl_sdk_wrapper->get_data_from_device( - extern_control_table.analog_a1.addr, - extern_control_table.analog_a1.length); - analog_msg->data[2] = dxl_sdk_wrapper->get_data_from_device( - extern_control_table.analog_a2.addr, - extern_control_table.analog_a2.length); - analog_msg->data[3] = dxl_sdk_wrapper->get_data_from_device( - extern_control_table.analog_a3.addr, - extern_control_table.analog_a3.length); - analog_msg->data[4] = dxl_sdk_wrapper->get_data_from_device( - extern_control_table.analog_a4.addr, - extern_control_table.analog_a4.length); - analog_msg->data[5] = dxl_sdk_wrapper->get_data_from_device( - extern_control_table.analog_a5.addr, - extern_control_table.analog_a5.length); + // Read values only from configured pins + for (size_t i = 0; i < configured_pins_.size(); ++i) { + int pin = configured_pins_[i]; + if (pin >= 0 && pin <= 5) { + analog_msg->data[i] = dxl_sdk_wrapper->get_data_from_device( + analog_entries[pin]->addr, + analog_entries[pin]->length); + } else { + analog_msg->data[i] = 0; // Invalid pin, set to 0 + } + } // Publish the message analog_publisher_->publish(std::move(analog_msg)); From 7e6ef5ca91aed202fb83f91ec775a289c4d5925a Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Mon, 25 Aug 2025 17:07:40 -0700 Subject: [PATCH 31/38] Change variable types to make Colcon happy --- .../include/turtlebot3_node/sensors/analog_pins.hpp | 2 +- turtlebot3_node/src/sensors/analog_pins.cpp | 12 +++++++----- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp index 433b6e2ef..3df1e6dbb 100644 --- a/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp +++ b/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp @@ -43,7 +43,7 @@ class AnalogPins : public Sensors private: rclcpp::Publisher::SharedPtr analog_publisher_; - std::vector configured_pins_; // Which pins to read from + std::vector configured_pins_; // Which pins to read from }; } // namespace sensors } // namespace turtlebot3 diff --git a/turtlebot3_node/src/sensors/analog_pins.cpp b/turtlebot3_node/src/sensors/analog_pins.cpp index 8b077a563..d1c760e52 100644 --- a/turtlebot3_node/src/sensors/analog_pins.cpp +++ b/turtlebot3_node/src/sensors/analog_pins.cpp @@ -18,6 +18,8 @@ #include #include +#include "turtlebot3_node/control_table.hpp" + using robotis::turtlebot3::sensors::AnalogPins; AnalogPins::AnalogPins( @@ -30,13 +32,13 @@ AnalogPins::AnalogPins( analog_publisher_ = nh->create_publisher(topic_name, qos); // Read analog pin configuration from parameters - nh->declare_parameter>("sensors.analog_pins"); - nh->get_parameter_or>("sensors.analog_pins", configured_pins_, {0, 1, 2, 3, 4, 5}); + nh->declare_parameter>("sensors.analog_pins"); + nh->get_parameter_or>("sensors.analog_pins", configured_pins_, {0, 1, 2, 3, 4, 5}); // Validate pin numbers (must be 0-5) for (auto pin : configured_pins_) { if (pin < 0 || pin > 5) { - RCLCPP_WARN(nh->get_logger(), "Invalid analog pin %d, must be 0-5", pin); + RCLCPP_WARN(nh->get_logger(), "Invalid analog pin %ld, must be 0-5", pin); } } @@ -73,7 +75,7 @@ void AnalogPins::publish( analog_msg->data.resize(configured_pins_.size()); // Array of control table entries for easy access - const auto* analog_entries[] = { + const robotis::turtlebot3::ControlItem* analog_entries[] = { &extern_control_table.analog_a0, &extern_control_table.analog_a1, &extern_control_table.analog_a2, @@ -84,7 +86,7 @@ void AnalogPins::publish( // Read values only from configured pins for (size_t i = 0; i < configured_pins_.size(); ++i) { - int pin = configured_pins_[i]; + int pin = static_cast(configured_pins_[i]); if (pin >= 0 && pin <= 5) { analog_msg->data[i] = dxl_sdk_wrapper->get_data_from_device( analog_entries[pin]->addr, From 7a1a86122213306397f1b11113c9f47cd801b6d2 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Mon, 25 Aug 2025 17:16:53 -0700 Subject: [PATCH 32/38] Provide default analog_pins parameter at declaration --- turtlebot3_node/src/sensors/analog_pins.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/turtlebot3_node/src/sensors/analog_pins.cpp b/turtlebot3_node/src/sensors/analog_pins.cpp index d1c760e52..99d769439 100644 --- a/turtlebot3_node/src/sensors/analog_pins.cpp +++ b/turtlebot3_node/src/sensors/analog_pins.cpp @@ -32,8 +32,8 @@ AnalogPins::AnalogPins( analog_publisher_ = nh->create_publisher(topic_name, qos); // Read analog pin configuration from parameters - nh->declare_parameter>("sensors.analog_pins"); - nh->get_parameter_or>("sensors.analog_pins", configured_pins_, {0, 1, 2, 3, 4, 5}); + nh->declare_parameter>("sensors.analog_pins", std::vector{0, 1, 2, 3, 4, 5}); + configured_pins_ = nh->get_parameter("sensors.analog_pins").as_integer_array(); // Validate pin numbers (must be 0-5) for (auto pin : configured_pins_) { From 336094c942c3fc37fda793e4465f87e0cb122eb6 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Tue, 26 Aug 2025 11:59:30 -0700 Subject: [PATCH 33/38] Add launch parameter to list analog pins to read --- turtlebot3_bringup/param/humble/burger.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/turtlebot3_bringup/param/humble/burger.yaml b/turtlebot3_bringup/param/humble/burger.yaml index b32b3178d..b44890add 100644 --- a/turtlebot3_bringup/param/humble/burger.yaml +++ b/turtlebot3_bringup/param/humble/burger.yaml @@ -26,6 +26,9 @@ illumination: 0 ir: 0 sonar: 0 + # Analog pins to read from (0-5). Must match firmware CONNECTED_ANALOG_PINS configuration + analog_pins: [0, 1, 2, 3, 4, 5] + /**: diff_drive_controller: ros__parameters: From 9147c502a26f0abcfa95d66d62a617241a1123cf Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Wed, 27 Aug 2025 18:04:05 -0700 Subject: [PATCH 34/38] Make use of custom AnalogPin ROS2 msg --- turtlebot3_node/CMakeLists.txt | 2 ++ .../turtlebot3_node/sensors/analog_pins.hpp | 4 +-- turtlebot3_node/package.xml | 1 + turtlebot3_node/src/sensors/analog_pins.cpp | 29 +++++++++---------- 4 files changed, 18 insertions(+), 18 deletions(-) diff --git a/turtlebot3_node/CMakeLists.txt b/turtlebot3_node/CMakeLists.txt index 5d8a766e0..693433f06 100644 --- a/turtlebot3_node/CMakeLists.txt +++ b/turtlebot3_node/CMakeLists.txt @@ -17,6 +17,7 @@ endif() ################################################################################ find_package(ament_cmake REQUIRED) find_package(dynamixel_sdk REQUIRED) +find_package(ez_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) find_package(message_filters REQUIRED) find_package(nav_msgs REQUIRED) @@ -54,6 +55,7 @@ add_library(${PROJECT_NAME}_lib set(DEPENDENCIES "dynamixel_sdk" + "ez_interfaces" "geometry_msgs" "message_filters" "nav_msgs" diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp index 3df1e6dbb..d447f1653 100644 --- a/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp +++ b/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp @@ -20,7 +20,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/u_int16_multi_array.hpp" +#include "ez_interfaces/msg/analog_pins.hpp" #include "turtlebot3_node/sensors/sensors.hpp" @@ -42,7 +42,7 @@ class AnalogPins : public Sensors std::shared_ptr & dxl_sdk_wrapper) override; private: - rclcpp::Publisher::SharedPtr analog_publisher_; + rclcpp::Publisher::SharedPtr analog_publisher_; std::vector configured_pins_; // Which pins to read from }; } // namespace sensors diff --git a/turtlebot3_node/package.xml b/turtlebot3_node/package.xml index 6e4b9a512..3258a0070 100644 --- a/turtlebot3_node/package.xml +++ b/turtlebot3_node/package.xml @@ -17,6 +17,7 @@ Hyungyu Kim ament_cmake dynamixel_sdk + ez_interfaces geometry_msgs message_filters nav_msgs diff --git a/turtlebot3_node/src/sensors/analog_pins.cpp b/turtlebot3_node/src/sensors/analog_pins.cpp index 99d769439..ec25ed307 100644 --- a/turtlebot3_node/src/sensors/analog_pins.cpp +++ b/turtlebot3_node/src/sensors/analog_pins.cpp @@ -19,6 +19,7 @@ #include #include "turtlebot3_node/control_table.hpp" +#include "ez_interfaces/msg/analog_pins.hpp" using robotis::turtlebot3::sensors::AnalogPins; @@ -29,7 +30,7 @@ AnalogPins::AnalogPins( { auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); - analog_publisher_ = nh->create_publisher(topic_name, qos); + analog_publisher_ = nh->create_publisher(topic_name, qos); // Read analog pin configuration from parameters nh->declare_parameter>("sensors.analog_pins", std::vector{0, 1, 2, 3, 4, 5}); @@ -59,20 +60,12 @@ void AnalogPins::publish( const rclcpp::Time & now, std::shared_ptr & dxl_sdk_wrapper) { - (void)now; // Mark as unused intentionally to suppress warning - try { - auto analog_msg = std::make_unique(); + auto analog_msg = std::make_unique(); - // Set up dimensions for the message based on configured pins - analog_msg->layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); - analog_msg->layout.dim[0].label = "analog_pins"; - analog_msg->layout.dim[0].size = configured_pins_.size(); - analog_msg->layout.dim[0].stride = configured_pins_.size(); - analog_msg->layout.data_offset = 0; - - // Initialize data array to hold configured pin values - analog_msg->data.resize(configured_pins_.size()); + // Set timestamp + analog_msg->header.stamp = now; + analog_msg->header.frame_id = "base_link"; // Array of control table entries for easy access const robotis::turtlebot3::ControlItem* analog_entries[] = { @@ -84,15 +77,19 @@ void AnalogPins::publish( &extern_control_table.analog_a5 }; - // Read values only from configured pins + // Create AnalogPin message for each configured pin + analog_msg->pins.resize(configured_pins_.size()); + for (size_t i = 0; i < configured_pins_.size(); ++i) { int pin = static_cast(configured_pins_[i]); if (pin >= 0 && pin <= 5) { - analog_msg->data[i] = dxl_sdk_wrapper->get_data_from_device( + analog_msg->pins[i].pin = static_cast(pin); + analog_msg->pins[i].value = dxl_sdk_wrapper->get_data_from_device( analog_entries[pin]->addr, analog_entries[pin]->length); } else { - analog_msg->data[i] = 0; // Invalid pin, set to 0 + analog_msg->pins[i].pin = static_cast(pin); + analog_msg->pins[i].value = 0; // Invalid pin, set to 0 } } From 3209053a8e8139e89be902ab0ca46d9d478f114f Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Thu, 28 Aug 2025 15:05:05 -0700 Subject: [PATCH 35/38] Publish temperature and humidity --- AGENT.md => AGENTS.md | 0 turtlebot3_bringup/param/humble/burger.yaml | 2 +- turtlebot3_node/CMakeLists.txt | 2 + .../include/turtlebot3_node/control_table.hpp | 8 +- .../turtlebot3_node/sensors/humidity.hpp | 52 ++++++++++++ .../turtlebot3_node/sensors/temperature.hpp | 52 ++++++++++++ .../include/turtlebot3_node/turtlebot3.hpp | 5 +- turtlebot3_node/src/sensors/humidity.cpp | 73 +++++++++++++++++ turtlebot3_node/src/sensors/temperature.cpp | 70 ++++++++++++++++ turtlebot3_node/src/turtlebot3.cpp | 81 +++---------------- 10 files changed, 268 insertions(+), 77 deletions(-) rename AGENT.md => AGENTS.md (100%) create mode 100644 turtlebot3_node/include/turtlebot3_node/sensors/humidity.hpp create mode 100644 turtlebot3_node/include/turtlebot3_node/sensors/temperature.hpp create mode 100644 turtlebot3_node/src/sensors/humidity.cpp create mode 100644 turtlebot3_node/src/sensors/temperature.cpp diff --git a/AGENT.md b/AGENTS.md similarity index 100% rename from AGENT.md rename to AGENTS.md diff --git a/turtlebot3_bringup/param/humble/burger.yaml b/turtlebot3_bringup/param/humble/burger.yaml index b44890add..c01c9462d 100644 --- a/turtlebot3_bringup/param/humble/burger.yaml +++ b/turtlebot3_bringup/param/humble/burger.yaml @@ -27,7 +27,7 @@ ir: 0 sonar: 0 # Analog pins to read from (0-5). Must match firmware CONNECTED_ANALOG_PINS configuration - analog_pins: [0, 1, 2, 3, 4, 5] + analog_pins: [2, 3, 4] /**: diff_drive_controller: diff --git a/turtlebot3_node/CMakeLists.txt b/turtlebot3_node/CMakeLists.txt index 693433f06..555d465f0 100644 --- a/turtlebot3_node/CMakeLists.txt +++ b/turtlebot3_node/CMakeLists.txt @@ -51,6 +51,8 @@ add_library(${PROJECT_NAME}_lib "src/sensors/imu.cpp" "src/sensors/joint_state.cpp" "src/sensors/analog_pins.cpp" + "src/sensors/temperature.cpp" + "src/sensors/humidity.cpp" ) set(DEPENDENCIES diff --git a/turtlebot3_node/include/turtlebot3_node/control_table.hpp b/turtlebot3_node/include/turtlebot3_node/control_table.hpp index 03d24fe4f..aa8a72913 100644 --- a/turtlebot3_node/include/turtlebot3_node/control_table.hpp +++ b/turtlebot3_node/include/turtlebot3_node/control_table.hpp @@ -48,7 +48,6 @@ typedef struct ControlItem millis = {10, RAM, 4, READ}; // ControlItem micros = {14, RAM, 4, READ}; // Does not match firmware - ControlItem device_ready = {17, RAM, 1, READ}; ControlItem device_status = {18, RAM, 1, READ}; ControlItem heartbeat = {19, RAM, 1, READ_WRITE}; @@ -63,10 +62,6 @@ typedef struct ControlItem bumper_1 = {28, RAM, 1, READ}; ControlItem bumper_2 = {29, RAM, 1, READ}; - // ControlItem illumination = {30, RAM, 4, READ}; - // ControlItem ir = {34, RAM, 4, READ}; - // ControlItem sonar = {38, RAM, 4, READ}; - ControlItem analog_a0 = {30, RAM, 2, READ}; ControlItem analog_a1 = {32, RAM, 2, READ}; ControlItem analog_a2 = {34, RAM, 2, READ}; @@ -95,6 +90,9 @@ typedef struct ControlItem imu_orientation_y = {104, RAM, 4, READ}; ControlItem imu_orientation_z = {108, RAM, 4, READ}; + ControlItem temperature = {112, RAM, 4, READ}; + ControlItem humidity = {116, RAM, 4, READ}; + ControlItem present_current_left = {120, RAM, 4, READ}; ControlItem present_current_right = {124, RAM, 4, READ}; ControlItem present_velocity_left = {128, RAM, 4, READ}; diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/humidity.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/humidity.hpp new file mode 100644 index 000000000..42bebda1b --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/sensors/humidity.hpp @@ -0,0 +1,52 @@ +// Copyright 2019 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Darby Lim + +#ifndef TURTLEBOT3_NODE__SENSORS__HUMIDITY_HPP_ +#define TURTLEBOT3_NODE__SENSORS__HUMIDITY_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/relative_humidity.hpp" + +#include "turtlebot3_node/sensors/sensors.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +namespace sensors +{ +class Humidity : public Sensors +{ +public: + explicit Humidity( + std::shared_ptr & nh, + const std::string & topic_name); + + void publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) override; + +private: + rclcpp::Publisher::SharedPtr humidity_publisher_; +}; +} // namespace sensors +} // namespace turtlebot3 +} // namespace robotis + +#endif // TURTLEBOT3_NODE__SENSORS__HUMIDITY_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/temperature.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/temperature.hpp new file mode 100644 index 000000000..d697b5abd --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/sensors/temperature.hpp @@ -0,0 +1,52 @@ +// Copyright 2019 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Darby Lim + +#ifndef TURTLEBOT3_NODE__SENSORS__TEMPERATURE_HPP_ +#define TURTLEBOT3_NODE__SENSORS__TEMPERATURE_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/temperature.hpp" + +#include "turtlebot3_node/sensors/sensors.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +namespace sensors +{ +class Temperature : public Sensors +{ +public: + explicit Temperature( + std::shared_ptr & nh, + const std::string & topic_name); + + void publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) override; + +private: + rclcpp::Publisher::SharedPtr temperature_publisher_; +}; +} // namespace sensors +} // namespace turtlebot3 +} // namespace robotis + +#endif // TURTLEBOT3_NODE__SENSORS__TEMPERATURE_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp index f4d0b77db..9c71d663a 100644 --- a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp +++ b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp @@ -43,7 +43,6 @@ #include "turtlebot3_node/devices/sound.hpp" #include "turtlebot3_node/dynamixel_sdk_wrapper.hpp" #include "turtlebot3_node/odometry.hpp" -#include "turtlebot3_node/sensors/analog_pins.hpp" #include "turtlebot3_node/sensors/battery_state.hpp" #include "turtlebot3_node/sensors/imu.hpp" #include "turtlebot3_node/sensors/joint_state.hpp" @@ -51,6 +50,10 @@ #include "turtlebot3_node/sensors/sensors.hpp" #include "turtlebot3_node/twist_subscriber.hpp" +#include "turtlebot3_node/sensors/analog_pins.hpp" +#include "turtlebot3_node/sensors/temperature.hpp" +#include "turtlebot3_node/sensors/humidity.hpp" + namespace robotis { namespace turtlebot3 diff --git a/turtlebot3_node/src/sensors/humidity.cpp b/turtlebot3_node/src/sensors/humidity.cpp new file mode 100644 index 000000000..0a42ef121 --- /dev/null +++ b/turtlebot3_node/src/sensors/humidity.cpp @@ -0,0 +1,73 @@ +// Copyright 2019 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Darby Lim + +#include "turtlebot3_node/sensors/humidity.hpp" + +#include +#include +#include + +#include "turtlebot3_node/control_table.hpp" +#include "sensor_msgs/msg/relative_humidity.hpp" + +using robotis::turtlebot3::sensors::Humidity; + +Humidity::Humidity( + std::shared_ptr & nh, + const std::string & topic_name) +: Sensors(nh, "base_link") +{ + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); + + humidity_publisher_ = nh->create_publisher(topic_name, qos); + + RCLCPP_INFO(nh->get_logger(), "Succeeded to create humidity publisher"); +} + +void Humidity::publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) +{ + try { + auto humidity_msg = std::make_unique(); + + // Set timestamp and frame + humidity_msg->header.stamp = now; + humidity_msg->header.frame_id = frame_id_; + + // Read humidity from control table (address 116, 4 bytes) + float humidity_percent = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.humidity.addr, + extern_control_table.humidity.length); + + // Convert percentage to ratio (0.0-1.0) for ROS standard compliance + humidity_msg->relative_humidity = humidity_percent / 100.0; + + // Set variance to 0 (unknown) + humidity_msg->variance = 0.0; + + // Publish the message + humidity_publisher_->publish(std::move(humidity_msg)); + } catch (const std::exception & e) { + RCLCPP_ERROR( + rclcpp::get_logger("humidity"), + "Exception in humidity publish: %s", e.what()); + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("humidity"), + "Unknown exception in humidity publish"); + } +} diff --git a/turtlebot3_node/src/sensors/temperature.cpp b/turtlebot3_node/src/sensors/temperature.cpp new file mode 100644 index 000000000..9d70c9667 --- /dev/null +++ b/turtlebot3_node/src/sensors/temperature.cpp @@ -0,0 +1,70 @@ +// Copyright 2019 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Darby Lim + +#include "turtlebot3_node/sensors/temperature.hpp" + +#include +#include +#include + +#include "turtlebot3_node/control_table.hpp" +#include "sensor_msgs/msg/temperature.hpp" + +using robotis::turtlebot3::sensors::Temperature; + +Temperature::Temperature( + std::shared_ptr & nh, + const std::string & topic_name) +: Sensors(nh, "base_link") +{ + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); + + temperature_publisher_ = nh->create_publisher(topic_name, qos); + + RCLCPP_INFO(nh->get_logger(), "Succeeded to create temperature publisher"); +} + +void Temperature::publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) +{ + try { + auto temp_msg = std::make_unique(); + + // Set timestamp and frame + temp_msg->header.stamp = now; + temp_msg->header.frame_id = frame_id_; + + // Read temperature from control table (address 112, 4 bytes) + temp_msg->temperature = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.temperature.addr, + extern_control_table.temperature.length); + + // Set variance to 0 (unknown) + temp_msg->variance = 0.0; + + // Publish the message + temperature_publisher_->publish(std::move(temp_msg)); + } catch (const std::exception & e) { + RCLCPP_ERROR( + rclcpp::get_logger("temperature"), + "Exception in temperature publish: %s", e.what()); + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("temperature"), + "Unknown exception in temperature publish"); + } +} diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 7a190b8b2..7f16b2850 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -139,76 +139,6 @@ void TurtleBot3::add_wheels() this->get_parameter_or("wheels.radius", wheels_.radius, 0.033); } -// Original add_sensors function - commented out for analog pins implementation -// void TurtleBot3::add_sensors() -// { -// RCLCPP_INFO(this->get_logger(), "Add Sensors"); - -// uint8_t is_connected_bumper_1 = 0; -// uint8_t is_connected_bumper_2 = 0; -// uint8_t is_connected_illumination = 0; -// uint8_t is_connected_ir = 0; -// uint8_t is_connected_sonar = 0; - -// this->declare_parameter("sensors.bumper_1"); -// this->declare_parameter("sensors.bumper_2"); -// this->declare_parameter("sensors.illumination"); -// this->declare_parameter("sensors.ir"); -// this->declare_parameter("sensors.sonar"); - -// this->get_parameter_or( -// "sensors.bumper_1", -// is_connected_bumper_1, -// 0); -// this->get_parameter_or( -// "sensors.bumper_2", -// is_connected_bumper_2, -// 0); -// this->get_parameter_or( -// "sensors.illumination", -// is_connected_illumination, -// 0); -// this->get_parameter_or( -// "sensors.ir", -// is_connected_ir, -// 0); -// this->get_parameter_or( -// "sensors.sonar", -// is_connected_sonar, -// 0); - -// sensors_.push_back( -// new sensors::BatteryState( -// node_handle_, -// "battery_state")); - -// sensors_.push_back( -// new sensors::Imu( -// node_handle_, -// "imu", -// "magnetic_field", -// "imu_link")); - -// sensors_.push_back( -// new sensors::SensorState( -// node_handle_, -// "sensor_state", -// is_connected_bumper_1, -// is_connected_bumper_2, -// is_connected_illumination, -// is_connected_ir, -// is_connected_sonar)); - -// dxl_sdk_wrapper_->read_data_set(); -// sensors_.push_back( -// new sensors::JointState( -// node_handle_, -// dxl_sdk_wrapper_, -// "joint_states", -// "base_link")); -// } - -// New add_sensors function with analog pins support void TurtleBot3::add_sensors() { RCLCPP_INFO(this->get_logger(), "Add Sensors"); @@ -232,6 +162,17 @@ void TurtleBot3::add_sensors() "analog_pins"); sensors_.push_back(analog_pins_sensor_); + // Add temperature and humidity sensors + sensors_.push_back( + new sensors::Temperature( + node_handle_, + "temperature")); + + sensors_.push_back( + new sensors::Humidity( + node_handle_, + "humidity")); + RCLCPP_INFO(this->get_logger(), "Successfully added all sensors"); dxl_sdk_wrapper_->read_data_set(); From 0e27b02baa854c8e303e1c0902484d0934683f53 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Thu, 28 Aug 2025 16:23:33 -0700 Subject: [PATCH 36/38] Add debug messages for temperature and humidity --- turtlebot3_node/src/sensors/humidity.cpp | 9 ++++++--- turtlebot3_node/src/sensors/temperature.cpp | 14 +++++++++++++- turtlebot3_node/src/turtlebot3.cpp | 4 +++- 3 files changed, 22 insertions(+), 5 deletions(-) diff --git a/turtlebot3_node/src/sensors/humidity.cpp b/turtlebot3_node/src/sensors/humidity.cpp index 0a42ef121..d02bcdc81 100644 --- a/turtlebot3_node/src/sensors/humidity.cpp +++ b/turtlebot3_node/src/sensors/humidity.cpp @@ -48,13 +48,16 @@ void Humidity::publish( humidity_msg->header.stamp = now; humidity_msg->header.frame_id = frame_id_; - // Read humidity from control table (address 116, 4 bytes) - float humidity_percent = dxl_sdk_wrapper->get_data_from_device( + // Read humidity from control table (address 116, 4 bytes) + // Firmware stores as float from DHT sensor (percentage) + float raw_humidity = dxl_sdk_wrapper->get_data_from_device( extern_control_table.humidity.addr, extern_control_table.humidity.length); + RCLCPP_INFO(nh_->get_logger(), "Humidity raw value: %f%%", raw_humidity); + // Convert percentage to ratio (0.0-1.0) for ROS standard compliance - humidity_msg->relative_humidity = humidity_percent / 100.0; + humidity_msg->relative_humidity = raw_humidity / 100.0; // Set variance to 0 (unknown) humidity_msg->variance = 0.0; diff --git a/turtlebot3_node/src/sensors/temperature.cpp b/turtlebot3_node/src/sensors/temperature.cpp index 9d70c9667..0eba7adbc 100644 --- a/turtlebot3_node/src/sensors/temperature.cpp +++ b/turtlebot3_node/src/sensors/temperature.cpp @@ -41,6 +41,13 @@ void Temperature::publish( const rclcpp::Time & now, std::shared_ptr & dxl_sdk_wrapper) { + static int call_count = 0; + call_count++; + + if (call_count % 100 == 1) { // Log every 100th call to avoid spam + RCLCPP_INFO(nh_->get_logger(), "Temperature::publish called %d times", call_count); + } + try { auto temp_msg = std::make_unique(); @@ -49,10 +56,15 @@ void Temperature::publish( temp_msg->header.frame_id = frame_id_; // Read temperature from control table (address 112, 4 bytes) - temp_msg->temperature = dxl_sdk_wrapper->get_data_from_device( + // Firmware stores as float from DHT sensor + float raw_temp = dxl_sdk_wrapper->get_data_from_device( extern_control_table.temperature.addr, extern_control_table.temperature.length); + RCLCPP_INFO(nh_->get_logger(), "Temperature raw value: %f", raw_temp); + + temp_msg->temperature = raw_temp; + // Set variance to 0 (unknown) temp_msg->variance = 0.0; diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 7f16b2850..df88b6b49 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -163,17 +163,19 @@ void TurtleBot3::add_sensors() sensors_.push_back(analog_pins_sensor_); // Add temperature and humidity sensors + RCLCPP_INFO(this->get_logger(), "Adding Temperature sensor..."); sensors_.push_back( new sensors::Temperature( node_handle_, "temperature")); + RCLCPP_INFO(this->get_logger(), "Adding Humidity sensor..."); sensors_.push_back( new sensors::Humidity( node_handle_, "humidity")); - RCLCPP_INFO(this->get_logger(), "Successfully added all sensors"); + RCLCPP_INFO(this->get_logger(), "Successfully added all sensors (total: %zu)", sensors_.size()); dxl_sdk_wrapper_->read_data_set(); sensors_.push_back( From c0fd2e853c725aec5f5b2d40b953fa5c754ec242 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Thu, 28 Aug 2025 17:02:09 -0700 Subject: [PATCH 37/38] Remove ROS info log messages used to debug temp and RH --- turtlebot3_node/src/sensors/humidity.cpp | 6 ++---- turtlebot3_node/src/sensors/temperature.cpp | 13 +------------ turtlebot3_node/src/turtlebot3.cpp | 4 +--- 3 files changed, 4 insertions(+), 19 deletions(-) diff --git a/turtlebot3_node/src/sensors/humidity.cpp b/turtlebot3_node/src/sensors/humidity.cpp index d02bcdc81..af6c084ed 100644 --- a/turtlebot3_node/src/sensors/humidity.cpp +++ b/turtlebot3_node/src/sensors/humidity.cpp @@ -50,14 +50,12 @@ void Humidity::publish( // Read humidity from control table (address 116, 4 bytes) // Firmware stores as float from DHT sensor (percentage) - float raw_humidity = dxl_sdk_wrapper->get_data_from_device( + float humidity_percent = dxl_sdk_wrapper->get_data_from_device( extern_control_table.humidity.addr, extern_control_table.humidity.length); - RCLCPP_INFO(nh_->get_logger(), "Humidity raw value: %f%%", raw_humidity); - // Convert percentage to ratio (0.0-1.0) for ROS standard compliance - humidity_msg->relative_humidity = raw_humidity / 100.0; + humidity_msg->relative_humidity = humidity_percent / 100.0; // Set variance to 0 (unknown) humidity_msg->variance = 0.0; diff --git a/turtlebot3_node/src/sensors/temperature.cpp b/turtlebot3_node/src/sensors/temperature.cpp index 0eba7adbc..42bea6a8a 100644 --- a/turtlebot3_node/src/sensors/temperature.cpp +++ b/turtlebot3_node/src/sensors/temperature.cpp @@ -41,13 +41,6 @@ void Temperature::publish( const rclcpp::Time & now, std::shared_ptr & dxl_sdk_wrapper) { - static int call_count = 0; - call_count++; - - if (call_count % 100 == 1) { // Log every 100th call to avoid spam - RCLCPP_INFO(nh_->get_logger(), "Temperature::publish called %d times", call_count); - } - try { auto temp_msg = std::make_unique(); @@ -57,14 +50,10 @@ void Temperature::publish( // Read temperature from control table (address 112, 4 bytes) // Firmware stores as float from DHT sensor - float raw_temp = dxl_sdk_wrapper->get_data_from_device( + temp_msg->temperature = dxl_sdk_wrapper->get_data_from_device( extern_control_table.temperature.addr, extern_control_table.temperature.length); - RCLCPP_INFO(nh_->get_logger(), "Temperature raw value: %f", raw_temp); - - temp_msg->temperature = raw_temp; - // Set variance to 0 (unknown) temp_msg->variance = 0.0; diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index df88b6b49..7f16b2850 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -163,19 +163,17 @@ void TurtleBot3::add_sensors() sensors_.push_back(analog_pins_sensor_); // Add temperature and humidity sensors - RCLCPP_INFO(this->get_logger(), "Adding Temperature sensor..."); sensors_.push_back( new sensors::Temperature( node_handle_, "temperature")); - RCLCPP_INFO(this->get_logger(), "Adding Humidity sensor..."); sensors_.push_back( new sensors::Humidity( node_handle_, "humidity")); - RCLCPP_INFO(this->get_logger(), "Successfully added all sensors (total: %zu)", sensors_.size()); + RCLCPP_INFO(this->get_logger(), "Successfully added all sensors"); dxl_sdk_wrapper_->read_data_set(); sensors_.push_back( From 881f6c71736065aa5461e0fe785d13553cea51ba Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Thu, 28 Aug 2025 17:06:42 -0700 Subject: [PATCH 38/38] Fix copyright --- .../turtlebot3_node/sensors/humidity.hpp | 17 ++--------------- .../turtlebot3_node/sensors/temperature.hpp | 17 ++--------------- turtlebot3_node/src/sensors/humidity.cpp | 17 ++--------------- turtlebot3_node/src/sensors/temperature.cpp | 17 ++--------------- 4 files changed, 8 insertions(+), 60 deletions(-) diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/humidity.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/humidity.hpp index 42bebda1b..e879b9fbb 100644 --- a/turtlebot3_node/include/turtlebot3_node/sensors/humidity.hpp +++ b/turtlebot3_node/include/turtlebot3_node/sensors/humidity.hpp @@ -1,18 +1,5 @@ -// Copyright 2019 ROBOTIS CO., LTD. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Author: Darby Lim +// Copyright 2025 EyeZense, Inc. +// Author: Travis Mendoza #ifndef TURTLEBOT3_NODE__SENSORS__HUMIDITY_HPP_ #define TURTLEBOT3_NODE__SENSORS__HUMIDITY_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/temperature.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/temperature.hpp index d697b5abd..2cfdce799 100644 --- a/turtlebot3_node/include/turtlebot3_node/sensors/temperature.hpp +++ b/turtlebot3_node/include/turtlebot3_node/sensors/temperature.hpp @@ -1,18 +1,5 @@ -// Copyright 2019 ROBOTIS CO., LTD. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Author: Darby Lim +// Copyright 2025 EyeZense, Inc. +// Author: Travis Mendoza #ifndef TURTLEBOT3_NODE__SENSORS__TEMPERATURE_HPP_ #define TURTLEBOT3_NODE__SENSORS__TEMPERATURE_HPP_ diff --git a/turtlebot3_node/src/sensors/humidity.cpp b/turtlebot3_node/src/sensors/humidity.cpp index af6c084ed..93e34545a 100644 --- a/turtlebot3_node/src/sensors/humidity.cpp +++ b/turtlebot3_node/src/sensors/humidity.cpp @@ -1,18 +1,5 @@ -// Copyright 2019 ROBOTIS CO., LTD. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Author: Darby Lim +// Copyright 2025 EyeZense, Inc. +// Author: Travis Mendoza #include "turtlebot3_node/sensors/humidity.hpp" diff --git a/turtlebot3_node/src/sensors/temperature.cpp b/turtlebot3_node/src/sensors/temperature.cpp index 42bea6a8a..606ac5ced 100644 --- a/turtlebot3_node/src/sensors/temperature.cpp +++ b/turtlebot3_node/src/sensors/temperature.cpp @@ -1,18 +1,5 @@ -// Copyright 2019 ROBOTIS CO., LTD. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Author: Darby Lim +// Copyright 2025 EyeZense, Inc. +// Author: Travis Mendoza #include "turtlebot3_node/sensors/temperature.hpp"