logger - whole planned path is saved by the GlobalPlannerLogger

Author: rayvburn

srpb_logger/include/srpb_logger/global_planner_data.h

@@ -3,6 +3,8 @@
 #include <geometry_msgs/PoseStamped.h>
 #include <tf2/utils.h>
 
+#include <vector>
+
 namespace srpb {
 namespace logger {
 
@@ -13,21 +15,21 @@ class GlobalPlannerData {
         const geometry_msgs::PoseStamped& goal_pose,
         bool planning_success,
         double planning_time,
-        size_t plan_size
-    ): GlobalPlannerData(robot_pose.pose, goal_pose.pose, planning_success, planning_time, plan_size) {}
+        const std::vector<geometry_msgs::PoseStamped>& plan
+    ): GlobalPlannerData(robot_pose.pose, goal_pose.pose, planning_success, planning_time, plan) {}
 
     GlobalPlannerData(
         const geometry_msgs::Pose& robot_pose,
         const geometry_msgs::Pose& goal_pose,
         bool planning_success,
         double planning_time,
-        size_t plan_size
+        const std::vector<geometry_msgs::PoseStamped>& plan
     ):
         pose_(robot_pose),
         goal_(goal_pose),
         planning_success_(planning_success),
         planning_time_(planning_time),
-        plan_size_(plan_size)
+        plan_(plan)
     {}
 
     inline geometry_msgs::Pose getPose() const {
@@ -88,17 +90,27 @@ class GlobalPlannerData {
         return planning_time_;
     }
 
-    /// Number of poses the global path plan consists of
+    /// Returns a copy of a container with the global path plan @ref plan_
+    inline std::vector<geometry_msgs::PoseStamped> getPlan() const {
+        return plan_;
+    }
+
+    /// Returns a const reference to a container with the global path plan @ref plan_
+    inline const std::vector<geometry_msgs::PoseStamped>& getPlanCref() const {
+        return plan_;
+    }
+
+    /// Number of poses the global path plan @ref plan_ is expected to contain
     inline size_t getPlanSize() const {
-        return plan_size_;
+        return plan_.size();
     }
 
 protected:
     bool planning_success_;
     geometry_msgs::Pose pose_;
     geometry_msgs::Pose goal_;
     double planning_time_;
-    size_t plan_size_;
+    std::vector<geometry_msgs::PoseStamped> plan_;
 };
 
 } // namespace logger

srpb_logger/src/global_planner_logger.cpp

