Add gtest

roomba_500driver_meiji/CMakeLists.txt

@@ -42,7 +42,12 @@ target_link_libraries(
 ament_auto_add_executable(main500 src/main500.cpp)
 ament_auto_add_executable(roomba500sci src/roomba500sci.cpp)
 ament_auto_add_executable(serial src/serial.cpp)
-ament_auto_add_executable(twist_to_roombactrl_converter src/twist_to_roombactrl_converter.cpp)
+ament_auto_add_executable(twist_to_roombactrl_converter src/twist_to_roombactrl_converter_node.cpp src/twist_to_roombactrl_converter.cpp)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
 
 install(DIRECTORY
   launch
@@ -54,6 +59,11 @@ if(BUILD_TESTING)
   set(ament_cmake_copyright_FOUND TRUE)
   set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
+
+  ament_auto_add_gtest(test_twist_to_roombactrl_converter
+    test/test_twist_to_roombactrl_converter.cpp
+    src/twist_to_roombactrl_converter.cpp
+  )
 endif()
 
 ament_auto_package()

roomba_500driver_meiji/include/roomba_500driver_meiji/twist_to_roombactrl_converter.hpp

@@ -0,0 +1,29 @@
+// Copyright 2025 amsl
+
+#ifndef ROOMBA_500DRIVER_MEIJI__TWIST_TO_ROOMBACTRL_CONVERTER_HPP_
+#define ROOMBA_500DRIVER_MEIJI__TWIST_TO_ROOMBACTRL_CONVERTER_HPP_
+
+#include <memory>
+
+#include <geometry_msgs/msg/twist.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <roomba_500driver_meiji/msg/roomba_ctrl.hpp>
+
+namespace roomba_500driver_meiji
+{
+class TwistToRoombactrlConverter : public rclcpp::Node
+{
+public:
+  TwistToRoombactrlConverter(void);
+
+  void cmd_vel_callback(const std::shared_ptr<geometry_msgs::msg::Twist> msg);
+
+private:
+  rclcpp::Publisher<roomba_500driver_meiji::msg::RoombaCtrl>::SharedPtr
+    ctrl_pub_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
+};
+
+}  // namespace roomba_500driver_meiji
+
+#endif  // ROOMBA_500DRIVER_MEIJI__TWIST_TO_ROOMBACTRL_CONVERTER_HPP_

roomba_500driver_meiji/package.xml

@@ -20,6 +20,7 @@
   <exec_depend>rosidl_default_runtime</exec_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
   <member_of_group>rosidl_interface_packages</member_of_group>
 
   <export>

roomba_500driver_meiji/src/twist_to_roombactrl_converter.cpp

@@ -3,50 +3,33 @@
 #include "geometry_msgs/msg/twist.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "roomba_500driver_meiji/msg/roomba_ctrl.hpp"
+#include "roomba_500driver_meiji/twist_to_roombactrl_converter.hpp"
 
 namespace roomba_500driver_meiji
 {
-class TwistToRoombactrlConverter : public rclcpp::Node
+TwistToRoombactrlConverter::TwistToRoombactrlConverter(void)
+: Node("twist_to_roombactrl_converter")
 {
-public:
-  TwistToRoombactrlConverter(void)
-  : Node("twist_to_roombactrl_converter")
-  {
-    ctrl_pub_ = this->create_publisher<roomba_500driver_meiji::msg::RoombaCtrl>(
-      "roomba/control", 10
-    );
-    cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
-      "cmd_vel", 10,
-      std::bind(
-        &TwistToRoombactrlConverter::cmd_vel_callback, this,
-        std::placeholders::_1
-      )
-    );
-  }
-
-  void cmd_vel_callback(const std::shared_ptr<geometry_msgs::msg::Twist> msg)
-  {
-    roomba_500driver_meiji::msg::RoombaCtrl control;
-    control.mode = roomba_500driver_meiji::msg::RoombaCtrl::DRIVE_DIRECT;
-    control.cntl.linear.x = msg->linear.x;
-    control.cntl.angular.z = msg->angular.z;
-    ctrl_pub_->publish(control);
-  }
-
-private:
-  rclcpp::Publisher<roomba_500driver_meiji::msg::RoombaCtrl>::SharedPtr
-    ctrl_pub_;
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
-};
-
-}  // namespace roomba_500driver_meiji
+  ctrl_pub_ = this->create_publisher<roomba_500driver_meiji::msg::RoombaCtrl>(
+    "roomba/control", 10
+  );
+  cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
+    "cmd_vel", 10,
+    std::bind(
+      &TwistToRoombactrlConverter::cmd_vel_callback, this,
+      std::placeholders::_1
+    )
+  );
+}
 
