Implement pose

Squashed commit of the following:

commit 00d9bc9ffa
Author: Wojciech Kwolek <me@irth.pl>
Date:   Fri May 7 12:38:15 2021 +0000

    turns out the only incorrect thing was my assumptions

commit 6d653383a8
Author: Wojciech Kwolek <me@irth.pl>
Date:   Fri Apr 30 14:42:40 2021 +0000

    WIP: initial pose implementation

src/ros2aria/CMakeLists.txt

@@ -22,6 +22,7 @@ find_package(std_srvs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(tf2 REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -33,13 +34,15 @@ add_executable(ros2aria
   src/raiibot.cpp
   src/cmd_vel.cpp
   src/publish.cpp
-  src/sonar.cpp)
+  src/sonar.cpp
+  src/pose.cpp)
   
 ament_target_dependencies(ros2aria rclcpp)
 ament_target_dependencies(ros2aria std_srvs)
 ament_target_dependencies(ros2aria geometry_msgs)
 ament_target_dependencies(ros2aria sensor_msgs)
 ament_target_dependencies(ros2aria nav_msgs)
+ament_target_dependencies(ros2aria tf2)
 target_include_directories(ros2aria PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>

src/ros2aria/src/pose.cpp

@@ -0,0 +1,46 @@
+#include "ros2aria.hpp"
+#include "tf2/utils.h"
+#include "tf2/transform_datatypes.h"
+
+inline void convert_transform_to_pose(const tf2::Transform &trans, geometry_msgs::msg::Pose &pose)
+{
+    geometry_msgs::msg::Transform msg = tf2::toMsg(trans);
+    pose.orientation = msg.rotation;
+    pose.position.x = msg.translation.x;
+    pose.position.y = msg.translation.y;
+    pose.position.z = msg.translation.z;
+}
+
+nav_msgs::msg::Odometry Ros2Aria::handlePose(rclcpp::Time stamp)
+{
+    nav_msgs::msg::Odometry pose;
+
+    auto r = robot->getRobot();
+    ArPose p = r->getPose();
+
+    tf2::Quaternion rotation;
+    rotation.setRPY(0, 0, p.getTh() * M_PI / 180);
+
+    tf2::Vector3 position = tf2::Vector3(p.getX() / 1000, p.getY() / 1000, 0);
+
+    convert_transform_to_pose(tf2::Transform(rotation, position),
+                              pose.pose.pose);
+
+    pose.twist.twist.linear.x = r->getVel() / 1000;
+    pose.twist.twist.linear.y = r->getLatVel() / 1000;
+    pose.twist.twist.angular.z = r->getRotVel() * M_PI / 180;
+
+    pose.header.frame_id = "header.frame_id"; // TODO: use correct frame_id
+    pose.child_frame_id = "child_frame_id";   // TODO: use correct child_frame_id
+    pose.header.stamp = stamp;
+
+    return pose;
+}
+
+void Ros2Aria::publishPose(nav_msgs::msg::Odometry pose)
+{
+    if (this->pose_pub_->get_subscription_count() == 0)
+        return;
+
+    this->pose_pub_->publish(pose);
+}

src/ros2aria/src/publish.cpp

@@ -4,7 +4,11 @@ void Ros2Aria::publish()
 {
     // RCLCPP_INFO(this->get_logger(), "publish");
     rclcpp::Time t = robot->getClock()->now();
+
     sensor_msgs::msg::PointCloud sonarData = handleSonar(t);
     publishSonar(sonarData);
     publishSonarPointCloud2(sonarData);
+
+    nav_msgs::msg::Odometry pose = handlePose(t);
+    publishPose(pose);
 }

src/ros2aria/src/raiibot.cpp

@@ -34,8 +34,11 @@ public:
             // RCLCPP_INFO(this->get_logger(), "RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)");
             // rclcpp::shutdown();
         }
+
         this->robot->runAsync(true);
         this->robot->enableMotors();
+
+        this->startWatchdog();
     }
 
     ~RAIIBot()

src/ros2aria/src/ros2aria.cpp

@@ -17,6 +17,8 @@ Ros2Aria::Ros2Aria()
     sonar_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud>("sonar", 10);
     sonar_pointcloud2_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("sonar_pointcloud2", 10);
 
+    pose_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("pose", 1000);
+
     // listen to sensors
     auto r = robot->getRobot();
     r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb);

src/ros2aria/src/ros2aria.hpp

@@ -4,6 +4,7 @@
 #include "sensor_msgs/msg/point_cloud.hpp"
 #include "sensor_msgs/msg/point_cloud2.hpp"
 #include "geometry_msgs/msg/twist.hpp"
+#include "nav_msgs/msg/odometry.hpp"
 #include "std_srvs/srv/empty.hpp"
 
 #include "./raiibot.cpp"
@@ -25,13 +26,16 @@ private:
     // publishers
     void publish();
 
+    sensor_msgs::msg::PointCloud handleSonar(rclcpp::Time stamp);
     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);
 
+    nav_msgs::msg::Odometry handlePose(rclcpp::Time stamp);
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pose_pub_;
+    void publishPose(nav_msgs::msg::Odometry pose);
+
     // subscribers
     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
     void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);