Merge pull request #87 from lge-ros2/main

Merge into jazzy
lge-ros2 · Nov 14, 2024 · e283929 · e283929
2 parents 609add3 + e5af53a
commit e283929
Show file tree

Hide file tree

Showing 23 changed files with 74 additions and 22 deletions.
diff --git a/cloisim_ros_actor/package.xml b/cloisim_ros_actor/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_actor</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>node for actor plugin</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>

diff --git a/cloisim_ros_base/package.xml b/cloisim_ros_base/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_base</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>CLOiSim-ROS base class for other CLOiSim-ROS</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>

diff --git a/cloisim_ros_bridge_zmq/package.xml b/cloisim_ros_bridge_zmq/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_bridge_zmq</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>bridge for cloisim(simulator) connection through ZMQ</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>

diff --git a/cloisim_ros_bringup/package.xml b/cloisim_ros_bringup/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_bringup</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>Bringup scripts and configurations for cloisim_ros</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <maintainer email="sungkyu.kang@lge.com">Sungkyu Kang</maintainer>

diff --git a/cloisim_ros_bringup_param/package.xml b/cloisim_ros_bringup_param/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_bringup_param</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>Bringup scripts and configurations for cloisim_ros</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>

diff --git a/cloisim_ros_camera/package.xml b/cloisim_ros_camera/package.xml
@@ -2,13 +2,13 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_camera</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>Virtual camera/depth camera/segmentation camera for CLOiSim</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>
 
   <author email="sungkyu.kang@lge.com">Sungkyu Kang</author>
-  <author email="ljm6525@gmail.com">Jaemin Lee </author>
+  <author email="ljm6525@gmail.com">Jaemin Lee</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

diff --git a/cloisim_ros_elevator_system/package.xml b/cloisim_ros_elevator_system/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_elevator_system</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>elevator system for simulation</description>
   <maintainer email="sungkyu.kang@lge.com">Sungkyu Kang</maintainer>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>

diff --git a/cloisim_ros_gps/package.xml b/cloisim_ros_gps/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_gps</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>virtual gps for simulation</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>

diff --git a/cloisim_ros_ground_truth/package.xml b/cloisim_ros_ground_truth/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_ground_truth</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>world plugin to retrieve ground truth</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>

diff --git a/cloisim_ros_imu/package.xml b/cloisim_ros_imu/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_imu</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>virtual imu for simulation</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>

diff --git a/cloisim_ros_joint_control/package.xml b/cloisim_ros_joint_control/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_joint_control</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>joint_control package for simulator</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>

diff --git a/cloisim_ros_lidar/package.xml b/cloisim_ros_lidar/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_lidar</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>virtual lidar for simulation</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>

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/package.xml b/cloisim_ros_micom/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_micom</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>micom package for simulator</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>

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_msgs/package.xml b/cloisim_ros_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_msgs</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>interfaces package for cloisim_ros</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>

diff --git a/cloisim_ros_multicamera/package.xml b/cloisim_ros_multicamera/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_multicamera</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>virtual multi-camera for simulator</description>
   <maintainer email="sungkyu.kang@lge.com">Sungkyu Kang</maintainer>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>

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")
 

diff --git a/cloisim_ros_protobuf_msgs/package.xml b/cloisim_ros_protobuf_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_protobuf_msgs</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>CLOiSim-ROS interafces for communication between simulator and CLOiSim-ROS</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>

diff --git a/cloisim_ros_realsense/package.xml b/cloisim_ros_realsense/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_realsense</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>virtual realsense for simulator</description>
   <maintainer email="sungkyu.kang@lge.com">Sungkyu Kang</maintainer>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>

diff --git a/cloisim_ros_sonar/package.xml b/cloisim_ros_sonar/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_sonar</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>virtual sonar for simulation</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>

diff --git a/cloisim_ros_websocket_service/package.xml b/cloisim_ros_websocket_service/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_websocket_service</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>websocket service for cloisim(simulator) connection port control</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>

diff --git a/cloisim_ros_world/package.xml b/cloisim_ros_world/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_world</name>
-  <version>4.4.0</version>
+  <version>4.5.0</version>
   <description>Utilities to interface with Unity through ROS.</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <license>MIT</license>