@@ -56,20 +56,35 @@ std::string GlobalPlannerLogger::plannerToString(const GlobalPlannerData& planne
     /*  8 */ ss << std::setw(4) << std::setprecision(1) << static_cast<double>(planner.getPlanningStatus()) << " ";
     /*  9 */ ss << std::setw(9) << std::setprecision(4) << planner.getPlanningTime() << " ";
     /* 10 */ ss << std::setw(9) << std::setprecision(1) << planner.getPlanSize();
+
+    // dynamic size of the plan
+    ss << " / ";
+    const auto& plan = planner.getPlanCref();
+    for (const auto& pose_stamped: plan) {
+        ss << " " << std::setw(9) << std::setprecision(4) << pose_stamped.pose.position.x;
+        ss << " " << std::setw(9) << std::setprecision(4) << pose_stamped.pose.position.y;
+        ss << " " << std::setw(9) << std::setprecision(4) << tf2::getYaw(pose_stamped.pose.orientation);
+    }
     return ss.str();
 }
 
 std::pair<bool, GlobalPlannerData> GlobalPlannerLogger::plannerFromString(const std::string& str) {
-    auto vals = people_msgs_utils::parseString<double>(str, " ");
-    if (vals.size() != 11) {
+    // divide into 2 parts considering separator
+    size_t separator = str.find("/");
+    auto vals = people_msgs_utils::parseString<double>(str.substr(0, separator - 1), " ");
+    auto vals_plan_poses = people_msgs_utils::parseString<double>(str.substr(separator + 1), " ");
+
+    // static part of the entry has always the same length; a dynamic part must have 3 elements for each pose
+    bool plan_poses_in_triplets = vals_plan_poses.size() % 3 != 0;
+    if (vals.size() != 11 || plan_poses_in_triplets) {
         std::cout << "\x1B[31mFound corrupted data of a global planner:\r\n\t" << str << "\x1B[0m" << std::endl;
         // dummy planner data
         auto dummy_planner = GlobalPlannerData(
             geometry_msgs::Pose(),
             geometry_msgs::Pose(),
             false,
             0.0,
-            0
+            std::vector<geometry_msgs::PoseStamped>()
         );
         return {false, dummy_planner};
     }
@@ -97,9 +112,31 @@ std::pair<bool, GlobalPlannerData> GlobalPlannerLogger::plannerFromString(const
 
     bool planning_status = static_cast<bool>(static_cast<int>(vals.at(8)));
     double planning_time = vals.at(9);
+
     double plan_size = vals.at(10);
+    std::vector<geometry_msgs::PoseStamped> plan;
+    // each pose is represented by a triplet
+    for (
+        auto it = vals_plan_poses.cbegin();
+        it != vals_plan_poses.cend();
+        it = it + 3
+    ) {
+        double x = *it;
+        double y = *(it+1);
+        double yaw = *(it+2);
+        geometry_msgs::PoseStamped pose;
+        pose.pose.position.x = x;
+        pose.pose.position.y = y;
+
+        // NOTE: ctor of tf2::Quaternion using Euler angles is deprecated
+        tf2::Quaternion quat;
+        quat.setEuler(0.0, 0.0, yaw);
+        pose.pose.orientation = tf2::toMsg(quat);
+
+        plan.push_back(pose);
+    }
 
-    auto planner = GlobalPlannerData(pose, goal, planning_status, planning_time, plan_size);
+    auto planner = GlobalPlannerData(pose, goal, planning_status, planning_time, plan);
     return {true, planner};
 }
 

srpb_logger/test/test_global_planner_logger.cpp

@@ -1,6 +1,7 @@
 #include <gtest/gtest.h>
 
 #include <srpb_logger/global_planner_logger.h>
+#include <angles/angles.h>
 
 using namespace srpb::logger;
 
@@ -33,8 +34,27 @@ TEST(ConversionTest, planner) {
     bool planning_success = true;
     double planning_time = 0.135;
     size_t plan_size = 497;
+    // create a arbitrary path plan
+    std::vector<geometry_msgs::PoseStamped> plan;
+    plan.resize(plan_size);
+    double x_plan = -5.159;
+    double y_plan = 20.951;
+    double yaw_plan = 0.357;
+    for (auto& pose_stamped: plan) {
+        // modify vector element
+        pose_stamped.pose.position.x = x_plan;
+        pose_stamped.pose.position.y = y_plan;
+        // NOTE: ctor of tf2::Quaternion using Euler angles is deprecated
+        tf2::Quaternion quat;
+        quat.setEuler(0.0, 0.0, yaw_plan);
+        pose_stamped.pose.orientation = tf2::toMsg(quat);
+        // prep coords for the next iteration
+        x_plan += 0.142;
+        y_plan -= 0.249;
+        yaw_plan = angles::normalize_angle(yaw_plan + 0.029);
+    }
 
-    GlobalPlannerData data_in(pose, goal, planning_success, planning_time, plan_size);
+    GlobalPlannerData data_in(pose, goal, planning_success, planning_time, plan);
 
     // 1. evaluate if attributes were saved correctly
     ASSERT_EQ(data_in.getPositionX(), pose.position.x);
@@ -51,6 +71,7 @@ TEST(ConversionTest, planner) {
     ASSERT_EQ(data_in.getPlanningStatus(), planning_success);
     ASSERT_EQ(data_in.getPlanningTime(), planning_time);
     ASSERT_EQ(data_in.getPlanSize(), plan_size);
+    ASSERT_EQ(data_in.getPlan(), plan);
 
     auto data_in_str = GlobalPlannerLogger::plannerToString(data_in);
     auto conversion = GlobalPlannerLogger::plannerFromString(data_in_str);
@@ -74,6 +95,24 @@ TEST(ConversionTest, planner) {
     ASSERT_EQ(data_out.getPlanningStatus(), data_in.getPlanningStatus());
     ASSERT_EQ(data_out.getPlanningTime(), data_in.getPlanningTime());
     ASSERT_EQ(data_out.getPlanSize(), data_in.getPlanSize());
+    /* NOTE: cannot simply use:
+     *   ASSERT_EQ(data_out.getPlan(), data_in.getPlan());
+     * here, as it uses a serialized version (?) of the message structure; despite the same contents (with 1e-04
+     * accuracy), there may be some whitespace differences
+     */
+    auto out_plan = data_out.getPlanCref();
+    auto in_plan = data_in.getPlanCref();
+    for (
+        auto out_it = out_plan.cbegin(), in_it = in_plan.cbegin();
+        out_it != out_plan.cend() || in_it != in_plan.cend();
+        out_it++, in_it++
+    ) {
+        auto out_pose = *out_it;
+        auto in_pose = *in_it;
+        ASSERT_NEAR(out_pose.pose.position.x, in_pose.pose.position.x, 1e-04);
+        ASSERT_NEAR(out_pose.pose.position.y, in_pose.pose.position.y, 1e-04);
+        ASSERT_NEAR(tf2::getYaw(out_pose.pose.orientation), tf2::getYaw(in_pose.pose.orientation), 1e-04);
+    }
 }
 
 int main(int argc, char** argv) {