add parameter for serial port

.vscode/settings.json

@@ -2,5 +2,15 @@
     "ros.distro": "humble",
     "files.associations": {
         "chrono": "cpp"
-    }
+    },
+    "python.autoComplete.extraPaths": [
+        "/home/ubuntu/ros2_ws/install/px4_msgs/lib/python3.8/site-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/ubuntu/ros2_ws/install/px4_msgs/lib/python3.8/site-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ]
 }

src/beacon_positioning/CMakeLists.txt

@@ -28,8 +28,8 @@ add_executable(tracker_position src/tracker_position.cpp)
 ament_target_dependencies(tracker_position rclcpp std_msgs)
 
 target_link_libraries(tracker_position
-  positioning_systems_api::follow_me_driver
+positioning_systems_api::serial_communication
-  positioning_systems_api::serial_communication
+  positioning_systems_api::rtls_driver
 )
 
 target_include_directories(tracker_position PUBLIC

src/beacon_positioning/src/tracker_position.cpp

@@ -6,33 +6,92 @@
 
 #include "rtls_driver/rtls_driver.hpp"
 
+// from the example: https://github.com/Terabee/positioning_systems_api/blob/master/examples/rtls_tracker_example.cpp
+#include "serial_communication/serial.hpp"
+using SerialInterface = terabee::serial_communication::Serial;
+
 using namespace std::chrono_literals;
 
 class BeaconPositioningPublisher : public rclcpp::Node
 {
-  public:
+public:
-    BeaconPositioningPublisher() : Node("beacon_positioning_publisher") {
+  BeaconPositioningPublisher() : Node("beacon_positioning_publisher")
-      publisher_ = this->create_publisher<std_msgs::msg::String>("beacon_positioning", 10);
+  {
-      timer_ = this->create_wall_timer(
+
+    this->declare_parameter("tracker_serial_port", "/dev/ttyUSB0");
+
+    std::string serial_port_name = this->get_parameter("tracker_serial_port").as_string();
+
+    RCLCPP_INFO(this->get_logger(), "serial port is %s\n", serial_port_name.c_str());
+
+    serial_port = std::make_shared<SerialInterface>("/dev/ttyUSB0");
+    serial_port->setBaudrate(115200);
+    serial_port->setTimeout(800ms);
+
+    RCLCPP_INFO(this->get_logger(), "Opening serial port of tracker!");
+    serial_port->open();
+
+    if (!serial_port->isOpen())
+    {
+      RCLCPP_ERROR(this->get_logger(), "Failed to open serial port!");
+      return;
+    }
+
+    rtls_device = terabee::RtlsDevice(serial_port);
+
+    publisher_ = this->create_publisher<std_msgs::msg::String>("beacon_positioning", 10);
+    timer_ = this->create_wall_timer(
         500ms, std::bind(&BeaconPositioningPublisher::timer_callback, this));
+  }
+
+private:
+  void timer_callback()
+  {
+    auto message = std_msgs::msg::String();
+    message.data = "Hello beacons!";
+    RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
+    publisher_->publish(message);
+  }
+
+  void setup_rtlsdevice(int priority, int label, int update_time, int network_id, bool long_message)
+  {
+    rtls_device.disableTrackerStream();
+    serial_port->flushInput();
+    rtls_device.setDevice(terabee::RtlsDevice::device_type::tracker, priority);
+    rtls_device.setLabel(label);
+    rtls_device.setUpdateTime(update_time);
+    rtls_device.setNetworkId(network_id);
+    if (long_message)
+    {
+      rtls_device.setTrackerMessageLong();
     }
-  private:
+    else
-    void timer_callback() {
+    {
-      auto message = std_msgs::msg::String();
+      rtls_device.setTrackerMessageShort();
-      message.data = "Hello beacons!";
-      RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
-      publisher_->publish(message);
     }
-    rclcpp::TimerBase::SharedPtr timer_; // timer to trigger the 
+    rtls_device.enableLED();
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_; // pointer to publisher object
+    rtls_device.requestConfig();
+    device_configuration = rtls_device.getConfig();
+
+    rtls_device.enableTrackerStream();
+  }
+
+  rclcpp::TimerBase::SharedPtr timer_;                            // timer to trigger the
+  rclcpp::Publisher<std_msgs::msg::String>::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
+  terabee::RtlsDevice rtls_device;
+  terabee::RtlsDevice::config_t device_configuration;
+  terabee::RtlsDevice::OnTrackerDataCallback tracker_data_callback_;
 };
 
-int main(int argc, char ** argv)
+int main(int argc, char **argv)
 {
-  (void) argc;
+  (void)argc;
-  (void) argv;
+  (void)argv;
 
-  printf("hello world beacon_positioning package\n");
+  printf("starting tracker node of beacon_positioning package\n");
 
   rclcpp::init(argc, argv);
   rclcpp::spin(std::make_shared<BeaconPositioningPublisher>());