From 0921c6db5a11577e60556e5c824d83ea771bd53b Mon Sep 17 00:00:00 2001
From: "Hyunseok Yang (YG)" <hyunseok7.yang@lge.com>
Date: Wed, 20 Nov 2024 19:02:35 +0900
Subject: [PATCH] publish bumper/irr/us topic in cloisim_ros_micom

---
 cloisim_ros_micom/CMakeLists.txt              |  2 +
 .../include/cloisim_ros_micom/micom.hpp       | 17 +++++
 cloisim_ros_micom/src/micom.cpp               | 63 ++++++++++++++++++-
 3 files changed, 80 insertions(+), 2 deletions(-)

diff --git a/cloisim_ros_micom/CMakeLists.txt b/cloisim_ros_micom/CMakeLists.txt
index 0bbd7054..5add3c91 100644
--- a/cloisim_ros_micom/CMakeLists.txt
+++ b/cloisim_ros_micom/CMakeLists.txt
@@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(cloisim_ros_base REQUIRED)
 find_package(cloisim_ros_bringup_param REQUIRED)
@@ -22,6 +23,7 @@ set(dependencies
   sensor_msgs
   geometry_msgs
   nav_msgs
+  std_msgs
   std_srvs
   cloisim_ros_base
   cloisim_ros_bringup_param
diff --git a/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp b/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp
index fdd94786..4933d646 100644
--- a/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp
+++ b/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp
@@ -29,6 +29,8 @@
 #include <std_srvs/srv/empty.hpp>
 #include <std_msgs/msg/float32.hpp>
 #include <std_msgs/msg/u_int16.hpp>
+#include <std_msgs/msg/u_int8_multi_array.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
 
 namespace cloisim_ros
 {
@@ -64,6 +66,9 @@ class Micom : public Base
   void UpdateOdom();
   void UpdateImu();
   void UpdateBattery();
+  void UpdateBumper();
+  void UpdateIR();
+  void UpdateUSS();
 
 private:
   zmq::Bridge * info_bridge_ptr;
@@ -82,10 +87,22 @@ class Micom : public Base
   // Battery
   sensor_msgs::msg::BatteryState msg_battery_;
 
+  // Bumper
+  std_msgs::msg::UInt8MultiArray msg_bumper_;
+
+  // USS
+  std_msgs::msg::Float64MultiArray msg_uss_;
+
+  // IR
+  std_msgs::msg::Float64MultiArray msg_ir_;
+
   // ROS2 micom publisher
   rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu_;
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom_;
   rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr pub_battery_;
+  rclcpp::Publisher<std_msgs::msg::UInt8MultiArray>::SharedPtr pub_bumper_;
+  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr pub_ir_;
+  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr pub_uss_;
 
   // wheel command subscriber
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_cmd_;
diff --git a/cloisim_ros_micom/src/micom.cpp b/cloisim_ros_micom/src/micom.cpp
index f4a4856e..707efdcc 100644
--- a/cloisim_ros_micom/src/micom.cpp
+++ b/cloisim_ros_micom/src/micom.cpp
@@ -72,6 +72,15 @@ void Micom::Initialize()
   pub_battery_ =
     create_publisher<sensor_msgs::msg::BatteryState>("battery_state", rclcpp::SensorDataQoS());
 
+  pub_bumper_ =
+    create_publisher<std_msgs::msg::UInt8MultiArray>("bumper", rclcpp::SensorDataQoS());
+
+  pub_ir_ =
+    create_publisher<std_msgs::msg::Float64MultiArray>("ir", rclcpp::SensorDataQoS());
+
+  pub_uss_ =
+    create_publisher<std_msgs::msg::Float64MultiArray>("uss", rclcpp::SensorDataQoS());
+
   {
     msg_odom_.header.frame_id = "odom";
     msg_odom_.child_frame_id = "base_footprint";
@@ -198,8 +207,7 @@ string Micom::MakeControlMessage(const sensor_msgs::msg::Joy::SharedPtr msg) con
   rotation_ptr->set_z(yaw);
 
   // std::cout << "msg Button=";
-  for (const auto& msg_button : msg->buttons)
-  {
+  for (const auto & msg_button : msg->buttons) {
     joyBuf.add_buttons(msg_button);
     // std::cout << msg_button << ", ";
   }
@@ -253,6 +261,9 @@ void Micom::PublishData(const string & buffer)
   UpdateOdom();
   UpdateImu();
   UpdateBattery();
+  UpdateBumper();
+  UpdateIR();
+  UpdateUSS();
 
   // publish data
   PublishTF(odom_tf_);
@@ -260,6 +271,9 @@ void Micom::PublishData(const string & buffer)
   pub_odom_->publish(msg_odom_);
   pub_imu_->publish(msg_imu_);
   pub_battery_->publish(msg_battery_);
+  pub_bumper_->publish(msg_bumper_);
+  pub_uss_->publish(msg_uss_);
+  pub_ir_->publish(msg_ir_);
 }
 
 void Micom::UpdateOdom()
@@ -324,4 +338,49 @@ void Micom::UpdateBattery()
   }
 }
 
+void Micom::UpdateBumper()
+{
+  if (pb_micom_.has_bumper()) {
+    // std::cout << "bumper Size " <<  pb_micom_.bumper().bumped_size() << std::endl;
+    msg_bumper_.data.clear();
+    msg_bumper_.data.resize(pb_micom_.bumper().bumped_size());
+
+    for (auto i = 0; i < pb_micom_.bumper().bumped_size(); i++) {
+      // std::cout << pb_micom_.bumper().bumped(i) << " ";
+      msg_bumper_.data[i] = pb_micom_.bumper().bumped(i);
+    }
+    // std::cout << std::endl;
+  }
+}
+
+void Micom::UpdateIR()
+{
+  if (pb_micom_.has_ir()) {
+    // std::cout << "bumper Size " <<  pb_micom_.bumper().bumped_size() << std::endl;
+    msg_ir_.data.clear();
+    msg_ir_.data.resize(pb_micom_.ir().distance_size());
+
+    for (auto i = 0; i < pb_micom_.ir().distance_size(); i++) {
+      // std::cout << pb_micom_.bumper().bumped(i) << " ";
+      msg_ir_.data[i] = pb_micom_.ir().distance(i);
+    }
+    // std::cout << std::endl;
+  }
+}
+
+void Micom::UpdateUSS()
+{
+  if (pb_micom_.has_uss()) {
+    // std::cout << "bumper Size " <<  pb_micom_.bumper().bumped_size() << std::endl;
+    msg_uss_.data.clear();
+    msg_uss_.data.resize(pb_micom_.uss().distance_size());
+
+    for (auto i = 0; i < pb_micom_.uss().distance_size(); i++) {
+      // std::cout << pb_micom_.bumper().bumped(i) << " ";
+      msg_uss_.data[i] = pb_micom_.uss().distance(i);
+    }
+    // std::cout << std::endl;
+  }
+}
+
 }  // namespace cloisim_ros