From 5f192e0906a29cc0b229161c2a9a3c9279d00601 Mon Sep 17 00:00:00 2001 From: Jari Nippula Date: Thu, 1 Sep 2022 14:27:36 +0300 Subject: [PATCH 1/3] px4 topic name changed for microxrce --- src/mocap_pose.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mocap_pose.cpp b/src/mocap_pose.cpp index 6a3dc79..af1cd22 100644 --- a/src/mocap_pose.cpp +++ b/src/mocap_pose.cpp @@ -211,7 +211,7 @@ MocapPose::MocapPose() : Node("MocapPose"), impl_(new MocapPose::Impl()), minTim impl_->cb_handle_lon = impl_->param_subscriber->add_parameter_callback("home_lon", callback); impl_->cb_handle_alt = impl_->param_subscriber->add_parameter_callback("home_alt", callback); - impl_->publisher = create_publisher("fmu/sensor_gps/in", 10); + impl_->publisher = create_publisher("/fmu/in/SensorGps", 10); impl_->worker_thread_running = true; impl_->worker_thread = std::thread(&MocapPose::WorkerThread, this); } From 48334d49c6bded4bb6eab04986e143c7c5bc8248 Mon Sep 17 00:00:00 2001 From: Jari Nippula Date: Mon, 26 Sep 2022 11:27:49 +0300 Subject: [PATCH 2/3] Move sensor_gps topic name to header --- include/mocap_pose/mocap_pose.hpp | 2 ++ src/mocap_pose.cpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/include/mocap_pose/mocap_pose.hpp b/include/mocap_pose/mocap_pose.hpp index 6b8551e..971a6a1 100644 --- a/include/mocap_pose/mocap_pose.hpp +++ b/include/mocap_pose/mocap_pose.hpp @@ -6,6 +6,8 @@ #include //#include +const std::string kGpsSensorTopic = "/fmu/in/SensorGps"; + /// ROS2 Driver for Qualisys Motion Capture (Mocap) system /// Provides "fake GPS" messages for selected Mocap object (set up with ROS2 parameters) class MocapPose : public rclcpp::Node diff --git a/src/mocap_pose.cpp b/src/mocap_pose.cpp index af1cd22..31b2dfd 100644 --- a/src/mocap_pose.cpp +++ b/src/mocap_pose.cpp @@ -211,7 +211,7 @@ MocapPose::MocapPose() : Node("MocapPose"), impl_(new MocapPose::Impl()), minTim impl_->cb_handle_lon = impl_->param_subscriber->add_parameter_callback("home_lon", callback); impl_->cb_handle_alt = impl_->param_subscriber->add_parameter_callback("home_alt", callback); - impl_->publisher = create_publisher("/fmu/in/SensorGps", 10); + impl_->publisher = create_publisher(kGpsSensorTopic, 10); impl_->worker_thread_running = true; impl_->worker_thread = std::thread(&MocapPose::WorkerThread, this); } From ba971d9d24aa4474cbda42c037bef951135d05c1 Mon Sep 17 00:00:00 2001 From: Jari Nippula Date: Mon, 26 Sep 2022 15:23:05 +0300 Subject: [PATCH 3/3] Use rclcpp timestamp for sensor_gps msg timestamp rclcpp timestamp needed for microxrce timesync time adjustment. --- src/mocap_pose.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mocap_pose.cpp b/src/mocap_pose.cpp index 31b2dfd..0b1a8c7 100644 --- a/src/mocap_pose.cpp +++ b/src/mocap_pose.cpp @@ -354,7 +354,7 @@ void MocapPose::WorkerThread() #endif const auto timestamp = rclcpp::Clock().now(); const auto gps_timestamp = QualisysToRosTimestamp(rtPacket->GetTimeStamp()); - const auto gps_msg = impl_->PrepareGpsMessage(Pos, Q, gps_timestamp, timestamp); + const auto gps_msg = impl_->PrepareGpsMessage(Pos, Q, timestamp, timestamp); const bool time_to_publish = (impl_->last_published_timestamp.seconds() + impl_->publishing_timestep) <= timestamp.seconds();