publish sonar values as pointcloud2

.gitignore

@@ -51,3 +51,6 @@ qtcreator-*
 
 # Catkin custom files
 CATKIN_IGNORE
+
+*.code-workspace
+.uploaded

Dockerfile.devcontainer

@@ -2,15 +2,15 @@ FROM athackst/ros2:foxy-dev
 
 # ** [Optional] Uncomment this section to install additional packages. **
 #
-# ENV DEBIAN_FRONTEND=noninteractive
+ENV DEBIAN_FRONTEND=noninteractive
-# RUN apt-get update \
+RUN apt-get update \
-#    && apt-get -y install --no-install-recommends <your-package-list-here> \
+    && apt-get -y install --no-install-recommends rsync \
-#    #
+    #
-#    # Clean up
+    # Clean up
-#    && apt-get autoremove -y \
+    && apt-get autoremove -y \
-#    && apt-get clean -y \
+    && apt-get clean -y \
-#    && rm -rf /var/lib/apt/lists/*
+    && rm -rf /var/lib/apt/lists/*
-# ENV DEBIAN_FRONTEND=dialog
+ENV DEBIAN_FRONTEND=dialog
 
 # Set up auto-source of workspace for ros user
 

Makefile

@@ -0,0 +1,20 @@
+.PHONY: build run upload
+
+SOURCES := $(wildcard src/ros2aria/src/*.cpp)
+HEADERS := $(wildcard src/ros2aria/src/*.hpp)
+
+build/ros2aria/ros2aria: $(SOURCES) $(HEADERS)
+	colcon build
+
+build: build/ros2aria/ros2aria
+
+.uploaded: build/ros2aria/ros2aria
+	rsync -r  build/ros2aria/ros2aria lab1_5@pionier6:~/ros2aria
+	touch .uploaded
+
+upload: .uploaded
+
+run: upload
+	ssh lab1_5@pionier6 -t -- docker run --rm --network=host -it --device /dev/ttyS0 -v /home/lab1_5:/ws irth7/ros2aria-dev /ws/ros2aria
+
+

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