Improve navstate print including the time

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>

Author: fmrico

localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp

@@ -177,7 +177,8 @@ AMCLLocalizer::AMCLLocalizer()
       double roll, pitch, yaw;
       tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
 
-      ret << "Odometry with pose: (x: " << x << ", y: " << y << ", yaw: " << yaw << ")";
+      ret << "{" << rclcpp::Time(odom.header.stamp).seconds() << " } Odometry with pose: (x: " <<
+        x << ", y: " << y << ", yaw: " << yaw << ")";
       return ret.str();
     });
 }

localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp

@@ -357,7 +357,8 @@ AMCLLocalizer::AMCLLocalizer()
       tf2::Quaternion q(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y,
                         odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
       double r, p, y; tf2::Matrix3x3(q).getRPY(r, p, y);
-      ret << "Odometry with pose: (x: " << odom.pose.pose.position.x
+      ret << "{" << rclcpp::Time(odom.header.stamp).seconds() << "} Odometry with pose: (x: " <<
+        odom.pose.pose.position.x
           << ", y: " << odom.pose.pose.position.y << ", yaw: " << y << ")";
       return ret.str();
     });

localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp

@@ -175,7 +175,8 @@ AMCLLocalizer::AMCLLocalizer()
       double roll, pitch, yaw;
       tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
 
-      ret << "Odometry with pose: (x: " << x << ", y: " << y << ", yaw: " << yaw << ")";
+      ret << "{" << rclcpp::Time(odom.header.stamp).seconds() << "} Odometry with pose: (x: " <<
+        x << ", y: " << y << ", yaw: " << yaw << ")";
       return ret.str();
     });
 }

maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp

@@ -19,17 +19,6 @@ class CostmapMapsManagerTest : public ::testing::Test
   void SetUp() override
   {
     rclcpp::init(0, nullptr);
-    easynav::NavState::register_printer<easynav::PointPerceptions>(
-      [](const easynav::PointPerceptions & perceptions) {
-        std::ostringstream ret;
-        ret << "PointPerception " << perceptions.size() << " with:\n";
-        for (const auto & perception : perceptions) {
-          ret << "\t[" << static_cast<const void *>(perception.get()) << "] --> "
-              << perception->data.size() << " points in frame [" << perception->frame_id
-              << "] with ts " << perception->stamp.seconds() << "\n";
-        }
-        return ret.str();
-      });
   }
 
   void TearDown() override

maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp

@@ -22,17 +22,6 @@ class NavMapMapsManagerTest : public ::testing::Test
   void SetUp() override
   {
     rclcpp::init(0, nullptr);
-    ::easynav::NavState::register_printer<easynav::PointPerceptions>(
-      [](const easynav::PointPerceptions & perceptions) {
-        std::ostringstream ret;
-        ret << "PointPerception " << perceptions.size() << " with:\n";
-        for (const auto & perception : perceptions) {
-          ret << "\t[" << static_cast<const void *>(perception.get()) << "] --> "
-              << perception->data.size() << " points in frame [" << perception->frame_id
-              << "] with ts " << perception->stamp.seconds() << "\n";
-        }
-        return ret.str();
-      });
   }
 
   void TearDown() override

maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp

@@ -40,18 +40,6 @@ class SimpleMapsManagerTest : public ::testing::Test
   void SetUp() override
   {
     rclcpp::init(0, nullptr);
-
-    easynav::NavState::register_printer<easynav::PointPerceptions>(
-      [](const easynav::PointPerceptions & perceptions) {
-        std::ostringstream ret;
-        ret << "PointPerception " << perceptions.size() << " with:\n";
-        for (const auto & perception : perceptions) {
-          ret << "\t[" << static_cast<const void *>(perception.get()) << "] --> "
-              << perception->data.size() << " points in frame [" << perception->frame_id
-              << "] with ts " << perception->stamp.seconds() << "\n";
-        }
-        return ret.str();
-      });
   }
 
   void TearDown() override

planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp

@@ -117,7 +117,8 @@ CostmapPlanner::CostmapPlanner()
   NavState::register_printer<nav_msgs::msg::Path>(
     [](const nav_msgs::msg::Path & path) {
       std::ostringstream ret;
-      ret << "Path with " << path.poses.size() << " poses and length "
+      ret << "{ " << rclcpp::Time(path.header.stamp).seconds() << " } Path with " <<
+        path.poses.size() << " poses and length "
           << compute_path_length(path) << " m.";
       return ret.str();
     });

planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp

@@ -56,7 +56,8 @@ AStarPlanner::AStarPlanner()
   NavState::register_printer<nav_msgs::msg::Path>(
     [](const nav_msgs::msg::Path & path) {
       std::ostringstream ret;
-      ret << "Path with " << path.poses.size() << " poses and length "
+      ret << "{ " << rclcpp::Time(path.header.stamp).seconds() << " } Path with " <<
+        path.poses.size() << " poses and length "
           << compute_path_length(path) << " m.";
       return ret.str();
     });

planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp

@@ -79,7 +79,8 @@ SimplePlanner::SimplePlanner()
     [](const nav_msgs::msg::Path & path) {
       std::ostringstream ret;
 
-      ret << "Path with " << path.poses.size() << " poses and length" <<
+      ret << "{ " << rclcpp::Time(path.header.stamp).seconds() << " } Path with " <<
+        path.poses.size() << " poses and length " <<
         compute_path_length(path) << " m.";
 
       return ret.str();