Merge pull request #15 from tiiuae/microxrce-migrate

MicroXRCE migrate
tiiuae · Sep 27, 2022 · 0be09ff · 0be09ff
2 parents da1658f + ba971d9
commit 0be09ff
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 2 deletions.
diff --git a/include/mocap_pose/mocap_pose.hpp b/include/mocap_pose/mocap_pose.hpp
@@ -6,6 +6,8 @@
 #include <rclcpp/rclcpp.hpp>
 //#include <std_msgs/msg/string.hpp>
 
+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
@@ -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<px4_msgs::msg::SensorGps>("fmu/sensor_gps/in", 10);
+    impl_->publisher = create_publisher<px4_msgs::msg::SensorGps>(kGpsSensorTopic, 10);
     impl_->worker_thread_running = true;
     impl_->worker_thread = std::thread(&MocapPose::WorkerThread, this);
 }
@@ -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();