From b726e8a4fce8052db69b5641863c1ceb856d48bf Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Tue, 18 Apr 2023 10:37:23 +0200
Subject: [PATCH] add publishing of custom message

---
 src/beacon_positioning/CMakeLists.txt         |  2 +-
 .../msg/TrackerPosition.msg                   |  1 -
 .../src/tracker_position.cpp                  | 19 ++++++++++++++-----
 3 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/src/beacon_positioning/CMakeLists.txt b/src/beacon_positioning/CMakeLists.txt
index 394101c3..ab0b42f6 100644
--- a/src/beacon_positioning/CMakeLists.txt
+++ b/src/beacon_positioning/CMakeLists.txt
@@ -25,7 +25,7 @@ find_package(std_msgs REQUIRED)
 find_package(positioning_systems_api REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
-rosidl_generate_interfaces($PROJECT_NAME
+rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/TrackerPosition.msg" # message for tracker position
 )
 
diff --git a/src/beacon_positioning/msg/TrackerPosition.msg b/src/beacon_positioning/msg/TrackerPosition.msg
index 23ecbff4..f0c50fa6 100644
--- a/src/beacon_positioning/msg/TrackerPosition.msg
+++ b/src/beacon_positioning/msg/TrackerPosition.msg
@@ -1,4 +1,3 @@
-int32 id
 int64 x_pos
 int64 y_pos
 int64 z_pos
diff --git a/src/beacon_positioning/src/tracker_position.cpp b/src/beacon_positioning/src/tracker_position.cpp
index 36a1f2f7..66f459f7 100644
--- a/src/beacon_positioning/src/tracker_position.cpp
+++ b/src/beacon_positioning/src/tracker_position.cpp
@@ -3,6 +3,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
+#indlude "beacon_positioning/msg/TrackerPosition.hpp"
 
 #include "rtls_driver/rtls_driver.hpp"
 
@@ -43,7 +44,7 @@ public:
       return;
     }
 
-    publisher_ = this->create_publisher<std_msgs::msg::String>("beacon_positioning", 10);
+    publisher_ = this->create_publisher<beacon_positioning::msg::TrackerPosition>("beacon_positioning", 10);
     timer_ = this->create_wall_timer(
         500ms, std::bind(&BeaconPositioningPublisher::timer_callback, this));
   }
@@ -78,7 +79,7 @@ public:
     return serial_port;
   }
 
-  void publish(std_msgs::msg::String msg)
+  void publish(beacon_positioning::msg::TrackerPosition msg)
   {
     publisher_->publish(msg);
   }
@@ -94,7 +95,7 @@ private:
   }
 
   rclcpp::TimerBase::SharedPtr timer_;                            // timer to trigger the
-  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_; // pointer to publisher object
+  rclcpp::Publisher<beacon_positioning::msg::TrackerPosition>::SharedPtr publisher_; // pointer to publisher object
 
   // terabee tower evo variables
   std::shared_ptr<terabee::serial_communication::ISerial> serial_port; // serial port for communicating with tracker
@@ -135,8 +136,16 @@ int main(int argc, char **argv)
         RCLCPP_INFO(node->get_logger(), "anchor number= %d, distance = %d, x = %d, y = %d, z = %d", tracker_msg.anchors_data[i].number, tracker_msg.anchors_data[i].distance, tracker_msg.anchors_data[i].pos_x, tracker_msg.anchors_data[i].pos_y, tracker_msg.anchors_data[i].pos_z);
       }
       RCLCPP_INFO(node->get_logger(), "");
-      auto message = std_msgs::msg::String();
-      message.data = "Hello from tracker callback!";
+      auto message = beacon_positioning::msg::TrackerPosition();
+      message.x_pos = tracker_msg.tracker_position_xyz.at(0);
+      message.y_pos = tracker_msg.tracker_position_xyz.at(1);
+      message.z_pos = tracker_msg.tracker_position_xyz.at(2);
+      message.anchor_distances = {0, 0, 0, 0};
+      for (int i = 0; i < tracker_msg.anchors_data.size(); i++)
+      {
+        message.anchor_distances[i] = tracker_msg.anchors_data[i].distance;
+      }
+
       node->publish(message);
     }
     else