read sonar data; split Ros2Aria into multiple files

.vscode/settings.json

@@ -62,5 +62,8 @@
         "cinttypes": "cpp",
         "typeindex": "cpp",
         "typeinfo": "cpp"
-    }
+    },
+    "cSpell.words": [
+        "rclcpp"
+    ]
 }

src/ros2aria/CMakeLists.txt

@@ -20,15 +20,24 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
 
-add_executable(ros2aria src/ros2aria.cpp src/raiibot.cpp)
+add_executable(ros2aria
+  src/main.cpp
+  src/ros2aria.cpp
+  src/raiibot.cpp
+  src/cmd_vel.cpp
+  src/publish.cpp
+  src/sonar.cpp)
+  
 ament_target_dependencies(ros2aria rclcpp)
 ament_target_dependencies(ros2aria std_srvs)
 ament_target_dependencies(ros2aria geometry_msgs)
+ament_target_dependencies(ros2aria sensor_msgs)
 target_include_directories(ros2aria PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>

src/ros2aria/package.xml

@@ -15,6 +15,7 @@
   <depend>rclcpp</depend>
   <depend>std_srvs</depend>
   <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/ros2aria/src/cmd_vel.cpp

@@ -0,0 +1,20 @@
+#include "ros2aria.hpp"
+
+void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
+{
+    float x, y, z;
+    x = msg->linear.x;
+    y = msg->linear.y;
+    z = msg->angular.z;
+    RCLCPP_INFO(this->get_logger(), "cmd_vel: x:%f y:%f z:%f", x, y, z);
+
+    std::lock_guard<RAIIBot> lock(*robot.get());
+
+    this->robot->pokeWatchdog();
+
+    auto r = robot->getRobot();
+    r->setVel(x * 1e3);
+    if (r->hasLatVel())
+        r->setLatVel(y * 1e3);
+    r->setRotVel(z * 180 / M_PI);
+}

src/ros2aria/src/main.cpp

@@ -0,0 +1,18 @@
+#include <cstdio>
+#include <memory>
+
+#include "ros2aria.hpp"
+
+int main(int argc, char **argv)
+{
+  (void)argc;
+  (void)argv;
+
+  printf("hello world ros2aria package\n");
+
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<Ros2Aria>());
+  rclcpp::shutdown();
+
+  return 0;
+}

src/ros2aria/src/publish.cpp

@@ -0,0 +1,8 @@
+#include "ros2aria.hpp"
+
+void Ros2Aria::publish()
+{
+    // RCLCPP_INFO(this->get_logger(), "publish");
+    rclcpp::Time t = robot->getClock()->now();
+    sensor_msgs::msg::PointCloud sonarData = handleSonar(t);
+}

src/ros2aria/src/raiibot.cpp

@@ -73,6 +73,11 @@ public:
         return this->robot;
     }
 
+    rclcpp::Clock::SharedPtr getClock()
+    {
+        return this->clock;
+    }
+
     void lock()
     {
         if (this->robot != nullptr)

src/ros2aria/src/ros2aria.cpp

@@ -1,27 +1,13 @@
-#include <cstdio>
+#include "ros2aria.hpp"
-#include <memory>
-
-#include "Aria/Aria.h"
-
-#include "rclcpp/rclcpp.hpp"
-#include "std_msgs/msg/string.hpp"
-#include "std_srvs/srv/empty.hpp"
-#include "geometry_msgs/msg/twist.hpp"
-
-#include "./raiibot.cpp"
 
 using std::placeholders::_1;
 using std::placeholders::_2;
-using namespace std::chrono_literals;
 
-class Ros2Aria : public rclcpp::Node
+Ros2Aria::Ros2Aria()
+    : Node("ros2aria"),
+      sensorCb(this, &Ros2Aria::publish)
 {
-public:
+    this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0"); // TODO: use args for port
-  Ros2Aria()
-      : Node("ros2aria"),
-        sensorCb(this, &Ros2Aria::publish)
-  {
-    this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0");
 
     RCLCPP_INFO(this->get_logger(), "starting subscribers and services");
     cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
@@ -31,75 +17,18 @@ public:
     // listen to sensors
     auto r = robot->getRobot();
     r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb);
-  }
 
-  ~Ros2Aria()
+    sonar_frame_id = "pionier6"; // TODO: use args
-  {
+}
+
+Ros2Aria::~Ros2Aria()
+{
     auto r = robot->getRobot();
     r->remSensorInterpTask("ROSPublishingTask");
-  }
+}
 
