Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
@pre-commit-ci
pre-commit-ci[bot] committed Feb 24, 2025
1 parent 42f1ec4 commit c0e1e30
Show file tree
Hide file tree
Showing 6 changed files with 341 additions and 259 deletions.
43 changes: 24 additions & 19 deletions map/autoware_tp_manager/README.md
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
# autoware_tp_manager

Here are some tools for collecting average TPs of PCD maps. Currently, we consider the decrease of TPs as a sign of map decay. However, we don't know what TPs are 'abnormal', e.g. in some areas the TPs range around 2.0 ~ 3.0, while in others TPs float around 5.0. This package provides some tools to check it, including:

- TP collector: collect the average TPs per segment of a PCD map
- TP checker: compare a rosbag's TPs with a map's TPs and highlight the map areas where the rosbag's TPs differ significantly from the map's TPs.

Expand All @@ -22,45 +23,49 @@ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-skip-building-test
ros2 run autoware_tp_manager tp_collector.py <path_to_pcd_dir> <path_to_rosbag> <path_to_output_dir> [--resolution:=<resolution>] [--pose_topic:=<topic_of_poses>] [--tp_topic:=<topic_of_TPs>] [--scan_topic:=<topic_of_scans>]
```

| Name | Description |
| ----------------- | ------------------------------------------- |
| path_to_pcd_dir | Directory that contains the input PCD files |
| path_to_rosbag | Path to the input rosbag |
| path_to_output_dir| Path to the output directory |
| resolution | Resolution to segment the input PCD. The TPs are collected on these segments|
| topic_of_poses | Topic of poses messages in the input rosbag|
| topic_of_TPs | Topic of TPs in the input rosbag|
| topic_of_scans | Topic of downsampled scans in the input rosbag|
| Name | Description |
| ------------------ | ---------------------------------------------------------------------------- |
| path_to_pcd_dir | Directory that contains the input PCD files |
| path_to_rosbag | Path to the input rosbag |
| path_to_output_dir | Path to the output directory |
| resolution | Resolution to segment the input PCD. The TPs are collected on these segments |
| topic_of_poses | Topic of poses messages in the input rosbag |
| topic_of_TPs | Topic of TPs in the input rosbag |
| topic_of_scans | Topic of downsampled scans in the input rosbag |

Paths to folders should be specified as **absolute paths**.

The rosbag should contain the following topics

- /localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias
- /localization/pose_estimator/transform_probability
- /localization/util/downsample/pointcloud

The average TPs can be visualized on Rviz2 by running the following command

```bash
python3 install/autoware_tp_manager/lib/autoware_tp_manager/tp_visualizer.py <path_to_output_dir>
```
| Name | Description |
| ----------------- | ------------------------------------------- |
| path_to_output_dir| Path to the output directory of TP_collector|

| Name | Description |
| ------------------ | -------------------------------------------- |
| path_to_output_dir | Path to the output directory of TP_collector |

then open another terminal to launch Rviz2 and add the topic /autoware_tp_visualizer.

- Compare a rosbags' TPs with a map's TPs by TP_checker

```bash
ros2 run autoware_tp_manager tp_checker.py <path_to_score_dir> <path_to_rosbag> [--pose_topic:=<topic_of_poses>] [--tp_topic:=<topic_of_TPs>] [--scan_topic:=<topic_of_scans>]
```

| Name | Description |
| ----------------- | ------------------------------------------- |
| path_to_score_dir | Directory that contains the TP file (.csv) and the downsampled PCD map. This is the output directory of the tp_collector. |
| path_to_rosbag | Path to the input rosbag to be evaluated|
| topic_of_poses | Topic of poses in the evaluation rosbag |
| topic_of_TPs | Topic of TPs in the evaluation rosbag |
| topic_of_scans | Topic of scans in the evaluation rosbag |
| Name | Description |
| ----------------- | ------------------------------------------------------------------------------------------------------------------------- |
| path_to_score_dir | Directory that contains the TP file (.csv) and the downsampled PCD map. This is the output directory of the tp_collector. |
| path_to_rosbag | Path to the input rosbag to be evaluated |
| topic_of_poses | Topic of poses in the evaluation rosbag |
| topic_of_TPs | Topic of TPs in the evaluation rosbag |
| topic_of_scans | Topic of scans in the evaluation rosbag |

The results of checking are published to the topic /autoware_tp_checker, and can also be displayed on Rviz2. The red points

Expand Down
2 changes: 1 addition & 1 deletion map/autoware_tp_manager/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,8 @@
<buildtool_depend>tier4_debug_msgs</buildtool_depend>

<depend>libpcl-all-dev</depend>
<depend>yaml-cpp</depend>
<depend>tier4_debug_msgs</depend>
<depend>yaml-cpp</depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
155 changes: 89 additions & 66 deletions map/autoware_tp_manager/scripts/tp_checker.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,35 +14,34 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import os
import argparse
from rosbag2_py import (
SequentialReader,
StorageOptions,
ConverterOptions,
Info
)

from rclpy.serialization import deserialize_message, serialize_message
import os
import csv
import yaml
from scipy import spatial as sp
import numpy as np
import os
import shutil
import struct
import time

from builtin_interfaces.msg import Time
from geometry_msgs.msg import PoseWithCovarianceStamped
import sensor_msgs.msg as sensor_msgs
import std_msgs.msg as std_msgs
import numpy as np
import open3d as o3d
import rclpy
from rclpy.node import Node
from tier4_debug_msgs.msg import Float32Stamped
from builtin_interfaces.msg import Time
import tqdm
import time
import shutil
import open3d as o3d
from rclpy.serialization import deserialize_message
from rclpy.serialization import serialize_message
from rosbag2_py import ConverterOptions
from rosbag2_py import Info
from rosbag2_py import SequentialReader
from rosbag2_py import StorageOptions
from scipy import spatial as sp
import sensor_msgs.msg as sensor_msgs
import sensor_msgs_py.point_cloud2 as pc2
import struct
import std_msgs.msg as std_msgs
from tier4_debug_msgs.msg import Float32Stamped
import tp_utility as tpu
import tqdm
import yaml


def mark_changes(candidate_segments, rosbag_tp: float, segment_dict, segment_df):
tp_sum = 0.0
Expand All @@ -57,21 +56,23 @@ def mark_changes(candidate_segments, rosbag_tp: float, segment_dict, segment_df)
else:
expected_tp = 0

if (expected_tp > 0 and abs(expected_tp - rosbag_tp) / expected_tp >= 0.2) or \
(expected_tp == 0 and rosbag_tp > 0):
if (expected_tp > 0 and abs(expected_tp - rosbag_tp) / expected_tp >= 0.2) or (
expected_tp == 0 and rosbag_tp > 0
):
for key in candidate_segments:
if key in segment_dict:
segment_df[segment_dict[key], 2] += 1


class TPChecker(Node):

def __init__(self):
super().__init__('tp_checker')
super().__init__("tp_checker")
self.segment_df = None # Segment indices, TP
self.segment_dict = {} # key: segment name, value: index to the segment_df
self.pcd_path = None # Path to the directory containing PCD segments
self.changed_dir = None # A directory contains the segments that need to be examined
self.result_csv = None # Path to the result CSV file
self.pcd_path = None # Path to the directory containing PCD segments
self.changed_dir = None # A directory contains the segments that need to be examined
self.result_csv = None # Path to the result CSV file
self.tp_path = None # Path to the file that contains TPs of map segments

def __initialize(self, score_dir: str):
Expand All @@ -94,7 +95,11 @@ def __initialize(self, score_dir: str):
self.tp_path = os.path.join(score_dir, "scores.csv")

if not os.path.exists(self.tp_path):
print("Error: A TP file, which contains the TPs of map segments, is not found at {0}! Abort!".format(self.tp_path))
print(
"Error: A TP file, which contains the TPs of map segments, is not found at {0}! Abort!".format(
self.tp_path
)
)
exit()

# Read the input map directory and setup the segment dictionary
Expand All @@ -112,8 +117,8 @@ def __get_pcd_segments_and_scores(self):
elif key == "x_resolution":
self.resolution = value

self.segment_df = np.array(self.segment_df, dtype = object)
self.segment_df = np.array(self.segment_df, dtype=object)

# Load the TPs
with open(self.tp_path, "r") as f:
reader = csv.reader(f)
Expand All @@ -130,15 +135,21 @@ def __show(self):
dtype = np.float32
itemsize = np.dtype(dtype).itemsize

fields = [sensor_msgs.PointField(name = "x", offset = 0, datatype = ros_float_dtype, count = 1),
sensor_msgs.PointField(name = "y", offset = itemsize, datatype = ros_float_dtype, count = 1),
sensor_msgs.PointField(name = "z", offset = itemsize * 2, datatype = ros_float_dtype, count = 1),
sensor_msgs.PointField(name = "rgba", offset = itemsize * 3, datatype = ros_uint32_dtype, count = 1)]
fields = [
sensor_msgs.PointField(name="x", offset=0, datatype=ros_float_dtype, count=1),
sensor_msgs.PointField(name="y", offset=itemsize, datatype=ros_float_dtype, count=1),
sensor_msgs.PointField(
name="z", offset=itemsize * 2, datatype=ros_float_dtype, count=1
),
sensor_msgs.PointField(
name="rgba", offset=itemsize * 3, datatype=ros_uint32_dtype, count=1
),
]

points = []
pc2_width = 0

progress_bar = tqdm.tqdm(total = len(self.segment_df))
progress_bar = tqdm.tqdm(total=len(self.segment_df))
origin = None

for i in range(self.segment_df.shape[0]):
Expand All @@ -161,62 +172,76 @@ def __show(self):
header.frame_id = "map"

pc2_msg = pc2.create_cloud(header, fields, points)
pcd_publisher = self.create_publisher(sensor_msgs.PointCloud2, '/autoware_tp_checker', 10)
pcd_publisher = self.create_publisher(sensor_msgs.PointCloud2, "/autoware_tp_checker", 10)

while True:
pcd_publisher.publish(pc2_msg)
time.sleep(5)


def __set_color_based_on_mark(self, mark) -> int:
# The may-have-changed segments are colored red
# The may-not-have-changed segments are colored white
if mark >= 100:
r = 255
g = 0
b = 0
b = 0
else:
r = 255
g = 255
b = 255
a = 255

tmp_rgb = struct.pack('BBBB', b, g, r, a)
rgba = struct.unpack('I', tmp_rgb)[0]
tmp_rgb = struct.pack("BBBB", b, g, r, a)
rgba = struct.unpack("I", tmp_rgb)[0]

return rgba

def processing(self,
score_path: str,
rosbag_path: str,
pose_topic: str,
tp_topic: str,
scan_topic: str):
def processing(
self, score_path: str, rosbag_path: str, pose_topic: str, tp_topic: str, scan_topic: str
):
self.__initialize(score_path)
self.__get_pcd_segments_and_scores()

tpu.collect_rosbag_tp(rosbag_path, pose_topic, tp_topic, scan_topic, self.resolution,
mark_changes, self.segment_dict, self.segment_df)
tpu.collect_rosbag_tp(
rosbag_path,
pose_topic,
tp_topic,
scan_topic,
self.resolution,
mark_changes,
self.segment_dict,
self.segment_df,
)

self.__show()


if __name__ == "__main__":
rclpy.init()
parser = argparse.ArgumentParser()
parser.add_argument("score_path", help="The path to the folder containing the TP file")
parser.add_argument("bag_path", help="The path to the input rosbag")
parser.add_argument("--pose_topic",
help = "Pose topic",
default = "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias",
required = False, type = str)
parser.add_argument("--tp_topic",
help = "TP topic",
default = "/localization/pose_estimator/transform_probability",
required = False, type = str)
parser.add_argument("--scan_topic",
help = "Point cloud topic",
default = "/localization/util/downsample/pointcloud",
required = False, type = str)
parser.add_argument(
"--pose_topic",
help="Pose topic",
default="/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias",
required=False,
type=str,
)
parser.add_argument(
"--tp_topic",
help="TP topic",
default="/localization/pose_estimator/transform_probability",
required=False,
type=str,
)
parser.add_argument(
"--scan_topic",
help="Point cloud topic",
default="/localization/util/downsample/pointcloud",
required=False,
type=str,
)

args = parser.parse_args()

Expand All @@ -230,8 +255,6 @@ def processing(self,
# Run
checker = TPChecker()

checker.processing(args.score_path,
args.bag_path,
args.pose_topic,
args.tp_topic,
args.scan_topic)
checker.processing(
args.score_path, args.bag_path, args.pose_topic, args.tp_topic, args.scan_topic
)
Loading

0 comments on commit c0e1e30

Please sign in to comment.