diff --git a/.vscode/settings.json b/.vscode/settings.json index 8c455574..7919a835 100644 --- a/.vscode/settings.json +++ b/.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" + ] } \ No newline at end of file diff --git a/src/beacon_positioning/CMakeLists.txt b/src/beacon_positioning/CMakeLists.txt index cc0fb27f..cc31f4e0 100644 --- a/src/beacon_positioning/CMakeLists.txt +++ b/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 diff --git a/src/beacon_positioning/src/tracker_position.cpp b/src/beacon_positioning/src/tracker_position.cpp index e493077e..e7a8aa31 100644 --- a/src/beacon_positioning/src/tracker_position.cpp +++ b/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: - BeaconPositioningPublisher() : Node("beacon_positioning_publisher") { - publisher_ = this->create_publisher("beacon_positioning", 10); - timer_ = this->create_wall_timer( +public: + BeaconPositioningPublisher() : Node("beacon_positioning_publisher") + { + + 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("/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("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: - 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); + else + { + rtls_device.setTrackerMessageShort(); } - rclcpp::TimerBase::SharedPtr timer_; // timer to trigger the - rclcpp::Publisher::SharedPtr publisher_; // pointer to publisher object + rtls_device.enableLED(); + rtls_device.requestConfig(); + device_configuration = rtls_device.getConfig(); + + rtls_device.enableTrackerStream(); + } + + rclcpp::TimerBase::SharedPtr timer_; // timer to trigger the + rclcpp::Publisher::SharedPtr publisher_; // pointer to publisher object + + // terabee tower evo variables + std::shared_ptr 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) argv; + (void)argc; + (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());