diff --git a/driving_log_replayer/launch/ground_segmentation.launch.py b/driving_log_replayer/launch/ground_segmentation.launch.py index f30a94c95..5c56e0fae 100644 --- a/driving_log_replayer/launch/ground_segmentation.launch.py +++ b/driving_log_replayer/launch/ground_segmentation.launch.py @@ -13,6 +13,7 @@ # limitations under the License. import launch +from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration import driving_log_replayer.launch_common as cmn @@ -25,6 +26,12 @@ def generate_launch_description() -> launch.LaunchDescription: launch_arguments = cmn.get_launch_arguments() + launch_arguments.append( + DeclareLaunchArgument( + "evaluation_target_topic", + default_value="/perception/obstacle_segmentation/pointcloud", + ), + ) autoware_launch = cmn.get_autoware_launch( sensing="false", localization="false", @@ -36,7 +43,10 @@ def generate_launch_description() -> launch.LaunchDescription: evaluator_node = cmn.get_evaluator_node( "ground_segmentation", - addition_parameter={"vehicle_model": LaunchConfiguration("vehicle_model")}, + addition_parameter={ + "vehicle_model": LaunchConfiguration("vehicle_model"), + "evaluation_target_topic": LaunchConfiguration("evaluation_target_topic"), + }, ) player = cmn.get_player() diff --git a/driving_log_replayer/scripts/ground_segmentation_evaluator_node.py b/driving_log_replayer/scripts/ground_segmentation_evaluator_node.py index c9a276a18..f92ea2ed5 100755 --- a/driving_log_replayer/scripts/ground_segmentation_evaluator_node.py +++ b/driving_log_replayer/scripts/ground_segmentation_evaluator_node.py @@ -43,6 +43,13 @@ def __init__(self, name: str) -> None: eval_condition: Condition = self._scenario.Evaluation.Conditions self.ground_label = eval_condition.ground_label self.obstacle_label = eval_condition.obstacle_label + self.declare_parameter( + "evaluation_target_topic", + "/perception/obstacle_segmentation/pointcloud", + ) + self.eval_target_topic = ( + self.get_parameter("evaluation_target_topic").get_parameter_value().string_value + ) if eval_condition.Method == "annotated_pcd": # pcd eval mode @@ -62,7 +69,7 @@ def __init__(self, name: str) -> None: self.__sub_pointcloud = self.create_subscription( PointCloud2, - "/perception/obstacle_segmentation/single_frame/pointcloud", + self.eval_target_topic, self.annotated_pcd_eval_cb, qos_profile_sensor_data, ) @@ -78,7 +85,7 @@ def __init__(self, name: str) -> None: self.__sub_eval_target_cloud = message_filters.Subscriber( self, PointCloud2, - "/perception/obstacle_segmentation/single_frame/pointcloud", + self.eval_target_topic, qos_profile=qos_profile_sensor_data, ) self.__sync_sub = message_filters.TimeSynchronizer(