publish bumper/irr/us topic in cloisim_ros_micom

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

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_;

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,13 +261,19 @@ void Micom::PublishData(const string & buffer)
   UpdateOdom();
   UpdateImu();
   UpdateBattery();
+  UpdateBumper();
+  UpdateIR();
+  UpdateUSS();
 
   // publish data
   PublishTF(odom_tf_);
 
   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