-  void publish()
+void Ros2Aria::stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const
-  {
+{
-    RCLCPP_INFO(this->get_logger(), "publish");
-  }
-
-private:
-  RAIIBot::SharedPtr robot;
-
-  ArFunctorC<Ros2Aria> sensorCb;
-
-  void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
-  {
-    float x, y, z;
-    x = msg->linear.x;
-    y = msg->linear.y;
-    z = msg->angular.z;
-    RCLCPP_INFO(this->get_logger(), "cmd_vel: x:%f y:%f z:%f", x, y, z);
-
-    std::lock_guard<RAIIBot> lock(*robot.get());
-
-    this->robot->pokeWatchdog();
-
-    auto r = robot->getRobot();
-    r->setVel(x * 1e3);
-    if (r->hasLatVel())
-      r->setLatVel(y * 1e3);
-    r->setRotVel(z * 180 / M_PI);
-  }
-
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
-
-  void stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const
-  {
     RCLCPP_INFO(this->get_logger(), "stop");
     rclcpp::shutdown();
-  }
+}
-  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_service_;
-};
-
-int main(int argc, char **argv)
-{
-  (void)argc;
-  (void)argv;
-
-  printf("hello world ros2aria package\n");
-  // printf("Enabled motors\n");
-  // robot->setVel(2);
-  // robot->move(500);
-
-  // sleep(10);
-
-  // robot->move(-500);
-  // sleep(2);
-  // conn->disconnectAll();
-
-  // return 0;
-
-  // return 0;
-  rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<Ros2Aria>());
-  rclcpp::shutdown();
-
-  return 0;
-}

src/ros2aria/src/ros2aria.hpp

@@ -0,0 +1,35 @@
+#include "rclcpp/rclcpp.hpp"
+#include "Aria/Aria.h"
+
+#include "sensor_msgs/msg/point_cloud.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "std_srvs/srv/empty.hpp"
+
+#include "./raiibot.cpp"
+
+class Ros2Aria : public rclcpp::Node
+{
+public:
+    Ros2Aria();
+    ~Ros2Aria();
+
+private:
+    // robot
+    RAIIBot::SharedPtr robot;
+    ArFunctorC<Ros2Aria> sensorCb;
+
+    // config
+    std::string sonar_frame_id;
+
+    // publishers
+    void publish();
+    sensor_msgs::msg::PointCloud handleSonar(rclcpp::Time stamp);
+
+    // subscribers
+    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
+    void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
+
+    // services
+    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_service_;
+    void stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const;
+};

src/ros2aria/src/sonar.cpp

@@ -0,0 +1,44 @@
+#include "ros2aria.hpp"
+
+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
+    cloud.header.frame_id = this->sonar_frame_id;
+
+    auto r = robot->getRobot();
+
+    for (int i = 0; i < r->getNumSonar(); i++)
+    {
+        ArSensorReading *reading = NULL;
+        reading = r->getSonarReading(i);
+        if (!reading)
+        {
+            RCLCPP_INFO(this->get_logger(), "did not receive a sonar reading.");
+            continue;
+        }
+
+        // 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.
+        // sonar_debug_info << "(" << reading->getLocalX()
+        //                  << ", " << reading->getLocalY()
+        //                  << ") from (" << sensor.getX() << ", "
+        //                  << sensor.getY() << ") ;; " ;
+        //add sonar readings (robot-local coordinate frame) to cloud
+        geometry_msgs::msg::Point32 p;
+        p.x = reading->getLocalX() / 1000.0;
+        p.y = reading->getLocalY() / 1000.0;
+        p.z = 0.0;
+        cloud.points.push_back(p);
+    }
+    return cloud;
+}