Add use_fixed_sensor_pose flag

MOLAorg · Mar 15, 2024 · 0a6a43e · 0a6a43e
1 parent 6ff2c3e
commit 0a6a43e
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 5 deletions.
diff --git a/mola_demos/mola-cli-launchs/ros2_ouster_just_view.yaml b/mola_demos/mola-cli-launchs/ros2_ouster_just_view.yaml
@@ -37,6 +37,7 @@ modules:
           output_sensor_label: lidar_points
           # If present, this will override whatever /tf tells about the sensor pose:
           fixed_sensor_pose: "0 0 0 0 0 0"  # 'x y z yaw_deg pitch_deg roll_deg''
+          #use_fixed_sensor_pose: true
           #queue_size: 100  # QoS "history" length
         - topic: /odom
           type: Odometry

diff --git a/mola_input_ros2/src/InputROS2.cpp b/mola_input_ros2/src/InputROS2.cpp
@@ -71,7 +71,9 @@ void InputROS2::ros_node_thread_main(Yaml cfg)
         char const* const argv[2] = {NODE_NAME, nullptr};
 
         // Initialize ROS:
-        rclcpp::init(argc, argv);
+        // Initialize ROS (only once):
+        if (!rclcpp::ok()) { rclcpp::init(argc, argv); }
+
         auto node = std::make_shared<rclcpp::Node>(NODE_NAME);
 
         {
@@ -120,7 +122,9 @@ void InputROS2::ros_node_thread_main(Yaml cfg)
 
             // Optional: fixed sensorPose (then ignores/don't need "tf" data):
             std::optional<mrpt::poses::CPose3D> fixedSensorPose;
-            if (topic.has("fixed_sensor_pose"))
+            if (topic.has("fixed_sensor_pose") &&
+                (!topic.has("use_fixed_sensor_pose") ||
+                 !topic["use_fixed_sensor_pose"].as<bool>()))
             {
                 fixedSensorPose = mrpt::poses::CPose3D::FromString(
                     "["s + topic["fixed_sensor_pose"].as<std::string>() + "]"s);

diff --git a/mola_input_rosbag2/src/Rosbag2Dataset.cpp b/mola_input_rosbag2/src/Rosbag2Dataset.cpp
@@ -253,7 +253,9 @@ void Rosbag2Dataset::initialize_rds(const Yaml& c)
 
         // Optional: fixed sensorPose (then ignores/don't need "tf" data):
         std::optional<mrpt::poses::CPose3D> fixedSensorPose;
-        if (sensor.count("fixed_sensor_pose") != 0)
+        if (sensor.count("fixed_sensor_pose") != 0 &&
+            (sensor.count("use_fixed_sensor_pose") == 0 ||
+             !sensor.at("use_fixed_sensor_pose").as<bool>()))
         {
             fixedSensorPose = mrpt::poses::CPose3D::FromString(
                 "["s + sensor.at("fixed_sensor_pose").as<std::string>() + "]"s);

diff --git a/mola_output_ros2/src/OutputROS2.cpp b/mola_output_ros2/src/OutputROS2.cpp
@@ -85,8 +85,8 @@ void OutputROS2::ros_node_thread_main([[maybe_unused]] Yaml cfg)
         const int         argc    = 1;
         char const* const argv[2] = {NODE_NAME, nullptr};
 
-        // Initialize ROS:
-        rclcpp::init(argc, argv);
+        // Initialize ROS (only once):
+        if (!rclcpp::ok()) { rclcpp::init(argc, argv); }
 
         auto lckNode = mrpt::lockHelper(rosNodeMtx_);
 
@@ -463,6 +463,8 @@ void OutputROS2::internalOn(const mrpt::obs::CObservationRobotPose& obs)
 
 void OutputROS2::doLookForNewMolaSubs()
 {
+    using namespace std::string_literals;
+
     auto lck = mrpt::lockHelper(molaSubsMtx_);
 
     // RawDataSourceBase:
@@ -472,6 +474,12 @@ void OutputROS2::doLookForNewMolaSubs()
         auto rds = std::dynamic_pointer_cast<mola::RawDataSourceBase>(module);
         ASSERT_(rds);
 
+        // Skip ROS input module, as this would be a pointless re-publishing to
+        // ROS!
+        if (std::string(rds->GetRuntimeClass()->className) ==
+            "mola::InputROS2"s)
+            continue;
+
         if (molaSubs_.dataSources.count(rds) == 0)
         {
             MRPT_LOG_INFO_STREAM(