feat(perception_reproducer): add publish route option (#154)

* feat(perception_reproducer): publish route created from the initial pose and goal pose retrieved from the beginning and end of the rosbag

Signed-off-by: Kyoichi Sugahara <[email protected]>

* add readme

Signed-off-by: Kyoichi Sugahara <[email protected]>

---------

Signed-off-by: Kyoichi Sugahara <[email protected]>

planning/planning_debug_tools/README.md

@@ -195,7 +195,7 @@ This script can overlay the perception results from the rosbag on the planning s
 
 ### How it works
 
-Whenever the ego's position changes, a chronological `reproduce_sequence` queue is generated base on its position with a search radius (default to 2 m).
+Whenever the ego's position changes, a chronological `reproduce_sequence` queue is generated based on its position with a search radius (default to 2 m).
 If the queue is empty, the nearest odom message in the rosbag is added to the queue.
 When publishing perception messages, the first element in the `reproduce_sequence` is popped and published.
 
@@ -204,6 +204,18 @@ This design results in the following behavior:
 - When ego stops, the perception messages are published in chronological order until queue is empty.
 - When the ego moves, a perception message close to ego's position is published.
 
+### Available Options
+
+- `-b`, `--bag`: Rosbag file path (required)
+- `-d`, `--detected-object`: Publish detected objects
+- `-t`, `--tracked-object`: Publish tracked objects
+- `-r`, `--search-radius`: Set the search radius in meters (default: 1.5m). If set to 0, always publishes the nearest message
+- `-c`, `--reproduce-cool-down`: Set the cool down time in seconds (default: 80.0s)
+- `-p`, `--pub-route`: Initialize localization and publish a route based on poses from the rosbag
+- `-n`, `--noise`: Apply perception noise to objects when publishing repeated messages (default: True)
+- `-f`, `--rosbag-format`: Specify rosbag data format (default: "db3")
+- `-v`, `--verbose`: Output debug data
+
 ### How to use
 
 First, launch the planning simulator, and put the ego pose.
@@ -223,8 +235,17 @@ ros2 run planning_debug_tools perception_reproducer.py -b <dir-to-bag-files>
 
 Instead of publishing predicted objects, you can publish detected/tracked objects by designating `-d` or `-t`, respectively.
 
-You can use `-r` option to set the search radius in meters for the perception messages. If it is set to 0, the reproducer always publishes the nearest perception message as how did the old perception_reproducer work.
-`-c`(`--reproduce-cool-down`) option is to set the cool down time in seconds, aiming to prevent republishing recently published messages.
+The `--pub-route` option enables automatic route generation based on the rosbag data. When enabled, the script:
+
+1. Extracts the initial and goal poses from the beginning and end of the rosbag file
+2. Initializes the localization system with the initial pose
+3. Generates and publishes a route to the goal pose
+
+Example usage with route publication:
+
+```bash
+ros2 run planning_debug_tools perception_reproducer.py -b <bag-file> -p
+```
 
 ## Perception replayer
 

planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py

@@ -24,6 +24,7 @@
 import rclpy
 from utils import StopWatch
 from utils import create_empty_pointcloud
+from utils import pub_route
 from utils import translate_objects_coordinate
 
 
@@ -281,8 +282,8 @@ def add_perception_noise(
 
 
 if __name__ == "__main__":
-    parser = argparse.ArgumentParser()
-    parser.add_argument("-b", "--bag", help="rosbag", default=None)
+    parser = argparse.ArgumentParser(description="Perception Reproducer")
+    parser.add_argument("-b", "--bag", help="rosbag file path", required=True)
     parser.add_argument(
         "-n",
         "--noise",
@@ -316,10 +317,23 @@ def add_perception_noise(
         type=float,
         default=80.0,
     )
-
+    parser.add_argument(
+        "-p",
+        "--pub-route",
+        help=(
+            "publish route created from the initial pose and goal pose retrieved from the beginning and end of the rosbag. "
+            "By default, route are not published."
+        ),
+        action="store_true",
+    )
     args = parser.parse_args()
 
-    rclpy.init()
+    if not args.bag:
+        parser.error("The '--bag' argument is required.")
+
+    if args.pub_route:
+        pub_route(args.bag)
+
     node = PerceptionReproducer(args)
 
     try:

planning/planning_debug_tools/scripts/perception_replayer/utils.py

@@ -16,14 +16,30 @@
 
 import math
 import time
+from typing import Tuple
 
+from autoware_adapi_v1_msgs.srv import SetRoutePoints
+from geometry_msgs.msg import Point
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import PoseWithCovariance
+from geometry_msgs.msg import PoseWithCovarianceStamped
 from geometry_msgs.msg import Quaternion
 import numpy as np
+import rclpy
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.node import Node
+from rclpy.serialization import deserialize_message
 import rosbag2_py
+from rosbag2_py import ConverterOptions
+from rosbag2_py import SequentialReader
+from rosbag2_py import StorageOptions
+from rosidl_runtime_py.utilities import get_message
 from sensor_msgs.msg import PointCloud2
 from sensor_msgs.msg import PointField
+from std_msgs.msg import Header
 from tf_transformations import euler_from_quaternion
 from tf_transformations import quaternion_from_euler
+from tier4_localization_msgs.srv import InitializeLocalization
 
 
 def get_starting_time(uri: str):
@@ -131,6 +147,95 @@ def translate_objects_coordinate(ego_pose, log_ego_pose, objects_msg):
         object_pose.orientation = get_quaternion_from_yaw(log_object_yaw + log_ego_yaw - ego_yaw)
 
 
+def create_reader(bag_dir: str) -> SequentialReader:
+    storage_options = StorageOptions(uri=bag_dir, storage_id="sqlite3")
+    converter_options = ConverterOptions(
+        input_serialization_format="cdr", output_serialization_format="cdr"
+    )
+    reader = SequentialReader()
+    reader.open(storage_options, converter_options)
+    return reader
+
+
+def is_close_pose(p0, p1, min_dist, max_dist) -> bool:
+    dist = np.linalg.norm([p0.x - p1.x, p0.y - p1.y, p0.z - p1.z])
+    return dist < min_dist or dist > max_dist
+
+
+def get_pose_from_bag(input_path: str, interval=(0.1, 10000.0)) -> Tuple[Pose, Pose]:
+    reader = create_reader(input_path)
+    type_map = {
+        topic_type.name: topic_type.type for topic_type in reader.get_all_topics_and_types()
+    }
+    pose_list = []
+    is_first_pose = True
+    prev_trans = None
+
+    while reader.has_next():
+        topic, data, _ = reader.read_next()
+        if topic == "/tf":
+            msg_type = get_message(type_map[topic])
+            msg = deserialize_message(data, msg_type)
+            for transform in msg.transforms:
+                if transform.child_frame_id != "base_link":
+                    continue
+                trans = transform.transform.translation
+                rot = transform.transform.rotation
+                if is_first_pose:
+                    is_first_pose = False
+                    prev_trans = trans
+                elif is_close_pose(prev_trans, trans, interval[0], interval[1]):
+                    continue
+                pose_list.append((trans, rot))
+                prev_trans = trans
+
+    if not pose_list:
+        raise ValueError("No valid poses found in the bag file.")
+
+    initial_trans, initial_rot = pose_list[0]
+    goal_trans, goal_rot = pose_list[-1]
+
+    initial_pose = Pose()
+    initial_pose.position = Point(x=initial_trans.x, y=initial_trans.y, z=initial_trans.z)
+    initial_pose.orientation = initial_rot
+
+    goal_pose = Pose()
+    goal_pose.position = Point(x=goal_trans.x, y=goal_trans.y, z=goal_trans.z)
+    goal_pose.orientation = goal_rot
+
+    return initial_pose, goal_pose
+
+
+def pub_route(input_path: str):
+    rclpy.init()
+
+    try:
+        first_pose, last_pose = get_pose_from_bag(input_path)
+    except Exception as e:
+        print(f"Error retrieving poses from bag: {e}")
+        return
+
+    localization_client = LocalizationInitializer()
+    future_init = localization_client.send_initial_pose(first_pose)
+    rclpy.spin_until_future_complete(localization_client, future_init)
+    if future_init.result() is not None:
+        print("Successfully initialized localization.")
+    else:
+        print("Failed to initialize localization.")
+
+    # temporarily add a sleep because sometimes the route is not generated correctly without it.
+    # Need to consider a proper solution.
+    time.sleep(2)
+
+    route_client = RoutePointsClient()
+    future_route = route_client.send_request(last_pose)
+    rclpy.spin_until_future_complete(route_client, future_route)
+    if future_route.result() is not None:
+        print("Successfully set route points.")
+    else:
+        print("Failed to set route points.")
+
+
 class StopWatch:
     def __init__(self, verbose):
         # A dictionary to store the starting times
@@ -155,3 +260,52 @@ def toc(self, name):
 
         # Reset the starting time for the name
         del self.start_times[name]
+
+
+class LocalizationInitializer(Node):
+    def __init__(self):
+        super().__init__("localization_initializer")
+        self.callback_group = ReentrantCallbackGroup()
+        self.client = self.create_client(
+            InitializeLocalization,
+            "/localization/initialize",
+            callback_group=self.callback_group,
+        )
+        while not self.client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("Waiting for '/localization/initialize' service...")
+
+    def create_initial_pose(self, pose: Pose) -> PoseWithCovarianceStamped:
+        pose_with_cov_stamped = PoseWithCovarianceStamped()
+        pose_with_cov_stamped.header.frame_id = "map"
+        pose_with_cov_stamped.header.stamp = self.get_clock().now().to_msg()
+        pose_with_cov = PoseWithCovariance()
+        pose_with_cov.pose = pose
+        covariance = np.identity(6) * 0.01  # Create a 6x6 identity matrix scaled by 0.01
+        pose_with_cov.covariance = covariance.flatten().tolist()
+        pose_with_cov_stamped.pose = pose_with_cov
+        return pose_with_cov_stamped
+
+    def send_initial_pose(self, pose: Pose):
+        request = InitializeLocalization.Request()
+        request.pose_with_covariance = [self.create_initial_pose(pose)]
+        request.method = 1
+        self.get_logger().info("Sending initial pose request...")
+        return self.client.call_async(request)
+
+
+class RoutePointsClient(Node):
+    def __init__(self):
+        super().__init__("route_points_client")
+        self.client = self.create_client(SetRoutePoints, "/api/routing/set_route_points")
+        while not self.client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("Waiting for '/api/routing/set_route_points' service...")
+
+    def send_request(self, goal_pose: Pose):
+        request = SetRoutePoints.Request()
+        request.header = Header()
+        request.header.stamp = self.get_clock().now().to_msg()
+        request.header.frame_id = "map"
+        request.goal = goal_pose
+        request.waypoints = []
+        self.get_logger().info("Sending route points request...")
+        return self.client.call_async(request)