Added initialpose

Signed-off-by: Shintaro Sakoda <[email protected]>

localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py

@@ -9,9 +9,10 @@
 import pathlib
 
 import pandas as pd
-from rclpy.serialization import deserialize_message
+from rclpy.serialization import deserialize_message, serialize_message
 import rosbag2_py
 from rosidl_runtime_py.utilities import get_message
+from geometry_msgs.msg import PoseWithCovarianceStamped
 
 REQUIRED_FPS_PATTERN_A = {
     "/localization/kinematic_state": 40,
@@ -41,6 +42,9 @@ def parse_args():
             "/awsim/ground_truth/localization/kinematic_state",
         ],
     )
+    parser.add_argument(
+        "--add_initialpose",
+        action="store_true")
     return parser.parse_args()
 
 
@@ -116,6 +120,26 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict, required_fps: di
         print("The rosbag is not suitable for ndt evaluation")
         exit()
 
+    # get first msg from /localization/kinematic_state
+    first_reference = topic_name_to_msg_list[reference_topic_name][0]
+
+    # if there is not gnss topic or selected from args, add initialpose
+    save_initialpose = (
+        len(topic_name_to_msg_list["/sensing/gnss/pose_with_covariance"]) == 0 or args.add_initialpose)
+    if save_initialpose:
+        print("Add /initialpose")
+        type_map["/initialpose"] = "geometry_msgs/msg/PoseWithCovarianceStamped"
+        save_topics.append("/initialpose")
+        stamp = first_reference.header.stamp
+        msg = PoseWithCovarianceStamped()
+        msg.header.stamp = stamp
+        msg.pose = first_reference.pose
+        data = serialize_message(msg)
+        timestamp = int(stamp.sec * 1e9 + stamp.nanosec)
+        tuple_list.append(
+            ("/initialpose", data, timestamp)
+        )
+
     # write rosbag
     rosbag_dir = rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent
     filtered_rosbag_path = rosbag_dir / "input_bag"