Merge pull request #88 from lge-ros2/backport/humble/from_jazzy

Backport/humble/from jazzy
lge-ros2 · Nov 14, 2024 · b3b74c6 · b3b74c6
2 parents d052226 + 838091f
commit b3b74c6
Show file tree

Hide file tree

Showing 3 changed files with 53 additions and 1 deletion.
diff --git a/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp b/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp
@@ -25,6 +25,7 @@
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/battery_state.hpp>
 #include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/joy.hpp>
 #include <std_srvs/srv/empty.hpp>
 #include <std_msgs/msg/float32.hpp>
 #include <std_msgs/msg/u_int16.hpp>
@@ -54,6 +55,8 @@ class Micom : public Base
 
   std::string MakeControlMessage(const geometry_msgs::msg::Twist::SharedPtr msg) const;
 
+  std::string MakeControlMessage(const sensor_msgs::msg::Joy::SharedPtr msg) const;
+
   std::string MakeMowingBladeHeightMessage(const std_msgs::msg::Float32::SharedPtr msg) const;
 
   std::string MakeMowingRevSpeedMessage(const std_msgs::msg::UInt16::SharedPtr msg) const;
@@ -87,6 +90,9 @@ class Micom : public Base
   // wheel command subscriber
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_cmd_;
 
+  // joy command subscriber
+  rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr sub_joy_;
+
   // set height of blade
   rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr sub_blade_height_;
 

diff --git a/cloisim_ros_micom/src/micom.cpp b/cloisim_ros_micom/src/micom.cpp
@@ -15,6 +15,7 @@
 
 #include <cloisim_msgs/param.pb.h>
 #include <cloisim_msgs/twist.pb.h>
+#include <cloisim_msgs/joystick.pb.h>
 #include <tf2/LinearMath/Matrix3x3.h>
 #include <tf2/LinearMath/Quaternion.h>
 
@@ -105,6 +106,12 @@ void Micom::Initialize()
       SetBufferToSimulator(data_bridge_ptr, msgBuf);
     };
 
+  auto callback_sub_joy = [this,
+      data_bridge_ptr](const sensor_msgs::msg::Joy::SharedPtr msg) -> void {
+      const auto msgBuf = MakeControlMessage(msg);
+      SetBufferToSimulator(data_bridge_ptr, msgBuf);
+    };
+
   auto callback_sub_mowing_blade_height = [this,
       data_bridge_ptr](const std_msgs::msg::Float32::SharedPtr msg) -> void {
       const auto msgBuf = MakeMowingBladeHeightMessage(msg);
@@ -121,6 +128,9 @@ void Micom::Initialize()
   sub_cmd_ = create_subscription<geometry_msgs::msg::Twist>(
     "cmd_vel", rclcpp::SensorDataQoS(), callback_sub_cmdvel);
 
+  sub_joy_ = create_subscription<sensor_msgs::msg::Joy>(
+    "joy", rclcpp::SensorDataQoS(), callback_sub_joy);
+
   sub_blade_height_ = create_subscription<std_msgs::msg::Float32>(
     "mowing/blade/height", rclcpp::ServicesQoS(), callback_sub_mowing_blade_height);
 
@@ -164,6 +174,42 @@ string Micom::MakeControlMessage(const geometry_msgs::msg::Twist::SharedPtr msg)
   return message;
 }
 
+string Micom::MakeControlMessage(const sensor_msgs::msg::Joy::SharedPtr msg) const
+{
+  cloisim::msgs::Joystick joyBuf;
+
+  auto translation_ptr = joyBuf.mutable_translation();
+  auto rotation_ptr = joyBuf.mutable_rotation();
+
+  // std::cout << "msg Axis=";
+  // for (const auto & msg_axis : msg->axes) std::cout << msg_axis << ", ";
+  // std::cout << std::endl;
+  const auto roll = msg->axes[0];
+  const auto linear_x = msg->axes[1];
+  const auto yaw = msg->axes[2];  // angular
+  const auto pitch = msg->axes[3];
+  const auto linear_z = ((msg->axes[4] + 1.0) - (msg->axes[5] + 1.0)) * 0.5;
+
+  translation_ptr->set_x(linear_x);
+  translation_ptr->set_y(0);
+  translation_ptr->set_z(linear_z);
+  rotation_ptr->set_x(roll);
+  rotation_ptr->set_y(pitch);
+  rotation_ptr->set_z(yaw);
+
+  // std::cout << "msg Button=";
+  for (const auto& msg_button : msg->buttons)
+  {
+    joyBuf.add_buttons(msg_button);
+    // std::cout << msg_button << ", ";
+  }
+  // std::cout << std::endl;
+
+  string message;
+  joyBuf.SerializeToString(&message);
+  return message;
+}
+
 string Micom::MakeMowingBladeHeightMessage(const std_msgs::msg::Float32::SharedPtr msg) const
 {
   cloisim::msgs::Param paramBuf;

diff --git a/cloisim_ros_protobuf_msgs/CMakeLists.txt b/cloisim_ros_protobuf_msgs/CMakeLists.txt
@@ -25,7 +25,7 @@ list(APPEND PROTOBUF_DEFINITION_FILES "laserscan;laserscan_stamped")
 list(APPEND PROTOBUF_DEFINITION_FILES "micom;battery;pointcloud;gps")
 list(APPEND PROTOBUF_DEFINITION_FILES "world_stats;log_playback_stats")
 list(APPEND PROTOBUF_DEFINITION_FILES "perception;perception_v")
-list(APPEND PROTOBUF_DEFINITION_FILES "request;response")
+list(APPEND PROTOBUF_DEFINITION_FILES "request;response;joystick")
 list(APPEND PROTOBUF_DEFINITION_FILES "joint_state;joint_state_v;joint_cmd;joint_cmd_v;pid")
 list(APPEND PROTOBUF_DEFINITION_FILES "transform_stamped")