From 111adf2001b6cbfa121d4ad7214f12b534f326ee Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Thu, 8 Aug 2024 19:02:07 +0900 Subject: [PATCH 1/8] fix mf_localization_mapping demo mapping launch file - fix bugs in demo_2d_VLP16.launch.py - fix a bug in fix_filter node - fix a json serialization bug in tf2_beacons_listner node - add QoS overrides parameters to the launch file to prevent missing imu message subscription - add long sigterm/sigkill timeout to the mapping launch file to prevent the node from stopping before writing pbstream file - change grid resolution parameter to publish a coarse grained grid map and make the parameter configurable Signed-off-by: Masayuki Murata --- mf_localization/script/fix_filter.py | 22 +++++++++---------- .../script/tf2_beacons_listener.py | 6 +++-- .../launch/cartographer_2d_VLP16.launch.py | 21 ++++++++++++++++-- .../launch/demo_2d_VLP16.launch.py | 14 +++++++----- 4 files changed, 42 insertions(+), 21 deletions(-) diff --git a/mf_localization/script/fix_filter.py b/mf_localization/script/fix_filter.py index 264d868a..daf82453 100755 --- a/mf_localization/script/fix_filter.py +++ b/mf_localization/script/fix_filter.py @@ -29,11 +29,12 @@ class FixFilterNode: def __init__(self, node, status_threshold, stdev_threshold): - self.node = node - self.status_threshold = status_threshold - self.stdev_threshold = stdev_threshold - self.fix_sub = self.node.create_subscription(NavSatFix, "fix", self.fix_callback, 10) - self.fix_pub = self.node.create_publisher(NavSatFix, "fix_filtered", 10) + self._node = node + self._clock = node.get_clock() + self._status_threshold = status_threshold + self._stdev_threshold = stdev_threshold + self._fix_sub = self._node.create_subscription(NavSatFix, "fix", self.fix_callback, 10) + self._fix_pub = self._node.create_publisher(NavSatFix, "fix_filtered", 10) def fix_callback(self, msg: NavSatFix): navsat_status = msg.status @@ -41,10 +42,10 @@ def fix_callback(self, msg: NavSatFix): position_covariance = np.reshape(msg.position_covariance, (3, 3)) stdev = np.sqrt(position_covariance[0, 0]) - if self.status_threshold <= status and stdev <= self.stdev_threshold: - msg.header.stamp = self.clock.now() + if self._status_threshold <= status and stdev <= self._stdev_threshold: + msg.header.stamp = self._clock.now().to_msg() msg.altitude = 0.0 - self.fix_pub.publish(msg) + self._fix_pub.publish(msg) def main(): @@ -55,10 +56,7 @@ def main(): FixFilterNode(node, status_threshold, stdev_threshold) - try: - rclpy.spin(node) - except: # noqa: E722 - pass + rclpy.spin(node) if __name__ == "__main__": diff --git a/mf_localization/script/tf2_beacons_listener.py b/mf_localization/script/tf2_beacons_listener.py index fc9c8ee8..42b60894 100755 --- a/mf_localization/script/tf2_beacons_listener.py +++ b/mf_localization/script/tf2_beacons_listener.py @@ -107,6 +107,8 @@ def set_current_position(self, position): if d2d < self._position_interval: return + # convert timestamp to float because timestamp object is not JSON serializable + timestamp_sec = timestamp.nanoseconds * 1.0e-9 fp_data = { "information": { "x": t.transform.translation.x, @@ -121,7 +123,7 @@ def set_current_position(self, position): } }, "data": { - "timestamp": timestamp, + "timestamp": timestamp_sec, "beacons": [] # empty list } } @@ -129,7 +131,7 @@ def set_current_position(self, position): self._previous_fingerprint_time = timestamp self._previous_fingerprint_position = self._current_position if self._verbose: - self.logger.info(F"dummy fingerprint data created at t={timestamp}, x={t.transform.translation.x}, y={t.transform.translation.y}") + self.logger.info(F"dummy fingerprint data created at t={timestamp_sec}, x={t.transform.translation.x}, y={t.transform.translation.y}") def main(): diff --git a/mf_localization_mapping/launch/cartographer_2d_VLP16.launch.py b/mf_localization_mapping/launch/cartographer_2d_VLP16.launch.py index afa8ce31..4584d1fe 100644 --- a/mf_localization_mapping/launch/cartographer_2d_VLP16.launch.py +++ b/mf_localization_mapping/launch/cartographer_2d_VLP16.launch.py @@ -55,11 +55,15 @@ def generate_launch_description(): save_state_filename = LaunchConfiguration('save_state_filename') load_state_filename = LaunchConfiguration('load_state_filename') start_trajectory_with_default_topics = LaunchConfiguration('start_trajectory_with_default_topics') + grid_resolution = LaunchConfiguration('grid_resolution', default=0.05) # config for post processing convert_pbstream = LaunchConfiguration('convert_pbstream') save_state_directory = LaunchConfiguration('save_state_directory') save_state_filestem = LaunchConfiguration('save_state_filestem') + cartographer_sigterm_timeout = LaunchConfiguration('cartographer_sigterm_timeout', default=300.0) + cartographer_sigkill_timeout = LaunchConfiguration('cartographer_sigkill_timeout', default=300.0) + # function to configure a node command argument def add_cartographer_arguments(context, node): cmd = node.cmd.copy() @@ -143,11 +147,16 @@ def add_cartographer_arguments(context, node): DeclareLaunchArgument('save_state_filename', default_value=''), DeclareLaunchArgument('load_state_filename', default_value=''), DeclareLaunchArgument('start_trajectory_with_default_topics', default_value='true'), + DeclareLaunchArgument('grid_resolution', default_value='0.05'), # config for post processing DeclareLaunchArgument('convert_pbstream', default_value='false'), DeclareLaunchArgument('save_state_directory', default_value=''), DeclareLaunchArgument('save_state_filestem', default_value=''), + # sigterm/sigkill timeout + DeclareLaunchArgument('cartographer_sigterm_timeout', default_value='300.0'), + DeclareLaunchArgument('cartographer_sigkill_timeout', default_value='300.0'), + # use cabot_model argument if specified Node( package='robot_state_publisher', @@ -206,7 +215,15 @@ def add_cartographer_arguments(context, node): ("points2", points2), ("imu", imu), ("fix", fix), - ] + ], + parameters=[{ + 'qos_overrides': { + PythonExpression(['"/', points2, '"']): {"subscription": {"depth": 10}}, + PythonExpression(['"/', imu, '"']): {"subscription": {"depth": 100}} + } + }], + sigterm_timeout=cartographer_sigterm_timeout, + sigkill_timeout=cartographer_sigkill_timeout, )] ), @@ -224,6 +241,6 @@ def add_cartographer_arguments(context, node): name="cartographer_occupancy_grid_node", package="cartographer_ros", executable="cartographer_occupancy_grid_node", - arguments=["-resolution", "0.015"] + arguments=["-resolution", grid_resolution] ), ]) diff --git a/mf_localization_mapping/launch/demo_2d_VLP16.launch.py b/mf_localization_mapping/launch/demo_2d_VLP16.launch.py index e21429ad..7e48b7f7 100644 --- a/mf_localization_mapping/launch/demo_2d_VLP16.launch.py +++ b/mf_localization_mapping/launch/demo_2d_VLP16.launch.py @@ -89,14 +89,18 @@ def configure_ros2_bag_play(context, node): cmd.append(['/', scan]) cmd.append(['/', imu]) cmd.append('/beacons') + cmd.append('/esp32/wifi') cmd.append('/wireless/beacons') cmd.append('/wireless/wifi') + cmd.append('/ublox/fix') + cmd.append('/ublox/fix_velocity') if convert_imu.perform(context) == 'true' or convert_points.perform(context) == 'true': cmd.append('--remap') - if convert_imu.perform(context) == 'true': - cmd.append([imu, ':=', imu_temp]) - if convert_points.perform(context) == 'true': - cmd.append([points2, ':=', points2_temp]) + if convert_imu.perform(context) == 'true': + cmd.append([imu, ':=', imu_temp]) + if convert_points.perform(context) == 'true': + cmd.append([points2, ':=', points2_temp]) + cmd.append('--') cmd.append([bag_filename]) node.cmd.clear() # needs to be normalized @@ -113,7 +117,7 @@ def configure_ros2_bag_play(context, node): DeclareLaunchArgument('convert_imu', default_value='false'), DeclareLaunchArgument('convert_esp32', default_value='false'), DeclareLaunchArgument('robot', default_value='rover'), - DeclareLaunchArgument('cabot_model', default_value='cabot_model'), + DeclareLaunchArgument('cabot_model', default_value=''), DeclareLaunchArgument('wireless_topics', default_value="['/wireless/beacons','/wireless/wifi','/beacons','/esp32/wifi']"), DeclareLaunchArgument('rate', default_value='1.0'), DeclareLaunchArgument('start', default_value='0'), From aa62bd176e819b089ccdaf00c23625d3d57bbb96 Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Sat, 31 Aug 2024 01:28:58 +0900 Subject: [PATCH 2/8] update mf_localization_mapping to write tracked_pose topic and optimized trajectory to csv files Signed-off-by: Masayuki Murata --- mf_localization/CMakeLists.txt | 2 + .../script/tracked_pose_listener.py | 84 ++++++++++ mf_localization/script/trajectory_recorder.py | 151 ++++++++++++++++++ .../cartographer/cartographer_2d_mapping.lua | 1 + .../launch/demo_2d_VLP16.launch.py | 17 +- 5 files changed, 252 insertions(+), 3 deletions(-) create mode 100755 mf_localization/script/tracked_pose_listener.py create mode 100755 mf_localization/script/trajectory_recorder.py diff --git a/mf_localization/CMakeLists.txt b/mf_localization/CMakeLists.txt index 7b2ece7b..49c961db 100644 --- a/mf_localization/CMakeLists.txt +++ b/mf_localization/CMakeLists.txt @@ -56,6 +56,8 @@ install(PROGRAMS script/multi_floor_wireless_rss_localizer.py script/tf2_beacons_listener.py script/tf2_listener.py + script/tracked_pose_listener.py + script/trajectory_recorder.py script/trajectory_restarter.py script/str2str_node.py script/ublox_converter.py diff --git a/mf_localization/script/tracked_pose_listener.py b/mf_localization/script/tracked_pose_listener.py new file mode 100755 index 00000000..d3bd5767 --- /dev/null +++ b/mf_localization/script/tracked_pose_listener.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright (c) 2024 IBM Corporation +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +import csv + +import rclpy +import rclpy.time +import rclpy.duration +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped +import traceback + + +class TrackedPoseListener: + def __init__(self, node: Node, throttle_duration_sec: float = 10.0): + self.node = node + self.data_list = [] + self.throttle_duration_sec = throttle_duration_sec + self.tracked_pose_subscriber = self.node.create_subscription(PoseStamped, "/tracked_pose", self.tracked_pose_callback, 10) + + def tracked_pose_callback(self, msg: PoseStamped): + timestamp = float(msg.header.stamp.sec) + float(msg.header.stamp.nanosec)*1.0e-9 + frame_id = msg.header.frame_id + x = msg.pose.position.x + y = msg.pose.position.y + z = msg.pose.position.z + qx = msg.pose.orientation.x + qy = msg.pose.orientation.y + qz = msg.pose.orientation.z + qw = msg.pose.orientation.w + data = [timestamp, frame_id, x, y, z, qx, qy, qz, qw] + self.data_list.append(data) + + freq = self.data_list + if len(self.data_list) > 1: + t0 = self.data_list[0][0] + t1 = self.data_list[-1][0] + freq = len(self.data_list)/(t1 - t0) + self.node.get_logger().info(F"tracked_pose messages received at {freq:.2f} Hz (total count= {len(self.data_list)}). current tracked_pose = {data}", throttle_duration_sec=self.throttle_duration_sec) + + +def main(): + rclpy.init() + node = rclpy.create_node("tracked_pose_listener") + output = node.declare_parameter("output", "").value + tracked_pose_listner = TrackedPoseListener(node) + + def shutdown_hook(): + if output is not None and 0 < len(tracked_pose_listner.data_list): + print("wirinting to output") + with open(output, "w") as f: + writer = csv.writer(f, lineterminator="\n") + writer.writerow(["timestamp", "frame_id", "x", "y", "z", "qx", "qy", "qz", "qw"]) + writer.writerows(tracked_pose_listner.data_list) + + try: + rclpy.spin(node) + except: # noqa: E722 + traceback.print_exc() + shutdown_hook() + + +if __name__ == "__main__": + main() diff --git a/mf_localization/script/trajectory_recorder.py b/mf_localization/script/trajectory_recorder.py new file mode 100755 index 00000000..f289a583 --- /dev/null +++ b/mf_localization/script/trajectory_recorder.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright (c) 2024 IBM Corporation +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +import csv + +import rclpy +import rclpy.duration +import rclpy.time +from rclpy.node import Node +import traceback +from std_srvs.srv import Trigger +from cartographer_ros_msgs.srv import TrajectoryQuery + + +class TrajectoryRecorder(Node): + def __init__(self, node, output, timer_period=10.0): + self._node = node + self._output = output + self._timer_period = timer_period + + self._logger = self._node.get_logger() + + self._service = self._node.create_service(Trigger, '~/call_trajectory_query', self.call_trajectory_query_callback) + self._save_trajectory = self._node.create_service(Trigger, '~/save_trajectory', self.save_trajectory_callback) + + # cartographer_node /trajectory_query service + self._client = self._node.create_client(TrajectoryQuery, '/trajectory_query') + + self._timer = self._node.create_timer(self._timer_period, self.timer_callback) + + self._trajectory = None + + def call_trajectory_query_callback(self, request: Trigger.Request, response: Trigger.Response): + self._logger.info("TrajectoryRecorder.call_trajectory_query_callback called") + success, message = self.call_trajectory_query() + response.success = success + response.message = message + return response + + def timer_callback(self): + self._logger.info("TrajectoryRecorder.timer_callback called") + self.call_trajectory_query() + + def call_trajectory_query(self) -> bool | str: + success = False + message = "" + + if not self._client.service_is_ready(): + message = f"service is not ready. service={self._client.srv_name}" + self._logger.info(message) + success = False + return success, message + + trajectory_query_req = TrajectoryQuery.Request() + trajectory_query_req.trajectory_id = 0 # first trajectory + + self._future = self._client.call_async(trajectory_query_req) + + def done_callback(future): + self._logger.info("TrajectoryRecorder.call_trajectory_query.done_callback called.") + response: TrajectoryQuery.Response = future.result() + status = response.status + self._trajectory = response.trajectory + self._logger.info(f"status={status}") + + self._future.add_done_callback(done_callback) + success = True + message = f"called {self._client.srv_name} service" + return success, message + + def save_trajectory(self) -> bool | str: + success = False + message = "" + if (self._output is not None and self._output != "") and self._trajectory is not None: + self._logger.info("writing to output") + with open(self._output, "w") as f: + writer = csv.writer(f, lineterminator="\n") + writer.writerow(["timestamp", "frame_id", "x", "y", "z", "qx", "qy", "qz", "qw"]) + for pose_stamped in self._trajectory: + header = pose_stamped.header + pose = pose_stamped.pose + timestamp = header.stamp.sec + 1.0e-9 * header.stamp.nanosec + frame_id = header.frame_id + x = pose.position.x + y = pose.position.y + z = pose.position.z + qx = pose.orientation.x + qy = pose.orientation.y + qz = pose.orientation.z + qw = pose.orientation.w + writer.writerow([timestamp, frame_id, x, y, z, qx, qy, qz, qw]) + success = True + message = f"trajectory was written to output file (ouput={self._output})" + else: + success = False + message = f"trajectory was not written to output file (ouput={self._output}, trajectory={self._trajectory})" + self._logger.info(message) + return success, message + + def save_trajectory_callback(self, request: Trigger.Request, response: Trigger.Response): + self._logger.info("TrajectoryRecorder.save_trajectory_callback called") + success, message = self.save_trajectory() + response.success = success + response.message = message + return response + + def get_trajectory(self): + return self._trajectory + + def on_shutdown(self): + self._logger.info("TrajectoryRecorder.on_shutdown") + self.save_trajectory() + + +def main(): + rclpy.init() + node = Node("trajectory_recorder") + output = node.declare_parameter("output", "").value + timer_period = node.declare_parameter("timer_period", 10.0).value + + trajectory_recorder = TrajectoryRecorder(node, output, timer_period) + + try: + rclpy.spin(node) + except: # noqa: E722 + traceback.print_exc() + trajectory_recorder.on_shutdown() + + +if __name__ == "__main__": + main() diff --git a/mf_localization_mapping/configuration_files/cartographer/cartographer_2d_mapping.lua b/mf_localization_mapping/configuration_files/cartographer/cartographer_2d_mapping.lua index 175f4c4a..4da45aad 100644 --- a/mf_localization_mapping/configuration_files/cartographer/cartographer_2d_mapping.lua +++ b/mf_localization_mapping/configuration_files/cartographer/cartographer_2d_mapping.lua @@ -42,6 +42,7 @@ options = { num_point_clouds = 1, lookup_transform_timeout_sec = 0.2, submap_publish_period_sec = 0.3, + publish_tracked_pose = true, pose_publish_period_sec = 5e-3, trajectory_publish_period_sec = 30e-3, rangefinder_sampling_ratio = 1., diff --git a/mf_localization_mapping/launch/demo_2d_VLP16.launch.py b/mf_localization_mapping/launch/demo_2d_VLP16.launch.py index 7e48b7f7..2c61b806 100644 --- a/mf_localization_mapping/launch/demo_2d_VLP16.launch.py +++ b/mf_localization_mapping/launch/demo_2d_VLP16.launch.py @@ -220,11 +220,22 @@ def configure_ros2_bag_play(context, node): # write pose to csv file Node( - name="tf2_listener", + name="tracked_pose_listener", package="mf_localization", - executable="tf2_listener.py", + executable="tracked_pose_listener.py", parameters=[{ - "output": PythonExpression(['"', bag_filename, '.pose.csv" if "', save_pose, '"=="true" else ""']) + "output": PythonExpression(['"', bag_filename, '.tracked_pose.csv" if "', save_pose, '"=="true" else ""']) + }], + condition=IfCondition(save_pose), + ), + + # write optimized trajectory to csv file + Node( + name="trajectory_recorder", + package="mf_localization", + executable="trajectory_recorder.py", + parameters=[{ + "output": PythonExpression(['"', bag_filename, '.trajectory.csv" if "', save_pose, '"=="true" else ""']) }], condition=IfCondition(save_pose), ), From e2ceabd9e62d764986a7ee4c0a822a8d61bed3e5 Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Thu, 22 Aug 2024 15:27:34 +0900 Subject: [PATCH 3/8] add localization/Dockerfile.debug for development Signed-off-by: Masayuki Murata --- docker/localization/Dockerfile.debug | 127 +++++++++++++++++++++++++++ 1 file changed, 127 insertions(+) create mode 100644 docker/localization/Dockerfile.debug diff --git a/docker/localization/Dockerfile.debug b/docker/localization/Dockerfile.debug new file mode 100644 index 00000000..c80ca557 --- /dev/null +++ b/docker/localization/Dockerfile.debug @@ -0,0 +1,127 @@ +ARG PREFIX=cabot +ARG FROM_IMAGE=${PREFIX}__jammy-humble-custom-mesa + +FROM $FROM_IMAGE as build + +ARG TZ=UTC +ENV TZ=$TZ +RUN sudo ln -snf /usr/share/zoneinfo/$TZ /etc/localtime && echo $TZ | sudo tee /etc/timezone + +# TODO replace with rosdep install +RUN apt update && \ + apt install -y --no-install-recommends \ + imagemagick \ + libpcap-dev \ + nodejs \ + npm \ + python-is-python3 \ + python3-pip \ + python3-matplotlib \ + python3-sklearn \ + ros-${ROS_DISTRO}-diagnostic-updater \ + ros-${ROS_DISTRO}-gazebo-msgs \ + ros-${ROS_DISTRO}-realsense2-description \ + ros-${ROS_DISTRO}-rosbridge-suite \ + ros-${ROS_DISTRO}-tf-transformations \ + ros-${ROS_DISTRO}-tf2-geometry-msgs \ + ros-${ROS_DISTRO}-velodyne-description \ + ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \ + && \ + apt clean && \ + rm -rf /var/lib/apt/lists/* + +RUN pip install --no-cache-dir \ + requests \ + certifi \ + orjson==3.10.6 \ + pyproj \ + rosbags + +## install packages for ublox node and RTKLIB +RUN apt update && \ + apt-get install -y \ + libasio-dev \ + ros-${ROS_DISTRO}-rtcm-msgs \ + ros-${ROS_DISTRO}-nmea-msgs \ + gfortran \ + && \ + rm -rf /var/lib/apt/lists/* + +## install RTKLIB +WORKDIR /opt +COPY ./RTKLIB/ ./RTKLIB/ +RUN (cd RTKLIB/lib/iers/gcc/; make) && \ + (cd RTKLIB/app/consapp; make; make install) && \ + rm -rf RTKLIB + +# TODO merge +RUN pip install --no-cache-dir \ + transforms3d==0.4.1 \ + pyserial +RUN apt update && \ + apt-get install -y \ + ros-${ROS_DISTRO}-navigation2 \ + ros-${ROS_DISTRO}-pointcloud-to-laserscan \ + xterm \ + && \ + rm -rf /var/lib/apt/lists/* +# end TODO + +# start cartographer_ws setup +ENV CARTO_WS=/opt/cartographer_ws + +RUN mkdir -p $CARTO_WS/src +COPY ./bluespace_ai_xsens_ros_mti_driver /opt/cartographer_ws/src/bluespace_ai_xsens_ros_mti_driver +COPY ./ublox /opt/cartographer_ws/src/ublox +COPY ./ntrip_client /opt/cartographer_ws/src/ntrip_client + +WORKDIR /opt/cartographer_ws/ + +# build packages in /opt/cartographer_ws other than cartographer +RUN . /opt/custom_ws/install/setup.sh && \ + colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo + +# start build cartographer +COPY ./cartographer /opt/cartographer_ws/src/cartographer +COPY ./cartographer_ros /opt/cartographer_ws/src/cartographer_ros + +RUN apt update && rosdep update && rosdep install -q -y \ +--from-paths src \ + --ignore-src + +RUN src/cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +RUN src/cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* + +RUN . /opt/custom_ws/install/setup.sh && \ + colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo +# end build cartographer + +ARG ROS_DISTRO +RUN sed -i 's:custom_ws:cartographer_ws:' /ros_entrypoint.sh + +# end cartographer_ws setup + +WORKDIR /opt + +COPY ./launch.sh / +COPY ./launch_rtklib.sh / + +# setup for display +ENV USERNAME developer + +### replace 1000 with your user/group id +ARG UID=1000 +RUN useradd -m $USERNAME && \ + echo "$USERNAME:$USERNAME" | chpasswd && \ + usermod --shell /bin/bash $USERNAME && \ + usermod -aG sudo $USERNAME && \ + mkdir -p /etc/sudoers.d/ && \ + echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers.d/$USERNAME && \ + chmod 0440 /etc/sudoers.d/$USERNAME && \ + usermod --uid $UID $USERNAME && \ + groupmod --gid $UID $USERNAME + +USER $USERNAME + +ENV HOME /home/$USERNAME +WORKDIR $HOME/loc_ws From 9a4a16caf57327a7d1707fac79167ce8e6249abc Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Fri, 13 Sep 2024 18:54:52 +0900 Subject: [PATCH 4/8] add scripts for debugging and evaluating mapping with gnss fix data - add export_gnss_fix.py to read bag data and write gnss fix topic to a csv file - add extract_fixed_frame_pose_data.py to extract fixed_frame_pose_data from pbstream info for debugging purpose - add compare_trajectory_and_gnss.py to compare optimized trajectory, gnss data, and fixed frame pose data (optional) for evaluation/debug purpose Signed-off-by: Masayuki Murata --- mf_localization_mapping/CMakeLists.txt | 1 + .../script/compare_trajectory_and_gnss.py | 121 ++++++++++++++++++ .../script/export_gnss_fix.py | 72 +++++++++++ .../script/extract_fixed_frame_pose_data.py | 79 ++++++++++++ 4 files changed, 273 insertions(+) create mode 100755 mf_localization_mapping/script/compare_trajectory_and_gnss.py create mode 100755 mf_localization_mapping/script/export_gnss_fix.py create mode 100755 mf_localization_mapping/script/extract_fixed_frame_pose_data.py diff --git a/mf_localization_mapping/CMakeLists.txt b/mf_localization_mapping/CMakeLists.txt index f51b19db..0f090aac 100644 --- a/mf_localization_mapping/CMakeLists.txt +++ b/mf_localization_mapping/CMakeLists.txt @@ -14,6 +14,7 @@ endif() install(PROGRAMS script/check_topic_size.py + script/export_gnss_fix.py script/extract_floormap_info_from_yaml.py script/filter_rss_samples.py script/timeout_thread.py diff --git a/mf_localization_mapping/script/compare_trajectory_and_gnss.py b/mf_localization_mapping/script/compare_trajectory_and_gnss.py new file mode 100755 index 00000000..8beea1f7 --- /dev/null +++ b/mf_localization_mapping/script/compare_trajectory_and_gnss.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2024 IBM Corporation +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +import argparse +import numpy as np +import matplotlib.pyplot as plt +import pandas as pd +import scipy +import scipy.interpolate + +from mf_localization import geoutil + + +def main(): + parser = argparse.ArgumentParser("Compare optimized trajectory and gnss data for evaluation/debug purpose") + parser.add_argument("--trajectory", required=True, help="trajectory log csv file") + parser.add_argument("--gnss", default=None, help="gnss log csv file") + parser.add_argument("--fixed_frame_pose", default=None, help="fixed frame pose data csv file") + parser.add_argument("--gnss_status_threshold", default=2, type=int, help="status threshold for gnss data. gnss data with gnss_status_threshold<=status are used to evaluate error") + args = parser.parse_args() + status_threshold = args.gnss_status_threshold + + # trajectory data + # expected format + # timestamp,frame_id,x,y,z,qx,qy,qz,qw + df_trajectory = pd.read_csv(args.trajectory) + + # gnss data + # expected format + # timestamp,header_stamp,frame_id,status,service,latitude,longitude,altitude,position_covariance_type,position_covariance_0,... + if args.gnss is not None: + df_gnss = pd.read_csv(args.gnss) + point = df_gnss.iloc[0] + gnss_anchor = geoutil.Anchor(lat=point.latitude, lng=point.longitude, rotate=0.0) + + X_gnss = [] # gnss local coordinate + for index, point in df_gnss.iterrows(): + # convert to local coordinate + latlng = geoutil.Latlng(lat=point.latitude, lng=point.longitude) + xy = geoutil.global2local(latlng, gnss_anchor) + X_gnss.append([xy.x, xy.y]) + X_gnss = np.array(X_gnss) + df_gnss["x"] = X_gnss[:, 0] + df_gnss["y"] = X_gnss[:, 1] + + plt.scatter(df_gnss["x"], df_gnss["y"], label="GNSS position converted by geoutil") + + # fixed frame pose data + # expected format + # timestamp,x,y,z,w + if args.fixed_frame_pose is not None: + df_pose = pd.read_csv(args.fixed_frame_pose) + plt.scatter(df_pose["x"], df_pose["y"], label="Fixed frame pose data") + + plt.scatter(df_trajectory["x"], df_trajectory["y"], label="Optimized trajectory") + + plt.legend() + plt.gca().set_aspect('equal') + plt.show() + + # error evaluation + if args.gnss is not None: + error_data = [] + interpolator = scipy.interpolate.interp1d(df_gnss["timestamp"].values, df_gnss[["x", "y", "status"]].T) + for index, point in df_trajectory.iterrows(): + x_traj = point.x + y_traj = point.y + try: + x_interp, y_interp, status_interp = interpolator(point.timestamp) + except ValueError as e: + print(F"Skipped interploation at timestamp = {point.timestamp} because an error is raised. ValueError: {e}") + continue + if status_threshold <= status_interp: + error = np.linalg.norm([x_interp - x_traj, y_interp - y_traj]) + error_data.append([point.timestamp, point.x, point.y, error]) + error_data = np.array(error_data) + + print(F"error stats: count={len(error_data)}, mean={np.mean(error_data[:, 3])}, min={np.min(error_data[:, 3])}, 95%-ile={np.percentile(error_data[:, 3], q=95)}, max={np.max(error_data[:, 3])}") + + # plot error time series + plt.scatter(error_data[:, 0], error_data[:, 3]) + plt.xlabel("timestamp") + plt.ylabel("error [m]") + plt.show() + + # plot error histogram + plt.hist(error_data[:, 3], bins=1000, cumulative=True, density=True, histtype="step") + plt.xlim(0) + plt.ylim([0, 1]) + plt.xlabel("error [m]") + plt.ylabel("cumularive distribution") + plt.show() + + # plot error distribution + plt.scatter(error_data[:, 1], error_data[:, 2], c=error_data[:, 3]) + plt.colorbar(label="error [m]") + plt.gca().set_aspect('equal') + plt.show() + + +if __name__ == "__main__": + main() diff --git a/mf_localization_mapping/script/export_gnss_fix.py b/mf_localization_mapping/script/export_gnss_fix.py new file mode 100755 index 00000000..9e26b5d9 --- /dev/null +++ b/mf_localization_mapping/script/export_gnss_fix.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2024 IBM Corporation +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +import argparse +import csv +from pathlib import Path +from rosbags.highlevel import AnyReader + + +def main(): + parser = argparse.ArgumentParser("Read ROS bag and output fix messages to a csv file") + parser.add_argument("-i", "--input", required=True, help="ROS bag directory") # bag + parser.add_argument("-o", "--output", default=None, help="output csv file path") # csv + parser.add_argument("--fix_topic", default="/ublox/fix") + args = parser.parse_args() + + bagpath = Path(args.input) + + data = [] + # read bag + with AnyReader([bagpath]) as reader: + connections = [x for x in reader.connections if x.topic == args.fix_topic] + for connection, timestamp, rawdata in reader.messages(connections=connections): + # sensor_msgs/msg/NavSatFix + msg = reader.deserialize(rawdata, connection.msgtype) + + t_sec = timestamp * 1.0e-9 # nanoseconds -> seconds + header_stamp = msg.header.stamp.sec + 1.0e-9 * msg.header.stamp.nanosec + frame_id = msg.header.frame_id + status = msg.status.status + service = msg.status.service + latitude = msg.latitude + longitude = msg.longitude + altitude = msg.altitude + position_covariance = msg.position_covariance + position_covariance_type = msg.position_covariance_type + + data.append([t_sec, header_stamp, frame_id, status, service, latitude, longitude, altitude, position_covariance_type, *position_covariance]) + + # save to a file + output = args.output + if output is not None and output != "": + header = ["timestamp", "header_stamp", "frame_id", "status", "service", "latitude", "longitude", "altitude", "position_covariance_type"] + header.extend(["position_covariance_" + str(i) for i in range(9)]) + + with open(output, "w") as f: + writer = csv.writer(f, lineterminator="\n") + writer.writerow(header) + writer.writerows(data) + + +if __name__ == "__main__": + main() diff --git a/mf_localization_mapping/script/extract_fixed_frame_pose_data.py b/mf_localization_mapping/script/extract_fixed_frame_pose_data.py new file mode 100755 index 00000000..be8f83b6 --- /dev/null +++ b/mf_localization_mapping/script/extract_fixed_frame_pose_data.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2024 IBM Corporation +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +import argparse +import csv +import re + + +def main(): + parser = argparse.ArgumentParser("Extract fixed_frame_pose_data from output of cartographer_pbstream info command for debugging purpose") + parser.add_argument("-i", "--input", required=True, help="input file") + parser.add_argument("-o", "--output", default=None, help="output csv file") + args = parser.parse_args() + + # input file is stderr output of cartographer_pbstream info command + # example command to write stderr output to a file + # $ PATH_TO_CARTOGRAPHER_BIN/cartographer_pbstream info YOUR_PBSTREAM_FILE.pbstream --all_debug_strings 2> YOUR_PBSTREAM_INFO.txt + with open(args.input) as f: + string = f.read() + + # extract fixed_from_pose_data by pattern matching + # expected pattern + # + # Serialized data: fixed_frame_pose_data { + # fixed_frame_pose_data { + # timestamp: timestamp + # pose { + # translation { + # x: x + # y: y + # z: z + # } + # rotation { + # w: w + # } + # } + # } + # } + pattern = r'Serialized data: fixed_frame_pose_data {\s*fixed_frame_pose_data {\s*timestamp: (\d+)\s*pose {\s*translation {\s*x: ([\d\-.]+)\s*y: ([\d\-.]+)\s*z: ([\d\-.]+)\s*}\s*rotation {\s*w: ([\d\-.]+)' + matches = re.findall(pattern, string, re.DOTALL) + + # save the extracted fixed_frame_pose data to a file + data = [] + for match in matches: + timestamp, x, y, z, w = match + print(f"timestamp: {timestamp}, x: {x}, y: {y}, z: {z}, w: {w}") + data.append([timestamp, x, y, z, w]) + + # save to a csv file + output = args.output + if output is not None and output != "": + with open(output, "w") as f: + writer = csv.writer(f, lineterminator="\n") + header = ["timestamp", "x", "y", "z", "w"] + writer.writerow(header) + writer.writerows(data) + + +if __name__ == "__main__": + main() From 45330ea5bd17750734de2019e96398e6d5bff796 Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Wed, 18 Sep 2024 17:26:07 +0900 Subject: [PATCH 5/8] update typical cartographer configuration used for mapping with gnss data Signed-off-by: Masayuki Murata --- .../cartographer_2d_mapping_gnss.lua | 29 ++++++++++++++++--- 1 file changed, 25 insertions(+), 4 deletions(-) diff --git a/mf_localization_mapping/configuration_files/cartographer/cartographer_2d_mapping_gnss.lua b/mf_localization_mapping/configuration_files/cartographer/cartographer_2d_mapping_gnss.lua index d70d23b2..bcec7a7c 100644 --- a/mf_localization_mapping/configuration_files/cartographer/cartographer_2d_mapping_gnss.lua +++ b/mf_localization_mapping/configuration_files/cartographer/cartographer_2d_mapping_gnss.lua @@ -20,12 +20,33 @@ include "cartographer_2d_mapping.lua" +-- Handle NavSatFix topic options.use_nav_sat = true -options.fixed_frame_pose_sampling_ratio = 1.0 -- 10 Hz * sampling_ratio = 1.0 Hz +-- Customized settings to convert geodetic coordinate to ENU (East-North-Up) coordinate by spherical mercator projection +options.nav_sat_use_enu_local_frame = true -- default: false +options.nav_sat_use_spherical_mercator = true -- default: false + +-- Use predifined ENU frame at specific point (latitude and logitude) if necessary. If not, the first NavSatFix message is used to initialize ECEF to local frame conversion +-- options.nav_sat_use_predefined_enu_frame = true +-- options.nav_sat_predefined_enu_frame_latitude = (float) +-- options.nav_sat_predefined_enu_frame_longitude = (float) + +-- Tune this value if the submap data size becomes too large +-- TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05 -- default: 0.05 + +-- Disable collate_fixed_frame to allow non continous (intermittent) fixed frame inputs TRAJECTORY_BUILDER.collate_fixed_frame = false -- default: true -POSE_GRAPH.optimization_problem.fixed_frame_pose_translation_weight = 1e1 -- default: 1e1 -POSE_GRAPH.optimization_problem.fixed_frame_pose_rotation_weight = 0.0 -- GPS provides no orientation. -POSE_GRAPH.optimization_problem.fixed_frame_pose_use_tolerant_loss = true -- use tolerant loss + +-- Settings for optimization problem using fixed frame constraint +POSE_GRAPH.optimization_problem.fixed_frame_pose_translation_weight = 1e4 -- default: 1e1 +POSE_GRAPH.optimization_problem.fixed_frame_pose_rotation_weight = 0.0 -- GPS provides no orientation +POSE_GRAPH.optimization_problem.fixed_frame_pose_use_tolerant_loss = true -- default: false + +-- Customized settings to align cartographer coordinate to ENU coordinate. +POSE_GRAPH.optimization_problem.set_constant_fixed_frame_origin = true -- default: false +POSE_GRAPH.optimization_problem.zero_initialize_fixed_frame_origin = true -- default: false +POSE_GRAPH.optimization_problem.set_constant_first_submap_translation = true -- default: true +POSE_GRAPH.optimization_problem.set_constant_first_submap_rotation = false -- default: true return options From ede61633411fc870215eab9b7b5bb7056e0f66c7 Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Thu, 19 Sep 2024 19:36:41 +0900 Subject: [PATCH 6/8] add a script to interpolate poses in samples data by optimized trajectory data Signed-off-by: Masayuki Murata --- mf_localization/CMakeLists.txt | 1 + .../script/trajectory_based_interpolator.py | 166 ++++++++++++++++++ 2 files changed, 167 insertions(+) create mode 100755 mf_localization/script/trajectory_based_interpolator.py diff --git a/mf_localization/CMakeLists.txt b/mf_localization/CMakeLists.txt index 49c961db..b3c4938e 100644 --- a/mf_localization/CMakeLists.txt +++ b/mf_localization/CMakeLists.txt @@ -57,6 +57,7 @@ install(PROGRAMS script/tf2_beacons_listener.py script/tf2_listener.py script/tracked_pose_listener.py + script/trajectory_based_interpolator.py script/trajectory_recorder.py script/trajectory_restarter.py script/str2str_node.py diff --git a/mf_localization/script/trajectory_based_interpolator.py b/mf_localization/script/trajectory_based_interpolator.py new file mode 100755 index 00000000..2a4b7f85 --- /dev/null +++ b/mf_localization/script/trajectory_based_interpolator.py @@ -0,0 +1,166 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright (c) 2024 IBM Corporation +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +import argparse +import json +import matplotlib.pyplot as plt +import numpy as np +import pandas as pd +import scipy +import scipy.interpolate +from scipy.spatial.transform import Rotation +from scipy.spatial.transform import Slerp + +from geometry_msgs.msg import PoseStamped, Point, Quaternion + + +def csv_to_trajectory(csv_file: str): + df_trajectory: pd.DataFrame = pd.read_csv(csv_file) + trajectory: list[PoseStamped] = [] + for index, point in df_trajectory.iterrows(): + pose_stamped = PoseStamped() + sec = int(point.timestamp) + nanosec = int((point.timestamp - sec) * 1.0e9) + pose_stamped.header.stamp.sec = sec + pose_stamped.header.stamp.nanosec = nanosec + pose_stamped.header.frame_id = point.frame_id + pose_stamped.pose.position = Point(x=point.x, y=point.y, z=point.z) + pose_stamped.pose.orientation = Quaternion(x=point.qx, y=point.qy, z=point.qz, w=point.qw) + trajectory.append(pose_stamped) + return trajectory + + +def trajectory_to_dataframe(trajectory) -> pd.DataFrame: + columns = ["timestamp", "frame_id", "x", "y", "z", "qx", "qy", "qz", "qw"] + X = [] + for pose_stamped in trajectory: + header = pose_stamped.header + pose = pose_stamped.pose + timestamp = header.stamp.sec + 1.0e-9 * header.stamp.nanosec + frame_id = header.frame_id + x, y, z = pose.position.x, pose.position.y, pose.position.z + qx, qy, qz, qw = pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w + X.append([timestamp, frame_id, x, y, z, qx, qy, qz, qw]) + + return pd.DataFrame(X, columns=columns) + + +class TrajectoryBasedInterpolator: + def __init__(self, trajectory): + df_trajectory = trajectory_to_dataframe(trajectory) + # create xyz interpolator + self.xyz_interpolator = scipy.interpolate.interp1d(df_trajectory["timestamp"].values, df_trajectory[["x", "y", "z"]].T) + # create rotation interpolator + q_list = [] + for index, point in df_trajectory.iterrows(): + q = [point.qw, point.qx, point.qy, point.qz] # w, x, y, z order + q_list.append(q) + rotations = Rotation.from_quat(q_list) + self.slerp = Slerp(df_trajectory["timestamp"].values, rotations) + + def interpolate_pose(self, timestamp): + try: + xyz_interpolated = self.xyz_interpolator(timestamp) + rotation_interpolated = self.slerp(timestamp) + except ValueError as e: + raise e + q_interpolated = rotation_interpolated.as_quat() # w, x, y, z order + return [timestamp, xyz_interpolated[0], xyz_interpolated[1], xyz_interpolated[2], q_interpolated[1], q_interpolated[2], q_interpolated[3], q_interpolated[0]] # timestamp, x, y, z, qx, qy, qz, w + + def interpolate_samples(self, samples): + samples_new = [] + for s in samples: + timestamp = s["data"]["timestamp"] + + try: + pose_interp = self.interpolate_pose(timestamp) + except ValueError as e: + print(F"Skipped interpolation because a ValueError was raised. ValueError: {e}") + continue + + s_new = s.copy() + info = s["information"] + info_new = info.copy() + + info_new["x"], info_new["y"], info_new["z"] = pose_interp[1], pose_interp[2], pose_interp[3] + info_new["rotation"]["x"], info_new["rotation"]["y"], info_new["rotation"]["z"], info_new["rotation"]["w"] = pose_interp[4], pose_interp[5], pose_interp[6], pose_interp[7] + + s_new["information"] = info_new + samples_new.append(s_new) + + return samples_new + + +def interpolate_samples_pose_by_trajectory(samples, trajectory): + trajectory_based_interpolator = TrajectoryBasedInterpolator(trajectory) + samples_new = trajectory_based_interpolator.interpolate_samples(samples) + return samples_new + + +if __name__ == "__main__": + parser = argparse.ArgumentParser("interpolate poses in samples by optimized trajectory") + parser.add_argument("--samples", required=True, help="path to samples json data") + parser.add_argument("--trajectory", required=True, help="path to trajectory csv data") + parser.add_argument("--output_samples", default=None, help="output path to interpolated samples json data") + parser.add_argument("--plot", default=False, action="store_true", help="plot data") + args = parser.parse_args() + + input_samples = args.samples + input_trajectory = args.trajectory + output_samples = args.output_samples + plot = args.plot + + # load samples + with open(input_samples) as f: + samples = json.load(f) + + # load trajectory csv data as a trajectory object (PoseStamped[]) + trajectory = csv_to_trajectory(input_trajectory) + + samples_new = interpolate_samples_pose_by_trajectory(samples, trajectory) + + if output_samples is not None: + with open(output_samples, "w") as f: + json.dump(samples_new, f) + + # plot data + if plot: + X_raw = [] + X_new = [] + + for s in samples: + info = s["information"] + X_raw.append([info["x"], info["y"], info["z"]]) + + for s in samples_new: + info = s["information"] + X_new.append([info["x"], info["y"], info["z"]]) + + X_raw = np.array(X_raw) + X_new = np.array(X_new) + + plt.scatter(X_raw[:, 0], X_raw[:, 1], label="original") + plt.scatter(X_new[:, 0], X_new[:, 1], label="interpolated") + plt.legend() + plt.gca().set_aspect("equal") + plt.show() From 324570b43973fa93c643f0fc458e72b2b0590bde Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Fri, 20 Sep 2024 01:46:16 +0900 Subject: [PATCH 7/8] update tf2_beacons_listener.py to incorporate the functions of trajectory_recorder and trajectory_based_interpolator Signed-off-by: Masayuki Murata --- .../script/tf2_beacons_listener.py | 25 ++++++++++++++++++- .../launch/demo_2d_VLP16.launch.py | 20 ++++++--------- 2 files changed, 32 insertions(+), 13 deletions(-) diff --git a/mf_localization/script/tf2_beacons_listener.py b/mf_localization/script/tf2_beacons_listener.py index 42b60894..7ae04a76 100755 --- a/mf_localization/script/tf2_beacons_listener.py +++ b/mf_localization/script/tf2_beacons_listener.py @@ -30,6 +30,9 @@ import tf2_ros from std_msgs.msg import String +from trajectory_based_interpolator import TrajectoryBasedInterpolator +from trajectory_recorder import TrajectoryRecorder + class BeaconMapper: def __init__(self, node, save_empty_beacon_sample, data_inverval=1.2, position_interval=0.5, verbose=False): @@ -151,6 +154,11 @@ def main(): fingerprint_data_interval = node.declare_parameter("fingerprint_data_interval", 1.2).value # should be larger than 1.0 s because beacon data interval is about 1.0 s. fingerprint_position_interval = node.declare_parameter("fingerprint_position_interval", 0.5).value # to prevent the mapper from creating dummy fingerprint data at the same position + # use trajectory recorder extention + output_trajectory = node.declare_parameter("output_trajectory", '').value + interpolate_by_trajectory = node.declare_parameter("interpolate_by_trajectory", False).value + trajectory_recoder = None # default + mapper = BeaconMapper(node, save_empty_beacon_sample=save_empty_beacon_sample, data_inverval=fingerprint_data_interval, @@ -158,6 +166,9 @@ def main(): verbose=verbose ) + if output_trajectory != '' or interpolate_by_trajectory: + trajectory_recoder = TrajectoryRecorder(node, output_trajectory) + subscribers = [] for sub_topic in sub_topics: logger.info("set " + sub_topic + " subscriber.") @@ -184,11 +195,23 @@ def transform_check_loop(): except KeyboardInterrupt: node.get_logger().info("caught KeyboardInterrupt") + # trajectory recorder post process + if trajectory_recoder is not None: + trajectory_recoder.on_shutdown() + # save to a file after node shutdown if output and 0 < len(mapper._fingerprints): node.get_logger().info("output fingerprint data before exiting") + if interpolate_by_trajectory: + trajectory = trajectory_recoder.get_trajectory() + interpolator = TrajectoryBasedInterpolator(trajectory) + samples = interpolator.interpolate_samples(mapper._fingerprints) + node.get_logger().info(F"Reconstructed fingerprint poses from trajectory data. #samples={len(mapper._fingerprints)} -> #samples_converted={len(samples)}") + else: + samples = mapper._fingerprints + with open(output, "w") as f: - json.dump(mapper._fingerprints, f) + json.dump(samples, f) if __name__ == "__main__": diff --git a/mf_localization_mapping/launch/demo_2d_VLP16.launch.py b/mf_localization_mapping/launch/demo_2d_VLP16.launch.py index 2c61b806..180dc67c 100644 --- a/mf_localization_mapping/launch/demo_2d_VLP16.launch.py +++ b/mf_localization_mapping/launch/demo_2d_VLP16.launch.py @@ -72,6 +72,8 @@ def generate_launch_description(): save_empty_beacon_sample = LaunchConfiguration('save_empty_beacon_sample') quit_when_rosbag_finish = LaunchConfiguration('quit_when_rosbag_finish') + interpolate_samples_by_trajectory = LaunchConfiguration('interpolate_samples_by_trajectory') + def configure_ros2_bag_play(context, node): cmd = node.cmd.copy() cmd.extend(['--clock', '--rate', rate]) @@ -137,6 +139,8 @@ def configure_ros2_bag_play(context, node): DeclareLaunchArgument('save_empty_beacon_sample', default_value='true'), DeclareLaunchArgument('quit_when_rosbag_finish', default_value='false'), + DeclareLaunchArgument('interpolate_samples_by_trajectory', default_value='false'), + SetParameter('use_sim_time', ParameterValue(True)), IncludeLaunchDescription( @@ -206,6 +210,7 @@ def configure_ros2_bag_play(context, node): args=[ros2_bag_play] ), + # write samples data and optimized trajectory data Node( name="tf2_beacons_listener", package="mf_localization", @@ -213,7 +218,9 @@ def configure_ros2_bag_play(context, node): parameters=[{ 'output': [bag_filename, '.loc.samples.json'], 'topics': wireless_topics, - 'save_empty_beacon_sample': save_empty_beacon_sample + 'save_empty_beacon_sample': save_empty_beacon_sample, + 'output_trajectory': PythonExpression(['"', bag_filename, '.trajectory.csv" if "', save_pose, '"=="true" else ""']), + 'interpolate_by_trajectory': interpolate_samples_by_trajectory, }], condition=IfCondition(save_samples), ), @@ -229,17 +236,6 @@ def configure_ros2_bag_play(context, node): condition=IfCondition(save_pose), ), - # write optimized trajectory to csv file - Node( - name="trajectory_recorder", - package="mf_localization", - executable="trajectory_recorder.py", - parameters=[{ - "output": PythonExpression(['"', bag_filename, '.trajectory.csv" if "', save_pose, '"=="true" else ""']) - }], - condition=IfCondition(save_pose), - ), - RegisterEventHandler( OnProcessExit( target_action=ros2_bag_play, From 826b5671e8d5e9e741fb43fe8c592e9a698e3116 Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Tue, 24 Sep 2024 15:46:27 +0900 Subject: [PATCH 8/8] update mapping_post_process.sh for mapping with gnss data - use gnss fix topic in mapping when an environmental variable is set to true - save node options (enu frame latitude and longitude) to a yaml file - show error between optimized trajectory and gnss data Signed-off-by: Masayuki Murata --- .../script/mapping_post_process.sh | 54 +++++++++++++++- mf_localization_mapping/CMakeLists.txt | 2 + .../convert_rosbag_for_cartographer.launch.py | 18 +++++- .../script/compare_trajectory_and_gnss.py | 52 +++++++++------- .../script/extract_node_options.py | 62 +++++++++++++++++++ 5 files changed, 164 insertions(+), 24 deletions(-) create mode 100755 mf_localization_mapping/script/extract_node_options.py diff --git a/cabot_mf_localization/script/mapping_post_process.sh b/cabot_mf_localization/script/mapping_post_process.sh index c984b22d..a8852316 100755 --- a/cabot_mf_localization/script/mapping_post_process.sh +++ b/cabot_mf_localization/script/mapping_post_process.sh @@ -26,12 +26,16 @@ QUIT_WHEN_ROSBAG_FINISH=${QUIT_WHEN_ROSBAG_FINISH:-true} PLAYBAG_RATE_CARTOGRAPHER=${PLAYBAG_RATE_CARTOGRAPHER:-1.0} PLAYBAG_RATE_PC2_CONVERT=${PLAYBAG_RATE_PC2_CONVERT:-1.0} LIDAR_MODEL=${LIDAR_MODEL:-VLP16} +MAPPING_USE_GNSS=${MAPPING_USE_GNSS:-false} gazebo=${PROCESS_GAZEBO_MAPPING:-0} # topic points2_topic='/velodyne_points' imu_topic=/imu/data +fix_topic=/ublox/fix +fix_velocity_topic=/ublox/fix_velocity +fix_filtered_topic=/ublox/fix_filtered convert_points=true # VLP16 (default LIDAR_MODEL) if [[ $gazebo -eq 1 ]]; then @@ -43,6 +47,11 @@ fi echo "LIDAR_MODEL=$LIDAR_MODEL" +function red { + echo -en "\033[31m" ## red + echo $@ + echo -en "\033[0m" ## reset color +} function blue { echo -en "\033[36m" ## blue echo $@ @@ -73,6 +82,8 @@ if [[ ! -e $WORKDIR/${BAG_FILENAME}.carto-converted ]]; then ros2 launch mf_localization_mapping convert_rosbag_for_cartographer.launch.py \ points2:=${points2_topic} \ imu:=${imu_topic} \ + fix:=${fix_topic} \ + fix_velocity:=${fix_velocity_topic} \ rate:=${PLAYBAG_RATE_PC2_CONVERT} \ convert_points:=$convert_points \ bag_filename:=$WORKDIR/${BAG_FILENAME} @@ -90,11 +101,26 @@ if [[ ! -e $WORKDIR/${BAG_FILENAME}.carto-converted.loc.samples.json ]] || [[ ! rate:=${PLAYBAG_RATE_CARTOGRAPHER} \ quit_when_rosbag_finish:=${QUIT_WHEN_ROSBAG_FINISH} \ bag_filename:=$WORKDIR/${BAG_FILENAME}.carto-converted" + if [ $cabot_model != "" ]; then com="$com cabot_model:=$cabot_model" else com="$com robot:=$robot" fi + + # outdoor mapping + if [ "$MAPPING_USE_GNSS" = true ]; then + com="$com \ + save_pose:=true \ + fix:=$fix_filtered_topic \ + configuration_basename:=cartographer_2d_mapping_gnss.lua \ + interpolate_samples_by_trajectory:=true \ + " + fi + + # copy output to a file + com="$com | tee $WORKDIR/${BAG_FILENAME}.carto-converted.log" + echo $com eval $com else @@ -102,7 +128,7 @@ else blue "skipping $WORKDIR/${BAG_FILENAME}.carto-converted.pbstream" fi - +# convert pbstream to pgm if [[ ! -e $WORKDIR/${BAG_FILENAME}.carto-converted.pgm ]]; then ros2 run cartographer_ros cartographer_pbstream_to_ros_map \ -pbstream_filename $WORKDIR/${BAG_FILENAME}.carto-converted.pbstream \ @@ -129,4 +155,30 @@ if [[ ! -e $WORKDIR/${BAG_FILENAME}.carto-converted.info.txt ]]; then | tee -a $WORKDIR/${BAG_FILENAME}.carto-converted.info.txt else blue "skipping $WORKDIR/${BAG_FILENAME}.carto-converted.info.txt" +fi + +# post process for mapping with gnss data +if [ "$MAPPING_USE_GNSS" = true ]; then + # extract option info from log + if [[ ! -e $WORKDIR/${BAG_FILENAME}.carto-converted.node-options.yaml ]]; then + ros2 run mf_localization_mapping extract_node_options.py \ + -i $WORKDIR/${BAG_FILENAME}.carto-converted.log \ + -o $WORKDIR/${BAG_FILENAME}.carto-converted.node-options.yaml + else + blue "skipping $WORKDIR/${BAG_FILENAME}.carto-converted.node-options.yaml" + fi + + # export gnss from bag + if [[ ! -e $WORKDIR/${BAG_FILENAME}.carto-converted.gnss.csv ]]; then + ros2 run mf_localization_mapping export_gnss_fix.py \ + -i $WORKDIR/${BAG_FILENAME}.carto-converted \ + -o $WORKDIR/${BAG_FILENAME}.carto-converted.gnss.csv + else + blue "skipping $WORKDIR/${BAG_FILENAME}.carto-converted.gnss.csv" + fi + + # calculate error between trajectory and gnss + ros2 run mf_localization_mapping compare_trajectory_and_gnss.py \ + --trajectory $WORKDIR/${BAG_FILENAME}.carto-converted.trajectory.csv \ + --gnss $WORKDIR/${BAG_FILENAME}.carto-converted.gnss.csv --plot fi \ No newline at end of file diff --git a/mf_localization_mapping/CMakeLists.txt b/mf_localization_mapping/CMakeLists.txt index 0f090aac..3c93a0f5 100644 --- a/mf_localization_mapping/CMakeLists.txt +++ b/mf_localization_mapping/CMakeLists.txt @@ -14,8 +14,10 @@ endif() install(PROGRAMS script/check_topic_size.py + script/compare_trajectory_and_gnss.py script/export_gnss_fix.py script/extract_floormap_info_from_yaml.py + script/extract_node_options.py script/filter_rss_samples.py script/timeout_thread.py script/topic_checker.py diff --git a/mf_localization_mapping/launch/convert_rosbag_for_cartographer.launch.py b/mf_localization_mapping/launch/convert_rosbag_for_cartographer.launch.py index 011bad75..03a22ecf 100644 --- a/mf_localization_mapping/launch/convert_rosbag_for_cartographer.launch.py +++ b/mf_localization_mapping/launch/convert_rosbag_for_cartographer.launch.py @@ -49,6 +49,8 @@ def generate_launch_description(): scan = LaunchConfiguration('scan') points2 = LaunchConfiguration('points2') imu = LaunchConfiguration('imu') + fix = LaunchConfiguration('fix') + fix_velocity = LaunchConfiguration('fix_velocity') rate = LaunchConfiguration('rate') convert_points = LaunchConfiguration('convert_points') convert_imu = LaunchConfiguration('convert_imu') @@ -58,6 +60,7 @@ def generate_launch_description(): def configure_ros2_bag_play_arguments(context, node): cmd = node.cmd.copy() + cmd.extend(['--clock']) if float(start.perform(context)) > 0.0: cmd.extend(['--start-offset', start]) if convert_points.perform(context) == 'true' or convert_imu.perform(context) == 'true': @@ -81,6 +84,8 @@ def configure_ros2_bag_play_arguments(context, node): DeclareLaunchArgument('scan', default_value='velodyne_scan'), DeclareLaunchArgument('points2', default_value='velodyne_points'), DeclareLaunchArgument('imu', default_value='imu/data'), + DeclareLaunchArgument('fix', default_value='fix'), + DeclareLaunchArgument('fix_velocity', default_value='fix_velocity'), DeclareLaunchArgument('rate', default_value='1.0'), DeclareLaunchArgument('convert_points', default_value='false'), DeclareLaunchArgument('convert_imu', default_value='false'), @@ -121,7 +126,18 @@ def configure_ros2_bag_play_arguments(context, node): ), ExecuteProcess( - cmd=['ros2', 'bag', 'record', imu, scan, points2, '/beacons', '/wireless/beacons', '/wireless/wifi', '/esp32/wifi', '-o', save_filename], + cmd=['ros2', 'bag', 'record', + imu, + scan, + points2, + '/beacons', + '/wireless/beacons', + '/wireless/wifi', + '/esp32/wifi', + fix, + fix_velocity, + '--use-sim-time', + '-o', save_filename], on_exit=ShutdownAction(), ), diff --git a/mf_localization_mapping/script/compare_trajectory_and_gnss.py b/mf_localization_mapping/script/compare_trajectory_and_gnss.py index 8beea1f7..7e602ac2 100755 --- a/mf_localization_mapping/script/compare_trajectory_and_gnss.py +++ b/mf_localization_mapping/script/compare_trajectory_and_gnss.py @@ -36,6 +36,7 @@ def main(): parser.add_argument("--gnss", default=None, help="gnss log csv file") parser.add_argument("--fixed_frame_pose", default=None, help="fixed frame pose data csv file") parser.add_argument("--gnss_status_threshold", default=2, type=int, help="status threshold for gnss data. gnss data with gnss_status_threshold<=status are used to evaluate error") + parser.add_argument("-p", "--plot", default=False, action="store_true", help="plot errors") args = parser.parse_args() status_threshold = args.gnss_status_threshold @@ -75,7 +76,11 @@ def main(): plt.legend() plt.gca().set_aspect('equal') - plt.show() + + if args.plot: + plt.show() + else: + plt.cla() # error evaluation if args.gnss is not None: @@ -94,27 +99,30 @@ def main(): error_data.append([point.timestamp, point.x, point.y, error]) error_data = np.array(error_data) - print(F"error stats: count={len(error_data)}, mean={np.mean(error_data[:, 3])}, min={np.min(error_data[:, 3])}, 95%-ile={np.percentile(error_data[:, 3], q=95)}, max={np.max(error_data[:, 3])}") - - # plot error time series - plt.scatter(error_data[:, 0], error_data[:, 3]) - plt.xlabel("timestamp") - plt.ylabel("error [m]") - plt.show() - - # plot error histogram - plt.hist(error_data[:, 3], bins=1000, cumulative=True, density=True, histtype="step") - plt.xlim(0) - plt.ylim([0, 1]) - plt.xlabel("error [m]") - plt.ylabel("cumularive distribution") - plt.show() - - # plot error distribution - plt.scatter(error_data[:, 1], error_data[:, 2], c=error_data[:, 3]) - plt.colorbar(label="error [m]") - plt.gca().set_aspect('equal') - plt.show() + print(F"trajectory and gnss ({status_threshold}<=status) error stats: count={len(error_data)}, mean={np.mean(error_data[:, 3])}, min={np.min(error_data[:, 3])}, 95%-ile={np.percentile(error_data[:, 3], q=95)}, max={np.max(error_data[:, 3])}") + + if args.plot: + # plot error time series + plt.scatter(error_data[:, 0], error_data[:, 3]) + plt.xlabel("timestamp") + plt.ylabel("error [m]") + plt.show() + + # plot error histogram + plt.hist(error_data[:, 3], bins=1000, cumulative=True, density=True, histtype="step") + plt.xlim(0) + plt.ylim([0, 1]) + plt.xlabel("error [m]") + plt.ylabel("cumularive distribution") + plt.show() + + # plot error distribution + plt.scatter(error_data[:, 1], error_data[:, 2], c=error_data[:, 3]) + plt.colorbar(label="error [m]") + plt.gca().set_aspect('equal') + plt.show() + else: + plt.cla() if __name__ == "__main__": diff --git a/mf_localization_mapping/script/extract_node_options.py b/mf_localization_mapping/script/extract_node_options.py new file mode 100755 index 00000000..cd2ddb0d --- /dev/null +++ b/mf_localization_mapping/script/extract_node_options.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2024 IBM Corporation +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +import argparse +import re + + +def main(): + parser = argparse.ArgumentParser("Extract options from cartographer_node log") + parser.add_argument("-i", "--input_log", required=True, help="input log file") + parser.add_argument("-o", "--output", default=None, help="output yaml file") + args = parser.parse_args() + + # input file is cartographer node log + with open(args.input_log) as f: + string = f.read() + + pattern = r'.*Using NavSatFix.*lat = ([0-9.]+).*long = ([0-9.]+).*use_enu_local_frame = ([0-1]).*use_spherical_mercator = ([0-1]).*' + matches = re.findall(pattern, string) + lat, long, use_enu_local_frame, use_spherical_mercator = matches[0] + + # replace text + use_enu_local_frame = "true" if use_enu_local_frame in [1, "1"] else "false" + use_spherical_mercator = "true" if use_spherical_mercator in [1, "1"] else "false" + + lines = [ + F"options.nav_sat_predefined_enu_frame_latitude: {lat}", + F"options.nav_sat_predefined_enu_frame_longitude: {long}", + F"options.nav_sat_use_enu_local_frame: {use_enu_local_frame}", + F"options.nav_sat_use_spherical_mercator: {use_spherical_mercator}", + ] + + print("\n".join(lines)) + + # save to a yaml file + output = args.output + if output is not None and output != "": + with open(output, "w") as f: + f.write("\n".lines) + + +if __name__ == "__main__": + main()