-int main(int argc, char ** argv)
+void TwistToRoombactrlConverter::cmd_vel_callback(
+  const std::shared_ptr<geometry_msgs::msg::Twist> msg)
 {
-  rclcpp::init(argc, argv);
-  rclcpp::spin(
-    std::make_shared<roomba_500driver_meiji::TwistToRoombactrlConverter>()
-  );
-  rclcpp::shutdown();
-  return 0;
+  roomba_500driver_meiji::msg::RoombaCtrl control;
+  control.mode = roomba_500driver_meiji::msg::RoombaCtrl::DRIVE_DIRECT;
+  control.cntl.linear.x = msg->linear.x;
+  control.cntl.angular.z = msg->angular.z;
+  ctrl_pub_->publish(control);
 }
+
+}  // namespace roomba_500driver_meiji

roomba_500driver_meiji/src/twist_to_roombactrl_converter_node.cpp

@@ -0,0 +1,14 @@
+// Copyright 2025 amsl
+
+#include <rclcpp/rclcpp.hpp>
+#include <roomba_500driver_meiji/twist_to_roombactrl_converter.hpp>
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(
+    std::make_shared<roomba_500driver_meiji::TwistToRoombactrlConverter>()
+  );
+  rclcpp::shutdown();
+  return 0;
+}

roomba_500driver_meiji/test/test_twist_to_roombactrl_converter.cpp

@@ -0,0 +1,75 @@
+// Copyright 2025 amsl
+
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <roomba_500driver_meiji/msg/roomba_ctrl.hpp>
+#include <roomba_500driver_meiji/twist_to_roombactrl_converter.hpp>
+
+class TwistToRoombactrlConverterTest : public ::testing::Test
+{
+public:
+  void ctrl_callback(const roomba_500driver_meiji::msg::RoombaCtrl::SharedPtr msg)
+  {
+    received_control_ = msg;
+  }
+
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Subscription<roomba_500driver_meiji::msg::RoombaCtrl>::SharedPtr ctrl_sub_;
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+  roomba_500driver_meiji::msg::RoombaCtrl::SharedPtr received_control_;
+
+protected:
+  void SetUp() override
+  {
+    node_ = std::make_shared<rclcpp::Node>("test_twist_to_roombactrl_converter");
+    ctrl_sub_ = node_->create_subscription<roomba_500driver_meiji::msg::RoombaCtrl>(
+      "roomba/control", 10,
+      std::bind(&TwistToRoombactrlConverterTest::ctrl_callback, this, std::placeholders::_1)
+    );
+    cmd_vel_pub_ = node_->create_publisher<geometry_msgs::msg::Twist>(
+      "cmd_vel", 10
+    );
+  }
+
+  void TearDown() override
+  {
+    received_control_.reset();
+    ctrl_sub_.reset();
+    node_.reset();
+  }
+};
+
+TEST_F(TwistToRoombactrlConverterTest, cmd_vel)
+{
+  rclcpp::executors::SingleThreadedExecutor executor;
+  const auto converter_node =
+    std::make_shared<roomba_500driver_meiji::TwistToRoombactrlConverter>();
+
+  executor.add_node(converter_node);
+  executor.add_node(node_);
+
+  const auto twist_msg = std::make_shared<geometry_msgs::msg::Twist>();
+  twist_msg->linear.x = 1.0;
+  twist_msg->angular.z = 0.5;
+
+  cmd_vel_pub_->publish(*twist_msg);
+  while (rclcpp::ok()) {
+    if (received_control_) {
+      break;
+    }
+    executor.spin_once();
+  }
+  EXPECT_EQ(received_control_->mode, roomba_500driver_meiji::msg::RoombaCtrl::DRIVE_DIRECT);
+  EXPECT_EQ(received_control_->cntl.linear.x, twist_msg->linear.x);
+  EXPECT_EQ(received_control_->cntl.angular.z, twist_msg->angular.z);
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return ret;
+}