From 26e182a855f4d3830b4d0be020d5df6f5c427acb Mon Sep 17 00:00:00 2001
From: Wojciech Kwolek <me@irth.pl>
Date: Wed, 21 Apr 2021 09:32:05 +0000
Subject: [PATCH] publish sonar to ROS

---
 src/ros2aria/src/publish.cpp  |  1 +
 src/ros2aria/src/ros2aria.cpp |  2 ++
 src/ros2aria/src/ros2aria.hpp |  3 +++
 src/ros2aria/src/sonar.cpp    | 11 ++++++++++-
 4 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/src/ros2aria/src/publish.cpp b/src/ros2aria/src/publish.cpp
index 8d13e53..89b1017 100644
--- a/src/ros2aria/src/publish.cpp
+++ b/src/ros2aria/src/publish.cpp
@@ -5,4 +5,5 @@ void Ros2Aria::publish()
     // RCLCPP_INFO(this->get_logger(), "publish");
     rclcpp::Time t = robot->getClock()->now();
     sensor_msgs::msg::PointCloud sonarData = handleSonar(t);
+    publishSonar(sonarData);
 }
\ No newline at end of file
diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp
index e8da64c..076290b 100644
--- a/src/ros2aria/src/ros2aria.cpp
+++ b/src/ros2aria/src/ros2aria.cpp
@@ -14,6 +14,8 @@ Ros2Aria::Ros2Aria()
         "cmd_vel", 10, std::bind(&Ros2Aria::cmd_vel_callback, this, _1));
     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);
+
     // listen to sensors
     auto r = robot->getRobot();
     r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb);
diff --git a/src/ros2aria/src/ros2aria.hpp b/src/ros2aria/src/ros2aria.hpp
index 3ed55a3..435a66f 100644
--- a/src/ros2aria/src/ros2aria.hpp
+++ b/src/ros2aria/src/ros2aria.hpp
@@ -23,7 +23,10 @@ private:
 
     // publishers
     void publish();
+
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr sonar_pub_;
     sensor_msgs::msg::PointCloud handleSonar(rclcpp::Time stamp);
+    void publishSonar(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 01cf463..a3f6ad3 100644
--- a/src/ros2aria/src/sonar.cpp
+++ b/src/ros2aria/src/sonar.cpp
@@ -2,7 +2,6 @@
 
 sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)
 {
-
     sensor_msgs::msg::PointCloud cloud; //sonar readings.
     cloud.header.stamp = stamp;
     // sonar sensors relative to base_link
@@ -42,3 +41,13 @@ sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)
     }
     return cloud;
 }
+
+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);
+}
\ No newline at end of file