From 1988711531c8196151f688ed7be2a4f54db59b53 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Tue, 26 Aug 2025 14:02:49 +0900 Subject: [PATCH 1/3] =?UTF-8?q?launch=E3=83=95=E3=82=A1=E3=82=A4=E3=83=AB?= =?UTF-8?q?=E3=81=8B=E3=82=89active=E3=81=B8=E9=81=B7=E7=A7=BB=E3=81=95?= =?UTF-8?q?=E3=81=9B=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- launch/rt_usb_9axisimu_driver.launch.py | 56 ++++++++++++++++++++++--- 1 file changed, 50 insertions(+), 6 deletions(-) diff --git a/launch/rt_usb_9axisimu_driver.launch.py b/launch/rt_usb_9axisimu_driver.launch.py index b32a272..ae15576 100644 --- a/launch/rt_usb_9axisimu_driver.launch.py +++ b/launch/rt_usb_9axisimu_driver.launch.py @@ -1,4 +1,3 @@ - # rt_usb_9axisimu_driver.launch.py # # License: BSD-3-Clause @@ -30,17 +29,62 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -import launch -from launch_ros.actions import Node +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.events.lifecycle import ChangeState +from launch_ros.event_handlers import OnStateTransition +from launch.event_handlers.on_process_start import OnProcessStart +from launch.events import matches_action +from launch.actions import EmitEvent, LogInfo, RegisterEventHandler +from lifecycle_msgs.msg import Transition def generate_launch_description(): """Generate launch description with multiple components.""" - driver = Node( + imu_driver_node = LifecycleNode( name='rt_usb_9axisimu_driver', + namespace='', package='rt_usb_9axisimu_driver', executable='rt_usb_9axisimu_driver', - output='screen' + output='screen', + ) + + configure_event_handler = RegisterEventHandler( + OnProcessStart( + target_action=imu_driver_node, + on_start=[ + LogInfo(msg='Transitions from unconfigured to inactive.'), + EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(imu_driver_node), + transition_id=Transition.TRANSITION_CONFIGURE, + ) + ), + ], + ) ) - return launch.LaunchDescription([driver]) + activate_event_handler = RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=imu_driver_node, + start_state='configuring', + goal_state='inactive', + entities=[ + LogInfo(msg='Transitions from inactive to active.'), + EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(imu_driver_node), + transition_id=Transition.TRANSITION_ACTIVATE, + ) + ), + ], + ) + ) + + return LaunchDescription( + [ + imu_driver_node, + configure_event_handler, + activate_event_handler, + ] + ) From a3b30a1a9666af17cdc48dfd4c043ca5ed669f79 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Tue, 26 Aug 2025 14:38:14 +0900 Subject: [PATCH 2/3] =?UTF-8?q?activate=E6=99=82=E3=81=AE=E3=83=87?= =?UTF-8?q?=E3=83=BC=E3=82=BF=E5=8F=96=E5=BE=97=E3=83=81=E3=82=A7=E3=83=83?= =?UTF-8?q?=E3=82=AF=E3=81=AB=E3=82=BF=E3=82=A4=E3=83=A0=E3=82=A2=E3=82=A6?= =?UTF-8?q?=E3=83=88=E6=A9=9F=E8=83=BD=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../rt_usb_9axisimu_driver.hpp | 1 + src/rt_usb_9axisimu_driver.cpp | 14 ++++++++++++++ src/rt_usb_9axisimu_driver_component.cpp | 4 ++-- 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp b/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp index 05a20f8..dbee413 100644 --- a/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp +++ b/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp @@ -65,6 +65,7 @@ class RtUsb9axisimuRosDriver bool startCommunication(); void stopCommunication(void); void checkDataFormat(const double timeout = 5.0); + bool checkReadData(const double timeout = 5.0); bool hasAsciiDataFormat(void); bool hasBinaryDataFormat(void); bool hasRefreshedImuData(void); diff --git a/src/rt_usb_9axisimu_driver.cpp b/src/rt_usb_9axisimu_driver.cpp index 3bb4588..377cfa7 100644 --- a/src/rt_usb_9axisimu_driver.cpp +++ b/src/rt_usb_9axisimu_driver.cpp @@ -366,6 +366,20 @@ void RtUsb9axisimuRosDriver::checkDataFormat(const double timeout) } } +bool RtUsb9axisimuRosDriver::checkReadData(const double timeout) +{ + const auto start_time = std::chrono::system_clock::now(); + double time_elapsed = 0.0; + while (time_elapsed < timeout) { + const auto end_time = std::chrono::system_clock::now(); + time_elapsed = (double)std::chrono::duration_cast(end_time - start_time).count(); + if (readSensorData() == RtUsb9axisimuRosDriver::ReadStatus::SUCCESS) { + return true; + } + } + return false; +} + bool RtUsb9axisimuRosDriver::hasAsciiDataFormat(void) { return data_format_ == DataFormat::ASCII; diff --git a/src/rt_usb_9axisimu_driver_component.cpp b/src/rt_usb_9axisimu_driver_component.cpp index 6a846c6..cebc408 100644 --- a/src/rt_usb_9axisimu_driver_component.cpp +++ b/src/rt_usb_9axisimu_driver_component.cpp @@ -126,8 +126,8 @@ CallbackReturn Driver::on_activate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_activate() is called."); - if (driver_->readSensorData() == RtUsb9axisimuRosDriver::ReadStatus::FAILURE) { - RCLCPP_ERROR(this->get_logger(), "readSensorData() returns FAILURE, please check your devices."); + if (!driver_->checkReadData()) { + RCLCPP_ERROR(this->get_logger(), "checkReadData() returns false, please check your devices."); return CallbackReturn::ERROR; } imu_data_raw_pub_->on_activate(); From 33de59ec90218dde264abfbc7f08743c6886858c Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Wed, 3 Sep 2025 10:43:53 +0900 Subject: [PATCH 3/3] =?UTF-8?q?Revert=20"activate=E6=99=82=E3=81=AE?= =?UTF-8?q?=E3=83=87=E3=83=BC=E3=82=BF=E5=8F=96=E5=BE=97=E3=83=81=E3=82=A7?= =?UTF-8?q?=E3=83=83=E3=82=AF=E3=81=AB=E3=82=BF=E3=82=A4=E3=83=A0=E3=82=A2?= =?UTF-8?q?=E3=82=A6=E3=83=88=E6=A9=9F=E8=83=BD=E3=82=92=E8=BF=BD=E5=8A=A0?= =?UTF-8?q?"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This reverts commit a3b30a1a9666af17cdc48dfd4c043ca5ed669f79. --- .../rt_usb_9axisimu_driver.hpp | 1 - src/rt_usb_9axisimu_driver.cpp | 14 -------------- src/rt_usb_9axisimu_driver_component.cpp | 4 ++-- 3 files changed, 2 insertions(+), 17 deletions(-) diff --git a/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp b/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp index dbee413..05a20f8 100644 --- a/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp +++ b/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp @@ -65,7 +65,6 @@ class RtUsb9axisimuRosDriver bool startCommunication(); void stopCommunication(void); void checkDataFormat(const double timeout = 5.0); - bool checkReadData(const double timeout = 5.0); bool hasAsciiDataFormat(void); bool hasBinaryDataFormat(void); bool hasRefreshedImuData(void); diff --git a/src/rt_usb_9axisimu_driver.cpp b/src/rt_usb_9axisimu_driver.cpp index 377cfa7..3bb4588 100644 --- a/src/rt_usb_9axisimu_driver.cpp +++ b/src/rt_usb_9axisimu_driver.cpp @@ -366,20 +366,6 @@ void RtUsb9axisimuRosDriver::checkDataFormat(const double timeout) } } -bool RtUsb9axisimuRosDriver::checkReadData(const double timeout) -{ - const auto start_time = std::chrono::system_clock::now(); - double time_elapsed = 0.0; - while (time_elapsed < timeout) { - const auto end_time = std::chrono::system_clock::now(); - time_elapsed = (double)std::chrono::duration_cast(end_time - start_time).count(); - if (readSensorData() == RtUsb9axisimuRosDriver::ReadStatus::SUCCESS) { - return true; - } - } - return false; -} - bool RtUsb9axisimuRosDriver::hasAsciiDataFormat(void) { return data_format_ == DataFormat::ASCII; diff --git a/src/rt_usb_9axisimu_driver_component.cpp b/src/rt_usb_9axisimu_driver_component.cpp index cebc408..6a846c6 100644 --- a/src/rt_usb_9axisimu_driver_component.cpp +++ b/src/rt_usb_9axisimu_driver_component.cpp @@ -126,8 +126,8 @@ CallbackReturn Driver::on_activate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_activate() is called."); - if (!driver_->checkReadData()) { - RCLCPP_ERROR(this->get_logger(), "checkReadData() returns false, please check your devices."); + if (driver_->readSensorData() == RtUsb9axisimuRosDriver::ReadStatus::FAILURE) { + RCLCPP_ERROR(this->get_logger(), "readSensorData() returns FAILURE, please check your devices."); return CallbackReturn::ERROR; } imu_data_raw_pub_->on_activate();