From e0d20a50491756acbc0c6e76a505e611236e2115 Mon Sep 17 00:00:00 2001
From: Wojciech Kwolek <me@irth.pl>
Date: Fri, 30 Apr 2021 13:48:56 +0000
Subject: [PATCH] publish sonar values as pointcloud2

---
 src/ros2aria/src/publish.cpp  |  1 +
 src/ros2aria/src/ros2aria.cpp |  1 +
 src/ros2aria/src/ros2aria.hpp |  4 ++++
 src/ros2aria/src/sonar.cpp    | 19 ++++++++++++++-----
 4 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/src/ros2aria/src/publish.cpp b/src/ros2aria/src/publish.cpp
index 89b1017..506b29b 100644
--- a/src/ros2aria/src/publish.cpp
+++ b/src/ros2aria/src/publish.cpp
@@ -6,4 +6,5 @@ void Ros2Aria::publish()
     rclcpp::Time t = robot->getClock()->now();
     sensor_msgs::msg::PointCloud sonarData = handleSonar(t);
     publishSonar(sonarData);
+    publishSonarPointCloud2(sonarData);
 }
\ No newline at end of file
diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp
index 076290b..f4d1239 100644
--- a/src/ros2aria/src/ros2aria.cpp
+++ b/src/ros2aria/src/ros2aria.cpp
@@ -15,6 +15,7 @@ Ros2Aria::Ros2Aria()
     stop_service_ = this->create_service<std_srvs::srv::Empty>("stop", std::bind(&Ros2Aria::stop, this, _1, _2));
 
     sonar_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud>("sonar", 10);
+    sonar_pointcloud2_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("sonar_pointcloud2", 10);
 
     // listen to sensors
     auto r = robot->getRobot();
diff --git a/src/ros2aria/src/ros2aria.hpp b/src/ros2aria/src/ros2aria.hpp
index 435a66f..4a3f082 100644
--- a/src/ros2aria/src/ros2aria.hpp
+++ b/src/ros2aria/src/ros2aria.hpp
@@ -2,6 +2,7 @@
 #include "Aria/Aria.h"
 
 #include "sensor_msgs/msg/point_cloud.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "std_srvs/srv/empty.hpp"
 
@@ -25,8 +26,11 @@ private:
     void publish();
 
     rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr sonar_pub_;
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr sonar_pointcloud2_pub_;
     sensor_msgs::msg::PointCloud handleSonar(rclcpp::Time stamp);
+
     void publishSonar(sensor_msgs::msg::PointCloud cloud);
+    void publishSonarPointCloud2(sensor_msgs::msg::PointCloud cloud);
 
     // subscribers
     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
diff --git a/src/ros2aria/src/sonar.cpp b/src/ros2aria/src/sonar.cpp
index a3f6ad3..de5f5d4 100644
--- a/src/ros2aria/src/sonar.cpp
+++ b/src/ros2aria/src/sonar.cpp
@@ -1,5 +1,7 @@
 #include "ros2aria.hpp"
 
+#include "sensor_msgs/point_cloud_conversion.hpp"
+
 sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)
 {
     sensor_msgs::msg::PointCloud cloud; //sonar readings.
@@ -20,14 +22,12 @@ sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)
         }
 
         // getRange() will return an integer between 0 and 5000 (5m)
-        std::cout << reading->getRange() << " ";
-
         // local (x,y). Appears to be from the centre of the robot, since values may
         // exceed 5000. This is good, since it means we only need 1 transform.
         // x & y seem to be swapped though, i.e. if the robot is driving north
         // x is north/south and y is east/west.
         //
-        //ArPose sensor = reading->getSensorPosition();  //position of sensor.
+        //ArPose sensor = reading->getSensorPosition();  //position sensor.
         // sonar_debug_info << "(" << reading->getLocalX()
         //                  << ", " << reading->getLocalY()
         //                  << ") from (" << sensor.getX() << ", "
@@ -47,7 +47,16 @@ void Ros2Aria::publishSonar(sensor_msgs::msg::PointCloud cloud)
     if (this->sonar_pub_->get_subscription_count() == 0)
         return;
 
-    RCLCPP_INFO(this->get_logger(), "publishing sonar pointcloud");
-
     this->sonar_pub_->publish(cloud);
+}
+
+void Ros2Aria::publishSonarPointCloud2(sensor_msgs::msg::PointCloud cloud)
+{
+    if (this->sonar_pointcloud2_pub_->get_subscription_count() == 0)
+        return;
+
+    sensor_msgs::msg::PointCloud2 cloud2;
+    sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2);
+
+    this->sonar_pointcloud2_pub_->publish(cloud2);
 }
\ No newline at end of file