publish sonar values as pointcloud2

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);
 }

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();

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

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);
+}