From 12a6053beed74cb30b23064031fe612b7a832b5b Mon Sep 17 00:00:00 2001 From: kyo0221 Date: Tue, 5 May 2026 20:00:35 +0900 Subject: [PATCH 01/13] =?UTF-8?q?=F0=9F=94=A5remove:=E3=80=80=E4=B8=8D?= =?UTF-8?q?=E8=A6=81=E3=81=AA=E6=A9=9F=E8=83=BD=E3=83=BB=E3=83=95=E3=82=A1?= =?UTF-8?q?=E3=82=A4=E3=83=AB=E3=81=AE=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- e2e_planner/config/train.yaml | 5 - e2e_planner/data/.gitignore | 2 - e2e_planner/e2e_planner/__init__.py | 0 e2e_planner/e2e_planner/inference_node.py | 179 ------ e2e_planner/e2e_planner/inference_node_sim.py | 199 ------- e2e_planner/e2e_planner/zed_sdk.py | 164 ------ e2e_planner/launch/e2e_planner.launch.py | 35 -- e2e_planner/package.xml | 23 - e2e_planner/resource/e2e_planner | 0 e2e_planner/runs/.gitignore | 2 - e2e_planner/scripts/create_data.py | 283 ---------- e2e_planner/scripts/network.py | 25 - e2e_planner/scripts/train.py | 191 ------- e2e_planner/scripts/util/__init__.py | 4 - e2e_planner/scripts/util/slit_aug.py | 37 -- e2e_planner/scripts/util/yolop_processor.py | 67 --- e2e_planner/setup.cfg | 25 - e2e_planner/setup.py | 32 -- e2e_planner/weights/.gitignore | 2 - frenet_planner/CMakeLists.txt | 44 -- .../include/frenet_planner/frenet_planner.hpp | 226 -------- .../frenet_planner/frenet_planner_node.hpp | 55 -- .../frenet_planner/obstacle_detect.hpp | 57 -- .../frenet_planner/reference_curve.hpp | 29 - .../frenet_planner/risk_calculator.hpp | 43 -- .../frenet_planner/visibility_control.h | 32 -- frenet_planner/package.xml | 22 - frenet_planner/src/frenet_planner.cpp | 404 -------------- frenet_planner/src/frenet_planner_node.cpp | 93 ---- frenet_planner/src/obstacle_detect.cpp | 108 ---- frenet_planner/src/reference_curve.cpp | 119 ---- frenet_planner/src/risk_calculator.cpp | 78 --- gnssnav/CMakeLists.txt | 110 ---- gnssnav/config/course_data/gazebo_shihou.csv | 46 -- .../config/course_data/line_follow_test.csv | 406 -------------- gnssnav/config/course_data/obstacle.csv | 522 ------------------ gnssnav/config/course_data/pre_line.csv | 411 -------------- gnssnav/config/course_data/precourse.csv | 399 ------------- .../config/course_data/precourse_250128.csv | 476 ---------------- .../config/course_data/precourse_obstacle.csv | 33 -- .../course_data/shihou_241030_dynamic.csv | 380 ------------- .../course_data/shihou_241030_static.csv | 500 ----------------- gnssnav/config/course_data/shihou_gnssnav.csv | 294 ---------- gnssnav/config/rviz/path.rviz | 203 ------- gnssnav/include/gnssnav/follower_node.hpp | 103 ---- .../include/gnssnav/path_publisher_node.hpp | 73 --- gnssnav/include/gnssnav/visibility_control.h | 35 -- gnssnav/package.xml | 28 - gnssnav/src/follower_node.cpp | 244 -------- gnssnav/src/path_publisher_node.cpp | 128 ----- path_tracker/CMakeLists.txt | 49 -- .../path_tracker/pure_pursuit_global_node.hpp | 68 --- .../path_tracker/pure_pursuit_node.hpp | 40 -- .../include/path_tracker/visibility_control.h | 35 -- path_tracker/package.xml | 26 - path_tracker/src/pure_pursuit_global_node.cpp | 188 ------- path_tracker/src/pure_pursuit_node.cpp | 95 ---- 57 files changed, 7477 deletions(-) delete mode 100644 e2e_planner/config/train.yaml delete mode 100644 e2e_planner/data/.gitignore delete mode 100644 e2e_planner/e2e_planner/__init__.py delete mode 100644 e2e_planner/e2e_planner/inference_node.py delete mode 100644 e2e_planner/e2e_planner/inference_node_sim.py delete mode 100644 e2e_planner/e2e_planner/zed_sdk.py delete mode 100644 e2e_planner/launch/e2e_planner.launch.py delete mode 100644 e2e_planner/package.xml delete mode 100644 e2e_planner/resource/e2e_planner delete mode 100644 e2e_planner/runs/.gitignore delete mode 100755 e2e_planner/scripts/create_data.py delete mode 100644 e2e_planner/scripts/network.py delete mode 100755 e2e_planner/scripts/train.py delete mode 100644 e2e_planner/scripts/util/__init__.py delete mode 100644 e2e_planner/scripts/util/slit_aug.py delete mode 100644 e2e_planner/scripts/util/yolop_processor.py delete mode 100644 e2e_planner/setup.cfg delete mode 100644 e2e_planner/setup.py delete mode 100644 e2e_planner/weights/.gitignore delete mode 100644 frenet_planner/CMakeLists.txt delete mode 100644 frenet_planner/include/frenet_planner/frenet_planner.hpp delete mode 100644 frenet_planner/include/frenet_planner/frenet_planner_node.hpp delete mode 100644 frenet_planner/include/frenet_planner/obstacle_detect.hpp delete mode 100644 frenet_planner/include/frenet_planner/reference_curve.hpp delete mode 100644 frenet_planner/include/frenet_planner/risk_calculator.hpp delete mode 100644 frenet_planner/include/frenet_planner/visibility_control.h delete mode 100644 frenet_planner/package.xml delete mode 100644 frenet_planner/src/frenet_planner.cpp delete mode 100644 frenet_planner/src/frenet_planner_node.cpp delete mode 100644 frenet_planner/src/obstacle_detect.cpp delete mode 100644 frenet_planner/src/reference_curve.cpp delete mode 100644 frenet_planner/src/risk_calculator.cpp delete mode 100644 gnssnav/CMakeLists.txt delete mode 100644 gnssnav/config/course_data/gazebo_shihou.csv delete mode 100644 gnssnav/config/course_data/line_follow_test.csv delete mode 100644 gnssnav/config/course_data/obstacle.csv delete mode 100644 gnssnav/config/course_data/pre_line.csv delete mode 100644 gnssnav/config/course_data/precourse.csv delete mode 100644 gnssnav/config/course_data/precourse_250128.csv delete mode 100644 gnssnav/config/course_data/precourse_obstacle.csv delete mode 100644 gnssnav/config/course_data/shihou_241030_dynamic.csv delete mode 100644 gnssnav/config/course_data/shihou_241030_static.csv delete mode 100644 gnssnav/config/course_data/shihou_gnssnav.csv delete mode 100644 gnssnav/config/rviz/path.rviz delete mode 100644 gnssnav/include/gnssnav/follower_node.hpp delete mode 100644 gnssnav/include/gnssnav/path_publisher_node.hpp delete mode 100644 gnssnav/include/gnssnav/visibility_control.h delete mode 100644 gnssnav/package.xml delete mode 100644 gnssnav/src/follower_node.cpp delete mode 100644 gnssnav/src/path_publisher_node.cpp delete mode 100644 path_tracker/CMakeLists.txt delete mode 100644 path_tracker/include/path_tracker/pure_pursuit_global_node.hpp delete mode 100644 path_tracker/include/path_tracker/pure_pursuit_node.hpp delete mode 100644 path_tracker/include/path_tracker/visibility_control.h delete mode 100644 path_tracker/package.xml delete mode 100644 path_tracker/src/pure_pursuit_global_node.cpp delete mode 100644 path_tracker/src/pure_pursuit_node.cpp diff --git a/e2e_planner/config/train.yaml b/e2e_planner/config/train.yaml deleted file mode 100644 index c069a80d..00000000 --- a/e2e_planner/config/train.yaml +++ /dev/null @@ -1,5 +0,0 @@ -epochs: 200 -batch_size: 8 -learning_rate: 0.0002 -num_workers: 2 -weight_file: "e2e_model.pt" \ No newline at end of file diff --git a/e2e_planner/data/.gitignore b/e2e_planner/data/.gitignore deleted file mode 100644 index c96a04f0..00000000 --- a/e2e_planner/data/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -* -!.gitignore \ No newline at end of file diff --git a/e2e_planner/e2e_planner/__init__.py b/e2e_planner/e2e_planner/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/e2e_planner/e2e_planner/inference_node.py b/e2e_planner/e2e_planner/inference_node.py deleted file mode 100644 index 41458bfd..00000000 --- a/e2e_planner/e2e_planner/inference_node.py +++ /dev/null @@ -1,179 +0,0 @@ -import rclpy -from rclpy.node import Node -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.executors import MultiThreadedExecutor -from sensor_msgs.msg import Image, PointCloud2 -from std_msgs.msg import Header -from nav_msgs.msg import Path -from geometry_msgs.msg import PoseStamped, Pose, Point -from cv_bridge import CvBridge -import cv2 -import torch -import numpy as np -import os -from pathlib import Path as FilePath -from rclpy.qos import qos_profile_system_default, qos_profile_sensor_data -from ament_index_python.packages import get_package_share_directory -from scipy.interpolate import splprep, splev -from typing import Tuple -from .zed_sdk import ZedSdk - -from util.yolop_processor import YOLOPv2Processor - -def denormalize_waypoints(normalized: np.ndarray) -> np.ndarray: - denormalized = normalized.copy() - denormalized[0::2] = (normalized[0::2] + 1.0) * 5.0 - denormalized[1::2] = (normalized[1::2] + 1.0) * 3.0 - 3.0 - return denormalized - -class InferenceNode(Node): - def __init__(self) -> None: - super().__init__('inference_node') - - self.declare_parameter('model_name', 'model.pt') - self.declare_parameter('interval_ms', 100) - - model_path = self.get_parameter('model_name').value - interval_ms = self.get_parameter('interval_ms').value - - self.bridge = CvBridge() - self.device = torch.device('cuda') - self.zed = None - - self.cv_image = None - - self.sim_flag = False - - package_share_directory = get_package_share_directory('e2e_planner') - weight_path = os.path.join(package_share_directory, 'weights', model_path) - yolop_weight_path = FilePath(package_share_directory) / 'weights' / 'yolopv2.pt' - - if os.path.exists(weight_path): - self.model = torch.jit.load(weight_path, map_location=self.device) - self.model.eval() - else: - self.get_logger().warn(f'Model file not found: {weight_path}') - self.model = None - - self.yolop_processor = YOLOPv2Processor(yolop_weight_path, self.device) - self.zed = ZedSdk(self, self.sim_flag) - - self.pub_raw = self.create_publisher(Path, 'e2e_planner/path_raw', qos_profile_system_default) - self.pub = self.create_publisher(Path, 'e2e_planner/path', qos_profile_system_default) - self.pub_pointcloud = self.create_publisher(PointCloud2, '/zed/zed_node/pointcloud', qos_profile_sensor_data) - self.pub_debug_image = self.create_publisher(Image, 'e2e_planner/debug_image', qos_profile_system_default) - self.get_logger().info('Debug mode enabled: publishing preprocessed images to e2e_planner/debug_image') - - self.torch_cb_group = ReentrantCallbackGroup() - self.zed_cb_group = ReentrantCallbackGroup() - - self.torch_timer = self.create_timer( - interval_ms / 1000.0, - self.torch_callback, - callback_group=self.torch_cb_group, - ) - self.zed_timer = self.create_timer( - 10.0 / 1000.0, - self.zed_sensor_callback, - callback_group=self.zed_cb_group, - ) - - def preprocess_image(self, image: np.ndarray) -> Tuple[torch.Tensor, np.ndarray]: - if self.sim_flag: - mask = ((image[:, :, 2] > 200) & (image[:, :, 0] < 50) & (image[:, :, 1] < 50)).astype(np.uint8) - mask = cv2.resize(mask, (64, 48)) - else: - bgr_image = cv2.cvtColor(image, cv2.COLOR_BGRA2BGR) - mask = self.yolop_processor.process_image(bgr_image, (64, 48)) - - mask_normalized = mask.astype(np.float32) - tensor = torch.from_numpy(mask_normalized).unsqueeze(0).unsqueeze(0) - return tensor.to(self.device), mask - - def torch_callback(self) -> None: - if self.cv_image is None: - return - header = Header() - header.stamp = self.get_clock().now().to_msg() - header.frame_id = 'base_link' - - input_tensor, mask = self.preprocess_image(self.cv_image) - - resized_input = cv2.resize(cv2.cvtColor(self.cv_image, cv2.COLOR_BGRA2BGR), (64, 48)) - resized_input[mask == 1] = [0, 0, 255] - debug_msg = self.bridge.cv2_to_imgmsg(resized_input, encoding='bgr8') - debug_msg.header = header - self.pub_debug_image.publish(debug_msg) - - with torch.no_grad(): - output = self.model(input_tensor) - - output_normalized = output.cpu().numpy().flatten() - output_denormalized = denormalize_waypoints(output_normalized) - output_denormalized_tensor = torch.from_numpy(output_denormalized).unsqueeze(0) - - path_raw_msg = self.create_path_from_output(output_denormalized_tensor, header) - self.pub_raw.publish(path_raw_msg) - - path_smooth_msg = self.apply_bspline_smoothing(output_denormalized_tensor, header) - self.pub.publish(path_smooth_msg) - - def zed_sensor_callback(self) -> None: - if self.zed is None or not self.zed.grab(): - return - - self.cv_image = self.zed.get_image() - - header = Header() - header.stamp = self.get_clock().now().to_msg() - header.frame_id = 'base_link' - - pointcloud_msg = self.zed.get_pointcloud(header) - if pointcloud_msg is None: - pointcloud_msg = PointCloud2() - pointcloud_msg.header = header - self.pub_pointcloud.publish(pointcloud_msg) - - def apply_bspline_smoothing(self, output: torch.Tensor, header) -> Path: - waypoints = output.cpu().numpy().reshape(-1, 2) - x = waypoints[:, 0] - y = waypoints[:, 1] - - # s: smoothing factor（値が大きいほど滑らか、0だと補間） - # k: スプラインの次数（3次） - tck, u = splprep([x, y], s=0.1, k=3) - u_new = np.linspace(0, 1, 30) - x_smooth, y_smooth = splev(u_new, tck) - - path_msg = Path() - path_msg.header = header - path_msg.header.frame_id = 'base_link' - - path_msg.poses.append(PoseStamped(header=path_msg.header, pose=Pose(position=Point(x=0.0, y=0.0)))) - path_msg.poses.extend(PoseStamped(header=path_msg.header, pose=Pose(position=Point(x=float(x_smooth[i]), y=float(y_smooth[i])))) for i in range(len(x_smooth))) - - return path_msg - - def create_path_from_output(self, output: torch.Tensor, header) -> Path: - path_msg = Path() - path_msg.header = header - path_msg.header.frame_id = 'base_link' - waypoints = output.cpu().numpy().reshape(-1, 2) - - path_msg.poses.append(PoseStamped(header=path_msg.header, pose=Pose(position=Point(x=0.0, y=0.0)))) - path_msg.poses.extend(PoseStamped(header=path_msg.header, pose=Pose(position=Point(x=float(x), y=float(y)))) for x, y in waypoints) - - return path_msg - - -def main(args=None) -> None: - rclpy.init(args=args) - node = InferenceNode() - executor = MultiThreadedExecutor(num_threads=2) - executor.add_node(node) - executor.spin() - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/e2e_planner/e2e_planner/inference_node_sim.py b/e2e_planner/e2e_planner/inference_node_sim.py deleted file mode 100644 index c28690bd..00000000 --- a/e2e_planner/e2e_planner/inference_node_sim.py +++ /dev/null @@ -1,199 +0,0 @@ -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Image -from nav_msgs.msg import Path -from geometry_msgs.msg import PoseStamped, Pose, Point -from cv_bridge import CvBridge -import cv2 -import torch -import numpy as np -import os -from pathlib import Path as FilePath -from rclpy.qos import qos_profile_system_default, qos_profile_sensor_data -from ament_index_python.packages import get_package_share_directory -from scipy.interpolate import splprep, splev -from typing import Optional, Tuple - -try: - import pyzed.sl as sl - ZED_SDK_AVAILABLE = True -except ImportError: - ZED_SDK_AVAILABLE = False - -from util.yolop_processor import YOLOPv2Processor - -def denormalize_waypoints(normalized: np.ndarray) -> np.ndarray: - denormalized = normalized.copy() - denormalized[0::2] = (normalized[0::2] + 1.0) * 5.0 - denormalized[1::2] = (normalized[1::2] + 1.0) * 3.0 - 3.0 - return denormalized - -class InferenceNode(Node): - def __init__(self) -> None: - super().__init__('inference_node') - - self.declare_parameter('model_name', 'model.pt') - self.declare_parameter('interval_ms', 100) - self.declare_parameter('sdk_flag', False) - self.declare_parameter('debug_mode', True) - - model_path = self.get_parameter('model_name').value - interval_ms = self.get_parameter('interval_ms').value - self.sdk_flag_ = self.get_parameter('sdk_flag').value - self.debug_mode_ = self.get_parameter('debug_mode').value - - self.bridge = CvBridge() - self.device = torch.device('cuda') - self.latest_image = None - self.zed_camera: Optional[sl.Camera] = None - self.zed_image: Optional[sl.Mat] = None - self.zed_runtime_params: Optional[sl.RuntimeParameters] = None - - package_share_directory = get_package_share_directory('e2e_planner') - weight_path = os.path.join(package_share_directory, 'weights', model_path) - yolop_weight_path = FilePath(package_share_directory) / 'weights' / 'yolopv2.pt' - - if os.path.exists(weight_path): - self.model = torch.jit.load(weight_path, map_location=self.device) - self.model.eval() - else: - self.get_logger().warn(f'Model file not found: {weight_path}') - self.model = None - - self.yolop_processor = YOLOPv2Processor(yolop_weight_path, self.device) - - if self.sdk_flag_: - if not ZED_SDK_AVAILABLE: - self.get_logger().error('ZED SDK not available. Install pyzed package.') - raise RuntimeError('ZED SDK not available') - self._initialize_zed_camera() - else: - self.sub = self.create_subscription(Image, '/zed/zed_node/rgb/image_rect_color', self.image_callback, qos_profile_sensor_data) - - self.pub_raw = self.create_publisher(Path, 'e2e_planner/path_raw', qos_profile_system_default) - self.pub = self.create_publisher(Path, 'e2e_planner/path', qos_profile_system_default) - - if self.debug_mode_: - self.pub_debug_image = self.create_publisher(Image, 'e2e_planner/debug_image', qos_profile_system_default) - self.get_logger().info('Debug mode enabled: publishing preprocessed images to e2e_planner/debug_image') - - self.timer = self.create_timer(interval_ms / 1000.0, self.timer_callback) - - def _initialize_zed_camera(self) -> None: - self.zed_camera = sl.Camera() - init_params = sl.InitParameters() - init_params.camera_resolution = sl.RESOLUTION.SVGA - init_params.camera_fps = 30 - - err = self.zed_camera.open(init_params) - if err != sl.ERROR_CODE.SUCCESS: - self.get_logger().error(f'Failed to open ZED camera: {err}') - raise RuntimeError(f'Failed to open ZED camera: {err}') - - self.zed_image = sl.Mat() - self.zed_runtime_params = sl.RuntimeParameters() - self.get_logger().info('ZED camera initialized successfully') - - def _capture_image_from_zed(self) -> Optional[np.ndarray]: - if self.zed_camera.grab(self.zed_runtime_params) == sl.ERROR_CODE.SUCCESS: - self.zed_camera.retrieve_image(self.zed_image, sl.VIEW.LEFT) - image = self.zed_image.get_data() - # Resize image to half size - height, width = image.shape[:2] - resized_image = cv2.resize(image, (width // 2, height // 2)) - return resized_image - return None - - def preprocess_image(self, image: np.ndarray) -> Tuple[torch.Tensor, np.ndarray]: - - if self.sdk_flag_: - bgr_image = cv2.cvtColor(image, cv2.COLOR_BGRA2BGR) - mask = self.yolop_processor.process_image(bgr_image, (64, 48)) - else: - mask = ((image[:, :, 2] > 200) & (image[:, :, 0] < 50) & (image[:, :, 1] < 50)).astype(np.uint8) - mask = cv2.resize(mask, (64, 48)) - - mask_normalized = mask.astype(np.float32) - tensor = torch.from_numpy(mask_normalized).unsqueeze(0).unsqueeze(0) - return tensor.to(self.device), mask - - def image_callback(self, msg: Image) -> None: - self.latest_image = msg - - def timer_callback(self) -> None: - if self.sdk_flag_: - cv_image = self._capture_image_from_zed() - if cv_image is None: - return - from std_msgs.msg import Header - header = Header() - header.stamp = self.get_clock().now().to_msg() - header.frame_id = 'base_link' - else: - if self.latest_image is None: - return - cv_image = self.bridge.imgmsg_to_cv2(self.latest_image, desired_encoding='bgra8') - header = self.latest_image.header - - input_tensor, mask = self.preprocess_image(cv_image) - if self.debug_mode_: - resized_input = cv2.resize(cv2.cvtColor(cv_image, cv2.COLOR_BGRA2BGR), (64, 48)) - resized_input[mask == 1] = [0, 0, 255] - debug_msg = self.bridge.cv2_to_imgmsg(resized_input, encoding='bgr8') - debug_msg.header = header - self.pub_debug_image.publish(debug_msg) - - with torch.no_grad(): - output = self.model(input_tensor) - - output_normalized = output.cpu().numpy().flatten() - output_denormalized = denormalize_waypoints(output_normalized) - output_denormalized_tensor = torch.from_numpy(output_denormalized).unsqueeze(0) - - path_raw_msg = self.create_path_from_output(output_denormalized_tensor, header) - self.pub_raw.publish(path_raw_msg) - - path_smooth_msg = self.apply_bspline_smoothing(output_denormalized_tensor, header) - self.pub.publish(path_smooth_msg) - - def apply_bspline_smoothing(self, output: torch.Tensor, header) -> Path: - waypoints = output.cpu().numpy().reshape(-1, 2) - x = waypoints[:, 0] - y = waypoints[:, 1] - - # s: smoothing factor（値が大きいほど滑らか、0だと補間） - # k: スプラインの次数（3次） - tck, u = splprep([x, y], s=0.1, k=3) - u_new = np.linspace(0, 1, 30) - x_smooth, y_smooth = splev(u_new, tck) - - path_msg = Path() - path_msg.header = header - path_msg.header.frame_id = 'base_link' - - path_msg.poses.append(PoseStamped(header=path_msg.header, pose=Pose(position=Point(x=0.0, y=0.0)))) - path_msg.poses.extend(PoseStamped(header=path_msg.header, pose=Pose(position=Point(x=float(x_smooth[i]), y=float(y_smooth[i])))) for i in range(len(x_smooth))) - - return path_msg - - def create_path_from_output(self, output: torch.Tensor, header) -> Path: - path_msg = Path() - path_msg.header = header - path_msg.header.frame_id = 'base_link' - waypoints = output.cpu().numpy().reshape(-1, 2) - - path_msg.poses.append(PoseStamped(header=path_msg.header, pose=Pose(position=Point(x=0.0, y=0.0)))) - path_msg.poses.extend(PoseStamped(header=path_msg.header, pose=Pose(position=Point(x=float(x), y=float(y)))) for x, y in waypoints) - - return path_msg - - -def main(args=None) -> None: - rclpy.init(args=args) - node = InferenceNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/e2e_planner/e2e_planner/zed_sdk.py b/e2e_planner/e2e_planner/zed_sdk.py deleted file mode 100644 index 9a69395a..00000000 --- a/e2e_planner/e2e_planner/zed_sdk.py +++ /dev/null @@ -1,164 +0,0 @@ -from typing import Optional - -from rclpy.node import Node -from rclpy.qos import qos_profile_sensor_data -from sensor_msgs.msg import PointCloud2, PointField, Image -from cv_bridge import CvBridge -from sensor_msgs_py import point_cloud2 - -import cv2 -import numpy as np - - -class ZedRosMsg: - def __init__(self, node: Node) -> None: - self.latest_pointcloud: Optional[PointCloud2] = None - self.latest_image: Optional[Image] = None - - self.sub_pointcloud = node.create_subscription(PointCloud2, '/zed/zed_node/sim/pointcloud', self.pointcloud_callback, qos_profile_sensor_data) - self.sub_image = node.create_subscription(Image, '/zed/zed_node/rgb/image_rect_color', self.image_callback, qos_profile_sensor_data) - - def pointcloud_callback(self, msg: PointCloud2) -> None: - self.latest_pointcloud = msg - - def image_callback(self, msg: Image) -> None: - self.latest_image = msg - - -class ZedSdk: - def __init__(self, node: Node, sim_flag: bool) -> None: - self._node = node - self._logger = node.get_logger() - self._ros_msg = ZedRosMsg(node) - self._bridge = CvBridge() - self.sim_flag = sim_flag - self._sl = None - self._camera = None - self._image = None - self._pointcloud = None - self._runtime = None - if sim_flag: - self._logger.info('Using ZED camera in simulation mode') - return - - import pyzed.sl as sl - self._sl = sl - self._camera = sl.Camera() - - init_params = sl.InitParameters() - init_params.camera_resolution = sl.RESOLUTION.SVGA - init_params.camera_fps = 30 - init_params.coordinate_units = sl.UNIT.METER - - # depth init - init_params.depth_mode = sl.DEPTH_MODE.NEURAL - # init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE - # init_params.depth_mode = sl.DEPTH_MODE.ULTRA - init_params.depth_minimum_distance = 0.5 - init_params.depth_maximum_distance = 10.0 - - err = self._camera.open(init_params) - if err != sl.ERROR_CODE.SUCCESS: - raise RuntimeError(f'Failed to open ZED camera: {err}') - - self._image = sl.Mat() - self._pointcloud = sl.Mat() - self._runtime = sl.RuntimeParameters() - self._runtime.confidence_threshold = 20 - self._runtime.texture_confidence_threshold = 100 - - self._logger.info('ZED camera initialized successfully') - - def grab(self) -> bool: - if self.sim_flag: - return True - return self._camera.grab(self._runtime) == self._sl.ERROR_CODE.SUCCESS - - def get_image(self) -> Optional[np.ndarray]: - image: Optional[np.ndarray] = None - if self.sim_flag: - msg = self._ros_msg.latest_image - if msg is None: - return None - image = self._bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough') - image = cv2.cvtColor(image, cv2.COLOR_BGR2BGRA) - else: - self._camera.retrieve_image(self._image, self._sl.VIEW.LEFT) - image = self._image.get_data() - height, width = image.shape[:2] - cv2.resize(image, (width // 2, height // 2)) - if image is None: - return None - return image - - def get_pointcloud(self, header) -> Optional[PointCloud2]: - if self.sim_flag: - msg = self._ros_msg.latest_pointcloud - header = msg.header - points_list = [point for point in point_cloud2.read_points(msg, field_names=('x', 'y', 'z', 'rgb'), skip_nans=True)] - pointcloud = np.array(points_list, dtype=[('x', np.float32), ('y', np.float32), ('z', np.float32), ('rgba', np.float32)]) - else: - self._camera.retrieve_measure(self._pointcloud, self._sl.MEASURE.XYZRGBA, self._sl.MEM.CPU) - pointcloud = self._pointcloud.get_data() - filtered_pcl = self.filtered_pointcloud(header, pointcloud) - return filtered_pcl - - - def filtered_pointcloud(self, header, pointcloud: np.ndarray) -> Optional[PointCloud2]: - points = pointcloud.reshape(-1, pointcloud.shape[-1]).astype(np.float32, copy=False) - non_nan = (~np.isnan(points[:, 0])) & (~np.isnan(points[:, 1])) & (~np.isnan(points[:, 2])) - finite = np.isfinite(points[:, 0]) & np.isfinite(points[:, 1]) & np.isfinite(points[:, 2]) - valid = finite & non_nan - points = points[valid] - - # x = points[:, 0] - # in_range = (x >= -3.0) & (x <= 3.0) - # points = points[in_range] - - y = points[:, 1] - in_range = (y >= 0.0) & (y <= 0.5) - points = points[in_range] - - # z = points[:, 2] - # in_range = (z<=7) - # points = points[in_range] - - # if points.size == 0: - # return None - # min_idx = np.argmin(points[:, 2]) - # min_x = points[min_idx, 0] - # min_z = points[min_idx, 2] - # in_range = (np.abs(points[:, 0] - min_x) <= 0.5) & (np.abs(points[:, 2] - min_z) <= 0.5) - # points = points[in_range] - - if points.size == 0: - return None - - if points.shape[0] > 1024: - indices = np.random.choice(points.shape[0], 1024, replace=False) - points = points[indices] - - if points.shape[1] >= 4: - rgba = points[:, 3].view(np.uint32) - else: - rgba = np.zeros(points.shape[0], dtype=np.uint32) - cloud = np.zeros(points.shape[0], dtype=[ - ('x', np.float32), - ('y', np.float32), - ('z', np.float32), - ('rgba', np.uint32), - ]) - cloud['x'] = points[:, 2] - cloud['y'] = -1 * points[:, 0] - cloud['z'] = -1 * points[:, 1] - cloud['rgba'] = rgba - - fields = [ - PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), - PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), - PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), - PointField(name='rgba', offset=12, datatype=PointField.UINT32, count=1), - ] - - pointcloud_msg = point_cloud2.create_cloud(header, fields, cloud) - return pointcloud_msg diff --git a/e2e_planner/launch/e2e_planner.launch.py b/e2e_planner/launch/e2e_planner.launch.py deleted file mode 100644 index 71fbea31..00000000 --- a/e2e_planner/launch/e2e_planner.launch.py +++ /dev/null @@ -1,35 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - -def launch_setup(context, *args, **kwargs): - sim_flag = LaunchConfiguration('sim_flag').perform(context) - - if sim_flag.lower() == 'true': - executable_name = 'inference_node_sim.py' - else: - executable_name = 'inference_node' - - inference_node = Node( - package='e2e_planner', - executable=executable_name, - name='inference_node', - output='screen', - parameters=[{ - 'model_name': 'e2e_model.pt', - 'interval_ms': 100, - }] - ) - - return [inference_node] - -def generate_launch_description(): - return LaunchDescription([ - DeclareLaunchArgument( - 'sim_flag', - default_value='false', - description='Flag to use simulation inference node' - ), - OpaqueFunction(function=launch_setup) - ]) diff --git a/e2e_planner/package.xml b/e2e_planner/package.xml deleted file mode 100644 index 98dff090..00000000 --- a/e2e_planner/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - e2e_planner - 0.0.1 - End-to-end learning path planner - kyo yamashita - MIT - - rclpy - sensor_msgs - nav_msgs - cv_bridge - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/e2e_planner/resource/e2e_planner b/e2e_planner/resource/e2e_planner deleted file mode 100644 index e69de29b..00000000 diff --git a/e2e_planner/runs/.gitignore b/e2e_planner/runs/.gitignore deleted file mode 100644 index c96a04f0..00000000 --- a/e2e_planner/runs/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -* -!.gitignore \ No newline at end of file diff --git a/e2e_planner/scripts/create_data.py b/e2e_planner/scripts/create_data.py deleted file mode 100755 index 14afd23a..00000000 --- a/e2e_planner/scripts/create_data.py +++ /dev/null @@ -1,283 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Image, Joy, PointCloud2 -from geometry_msgs.msg import PoseWithCovarianceStamped -from cv_bridge import CvBridge -import cv2 -import numpy as np -import os -import time -import csv -import copy -from pathlib import Path -from collections import deque -from typing import Optional, List, Tuple, Deque -from tf_transformations import euler_from_quaternion -from rclpy.qos import qos_profile_sensor_data -import sensor_msgs_py.point_cloud2 as pc2 -import pymap3d as pm -try: - import pyzed.sl as sl - ZED_SDK_AVAILABLE = True -except ImportError: - ZED_SDK_AVAILABLE = False - -SAMPLE_INTERVAL = 0.2 -WAYPOINT_INTERVAL = 0.5 -NUM_WAYPOINTS = 10 - -class Sample: - def __init__(self, image: np.ndarray, timestamp: float, reference_pose: PoseWithCovarianceStamped, point_cloud: Optional[np.ndarray] = None): - self.image: np.ndarray = image - self.timestamp: float = timestamp - self.reference_pose: PoseWithCovarianceStamped = reference_pose - self.point_cloud: Optional[np.ndarray] = point_cloud - self.waypoints: List[Tuple[float, float]] = [] - self.target_times: List[float] = [timestamp + WAYPOINT_INTERVAL * (i + 1) for i in range(NUM_WAYPOINTS)] - -class DataCollectionNode(Node): - def __init__(self) -> None: - super().__init__('data_collection_node') - - self.declare_parameter('sdk_flag', True) - self.sdk_flag_ = self.get_parameter('sdk_flag').value - - self.bridge: CvBridge = CvBridge() - self.latest_image: Optional[Image] = None - self.latest_pose: Optional[PoseWithCovarianceStamped] = None - self.latest_pointcloud: Optional[PointCloud2] = None - - self.samples: List[Sample] = [] - self.pose_history: Deque[Tuple[float, PoseWithCovarianceStamped]] = deque() - self.collected_data: List[Tuple[np.ndarray, List[Tuple[float, float]], Optional[np.ndarray]]] = [] - self.last_sample_time: Optional[float] = None - - self.is_paused: bool = True - self.prev_button_state: int = 0 - - self.zed_camera: Optional[sl.Camera] = None - self.zed_image: Optional[sl.Mat] = None - self.zed_point_cloud: Optional[sl.Mat] = None - self.zed_pose: Optional[sl.Pose] = None - self.zed_runtime_params: Optional[sl.RuntimeParameters] = None - - if self.sdk_flag_: - if not ZED_SDK_AVAILABLE: - self.get_logger().error('ZED SDK not available. Install pyzed package.') - raise RuntimeError('ZED SDK not available') - self._initialize_zed_camera() - else: - self.create_subscription(Image, '/zed/zed_node/rgb/image_rect_color', self.image_callback, qos_profile_sensor_data) - self.create_subscription(PointCloud2, '/zed/zed_node/pointcloud', self.pointcloud_callback, qos_profile_sensor_data) - - # VectorNav pose subscription (used in both SDK and ROS modes) - self.create_subscription(PoseWithCovarianceStamped, '/vectornav/pose', self.pose_callback, qos_profile_sensor_data) - - self.create_subscription(Joy, '/joy', self.joy_callback, 10) - self.create_timer(0.1, self.timer_callback) - - self.get_logger().info('⚪Create data started') - - def _initialize_zed_camera(self) -> None: - self.zed_camera = sl.Camera() - init_params = sl.InitParameters() - init_params.camera_resolution = sl.RESOLUTION.SVGA - init_params.camera_fps = 30 - init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE - init_params.coordinate_units = sl.UNIT.METER - - err = self.zed_camera.open(init_params) - if err != sl.ERROR_CODE.SUCCESS: - self.get_logger().error(f'Failed to open ZED camera: {err}') - raise RuntimeError(f'Failed to open ZED camera: {err}') - - tracking_params = sl.PositionalTrackingParameters() - err = self.zed_camera.enable_positional_tracking(tracking_params) - if err != sl.ERROR_CODE.SUCCESS: - self.get_logger().error(f'Failed to enable positional tracking: {err}') - raise RuntimeError(f'Failed to enable positional tracking: {err}') - - self.zed_image = sl.Mat() - self.zed_point_cloud = sl.Mat() - self.zed_pose = sl.Pose() - self.zed_runtime_params = sl.RuntimeParameters() - self.get_logger().info('ZED camera initialized with tracking and depth sensing') - - def _capture_data_from_zed(self) -> Tuple[Optional[np.ndarray], Optional[np.ndarray]]: - if self.zed_camera.grab(self.zed_runtime_params) != sl.ERROR_CODE.SUCCESS: - return None, None - - self.zed_camera.retrieve_image(self.zed_image, sl.VIEW.LEFT) - image = self.zed_image.get_data() - height, width = image.shape[:2] - resized_image = cv2.resize(image, (width // 2, height // 2)) - - self.zed_camera.retrieve_measure(self.zed_point_cloud, sl.MEASURE.XYZRGBA) - point_cloud = self.zed_point_cloud.get_data() - - return resized_image, point_cloud - - def image_callback(self, msg: Image) -> None: - self.latest_image = msg - - def pose_callback(self, msg: PoseWithCovarianceStamped) -> None: - self.latest_pose = msg - - def pointcloud_callback(self, msg: PointCloud2) -> None: - self.latest_pointcloud = msg - - def _convert_pointcloud2_to_array(self, pointcloud_msg: PointCloud2) -> np.ndarray: - points_list = [[point[0], point[1], point[2], point[3]] - for point in pc2.read_points(pointcloud_msg, skip_nans=True, field_names=("x", "y", "z", "rgb"))] - return np.array(points_list, dtype=np.float32) - - def joy_callback(self, msg: Joy) -> None: - if len(msg.buttons) > 2: - current_button_state = msg.buttons[2] - if current_button_state == 1 and self.prev_button_state == 0: - self.is_paused = not self.is_paused - if self.is_paused: - self.get_logger().info('⏸️ Data collection paused') - else: - self.pose_history.clear() - self.samples.clear() - self.last_sample_time = None - self.get_logger().info('▶️ Data collection resumed') - self.prev_button_state = current_button_state - - def timer_callback(self) -> None: - if self.is_paused: - return - - current_time = time.time() - - if self.latest_pose is not None: - self.pose_history.append((current_time, copy.deepcopy(self.latest_pose))) - - if self.sdk_flag_: - image, point_cloud = self._capture_data_from_zed() - if image is None or self.latest_pose is None: - return - - if self.last_sample_time is None or current_time - self.last_sample_time >= SAMPLE_INTERVAL: - sample = Sample(image, current_time, copy.deepcopy(self.latest_pose), point_cloud) - self.samples.append(sample) - self.last_sample_time = current_time - else: - if self.latest_image is not None and self.latest_pose is not None: - if self.last_sample_time is None or current_time - self.last_sample_time >= SAMPLE_INTERVAL: - cv_image = self.bridge.imgmsg_to_cv2(self.latest_image, desired_encoding='bgra8') - point_cloud = self._convert_pointcloud2_to_array(self.latest_pointcloud) if self.latest_pointcloud is not None else None - sample = Sample(cv_image, current_time, copy.deepcopy(self.latest_pose), point_cloud) - self.samples.append(sample) - self.last_sample_time = current_time - - for sample in self.samples: - self.collect_waypoints_for_sample(sample) - - completed_samples = [sample for sample in self.samples if len(sample.waypoints) == NUM_WAYPOINTS] - for sample in completed_samples: - self.collected_data.append((sample.image, sample.waypoints, sample.point_cloud)) - self.get_logger().info(f'🟡Collected data #{len(self.collected_data)}') - - self.samples = [sample for sample in self.samples if len(sample.waypoints) < NUM_WAYPOINTS] - - self.cleanup_pose_history() - - def collect_waypoints_for_sample(self, sample: Sample) -> None: - for i in range(len(sample.waypoints), NUM_WAYPOINTS): - target_time = sample.target_times[i] - pose = self.find_closest_pose(target_time) - if pose is not None: - x, y = self.transform_to_robot_frame(sample.reference_pose, pose) - sample.waypoints.append((x, y)) - else: - break - - def find_closest_pose(self, target_time: float) -> Optional[PoseWithCovarianceStamped]: - for t, pose in self.pose_history: - if t >= target_time: - return pose - return None - - def cleanup_pose_history(self) -> None: - if not self.samples or not self.pose_history: - return - incomplete_samples = [s for s in self.samples if len(s.waypoints) < NUM_WAYPOINTS] - if not incomplete_samples: - return - min_target_time = min(sample.target_times[len(sample.waypoints)] for sample in incomplete_samples) - while self.pose_history and self.pose_history[0][0] < min_target_time: - self.pose_history.popleft() - - def transform_to_robot_frame(self, reference_pose: PoseWithCovarianceStamped, current_pose: PoseWithCovarianceStamped) -> Tuple[float, float]: - x0_ecef = reference_pose.pose.pose.position.x - y0_ecef = reference_pose.pose.pose.position.y - z0_ecef = reference_pose.pose.pose.position.z - lat0, lon0, alt0 = pm.ecef2geodetic(x0_ecef, y0_ecef, z0_ecef) - - q0 = reference_pose.pose.pose.orientation - _, _, yaw0 = euler_from_quaternion([q0.x, q0.y, q0.z, q0.w]) - - xi_ecef = current_pose.pose.pose.position.x - yi_ecef = current_pose.pose.pose.position.y - zi_ecef = current_pose.pose.pose.position.z - e, n, u = pm.ecef2enu(xi_ecef, yi_ecef, zi_ecef, lat0, lon0, alt0) - - x_robot = -e * np.sin(yaw0) + n * np.cos(yaw0) - y_robot = -e * np.cos(yaw0) - n * np.sin(yaw0) - - return x_robot, y_robot - - def save_data(self) -> None: - if len(self.collected_data) == 0: - self.get_logger().info('🔴No data to save') - return - - package_root = Path(__file__).parent.parent - data_base_dir = package_root / 'data' - timestamp = time.strftime('%Y%m%d_%H%M%S') - dataset_dir = data_base_dir / f'{timestamp}_dataset' - images_dir = dataset_dir / 'images' - path_dir = dataset_dir / 'path' - pointclouds_dir = dataset_dir / 'pointclouds' - - images_dir.mkdir(parents=True, exist_ok=True) - path_dir.mkdir(parents=True, exist_ok=True) - pointclouds_dir.mkdir(parents=True, exist_ok=True) - - for idx, (image, waypoints, point_cloud) in enumerate(self.collected_data, start=1): - image_path = images_dir / f'{idx:05d}.png' - waypoints_path = path_dir / f'{idx:05d}.csv' - pointcloud_path = pointclouds_dir / f'{idx:05d}.npy' - - cv2.imwrite(str(image_path), image) - - with open(str(waypoints_path), 'w', newline='') as csvfile: - csv_writer = csv.writer(csvfile) - csv_writer.writerow(['x', 'y']) - for x, y in waypoints: - csv_writer.writerow([x, y]) - - if point_cloud is not None: - np.save(str(pointcloud_path), point_cloud) - - self.get_logger().info(f'🔵Saved {len(self.collected_data)} samples to {dataset_dir}') - -def main(args=None) -> None: - rclpy.init(args=args) - node = DataCollectionNode() - - try: - rclpy.spin(node) - except KeyboardInterrupt: - node.get_logger().info('Interrupted by user') - finally: - node.save_data() - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/e2e_planner/scripts/network.py b/e2e_planner/scripts/network.py deleted file mode 100644 index 587eba0b..00000000 --- a/e2e_planner/scripts/network.py +++ /dev/null @@ -1,25 +0,0 @@ -import torch -import torch.nn as nn - -class Network(nn.Module): - def __init__(self, num_waypoints: int = 10): - super(Network, self).__init__() - - self.conv1 = nn.Conv2d(1, 32, kernel_size=8, stride=4) - self.conv2 = nn.Conv2d(32, 64, kernel_size=3, stride=2) - self.conv3 = nn.Conv2d(64, 64, kernel_size=3, stride=1) - self.fc1 = nn.Linear(960, 512) - self.fc2 = nn.Linear(512, num_waypoints * 2) - self.flatten = nn.Flatten() - - self.relu = nn.ReLU() - - def forward(self, x: torch.Tensor) -> torch.Tensor: - x = self.relu(self.conv1(x)) - x = self.relu(self.conv2(x)) - x = self.relu(self.conv3(x)) - x = self.flatten(x) - x = self.relu(self.fc1(x)) - x = self.fc2(x) - - return x diff --git a/e2e_planner/scripts/train.py b/e2e_planner/scripts/train.py deleted file mode 100755 index fdf3cedd..00000000 --- a/e2e_planner/scripts/train.py +++ /dev/null @@ -1,191 +0,0 @@ -#!/usr/bin/env python3 - -import sys -import yaml -import torch -import torch.nn as nn -from torch.utils.data import Dataset, DataLoader, random_split -from torch.utils.tensorboard import SummaryWriter -import cv2 -import csv -from pathlib import Path -import numpy as np -from typing import Tuple -from tqdm import tqdm -from schedulefree import RAdamScheduleFree -from network import Network - -IMAGE_WIDTH = 64 -IMAGE_HEIGHT = 48 -NUM_WAYPOINTS = 10 - - -class E2EDataset(Dataset): - def __init__(self, dataset_path: Path): - self.dataset_path = dataset_path - self.mask_images_dir = dataset_path / 'mask_images' - self.path_dir = dataset_path / 'path' - self.mask_files = sorted(list(self.mask_images_dir.glob('*.png'))) - - def __len__(self) -> int: - return len(self.mask_files) - - def __getitem__(self, idx: int) -> Tuple[torch.Tensor, torch.Tensor]: - mask_file = self.mask_files[idx] - csv_file = self.path_dir / f'{mask_file.stem}.csv' - - mask_bgr = cv2.imread(str(mask_file), cv2.IMREAD_COLOR) - - with open(csv_file, 'r') as f: - reader = csv.DictReader(f) - waypoints = [[float(row['x']), float(row['y'])] for row in reader] - - mask_binary = ((mask_bgr[:, :, 2] > 200) & (mask_bgr[:, :, 0] < 50) & (mask_bgr[:, :, 1] < 50)).astype(np.uint8) - - cropped_mask = mask_binary[:, 40:440] - resized_mask = cv2.resize(cropped_mask, (IMAGE_WIDTH, IMAGE_HEIGHT), interpolation=cv2.INTER_NEAREST) - - mask_normalized = resized_mask.astype(np.float32) - mask_tensor = torch.from_numpy(mask_normalized).unsqueeze(0) - - waypoints_tensor = torch.tensor(waypoints, dtype=torch.float32).flatten() - waypoints_tensor[0::2] = waypoints_tensor[0::2] / 5.0 - 1.0 - waypoints_tensor[1::2] = (waypoints_tensor[1::2] + 3.0) / 3.0 - 1.0 - - return mask_tensor, waypoints_tensor - -class Config: - def __init__(self, config_path: Path, package_root: Path): - self.package_root = package_root - - with open(config_path, 'r') as f: - config_dict = yaml.safe_load(f) - - self.epochs = config_dict['epochs'] - self.batch_size = config_dict['batch_size'] - self.learning_rate = config_dict['learning_rate'] - self.num_workers = config_dict['num_workers'] - self.weight_file = config_dict['weight_file'] - - self.weights_dir = package_root / 'weights' - self.weights_dir.mkdir(exist_ok=True) - - self.logs_dir = package_root / 'runs' - - self.device = torch.device('cuda') - -class Trainer: - def __init__(self, dataset_path: Path, config: Config): - self.config = config - self.device = config.device - - dataset = E2EDataset(dataset_path) - train_size = int(0.8 * len(dataset)) - val_size = len(dataset) - train_size - train_dataset, val_dataset = random_split(dataset, [train_size, val_size]) - - self.train_loader = DataLoader( - train_dataset, - batch_size=config.batch_size, - shuffle=True, - num_workers=config.num_workers - ) - self.val_loader = DataLoader( - val_dataset, - batch_size=config.batch_size, - shuffle=False, - num_workers=config.num_workers - ) - - self.model = Network(num_waypoints=NUM_WAYPOINTS).to(self.device) - self.optimizer = torch.optim.AdamW(self.model.parameters(), lr=config.learning_rate) - self.mseloss = nn.MSELoss() - self.writer = SummaryWriter(log_dir=str(config.logs_dir)) - - self.best_val_loss = float('inf') - - print(f'Using device: {self.device}') - print(f'Train size: {len(train_dataset)}, Val size: {len(val_dataset)}') - - def validate(self) -> float: - self.model.eval() - total_loss = 0.0 - - with torch.no_grad(): - pbar = tqdm(self.val_loader, desc='Validation') - for images, waypoints in pbar: - images = images.to(self.device) - waypoints = waypoints.to(self.device) - - outputs = self.model(images) - - loss = self.mseloss(outputs, waypoints) - total_loss += loss.item() - pbar.set_postfix({'loss': f'{loss.item():.6f}'}) - - return total_loss / len(self.val_loader) - - def save_checkpoint(self, val_loss: float) -> None: - if val_loss < self.best_val_loss: - self.best_val_loss = val_loss - weight_path = self.config.weights_dir / self.config.weight_file - scripted_model = torch.jit.script(self.model) - scripted_model.save(str(weight_path)) - print(f'Best model saved: {weight_path} (val_loss: {val_loss:.6f})') - - def train(self, epochs: int) -> None: - for epoch in range(1, epochs + 1): - self.model.train() - total_train_loss = 0.0 - - pbar = tqdm(self.train_loader, desc=f'Epoch {epoch} [Train]') - for images, waypoints in pbar: - images = images.to(self.device) - waypoints = waypoints.to(self.device) - - self.optimizer.zero_grad() - outputs = self.model(images) - - loss = self.mseloss(outputs, waypoints) - loss.backward() - self.optimizer.step() - - total_train_loss += loss.item() - pbar.set_postfix({'loss': f'{loss.item():.6f}'}) - - train_loss = total_train_loss / len(self.train_loader) - val_loss = self.validate() - - self.writer.add_scalar('Loss/train', train_loss, epoch) - self.writer.add_scalar('Loss/val', val_loss, epoch) - - print(f'Epoch [{epoch}/{epochs}], Train Loss: {train_loss:.6f}, Val Loss: {val_loss:.6f}') - - self.save_checkpoint(val_loss) - - self.writer.close() - -def main() -> None: - if len(sys.argv) != 2: - print('Usage: python3 train.py ') - sys.exit(1) - - dataset_path = Path(sys.argv[1]) - if not dataset_path.exists(): - print(f'Dataset path does not exist: {dataset_path}') - sys.exit(1) - - script_dir = Path(__file__).parent - package_root = script_dir.parent - config_path = package_root / 'config' / 'train.yaml' - - config = Config(config_path, package_root) - trainer = Trainer(dataset_path, config) - - print(f'Starting training for {config.epochs} epochs') - trainer.train(config.epochs) - - print('Training complete.') - -if __name__ == '__main__': - main() diff --git a/e2e_planner/scripts/util/__init__.py b/e2e_planner/scripts/util/__init__.py deleted file mode 100644 index e38709a7..00000000 --- a/e2e_planner/scripts/util/__init__.py +++ /dev/null @@ -1,4 +0,0 @@ -from .slit_aug import crop_images, rotate_waypoints, augment -from .yolop_processor import YOLOPv2Processor - -__all__ = ['crop_images', 'rotate_waypoints', 'augment', 'YOLOPv2Processor'] diff --git a/e2e_planner/scripts/util/slit_aug.py b/e2e_planner/scripts/util/slit_aug.py deleted file mode 100644 index 6f96153e..00000000 --- a/e2e_planner/scripts/util/slit_aug.py +++ /dev/null @@ -1,37 +0,0 @@ -import numpy as np -from typing import List, Tuple - - -def crop_images(image: np.ndarray) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: - center_crop = image[:, 40:440] - right_crop = image[:, 80:480] - left_crop = image[:, 0:400] - return center_crop, right_crop, left_crop - - -def rotate_waypoints(waypoints: List[List[float]], angle: float) -> List[List[float]]: - cos_theta = np.cos(angle) - sin_theta = np.sin(angle) - rotation_matrix = np.array([[cos_theta, -sin_theta], - [sin_theta, cos_theta]]) - - rotated_waypoints = [] - for waypoint in waypoints: - rotated = rotation_matrix @ np.array(waypoint) - rotated_waypoints.append(rotated.tolist()) - - return rotated_waypoints - - -def augment(image: np.ndarray, waypoints: List[List[float]]) -> List[Tuple[np.ndarray, List[List[float]]]]: - center_crop, right_crop, left_crop = crop_images(image) - - center_waypoints = waypoints - right_waypoints = rotate_waypoints(waypoints, 0.1745) - left_waypoints = rotate_waypoints(waypoints, -0.1745) - - return [ - (center_crop, center_waypoints), - (right_crop, right_waypoints), - (left_crop, left_waypoints) - ] diff --git a/e2e_planner/scripts/util/yolop_processor.py b/e2e_planner/scripts/util/yolop_processor.py deleted file mode 100644 index 3ea876a1..00000000 --- a/e2e_planner/scripts/util/yolop_processor.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python3 - -import cv2 -import numpy as np -import torch -from pathlib import Path -from typing import Tuple - - -class YOLOPv2Processor: - def __init__(self, model_path: Path, device: torch.device): - self.device = device - self.input_shape = (640, 640) - - if model_path.exists(): - self.model = torch.jit.load(str(model_path), map_location=device) - self.model.to(device) - self.model.eval() - else: - raise FileNotFoundError(f'YOLOPv2 model not found: {model_path}') - - def letterbox(self, img: np.ndarray, new_shape: Tuple[int, int], color: Tuple[int, int, int] = (114, 114, 114), stride: int = 32) -> Tuple[np.ndarray, float, Tuple[float, float]]: - shape = img.shape[:2] - r = min(new_shape[0] / shape[0], new_shape[1] / shape[1]) - - new_unpad = int(round(shape[1] * r)), int(round(shape[0] * r)) - dw, dh = new_shape[1] - new_unpad[0], new_shape[0] - new_unpad[1] - dw, dh = np.mod(dw, stride) / 2, np.mod(dh, stride) / 2 - - if shape[::-1] != new_unpad: - img = cv2.resize(img, new_unpad, interpolation=cv2.INTER_LINEAR) - - top, bottom = int(round(dh - 0.1)), int(round(dh + 0.1)) - left, right = int(round(dw - 0.1)), int(round(dw + 0.1)) - - img = cv2.copyMakeBorder( - img, top, bottom, left, right, - cv2.BORDER_CONSTANT, value=color - ) - - return img, r, (dw, dh) - - def lane_line_mask(self, ll = None): - ll_seg_mask = torch.nn.functional.interpolate(ll, scale_factor=2, mode='bilinear') - ll_seg_mask = torch.round(ll_seg_mask).squeeze(1) - ll_seg_mask = ll_seg_mask.int().squeeze().cpu().numpy() - return ll_seg_mask - - def process_image(self, image: np.ndarray, target_size: Tuple[int, int]) -> np.ndarray: - img_resized, ratio, (pad_left, pad_top) = self.letterbox(image, self.input_shape) - - img = img_resized.astype(np.float32) / 255.0 - img = torch.from_numpy(np.transpose(img, (2, 0, 1))).unsqueeze(0).to(self.device) - - with torch.no_grad(): - outputs = self.model(img) - [pred, anchor_grid], seg, ll = outputs - - ll_seg_mask = self.lane_line_mask(ll) - - resized_mask = cv2.resize( - ll_seg_mask, - target_size, - interpolation=cv2.INTER_NEAREST - ) - - return resized_mask diff --git a/e2e_planner/setup.cfg b/e2e_planner/setup.cfg deleted file mode 100644 index 64caacd5..00000000 --- a/e2e_planner/setup.cfg +++ /dev/null @@ -1,25 +0,0 @@ -[metadata] -name = e2e_planner -version = 0.0.1 -maintainer = kyo yamashita -maintainer_email = s21c1135sc@s.chibakoudai.jp -description = End-to-end learning path planner - -[options] -zip_safe = True -packages = find: -install_requires = - setuptools - -[options.packages.find] -exclude = - test - -[options.entry_points] -console_scripts = - inference_node = e2e_planner.inference_node:main - -[develop] -script_dir=$base/lib/e2e_planner -[install] -install_scripts=$base/lib/e2e_planner diff --git a/e2e_planner/setup.py b/e2e_planner/setup.py deleted file mode 100644 index 658b03f7..00000000 --- a/e2e_planner/setup.py +++ /dev/null @@ -1,32 +0,0 @@ -from setuptools import setup -import os -from glob import glob - -package_name = 'e2e_planner' - -setup( - name=package_name, - version='0.0.1', - packages=[package_name, 'util'], - package_dir={'util': 'scripts/util'}, - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), - (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), - (os.path.join('share', package_name, 'weights'), glob('weights/*.pt')), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='kyo yamashita', - maintainer_email='s21c1135sc@s.chibakoudai.jp', - description='End-to-end learning path planner', - license='TODO: License declaration', - entry_points={ - 'console_scripts': [ - 'inference_node = e2e_planner.inference_node:main', - 'inference_node_sim.py = e2e_planner.inference_node_sim:main', - ], - }, -) diff --git a/e2e_planner/weights/.gitignore b/e2e_planner/weights/.gitignore deleted file mode 100644 index c96a04f0..00000000 --- a/e2e_planner/weights/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -* -!.gitignore \ No newline at end of file diff --git a/frenet_planner/CMakeLists.txt b/frenet_planner/CMakeLists.txt deleted file mode 100644 index 9acfea77..00000000 --- a/frenet_planner/CMakeLists.txt +++ /dev/null @@ -1,44 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(frenet_planner) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -find_package(Eigen3 REQUIRED) - -ament_auto_add_library(frenet_planner_node SHARED - src/frenet_planner_node.cpp - src/frenet_planner.cpp - src/reference_curve.cpp - src/risk_calculator.cpp - src/obstacle_detect.cpp -) -target_compile_features(frenet_planner_node PUBLIC c_std_99 cxx_std_17) -target_include_directories(frenet_planner_node PUBLIC - $ - $ - ${EIGEN3_INCLUDE_DIRS} -) -target_link_libraries(frenet_planner_node Eigen3::Eigen) -target_compile_definitions(frenet_planner_node PRIVATE "FRENET_PLANNER_BUILDING_LIBRARY") - -ament_export_dependencies(Eigen3) -ament_export_include_directories(${EIGEN3_INCLUDE_DIRS}) - -install( - DIRECTORY include/ - DESTINATION include -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package() diff --git a/frenet_planner/include/frenet_planner/frenet_planner.hpp b/frenet_planner/include/frenet_planner/frenet_planner.hpp deleted file mode 100644 index 1f154d4f..00000000 --- a/frenet_planner/include/frenet_planner/frenet_planner.hpp +++ /dev/null @@ -1,226 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include "frenet_planner/obstacle_detect.hpp" -#include "frenet_planner/reference_curve.hpp" -#include "frenet_planner/risk_calculator.hpp" - -namespace frenet_planner { - -class QuinticPolynomial { -public: - QuinticPolynomial(double xs, double vxs, double axs, double xe, double vxe, double axe, double T) { - a0 = xs; - a1 = vxs; - a2 = axs / 2.0; - - Eigen::Matrix3d A; - A << T*T*T, T*T*T*T, T*T*T*T*T, - 3*T*T, 4*T*T*T, 5*T*T*T*T, - 6*T, 12*T*T, 20*T*T*T; - - Eigen::Vector3d b; - b << xe - a0 - a1*T - a2*T*T, - vxe - a1 - 2*a2*T, - axe - 2*a2; - - Eigen::Vector3d x = A.colPivHouseholderQr().solve(b); - - a3 = x(0); - a4 = x(1); - a5 = x(2); - } - - inline double calc_point(double t) const { - return a0 + a1*t + a2*t*t + a3*t*t*t + a4*t*t*t*t + a5*t*t*t*t*t; - } - - inline double calc_first_derivative(double t) const { - return a1 + 2*a2*t + 3*a3*t*t + 4*a4*t*t*t + 5*a5*t*t*t*t; - } - - inline double calc_second_derivative(double t) const { - return 2*a2 + 6*a3*t + 12*a4*t*t + 20*a5*t*t*t; - } - - inline double calc_third_derivative(double t) const { - return 6*a3 + 24*a4*t + 60*a5*t*t; - } - -private: - double a0, a1, a2, a3, a4, a5; -}; - -class QuarticPolynomial { -public: - QuarticPolynomial(double xs, double vxs, double axs, double vxe, double axe, double T) { - a0 = xs; - a1 = vxs; - a2 = axs / 2.0; - - Eigen::Matrix2d A; - A << 3*T*T, 4*T*T*T, - 6*T, 12*T*T; - - Eigen::Vector2d b; - b << vxe - a1 - 2*a2*T, - axe - 2*a2; - - Eigen::Vector2d x = A.colPivHouseholderQr().solve(b); - - a3 = x(0); - a4 = x(1); - } - - inline double calc_point(double t) const { - return a0 + a1*t + a2*t*t + a3*t*t*t + a4*t*t*t*t; - } - - inline double calc_first_derivative(double t) const { - return a1 + 2*a2*t + 3*a3*t*t + 4*a4*t*t*t; - } - - inline double calc_second_derivative(double t) const { - return 2*a2 + 6*a3*t + 12*a4*t*t; - } - - inline double calc_third_derivative(double t) const { - return 6*a3 + 24*a4*t; - } - -private: - double a0, a1, a2, a3, a4; -}; - -struct VehicleState { - double s; - double s_dot; - double s_ddot; - double d; - double d_dot; - double d_ddot; -}; - -struct TrajectoryPoint { - double s; - double d; - double x; - double y; - double yaw; - double curvature; - double velocity; - double acceleration; -}; - -struct FrenetState { - double s; - double s_dot; - double s_ddot; - double d; - double d_s; - double d_ss; -}; - -struct CartesianState { - double x; - double y; - double yaw; - double curvature; - double velocity; - double acceleration; -}; - -struct FrenetTrajectory { - std::vector points; - std::vector t; - std::vector s; - std::vector d; - std::vector d_s; - std::vector d_ss; - std::vector s_dot; - std::vector d_dot; - std::vector s_ddot; - std::vector d_ddot; - std::vector s_dddot; - std::vector d_dddot; - double cost; - bool valid; -}; - -class FrenetPlanner { -public: - explicit FrenetPlanner(RiskCalculator& risk_calculator); - - void set_parameters( - double max_speed, - double max_accel, - double max_curvature, - double dt, - double d_t_s, - int n_s_sample, - double target_speed, - double safety_margin - ); - - nav_msgs::msg::Path plan_local_path( - const nav_msgs::msg::Path::SharedPtr reference_path, - const std::vector& obstacles, - const geometry_msgs::msg::Twist& current_twist - ); - -private: - double max_speed_; - double max_accel_; - double max_curvature_; - const double max_road_width_left_ = 4.0; - const double max_road_width_right_ = 4.0; - const double d_road_width_ = 0.5; - double dt_; - double d_t_s_; - int n_s_sample_; - double target_speed_; - const double robot_radius_ = 0.5; - double safety_margin_; - - RiskCalculator& risk_calculator_; - - FrenetTrajectory generate_lateral_trajectory( - const VehicleState& current_state, - double target_d, - const FrenetTrajectory& longitudinal_traj); - - FrenetTrajectory generate_longitudinal_trajectory( - const VehicleState& current_state, - double target_speed, - double T); - - bool select_best_path( - const VehicleState& current_state, - const nav_msgs::msg::Path::SharedPtr& reference_path, - const std::vector& obstacles, - FrenetTrajectory& best_path, - size_t& total_count, - size_t& valid_count - ); - - void get_frenet_state( - const nav_msgs::msg::Path::SharedPtr& path, - const geometry_msgs::msg::Twist& current_twist, - VehicleState& state - ); - - void frenet_to_cartesian( - const ReferenceCurve& reference_curve, - const FrenetState& state, - CartesianState& out - ); -}; - -} // namespace frenet_planner diff --git a/frenet_planner/include/frenet_planner/frenet_planner_node.hpp b/frenet_planner/include/frenet_planner/frenet_planner_node.hpp deleted file mode 100644 index 225d4010..00000000 --- a/frenet_planner/include/frenet_planner/frenet_planner_node.hpp +++ /dev/null @@ -1,55 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include "frenet_planner/visibility_control.h" -#include "frenet_planner/frenet_planner.hpp" -#include "frenet_planner/obstacle_detect.hpp" - -namespace frenet_planner { - -class FrenetPlannerNode : public rclcpp::Node { -public: - FRENET_PLANNER_PUBLIC - explicit FrenetPlannerNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - - FRENET_PLANNER_PUBLIC - explicit FrenetPlannerNode(const std::string& name_space, const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - -private: - rclcpp::Subscription::SharedPtr sub_path_; - rclcpp::Subscription::SharedPtr sub_pointcloud_; - rclcpp::Subscription::SharedPtr sub_velocity_body_; - - rclcpp::Publisher::SharedPtr pub_local_path_; - rclcpp::Publisher::SharedPtr pub_obstacle_markers_; - - rclcpp::QoS qos_{ - rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data), - rmw_qos_profile_sensor_data - }; - - - nav_msgs::msg::Path::SharedPtr global_path_; - sensor_msgs::msg::PointCloud2::SharedPtr pointcloud_; - nav_msgs::msg::Path::SharedPtr local_path_; - geometry_msgs::msg::Twist current_twist_; - - RiskCalculator risk_calculator_; - FrenetPlanner frenet_planner_; - ObstacleDetector obstacle_detector_; - - const double caster_max_angle_; - - void path_callback(const nav_msgs::msg::Path::SharedPtr msg); - void pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void velocity_body_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); -}; - -} // namespace frenet_planner diff --git a/frenet_planner/include/frenet_planner/obstacle_detect.hpp b/frenet_planner/include/frenet_planner/obstacle_detect.hpp deleted file mode 100644 index 93cced18..00000000 --- a/frenet_planner/include/frenet_planner/obstacle_detect.hpp +++ /dev/null @@ -1,57 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace frenet_planner { - -struct Obstacle { - double x; - double y; - double s; - double d; -}; - -class ObstacleDetector { -public: - ObstacleDetector(); - - std::vector detect_obstacles( - const sensor_msgs::msg::PointCloud2::SharedPtr pointcloud - ); - - visualization_msgs::msg::MarkerArray obstacles_to_marker_array( - const std::vector& obstacles, - const std::string& frame_id, - const rclcpp::Time& stamp - ) const; - -private: - double voxel_size_; - - struct VoxelKey { - int x, y, z; - - bool operator==(const VoxelKey& other) const { - return x == other.x && y == other.y && z == other.z; - } - }; - - struct VoxelKeyHash { - std::size_t operator()(const VoxelKey& key) const { - std::size_t h1 = std::hash{}(key.x); - std::size_t h2 = std::hash{}(key.y); - std::size_t h3 = std::hash{}(key.z); - return h1 ^ (h2 << 1) ^ (h3 << 2); - } - }; - - VoxelKey get_voxel_key(double x, double y, double z) const; - Obstacle voxel_center(const VoxelKey& key) const; -}; - -} // namespace frenet_planner diff --git a/frenet_planner/include/frenet_planner/reference_curve.hpp b/frenet_planner/include/frenet_planner/reference_curve.hpp deleted file mode 100644 index 81db57d4..00000000 --- a/frenet_planner/include/frenet_planner/reference_curve.hpp +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#include - -namespace frenet_planner { - -class ReferenceCurve { -public: - ReferenceCurve(const std::vector& x, const std::vector& y); - bool valid() const; - double max_s() const; - void sample(double s, - double& x, - double& y, - double& yaw, - double& kappa, - double& dkappa) const; - -private: - std::vector s_; - std::vector x_; - std::vector y_; - std::vector yaw_; - std::vector kappa_; - std::vector dkappa_; - bool valid_{false}; -}; - -} // namespace frenet_planner diff --git a/frenet_planner/include/frenet_planner/risk_calculator.hpp b/frenet_planner/include/frenet_planner/risk_calculator.hpp deleted file mode 100644 index 6425620f..00000000 --- a/frenet_planner/include/frenet_planner/risk_calculator.hpp +++ /dev/null @@ -1,43 +0,0 @@ -#pragma once - -#include -#include -#include "frenet_planner/obstacle_detect.hpp" - -namespace frenet_planner { - -struct TrajectoryPoint; -struct FrenetTrajectory; - -class RiskCalculator { -public: - RiskCalculator(); - - void set_parameters( - double k_jerk, - double k_time, - double k_d, - double k_d_s, - double k_s_dot, - double k_lat, - double k_lon, - double target_speed - ); - - double calculate_cost( - const FrenetTrajectory& trajectory, - const std::vector& obstacles - ); - -private: - double k_jerk_; - double k_time_; - double k_d_; - double k_d_s_; - double k_s_dot_; - double k_lateral_ = 1.0; - double k_longitudinal_ = 1.0; - double target_speed_; -}; - -} // namespace frenet_planner diff --git a/frenet_planner/include/frenet_planner/visibility_control.h b/frenet_planner/include/frenet_planner/visibility_control.h deleted file mode 100644 index b535c7dd..00000000 --- a/frenet_planner/include/frenet_planner/visibility_control.h +++ /dev/null @@ -1,32 +0,0 @@ -#ifndef FRENET_PLANNER__VISIBILITY_CONTROL_H_ -#define FRENET_PLANNER__VISIBILITY_CONTROL_H_ - -#if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define FRENET_PLANNER_EXPORT __attribute__ ((dllexport)) - #define FRENET_PLANNER_IMPORT __attribute__ ((dllimport)) - #else - #define FRENET_PLANNER_EXPORT __declspec(dllexport) - #define FRENET_PLANNER_IMPORT __declspec(dllimport) - #endif - #ifdef FRENET_PLANNER_BUILDING_LIBRARY - #define FRENET_PLANNER_PUBLIC FRENET_PLANNER_EXPORT - #else - #define FRENET_PLANNER_PUBLIC FRENET_PLANNER_IMPORT - #endif - #define FRENET_PLANNER_PUBLIC_TYPE FRENET_PLANNER_PUBLIC - #define FRENET_PLANNER_LOCAL -#else - #define FRENET_PLANNER_EXPORT __attribute__ ((visibility("default"))) - #define FRENET_PLANNER_IMPORT - #if __GNUC__ >= 4 - #define FRENET_PLANNER_PUBLIC __attribute__ ((visibility("default"))) - #define FRENET_PLANNER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define FRENET_PLANNER_PUBLIC - #define FRENET_PLANNER_LOCAL - #endif - #define FRENET_PLANNER_PUBLIC_TYPE -#endif - -#endif // FRENET_PLANNER__VISIBILITY_CONTROL_H_ diff --git a/frenet_planner/package.xml b/frenet_planner/package.xml deleted file mode 100644 index cb16cc22..00000000 --- a/frenet_planner/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - frenet_planner - 0.0.0 - Frenet planner library node (no tracking) for local path generation. - kyo yamashita - MIT - - ament_cmake_auto - - rclcpp - geometry_msgs - nav_msgs - sensor_msgs - std_msgs - visualization_msgs - Eigen3 - - - ament_cmake - - diff --git a/frenet_planner/src/frenet_planner.cpp b/frenet_planner/src/frenet_planner.cpp deleted file mode 100644 index ce624628..00000000 --- a/frenet_planner/src/frenet_planner.cpp +++ /dev/null @@ -1,404 +0,0 @@ -#include "frenet_planner/frenet_planner.hpp" -#include -#include -#include -#include - -namespace frenet_planner { - -FrenetPlanner::FrenetPlanner(RiskCalculator& risk_calculator) - : risk_calculator_(risk_calculator) { -} - -void FrenetPlanner::set_parameters( - double max_speed, - double max_accel, - double max_curvature, - double dt, - double d_t_s, - int n_s_sample, - double target_speed, - double safety_margin -) { - max_speed_ = 10.0; - max_accel_ = max_accel; - max_curvature_ = max_curvature; - dt_ = dt; - d_t_s_ = d_t_s; - n_s_sample_ = n_s_sample; - target_speed_ = target_speed; - safety_margin_ = safety_margin; -} - -nav_msgs::msg::Path FrenetPlanner::plan_local_path( - const nav_msgs::msg::Path::SharedPtr reference_path, - const std::vector& obstacles, - const geometry_msgs::msg::Twist& current_twist -) { - nav_msgs::msg::Path local_path; - local_path.header = reference_path->header; - - VehicleState current_state; - get_frenet_state(reference_path, current_twist, current_state); - FrenetTrajectory best_path; - size_t total_count = 0; - size_t valid_count = 0; - const bool has_best = select_best_path( - current_state, - reference_path, - obstacles, - best_path, - total_count, - valid_count - ); - - RCLCPP_INFO(rclcpp::get_logger("frenet_planner"), - "Valid paths: %zu / %zu", valid_count, total_count); - - if (has_best && best_path.valid) { - RCLCPP_INFO(rclcpp::get_logger("frenet_planner"), - "Best path found with %zu points, cost=%.2f", - best_path.points.size(), best_path.cost); - for (const auto& point : best_path.points) { - geometry_msgs::msg::PoseStamped pose; - pose.header = local_path.header; - pose.pose.position.x = point.x; - pose.pose.position.y = point.y; - pose.pose.position.z = 0.0; - local_path.poses.push_back(pose); - } - } else { - RCLCPP_WARN(rclcpp::get_logger("frenet_planner"), - "No valid path found!"); - } - - return local_path; -} - -FrenetTrajectory FrenetPlanner::generate_longitudinal_trajectory( - const VehicleState& current_state, - double target_speed, - double T) { - FrenetTrajectory trajectory; - trajectory.valid = true; - - size_t n_points = static_cast(T / dt_) + 1; - trajectory.t.reserve(n_points); - trajectory.s.reserve(n_points); - trajectory.s_dot.reserve(n_points); - trajectory.s_ddot.reserve(n_points); - trajectory.s_dddot.reserve(n_points); - - QuarticPolynomial lon_qp( - current_state.s, - current_state.s_dot, - current_state.s_ddot, - target_speed, - 0.0, - T - ); - - for (double t = 0.0; t <= T; t += dt_) { - trajectory.t.push_back(t); - trajectory.s.push_back(lon_qp.calc_point(t)); - trajectory.s_dot.push_back(lon_qp.calc_first_derivative(t)); - trajectory.s_ddot.push_back(lon_qp.calc_second_derivative(t)); - trajectory.s_dddot.push_back(lon_qp.calc_third_derivative(t)); - } - - trajectory.cost = 0.0; - - return trajectory; -} - -bool FrenetPlanner::select_best_path( - const VehicleState& current_state, - const nav_msgs::msg::Path::SharedPtr& reference_path, - const std::vector& obstacles, - FrenetTrajectory& best_path, - size_t& total_count, - size_t& valid_count -) { - std::vector longitudinal_trajectories; - total_count = 0; - valid_count = 0; - - if (!reference_path || reference_path->poses.size() < 2) { - return false; - } - - std::vector ref_x; - std::vector ref_y; - ref_x.reserve(reference_path->poses.size()); - ref_y.reserve(reference_path->poses.size()); - for (const auto& pose : reference_path->poses) { - ref_x.push_back(pose.pose.position.x); - ref_y.push_back(pose.pose.position.y); - } - ReferenceCurve reference_curve(ref_x, ref_y); - - double max_s_available = reference_curve.max_s(); - double min_t = 0.95 * max_s_available / target_speed_; - double max_t = max_s_available / target_speed_; - - double max_t_for_path = max_s_available / std::max(1e-3, target_speed_); - double max_t_adjusted = std::min(max_t, max_t_for_path); - double min_t_adjusted = std::min(min_t, max_t_adjusted); - - size_t n_time_samples = static_cast((max_t_adjusted - min_t_adjusted) / dt_) + 1; - size_t n_lateral_samples = static_cast((max_road_width_left_ + max_road_width_right_) / d_road_width_) + 1; - - int n_speed_sample = std::max(0, n_s_sample_); - double dv = std::abs(d_t_s_); - size_t n_speed_samples = static_cast(2 * n_speed_sample + 1); - - longitudinal_trajectories.reserve(n_time_samples * n_speed_samples); - - for (size_t t_index = 0; t_index < n_time_samples; ++t_index) { - const double T = min_t_adjusted + static_cast(t_index) * dt_; - for (int i = -n_speed_sample; i <= n_speed_sample; ++i) { - double tv = target_speed_ + static_cast(i) * dv; - if (tv < 0.0) { - tv = 0.0; - } - FrenetTrajectory lon_traj = generate_longitudinal_trajectory(current_state, tv, T); - longitudinal_trajectories.push_back(std::move(lon_traj)); - } - } - - const bool has_obstacles = !obstacles.empty(); - const double collision_threshold = robot_radius_ + safety_margin_; - const double collision_threshold_sq = collision_threshold * collision_threshold; - bool has_best = false; - double best_cost = std::numeric_limits::max(); - - for (size_t i = 0; i < n_time_samples; ++i) { - const size_t lon_base = i * n_speed_samples; - - for (size_t j = 0; j < n_lateral_samples; ++j) { - const double target_d = -max_road_width_left_ + static_cast(j) * d_road_width_; - for (size_t k = 0; k < n_speed_samples; ++k) { - total_count++; - const auto& lon = longitudinal_trajectories[lon_base + k]; - - if (lon.t.empty()) { - continue; - } - - FrenetTrajectory lat = generate_lateral_trajectory(current_state, target_d, lon); - if (lat.t.size() != lon.t.size()) { - continue; - } - - FrenetTrajectory fp; - fp.t = lat.t; - fp.d = lat.d; - fp.d_s = lat.d_s; - fp.d_ss = lat.d_ss; - fp.d_dot = lat.d_dot; - fp.d_ddot = lat.d_ddot; - fp.d_dddot = lat.d_dddot; - fp.s = lon.s; - fp.s_dot = lon.s_dot; - fp.s_ddot = lon.s_ddot; - fp.s_dddot = lon.s_dddot; - fp.cost = 0.0; - fp.valid = false; - fp.points.reserve(fp.s.size()); - - bool valid = true; - for (size_t m = 0; m < fp.s.size(); ++m) { - FrenetState frenet_state{ - fp.s[m], - fp.s_dot[m], - fp.s_ddot[m], - fp.d[m], - fp.d_s[m], - fp.d_ss[m] - }; - CartesianState cartesian_state{}; - frenet_to_cartesian(reference_curve, frenet_state, cartesian_state); - - TrajectoryPoint point; - point.s = fp.s[m]; - point.d = fp.d[m]; - point.x = cartesian_state.x; - point.y = cartesian_state.y; - point.yaw = cartesian_state.yaw; - point.curvature = cartesian_state.curvature; - point.velocity = cartesian_state.velocity; - point.acceleration = cartesian_state.acceleration; - - if (std::abs(point.velocity) > max_speed_ || - std::abs(point.acceleration) > max_accel_ || - std::abs(point.curvature) > max_curvature_) { - valid = false; - break; - } - - if (has_obstacles) { - for (const auto& obs : obstacles) { - const double dx = point.x - obs.x; - const double dy = point.y - obs.y; - const double dist_sq = dx * dx + dy * dy; - if (dist_sq < collision_threshold_sq) { - valid = false; - break; - } - } - if (!valid) { - break; - } - } - - fp.points.push_back(point); - } - - if (!valid) { - continue; - } - - fp.valid = true; - fp.cost = risk_calculator_.calculate_cost(fp, obstacles); - valid_count++; - if (fp.cost < best_cost) { - best_cost = fp.cost; - best_path = std::move(fp); - has_best = true; - } - } - } - } - - return has_best; -} - -FrenetTrajectory FrenetPlanner::generate_lateral_trajectory( - const VehicleState& current_state, - double target_d, - const FrenetTrajectory& longitudinal_traj) { - FrenetTrajectory trajectory; - trajectory.valid = true; - - if (longitudinal_traj.s.empty()) { - trajectory.valid = false; - return trajectory; - } - - const double T = longitudinal_traj.t.back(); - QuinticPolynomial lat_qp( - current_state.d, - current_state.d_dot, - current_state.d_ddot, - target_d, - 0.0, - 0.0, - T - ); - - const size_t n_points = longitudinal_traj.s.size(); - trajectory.t = longitudinal_traj.t; - trajectory.s = longitudinal_traj.s; - trajectory.s_dot = longitudinal_traj.s_dot; - trajectory.s_ddot = longitudinal_traj.s_ddot; - trajectory.s_dddot = longitudinal_traj.s_dddot; - trajectory.d.reserve(n_points); - trajectory.d_s.reserve(n_points); - trajectory.d_ss.reserve(n_points); - trajectory.d_dot.reserve(n_points); - trajectory.d_ddot.reserve(n_points); - trajectory.d_dddot.reserve(n_points); - - for (size_t i = 0; i < n_points; ++i) { - const double t = longitudinal_traj.t[i]; - const double d = lat_qp.calc_point(t); - const double d_dot_t = lat_qp.calc_first_derivative(t); - const double d_ddot_t = lat_qp.calc_second_derivative(t); - const double d_dddot_t = lat_qp.calc_third_derivative(t); - - const double s_dot = longitudinal_traj.s_dot[i]; - const double s_ddot = longitudinal_traj.s_ddot[i]; - const double s_dot_inv = 1.0 / (std::abs(s_dot) + 1e-6); - const double s_dot_inv_sq = s_dot_inv * s_dot_inv; - - const double d_s_i = d_dot_t * s_dot_inv; - const double d_ss_i = (d_ddot_t - d_s_i * s_ddot) * s_dot_inv_sq; - - trajectory.d.push_back(d); - trajectory.d_s.push_back(d_s_i); - trajectory.d_ss.push_back(d_ss_i); - trajectory.d_dot.push_back(d_dot_t); - trajectory.d_ddot.push_back(d_ddot_t); - trajectory.d_dddot.push_back(d_dddot_t); - } - - trajectory.cost = 0.0; - return trajectory; -} - -void FrenetPlanner::get_frenet_state(const nav_msgs::msg::Path::SharedPtr& path, const geometry_msgs::msg::Twist& current_twist, VehicleState& state) { - state.s = 0.0; - state.d = 0.0; - state.d_ddot = 0.0; - state.s_ddot = 0.0; - - if (!path || path->poses.size() < 2) { - state.s_dot = 0.0; - state.d_dot = 0.0; - return; - } - - const double path1_x = path->poses[1].pose.position.x - path->poses[0].pose.position.x; - const double path1_y = path->poses[1].pose.position.y - path->poses[0].pose.position.y; - const double path_norm = std::hypot(path1_x, path1_y); - const double tx = (path_norm > 1e-6) ? (path1_x / path_norm) : 1.0; - const double ty = (path_norm > 1e-6) ? (path1_y / path_norm) : 0.0; - const double nx = -ty; - const double ny = tx; - - const double vx = std::abs(current_twist.linear.x); - const double vy = 0.0; - - state.s_dot = vx * tx + vy * ty; - state.d_dot = vx * nx + vy * ny; - -} - -void FrenetPlanner::frenet_to_cartesian( - const ReferenceCurve& reference_curve, - const FrenetState& state, - CartesianState& out -) { - double rx, ry, rtheta, rkappa, rdkappa; - reference_curve.sample(state.s, rx, ry, rtheta, rkappa, rdkappa); - - double cos_theta_r = std::cos(rtheta); - double sin_theta_r = std::sin(rtheta); - - out.x = rx - sin_theta_r * state.d; - out.y = ry + cos_theta_r * state.d; - - double one_minus_kappa_r_d = 1.0 - rkappa * state.d; - double tan_delta_theta = state.d_s / one_minus_kappa_r_d; - double delta_theta = std::atan2(state.d_s, one_minus_kappa_r_d); - double cos_delta_theta = std::cos(delta_theta); - - out.yaw = delta_theta + rtheta; - - double kappa_r_d_prime = rdkappa * state.d + rkappa * state.d_s; - - out.curvature = (((state.d_ss + kappa_r_d_prime * tan_delta_theta) * - cos_delta_theta * cos_delta_theta) / one_minus_kappa_r_d + rkappa) * - cos_delta_theta / one_minus_kappa_r_d; - - double d_dot = state.d_s * state.s_dot; - out.velocity = std::sqrt(one_minus_kappa_r_d * one_minus_kappa_r_d * state.s_dot * state.s_dot + d_dot * d_dot); - - double delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * out.curvature - rkappa; - - out.acceleration = (state.s_ddot * one_minus_kappa_r_d / cos_delta_theta + - state.s_dot * state.s_dot / cos_delta_theta * - (state.d_s * delta_theta_prime - kappa_r_d_prime)); -} - -} // namespace frenet_planner diff --git a/frenet_planner/src/frenet_planner_node.cpp b/frenet_planner/src/frenet_planner_node.cpp deleted file mode 100644 index 441bda54..00000000 --- a/frenet_planner/src/frenet_planner_node.cpp +++ /dev/null @@ -1,93 +0,0 @@ -#include "frenet_planner/frenet_planner_node.hpp" - -namespace frenet_planner { - -FrenetPlannerNode::FrenetPlannerNode(const rclcpp::NodeOptions& options) - : FrenetPlannerNode("", options) {} - -FrenetPlannerNode::FrenetPlannerNode(const std::string& name_space, const rclcpp::NodeOptions& options) -: rclcpp::Node("frenet_planner_node", name_space, options), - risk_calculator_(), - frenet_planner_(risk_calculator_), - obstacle_detector_(), - caster_max_angle_(get_parameter("steering_max.pos").as_double()) -{ - double max_curvature = caster_max_angle_; - - frenet_planner_.set_parameters( - get_parameter("linear_max.vel").as_double(), - get_parameter("linear_max.acc").as_double(), - max_curvature, - get_parameter("dt").as_double(), - get_parameter("d_t_s").as_double(), - get_parameter("n_s_sample").as_int(), - get_parameter("linear_max.vel").as_double(), - get_parameter("safety_margin").as_double() - ); - risk_calculator_.set_parameters( - get_parameter("k_jerk").as_double(), - get_parameter("k_time").as_double(), - get_parameter("k_d").as_double(), - get_parameter("k_d_s").as_double(), - get_parameter("k_s_dot").as_double(), - get_parameter("k_lat").as_double(), - get_parameter("k_lon").as_double(), - get_parameter("linear_max.vel").as_double() - ); - - sub_path_ = this->create_subscription( - "e2e_planner/path", - rclcpp::QoS(10), - std::bind(&FrenetPlannerNode::path_callback, this, std::placeholders::_1) - ); - sub_pointcloud_ = this->create_subscription( - "/zed/zed_node/pointcloud", - qos_, - std::bind(&FrenetPlannerNode::pointcloud_callback, this, std::placeholders::_1) - ); - sub_velocity_body_ = this->create_subscription( - "/vectornav/velocity_body", - rclcpp::QoS(10), - std::bind(&FrenetPlannerNode::velocity_body_callback, this, std::placeholders::_1) - ); - - pub_local_path_ = this->create_publisher("frenet_planner/path", rclcpp::QoS(10)); - pub_obstacle_markers_ = this->create_publisher("obstacle_markers", qos_); - - RCLCPP_INFO(this->get_logger(), "FrenetPlanner node has been initialized."); -} - -void FrenetPlannerNode::pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg){ - pointcloud_ = msg; - - if (!global_path_ || global_path_->poses.empty()) { - return; - } - - std::vector obstacles; - obstacles = obstacle_detector_.detect_obstacles(pointcloud_); - - local_path_ = obstacles.empty() ? global_path_ : std::make_shared(frenet_planner_.plan_local_path(global_path_, obstacles, current_twist_)); - - if (!local_path_) { - local_path_ = global_path_; - } - - auto marker_array = obstacle_detector_.obstacles_to_marker_array(obstacles, global_path_->header.frame_id, this->now()); - pub_obstacle_markers_->publish(marker_array); - pub_local_path_->publish(*local_path_); -} - -void FrenetPlannerNode::velocity_body_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg){ - current_twist_ = msg->twist.twist; -} - -void FrenetPlannerNode::path_callback(const nav_msgs::msg::Path::SharedPtr msg){ - if (msg->poses.empty()) { - return; - } - - global_path_ = msg; -} - -} // namespace frenet_planner diff --git a/frenet_planner/src/obstacle_detect.cpp b/frenet_planner/src/obstacle_detect.cpp deleted file mode 100644 index 19006067..00000000 --- a/frenet_planner/src/obstacle_detect.cpp +++ /dev/null @@ -1,108 +0,0 @@ -#include "frenet_planner/obstacle_detect.hpp" -#include - -namespace frenet_planner { - -ObstacleDetector::ObstacleDetector() - : voxel_size_(0.3) {} - -ObstacleDetector::VoxelKey ObstacleDetector::get_voxel_key(double x, double y, double z) const { - VoxelKey key; - key.x = static_cast(std::floor(x / voxel_size_)); - key.y = static_cast(std::floor(y / voxel_size_)); - key.z = static_cast(std::floor(z / voxel_size_)); - return key; -} - -Obstacle ObstacleDetector::voxel_center(const VoxelKey& key) const { - Obstacle obs; - obs.x = (static_cast(key.x) + 0.5) * voxel_size_; - obs.y = (static_cast(key.y) + 0.5) * voxel_size_; - obs.s = 0.0; - obs.d = 0.0; - return obs; -} - -std::vector ObstacleDetector::detect_obstacles( - const sensor_msgs::msg::PointCloud2::SharedPtr pointcloud -) { - if (!pointcloud || pointcloud->data.empty()) { - return {}; - } - - std::vector obstacles; - std::unordered_set voxel_set; - size_t point_count = static_cast(pointcloud->width) * static_cast(pointcloud->height); - if (point_count == 0 && pointcloud->point_step > 0) { - point_count = pointcloud->data.size() / pointcloud->point_step; - } - voxel_set.reserve(point_count); - - sensor_msgs::PointCloud2ConstIterator iter_x(*pointcloud, "x"); - sensor_msgs::PointCloud2ConstIterator iter_y(*pointcloud, "y"); - sensor_msgs::PointCloud2ConstIterator iter_z(*pointcloud, "z"); - - for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - double obs_x = *iter_x; - double obs_y = *iter_y; - double obs_z = *iter_z; - - VoxelKey voxel_key = get_voxel_key(obs_x, obs_y, obs_z); - voxel_set.insert(voxel_key); - } - - obstacles.reserve(voxel_set.size()); - for (const auto& key : voxel_set) { - obstacles.push_back(voxel_center(key)); - } - - return obstacles; -} - -visualization_msgs::msg::MarkerArray ObstacleDetector::obstacles_to_marker_array( - const std::vector& obstacles, - const std::string& frame_id, - const rclcpp::Time& stamp -) const { - visualization_msgs::msg::MarkerArray marker_array; - - visualization_msgs::msg::Marker delete_marker; - delete_marker.header.frame_id = frame_id; - delete_marker.header.stamp = stamp; - delete_marker.ns = "obstacles"; - delete_marker.id = 0; - delete_marker.action = visualization_msgs::msg::Marker::DELETEALL; - marker_array.markers.push_back(delete_marker); - - for (size_t i = 0; i < obstacles.size(); ++i) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = frame_id; - marker.header.stamp = stamp; - marker.ns = "obstacles"; - marker.id = i + 1; - marker.type = visualization_msgs::msg::Marker::SPHERE; - marker.action = visualization_msgs::msg::Marker::ADD; - - marker.pose.position.x = obstacles[i].x; - marker.pose.position.y = obstacles[i].y; - marker.pose.position.z = 0.0; - marker.pose.orientation.w = 1.0; - - marker.scale.x = voxel_size_ * 0.8; - marker.scale.y = voxel_size_ * 0.8; - marker.scale.z = voxel_size_ * 0.8; - - marker.color.r = 1.0; - marker.color.g = 0.0; - marker.color.b = 0.0; - marker.color.a = 0.7; - - marker.lifetime = rclcpp::Duration::from_seconds(0.2); - - marker_array.markers.push_back(marker); - } - - return marker_array; -} - -} // namespace frenet_planner diff --git a/frenet_planner/src/reference_curve.cpp b/frenet_planner/src/reference_curve.cpp deleted file mode 100644 index e7b11bb9..00000000 --- a/frenet_planner/src/reference_curve.cpp +++ /dev/null @@ -1,119 +0,0 @@ -#include "frenet_planner/reference_curve.hpp" - -#include -#include - -namespace frenet_planner { - -ReferenceCurve::ReferenceCurve(const std::vector& x, const std::vector& y) - : x_(x), y_(y) { - const size_t n = x.size(); - s_.resize(n, 0.0); - for (size_t i = 1; i < n; ++i) { - double dx = x_[i] - x_[i - 1]; - double dy = y_[i] - y_[i - 1]; - s_[i] = s_[i - 1] + std::hypot(dx, dy); - } - - yaw_.resize(n, 0.0); - kappa_.resize(n, 0.0); - dkappa_.resize(n, 0.0); - - std::vector dx_ds(n, 0.0); - std::vector dy_ds(n, 0.0); - std::vector ddx_ds(n, 0.0); - std::vector ddy_ds(n, 0.0); - - for (size_t i = 1; i + 1 < n; ++i) { - const double ds_prev = std::max(1e-6, s_[i] - s_[i - 1]); - const double ds_next = std::max(1e-6, s_[i + 1] - s_[i]); - const double ds_sum = ds_prev + ds_next; - - dx_ds[i] = (x_[i + 1] - x_[i - 1]) / ds_sum; - dy_ds[i] = (y_[i + 1] - y_[i - 1]) / ds_sum; - - const double dx_prev = (x_[i] - x_[i - 1]) / ds_prev; - const double dx_next = (x_[i + 1] - x_[i]) / ds_next; - const double dy_prev = (y_[i] - y_[i - 1]) / ds_prev; - const double dy_next = (y_[i + 1] - y_[i]) / ds_next; - - ddx_ds[i] = 2.0 * (dx_next - dx_prev) / ds_sum; - ddy_ds[i] = 2.0 * (dy_next - dy_prev) / ds_sum; - } - - dx_ds[0] = (x_[1] - x_[0]) / std::max(1e-6, s_[1] - s_[0]); - dy_ds[0] = (y_[1] - y_[0]) / std::max(1e-6, s_[1] - s_[0]); - dx_ds[n - 1] = (x_[n - 1] - x_[n - 2]) / std::max(1e-6, s_[n - 1] - s_[n - 2]); - dy_ds[n - 1] = (y_[n - 1] - y_[n - 2]) / std::max(1e-6, s_[n - 1] - s_[n - 2]); - - ddx_ds[0] = ddx_ds[1]; - ddy_ds[0] = ddy_ds[1]; - ddx_ds[n - 1] = ddx_ds[n - 2]; - ddy_ds[n - 1] = ddy_ds[n - 2]; - - for (size_t i = 0; i < n; ++i) { - yaw_[i] = std::atan2(dy_ds[i], dx_ds[i]); - const double v2 = dx_ds[i] * dx_ds[i] + dy_ds[i] * dy_ds[i]; - const double v = std::sqrt(v2); - if (v < 1e-6) { - kappa_[i] = 0.0; - } else { - kappa_[i] = (dx_ds[i] * ddy_ds[i] - dy_ds[i] * ddx_ds[i]) / (v2 * v); - } - } - - for (size_t i = 1; i + 1 < n; ++i) { - const double ds_prev = std::max(1e-6, s_[i] - s_[i - 1]); - const double ds_next = std::max(1e-6, s_[i + 1] - s_[i]); - dkappa_[i] = (kappa_[i + 1] - kappa_[i - 1]) / (ds_prev + ds_next); - } - dkappa_[0] = dkappa_[1]; - dkappa_[n - 1] = dkappa_[n - 2]; - - valid_ = true; -} - -bool ReferenceCurve::valid() const { - return valid_; -} - -double ReferenceCurve::max_s() const { - if (s_.empty()) { - return 0.0; - } - return s_.back(); -} - -void ReferenceCurve::sample(double s, - double& x, - double& y, - double& yaw, - double& kappa, - double& dkappa) const { - double clamped_s = std::max(0.0, std::min(s, s_.back())); - auto it = std::upper_bound(s_.begin(), s_.end(), clamped_s); - size_t i = 0; - if (it != s_.begin()) { - i = static_cast(it - s_.begin()) - 1; - } - if (i + 1 >= s_.size()) { - i = s_.size() - 2; - } - - const double s0 = s_[i]; - const double s1 = s_[i + 1]; - const double denom = std::max(1e-6, s1 - s0); - const double t = (clamped_s - s0) / denom; - - auto lerp = [t](double a, double b) { - return a + (b - a) * t; - }; - - x = lerp(x_[i], x_[i + 1]); - y = lerp(y_[i], y_[i + 1]); - yaw = lerp(yaw_[i], yaw_[i + 1]); - kappa = lerp(kappa_[i], kappa_[i + 1]); - dkappa = lerp(dkappa_[i], dkappa_[i + 1]); -} - -} // namespace frenet_planner diff --git a/frenet_planner/src/risk_calculator.cpp b/frenet_planner/src/risk_calculator.cpp deleted file mode 100644 index a1b14bfb..00000000 --- a/frenet_planner/src/risk_calculator.cpp +++ /dev/null @@ -1,78 +0,0 @@ -#include "frenet_planner/risk_calculator.hpp" -#include "frenet_planner/frenet_planner.hpp" -#include -#include - -namespace frenet_planner { - -RiskCalculator::RiskCalculator() - : k_jerk_(0.0), - k_time_(0.0), - k_d_(0.0), - k_d_s_(0.0), - k_s_dot_(0.0), - target_speed_(0.0) { -} - -void RiskCalculator::set_parameters( - double k_jerk, - double k_time, - double k_d, - double k_d_s, - double k_s_dot, - double k_lat, - double k_lon, - double target_speed -) { - k_jerk_ = k_jerk; - k_time_ = k_time; - k_d_ = k_d; - k_d_s_ = k_d_s; - k_s_dot_ = k_s_dot; - k_lateral_ = k_lat; - k_longitudinal_ = k_lon; - target_speed_ = target_speed; -} - -double RiskCalculator::calculate_cost( - const FrenetTrajectory& trajectory, - const std::vector& obstacles -) { - (void)obstacles; - if (trajectory.points.empty()) { - RCLCPP_ERROR(rclcpp::get_logger("frenet_planner"), - "calculate_cost: Empty trajectory!"); - return std::numeric_limits::max(); - } - - if (trajectory.t.empty() || trajectory.s.empty() || trajectory.d.empty() || - trajectory.s_dot.empty() || trajectory.d_s.empty()) { - RCLCPP_ERROR(rclcpp::get_logger("frenet_planner"), - "calculate_cost: Missing Frenet samples!"); - return std::numeric_limits::max(); - } - - double lateral_jerk_sum = 0.0; - for (const auto& jerk : trajectory.d_dddot) { - lateral_jerk_sum += jerk * jerk; - } - - double longitudinal_jerk_sum = 0.0; - for (const auto& jerk : trajectory.s_dddot) { - longitudinal_jerk_sum += jerk * jerk; - } - - const double T = trajectory.t.back(); - const double d_end = trajectory.d.back(); - const double d_s_end = trajectory.d_s.back(); - const double v_end = trajectory.s_dot.back(); - const double ds = target_speed_ - v_end; - - const double cd = k_jerk_ * lateral_jerk_sum + k_time_ * T + - k_d_ * d_end * d_end + k_d_s_ * d_s_end * d_s_end; - const double cv = k_jerk_ * longitudinal_jerk_sum + k_time_ * T + k_s_dot_ * ds * ds; - - return k_lateral_ * cd + k_longitudinal_ * cv; -} - -} // namespace frenet_planner diff --git a/gnssnav/CMakeLists.txt b/gnssnav/CMakeLists.txt deleted file mode 100644 index 50c20bb5..00000000 --- a/gnssnav/CMakeLists.txt +++ /dev/null @@ -1,110 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(gnssnav) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKFE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(ament_index_cpp REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(rclcpp REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(utilities REQUIRED) - -find_path(PROJ_INCLUDE_DIR NAMES proj.h PATHS /usr/include) -find_library(PROJ_LIBRARY NAMES proj PATHS /usr/lib/x86_64-linux-gnu) - -add_library(navigation_node - src/follower_node.cpp - src/path_publisher_node.cpp - ) -target_include_directories(navigation_node PUBLIC - $ - $) -ament_target_dependencies( - navigation_node - ament_index_cpp - rclcpp - nav_msgs - geometry_msgs - Eigen3 - tf2 - tf2_ros - tf2_geometry_msgs - utilities -) -target_link_libraries(navigation_node ${PROJ_LIBRARY}) - -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(navigation_node PRIVATE "GNSSNAV_BUILDING_LIBRARY") - - -ament_export_include_directories( - include -) -ament_export_libraries( - navigation_node -) -ament_export_targets( - export_${PROJECT_NAME} -) -ament_export_dependencies( - ament_index_cpp - rclcpp - nav_msgs - geometry_msgs - Eigen3 - tf2 - tf2_ros - tf2_geometry_msgs - utilities -) - -install( - DIRECTORY include/ - DESTINATION include -) -install( - TARGETS navigation_node - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install(DIRECTORY config - DESTINATION share/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - - - -ament_package() diff --git a/gnssnav/config/course_data/gazebo_shihou.csv b/gnssnav/config/course_data/gazebo_shihou.csv deleted file mode 100644 index 0629c5a7..00000000 --- a/gnssnav/config/course_data/gazebo_shihou.csv +++ /dev/null @@ -1,46 +0,0 @@ -36.11339585571819,139.9815010597697 -36.113395855685,139.98150106025167 -36.11339585555729,139.98150106310823 -36.113395855316384,139.9815010660582 -36.113395855112365,139.9815010693639 -36.11339585311715,139.98150109993202 -36.113399053075504,139.98156892854155 -36.11339616027104,139.98163969302706 -36.1133968899045,139.9817134639211 -36.11339715833002,139.9817872720855 -36.11338383945336,139.98185755792508 -36.11334840042976,139.9819322665014 -36.11330077461931,139.981972361871 -36.11324144043163,139.98199228034838 -36.11318527161366,139.9819952661564 -36.113122190339006,139.9819980714745 -36.113064012271096,139.98199940683378 -36.11300737536521,139.98199879808212 -36.11294615113052,139.98199913570113 -36.11288943138499,139.98199792147676 -36.1128222508607,139.9819892559794 -36.11276791063374,139.98195926897264 -36.11272629710545,139.9819022045721 -36.11270812577867,139.98183147569063 -36.11270758849688,139.98176156798567 -36.11272887876713,139.98168218182877 -36.11275240159456,139.98161658682272 -36.112775820003925,139.98154538282049 -36.112799780759936,139.98148110132905 -36.112821065922304,139.98141276527102 -36.11284644784925,139.98133906548497 -36.11286837687875,139.98127130754568 -36.11289513022984,139.9812004051492 -36.11292661051057,139.98114183505118 -36.11298290610209,139.9810935768338 -36.11303954348792,139.9810756399803 -36.113098227378906,139.98107369592125 -36.11316019976915,139.98107158857792 -36.11321647821682,139.98106946374097 -36.11328350500575,139.98108412930793 -36.113331538020695,139.98112281693503 -36.11337327298213,139.98117899607783 -36.11339344871117,139.98124552789508 -36.1133966044632,139.98131950811276 -36.11339844526289,139.98140069839454 -36.11339912508567,139.9814721930114 diff --git a/gnssnav/config/course_data/line_follow_test.csv b/gnssnav/config/course_data/line_follow_test.csv deleted file mode 100644 index 051c7891..00000000 --- a/gnssnav/config/course_data/line_follow_test.csv +++ /dev/null @@ -1,406 +0,0 @@ -36.11315511766887,139.9810702911582 -36.11315521561402,139.98107044621128 -36.11315507726436,139.9810705447352 -36.11315537507507,139.98107067299728 -36.11315550508416,139.9810707611711 -36.11315562831327,139.98107100024302 -36.11315551184435,139.98107102275853 -36.11315546170119,139.98107104090462 -36.11315532151501,139.98107100183236 -36.1131553219176,139.98107113000893 -36.11315505179268,139.98107119728652 -36.11315516508983,139.98107125547804 -36.1131551900855,139.9810712645883 -36.11315546606629,139.98107125783716 -36.113155424613375,139.98107141429747 -36.113155444579824,139.98107140963205 -36.11315535356906,139.98107163471047 -36.113155371253455,139.9810716757864 -36.11315542423593,139.98107195437774 -36.113155395471736,139.98107177933696 -36.11315555872425,139.98107187098702 -36.1131557968356,139.9810720897463 -36.113156038827086,139.98107220907326 -36.113156095199976,139.9810721917101 -36.11315608555058,139.98107197425531 -36.11315617691382,139.98107182860085 -36.1131560351005,139.9810719864047 -36.11315609378744,139.9810721262529 -36.113156165716674,139.9810723072242 -36.11315583376647,139.98107231096668 -36.11315298459846,139.98107231657903 -36.11314818738387,139.9810723579166 -36.113142201933776,139.9810728197389 -36.11313600435676,139.9810735291228 -36.11312893627366,139.98107450404316 -36.113121880993404,139.98107550670136 -36.113115004894624,139.981076543233 -36.11310800616907,139.98107726591857 -36.1131012470352,139.98107814588255 -36.113094378432564,139.98107898483377 -36.113087674461674,139.98108000917276 -36.113080868534034,139.9810807578879 -36.11307411341982,139.981081499148 -36.113067061844475,139.98108217266258 -36.11306023645254,139.98108267442177 -36.11305323778162,139.98108335594407 -36.11304610463992,139.98108403272158 -36.11303914716711,139.98108512024677 -36.113031857311285,139.98108561698206 -36.1130245298871,139.98108617835035 -36.113017208939645,139.98108674130057 -36.11301030524393,139.981087605271 -36.11300353075295,139.98108835155713 -36.11299677364164,139.98108927383979 -36.112990250821795,139.98109008042093 -36.112983630060164,139.98109105747443 -36.1129766546749,139.981092863519 -36.11296995962592,139.9810946532223 -36.112963336640625,139.98109654279747 -36.1129566413557,139.9810991957692 -36.11295010699514,139.98110240350846 -36.11294356959703,139.98110561633126 -36.112937220100335,139.98110925147304 -36.1129309043522,139.9811131526663 -36.11292480245278,139.98111661563948 -36.1129186952905,139.9811206213701 -36.11291281371037,139.9811249042035 -36.11290709158301,139.98112933880248 -36.11290169254988,139.9811343998573 -36.11289668791442,139.98114012263164 -36.112891713674394,139.9811461077675 -36.11288639431098,139.98115203492864 -36.11288142798969,139.98115836441397 -36.11287678286433,139.98116494175582 -36.11287238866676,139.98117137336075 -36.11286870306922,139.98117875404884 -36.11286542680672,139.98118613344587 -36.11286197849464,139.98119351740263 -36.112858725785586,139.98120089262682 -36.11285546441097,139.98120833192274 -36.11285274295007,139.9812161089917 -36.11285005367647,139.9812240408563 -36.11284780692394,139.9812319346342 -36.11284571285798,139.9812398883524 -36.11284347831592,139.9812483349102 -36.112841530328595,139.98125644269143 -36.11283956180027,139.98126471627222 -36.11283796882351,139.98127276829936 -36.11283603517821,139.98128129632855 -36.112834292753746,139.98128957304857 -36.112832323085875,139.98129788009808 -36.112830192240935,139.98130608032284 -36.11282816557436,139.98131381104855 -36.11282566800318,139.98132181530497 -36.112823244162286,139.98132996771352 -36.1128209666472,139.98133805195482 -36.11281854632607,139.98134626654564 -36.11281634399412,139.98135459532241 -36.112814132325234,139.9813627420602 -36.11281223536705,139.98137095293964 -36.11281019993618,139.9813793901002 -36.112808508836615,139.98138744488526 -36.11280660489296,139.98139524987135 -36.112804225990175,139.9814031580402 -36.11280195393936,139.98141121455362 -36.112799323886755,139.98141925448826 -36.11279707030733,139.9814271099277 -36.11279503220134,139.9814348345663 -36.112792650441214,139.98144308409059 -36.112790485415545,139.98145119759732 -36.11278778603202,139.9814594754494 -36.11278539690987,139.98146755502373 -36.112782954327436,139.98147564495363 -36.11278048860467,139.98148356202583 -36.11277779520927,139.98149189550168 -36.11277568788773,139.98150016155392 -36.11277399112084,139.98150895852515 -36.112772044347416,139.98151751030392 -36.11277039782091,139.98152608096052 -36.11276853839371,139.98153426499996 -36.11276670634103,139.98154244633895 -36.112764860545695,139.9815508977447 -36.11276289390926,139.98155912972695 -36.112760898635415,139.9815673683232 -36.11275908596916,139.9815755162883 -36.11275729899729,139.98158386411362 -36.11275495739728,139.98159238571154 -36.11275300349804,139.98160060688295 -36.11275077603295,139.9816086490165 -36.1127483696962,139.98161661964656 -36.11274583929438,139.98162471720087 -36.11274353381747,139.98163281937383 -36.11274115819431,139.9816407138363 -36.11273924657445,139.98164879088256 -36.112737066380035,139.981656825987 -36.11273489480198,139.98166521335153 -36.11273269861184,139.98167360868737 -36.11273066597415,139.98168164271573 -36.112728577604244,139.98168959483968 -36.112726576507555,139.98169771146894 -36.11272449146447,139.98170580721705 -36.11272246291115,139.98171398773522 -36.11272043811359,139.9817222598774 -36.11271850534922,139.981730551363 -36.112716422091665,139.98173863347265 -36.112714374242145,139.9817466153881 -36.112712522564834,139.98175493350968 -36.11271069008461,139.98176344321752 -36.112709322190874,139.98177194122056 -36.11270796418868,139.98178040797467 -36.11270700901944,139.981789309467 -36.11270653038481,139.9817979016455 -36.11270563127587,139.9818064954445 -36.11270517870597,139.98181507829295 -36.11270533114685,139.98182358239745 -36.112705855874026,139.9818320420985 -36.11270656701857,139.9818404779347 -36.11270774098952,139.9818489205173 -36.11270886430348,139.9818575789465 -36.11271042401298,139.98186623933663 -36.11271199607991,139.98187476507383 -36.11271411199347,139.98188306564168 -36.1127167764157,139.98189110313592 -36.11271961648152,139.98189924063163 -36.1127227022412,139.98190700831418 -36.11272607696103,139.98191471558386 -36.11272950466058,139.9819223131765 -36.112732434646816,139.98193024339315 -36.11273473151527,139.98193841119127 -36.11273863308853,139.98194516352606 -36.11274303584728,139.98195165221836 -36.11274744887228,139.98195827525797 -36.112752257825235,139.98196466519389 -36.11275729215994,139.98197082659428 -36.1127626814454,139.98197664208442 -36.11276813944969,139.98198226284563 -36.1127738606764,139.9819874424195 -36.11277992194128,139.9819922764551 -36.11278603979575,139.98199687537345 -36.11279242166856,139.98200071534947 -36.112798895104575,139.9820045583369 -36.11280545423575,139.98200784303285 -36.112811850298584,139.98201091602652 -36.11281845583777,139.9820136175355 -36.11282492663889,139.9820165189941 -36.1128319864459,139.98201914963906 -36.11283871810058,139.9820211899289 -36.11284567239914,139.9820223709404 -36.112852483893874,139.98202308521869 -36.11285947104333,139.98202346417074 -36.11286633640412,139.9820238250521 -36.11287336075838,139.98202428839937 -36.11288033074065,139.98202421164856 -36.11288719430501,139.98202367439814 -36.11289414672237,139.9820225761889 -36.112901030009944,139.98202182438894 -36.1129077004577,139.98202111299383 -36.11291450261193,139.98202095304924 -36.11292128600943,139.98202017569955 -36.112928067371236,139.98201949792744 -36.11293490272527,139.982018995596 -36.1129418426964,139.98201860917504 -36.11294862427706,139.98201799594437 -36.11295545012745,139.98201698983752 -36.112962352924015,139.9820158679116 -36.11296944966819,139.98201504172127 -36.11297640644198,139.9820142883473 -36.1129834100283,139.9820135510205 -36.11299048142184,139.98201304986924 -36.11299744943623,139.98201218773394 -36.11300458630669,139.9820114279238 -36.113011561169586,139.98201102446123 -36.11301857005281,139.98201068978963 -36.113025532633415,139.98201038622545 -36.113032201717154,139.98200950912306 -36.11303904494564,139.9820082847284 -36.11304595457902,139.98200711859943 -36.11305264220252,139.98200618819493 -36.113059722072286,139.98200501615722 -36.11306669880674,139.98200417864456 -36.113073663264935,139.98200327974328 -36.11308060150457,139.98200239661097 -36.11308756855927,139.98200172838867 -36.11309460187771,139.9820010008188 -36.113101732477595,139.98200035756108 -36.11310887370567,139.98199952912253 -36.11311572432729,139.9819986982943 -36.11312236863112,139.9819982108126 -36.113129092773036,139.98199735635785 -36.113135386496,139.98199655595218 -36.11314166576028,139.9819959461954 -36.113147908350456,139.98199520851634 -36.11315467951225,139.98199484176135 -36.11316170361162,139.98199411810708 -36.11316913314062,139.98199424069617 -36.113176257142605,139.98199373816507 -36.11318356413779,139.98199281188948 -36.11319070208476,139.98199200525673 -36.11319779361028,139.98199125783208 -36.11320492949369,139.98199027513712 -36.11321210256937,139.98198939867032 -36.11321897077812,139.98198866262985 -36.11322628286928,139.98198774608855 -36.11323324545422,139.98198683888484 -36.11324071918497,139.98198587035634 -36.113247655774394,139.9819846188299 -36.11325450427109,139.98198311674815 -36.113261452737696,139.98198141669587 -36.113268438553774,139.98197982078864 -36.11327513372897,139.9819776289777 -36.11328188308977,139.98197490132188 -36.11328866292606,139.98197230081416 -36.1132951761416,139.98196908096963 -36.11330149161057,139.98196531452598 -36.113307982652096,139.9819613282654 -36.11331436538985,139.98195689968387 -36.11332049944444,139.98195248398318 -36.11332657633912,139.98194797256846 -36.11333243673152,139.98194287940763 -36.1133380816108,139.98193791949376 -36.11334355675115,139.98193237326157 -36.11334892553966,139.98192648523428 -36.113354015592314,139.98192026007462 -36.113358885591964,139.9819143385872 -36.113363343733255,139.98190818466898 -36.11336753181586,139.98190147396352 -36.11337128960837,139.98189504254023 -36.11337493492503,139.98188850475782 -36.1133784512113,139.98188194904336 -36.11338179650221,139.98187504813 -36.11338474961751,139.98186762520493 -36.11338738965941,139.98186005123526 -36.11338969774184,139.98185275192924 -36.11339198978909,139.9818453344179 -36.11339451090319,139.98183777374766 -36.11339690218391,139.9818302972197 -36.11339906637007,139.98182269533743 -36.11340027608927,139.98181513528888 -36.113401258887365,139.98180728878654 -36.11340243005827,139.98179995386494 -36.113403029696165,139.98179259786903 -36.113403438179404,139.9817850927945 -36.1134032727914,139.98177712335365 -36.11340305452318,139.98176897404633 -36.11340310404787,139.98176156982532 -36.113402625733784,139.98175408378856 -36.11340200547338,139.98174697194892 -36.11340106534512,139.98173972429151 -36.113399880749164,139.98173290664982 -36.11339917596304,139.9817259997832 -36.11339824047987,139.9817186804924 -36.11339758370992,139.9817112773287 -36.113396635831336,139.98170375755208 -36.113395924272936,139.98169629578743 -36.1133951186618,139.98168862537185 -36.1133943413897,139.9816807783318 -36.11339374997768,139.98167297710754 -36.1133932766847,139.98166510427853 -36.11339309771276,139.98165703248523 -36.11339275157864,139.98164916761942 -36.113392507554394,139.98164132061993 -36.11339227366234,139.98163396706005 -36.11339167817025,139.98162631201257 -36.11339107403911,139.98161886165374 -36.11339048507987,139.9816113368147 -36.11339009257268,139.98160376146703 -36.113389385079344,139.9815962336684 -36.11338911262987,139.98158849765358 -36.113388330708375,139.98158090612333 -36.113387625781,139.9815733976156 -36.113387080279395,139.98156567394378 -36.11338646987389,139.98155777080575 -36.11338561997457,139.98155006420572 -36.11338529609176,139.98154234790675 -36.11338437823883,139.98153469198743 -36.11338354190011,139.98152701747497 -36.11338289694466,139.98151943431915 -36.113382397363274,139.98151177853052 -36.11338202604918,139.98150384175378 -36.11338151792524,139.98149621105927 -36.11338122838951,139.98148830642992 -36.113380841512495,139.98148084334593 -36.11338045962805,139.98147342001025 -36.1133796883058,139.98146582437474 -36.11337882066874,139.9814579802609 -36.11337808036887,139.9814503790166 -36.11337731990252,139.981442811509 -36.113376998299344,139.9814352012219 -36.1133764613018,139.98142722394041 -36.11337567982338,139.9814196961311 -36.113375358218505,139.98141205389948 -36.11337488728531,139.98140465457868 -36.113373946641,139.98139699053007 -36.113373385951945,139.98138941867 -36.1133729552183,139.98138187683725 -36.11337247322871,139.9813744169184 -36.113372171627226,139.9813667332769 -36.11337162688089,139.9813589459187 -36.11337160082474,139.98135134486145 -36.11337106893036,139.981343689847 -36.11337038570275,139.9813358879678 -36.1133701310925,139.981328631103 -36.113369974212056,139.98132078559146 -36.11336948591256,139.98131313107447 -36.113368897334304,139.981305388106 -36.11336801046761,139.9812976954275 -36.11336713142846,139.98129009163785 -36.113366249381095,139.98128255321015 -36.113365250820685,139.9812750742652 -36.113364181848354,139.9812673441137 -36.113363457070704,139.98125947909875 -36.11336271094208,139.98125185470778 -36.11336149471906,139.98124445721885 -36.11335997440995,139.98123714635653 -36.11335843632138,139.98123007337432 -36.113357019235,139.981222805523 -36.113355575646814,139.98121567429163 -36.113354042496766,139.9812086597971 -36.11335279042589,139.98120190351435 -36.11335051831019,139.98119553698336 -36.11334899366216,139.9811890767582 -36.113346633989075,139.98118284917717 -36.11334411229791,139.9811764658515 -36.11334161842446,139.98117070392792 -36.11333910017369,139.98116440155474 -36.11333596872258,139.98115851030943 -36.1133326884136,139.98115277560717 -36.11332949772255,139.98114693164328 -36.11332581227112,139.9811413315097 -36.11332206556954,139.98113598754625 -36.11331771029557,139.981130467358 -36.1133137922824,139.9811255412604 -36.11330923462335,139.98112066118938 -36.11330491098288,139.98111612786357 -36.11330026282476,139.98111125087976 -36.113295596875076,139.9811066313386 -36.11329088070784,139.98110266266374 -36.11328589918739,139.98109903965496 -36.113281193329925,139.9810960312265 -36.11327620466898,139.98109269304493 -36.113271178271454,139.98108934728154 -36.11326629970571,139.9810860436515 -36.11326137863285,139.98108310249054 -36.113256257346535,139.98108078462252 -36.113251004135286,139.9810785496479 -36.11324550551103,139.9810764691109 -36.113240048961664,139.98107454082864 -36.11323421645413,139.9810728792325 -36.11322858225767,139.9810708968704 -36.11322280591739,139.9810692519628 -36.11321706593883,139.98106810310094 -36.113211499808216,139.981066950336 -36.113205934165585,139.98106607941165 -36.11320066019975,139.98106566454575 -36.11319526912622,139.9810656919308 -36.11319243126258,139.98106567094342 -36.11319123712851,139.98106566214486 -36.1131911308733,139.98106578138257 -36.11319080484066,139.9810656218172 -36.113190797210244,139.9810654897615 -36.113190499586615,139.98106563254208 -36.11319052964253,139.98106569071444 -36.1131905011943,139.98106551326836 -36.11319050371774,139.98106539149728 -36.113190329718535,139.981065163018 -36.11319037341846,139.98106501100983 diff --git a/gnssnav/config/course_data/obstacle.csv b/gnssnav/config/course_data/obstacle.csv deleted file mode 100644 index d21e98d3..00000000 --- a/gnssnav/config/course_data/obstacle.csv +++ /dev/null @@ -1,522 +0,0 @@ -36.113390495307286,139.98145613266374 -36.11339066286108,139.98145624394087 -36.1133905583206,139.98145623298308 -36.11339044246417,139.98145622321982 -36.11339041349901,139.98145595658067 -36.11339081770246,139.98145570909946 -36.11339086222257,139.98145576233236 -36.113390879806744,139.98145574921293 -36.11339098019485,139.98145558043697 -36.11339112470182,139.98145574999722 -36.11339114918283,139.9814558371252 -36.1133908771098,139.98145557491986 -36.113390796655935,139.9814557354204 -36.113390609600025,139.9814555743563 -36.113390724398066,139.98145556292712 -36.1133905243866,139.98145533955642 -36.113390548094046,139.98145504542097 -36.11339064774914,139.9814549871078 -36.1133908307907,139.9814547158297 -36.113390899806774,139.98145457970287 -36.113390922928886,139.9814542787838 -36.11339114380395,139.98145456758982 -36.113391101253114,139.98145435863398 -36.113390954443396,139.98145430254624 -36.113390707607905,139.98145423481742 -36.11339034050362,139.98145448224685 -36.11339006245109,139.98145603956394 -36.11338989752917,139.98146016594643 -36.113389972383345,139.981465290367 -36.1133903859132,139.98147085733385 -36.11339046285918,139.98147683241223 -36.11339040312345,139.98148247784056 -36.113390630844705,139.98148834674902 -36.11339048181814,139.98149460724161 -36.113390855329456,139.9814991015425 -36.11339113433033,139.98150339581358 -36.113391758748605,139.98150800174093 -36.11339222319563,139.98151464971147 -36.113393144803034,139.98152303857182 -36.1133937510388,139.9815299807821 -36.113394308856144,139.98153668047448 -36.113394792684026,139.98154276761443 -36.1133955003794,139.98154925834152 -36.11339564173771,139.98155482839533 -36.11339618523531,139.98155983778088 -36.113396477014746,139.98156518688958 -36.113396786014945,139.98157085898586 -36.113396939716885,139.98157664896533 -36.11339733433174,139.98158217952377 -36.1133976941619,139.98158820865245 -36.11339813954361,139.98159412059704 -36.11339888346338,139.9816005135978 -36.11339961463464,139.9816066738578 -36.11340045100817,139.98161330104986 -36.11340137424824,139.98162042121672 -36.11340218649676,139.98162714361905 -36.1134030479814,139.981633678067 -36.1134038424057,139.9816396616733 -36.113404268539604,139.98164627472588 -36.113404976516314,139.9816523919091 -36.11340551038817,139.98165856705668 -36.11340564831204,139.98166398643374 -36.113406156303704,139.981670196021 -36.11340604752464,139.98167574207585 -36.11340638865183,139.98168231146536 -36.11340656460921,139.98168940546512 -36.11340719526562,139.98169648953754 -36.11340744522291,139.98170290309358 -36.11340763396702,139.98170912925968 -36.113407914755385,139.98171489277422 -36.113408141416336,139.98172059273497 -36.113408285365104,139.9817265335753 -36.11340865008965,139.98173235822847 -36.113408870905445,139.98173866405608 -36.11340912517137,139.9817452026071 -36.11340927454226,139.98175223369194 -36.113409181915316,139.98175897267447 -36.1134092485431,139.98176527998962 -36.11340902631757,139.98177135589017 -36.11340855026668,139.9817775123774 -36.113407621765845,139.9817849023112 -36.11340674180078,139.98179307708614 -36.11340571572026,139.98180166068494 -36.113403973720416,139.9818089970726 -36.11340197316958,139.98181579094498 -36.113399653689584,139.98182274616926 -36.11339765729412,139.9818295463123 -36.11339537600515,139.9818353425532 -36.11339347012891,139.98184143958855 -36.11339152937581,139.9818475443221 -36.113389520303514,139.98185370093285 -36.11338657069608,139.98185938967063 -36.11338356368852,139.9818642981757 -36.11337984405112,139.98186991779366 -36.11337607781585,139.9818756001211 -36.11337297733543,139.98188088081366 -36.11336945656711,139.98188567622975 -36.1133660882343,139.98189117251414 -36.11336206768203,139.9818968331227 -36.11335783506788,139.98190222766212 -36.1133539421469,139.98190730684178 -36.11334995833184,139.98191229119587 -36.113345774945984,139.9819169779652 -36.113341254513365,139.98192123712448 -36.11333668680261,139.98192575599182 -36.11333154757702,139.98193050727303 -36.11332627791476,139.9819348925 -36.11332122183311,139.9819388994189 -36.1133163800672,139.98194290517756 -36.113311187706586,139.98194690972272 -36.11330591226488,139.98195098299237 -36.11330063076035,139.98195426787447 -36.11329526270278,139.9819575801099 -36.113289930266724,139.98196035513473 -36.11328464698082,139.98196312589894 -36.11327945929643,139.98196558007473 -36.11327395181814,139.9819682059881 -36.113268012917544,139.98197077263228 -36.113262893970365,139.98197301700176 -36.11325755328621,139.9819751360148 -36.11325208054657,139.98197669974573 -36.113246894887865,139.98197787450107 -36.11324162236245,139.98197907802196 -36.11323650285049,139.98198001479415 -36.11323188280332,139.98198095089302 -36.11322730396622,139.9819820188656 -36.113223086771875,139.9819826782382 -36.113218603830845,139.9819835032697 -36.113213512309635,139.9819842505346 -36.11320881165744,139.98198477993859 -36.11320388932305,139.98198560984085 -36.11319876852069,139.98198613453118 -36.11319334774165,139.98198690874005 -36.113187785972464,139.98198744281174 -36.113182022803656,139.98198779414162 -36.11317611430998,139.98198821917694 -36.11316994411455,139.98198846835143 -36.11316451560685,139.9819889574776 -36.11315921653228,139.9819894203978 -36.1131537482057,139.9819897629294 -36.113148516081814,139.98199047056897 -36.11314337788606,139.98199115989587 -36.113138129182175,139.98199170776542 -36.113132403776504,139.98199228937793 -36.11312651184189,139.98199268450125 -36.113121152095296,139.98199283570355 -36.11311686075568,139.98199300672186 -36.113112660424804,139.9819933498643 -36.11310776925085,139.9819935575183 -36.11310242525464,139.9819935390228 -36.1130972347378,139.9819938290518 -36.11309203648017,139.98199437981074 -36.11308672034217,139.9819949018126 -36.11308193472023,139.98199504323873 -36.11307712743052,139.98199530345346 -36.11307244420771,139.98199537200546 -36.11306743002995,139.9819954827997 -36.113062443118544,139.98199547848074 -36.11305749519936,139.9819956667899 -36.11305249240626,139.9819954190719 -36.11304749973637,139.9819951061324 -36.113042622079924,139.9819946987267 -36.11303801713408,139.98199406864134 -36.11303318424771,139.98199416834723 -36.11302876798785,139.98199398529098 -36.113023893408844,139.9819936800788 -36.113019435023595,139.98199341723554 -36.1130151798924,139.98199354265677 -36.11301057586077,139.98199357735356 -36.11300613504319,139.98199388966094 -36.11300158916578,139.98199444149222 -36.112997122095116,139.98199533700415 -36.11299304199609,139.98199576888004 -36.1129886149265,139.9819965000795 -36.11298387506866,139.9819971293075 -36.11297940778527,139.98199792221948 -36.11297469397984,139.98199832508968 -36.11296971677292,139.98199888698423 -36.112964901245086,139.98199946192855 -36.11296000707915,139.98200001583103 -36.11295524627848,139.98200025720737 -36.1129501620137,139.98200060980923 -36.11294490459849,139.98200123656446 -36.112939931101785,139.98200166105946 -36.112935018705656,139.98200202212752 -36.11292987972544,139.98200250530036 -36.11292430825776,139.9820029505441 -36.11291923973536,139.98200347018087 -36.11291474238597,139.982004000902 -36.11291041317837,139.9820045832174 -36.112906313617614,139.982004919987 -36.11290301121595,139.98200540174807 -36.11289934921128,139.98200564127848 -36.11289498328455,139.98200623821444 -36.11288984812519,139.98200677319633 -36.11288446794599,139.9820071631222 -36.11287941800532,139.98200714961777 -36.11287434073864,139.98200725092434 -36.11286984551598,139.98200706536687 -36.11286507032815,139.98200636813 -36.1128601756267,139.98200571691982 -36.11285511832233,139.9820046515038 -36.11284952710174,139.98200336488506 -36.11284407484889,139.98200202592454 -36.11283840459082,139.98200015698893 -36.11283303791415,139.98199736335994 -36.11282725351028,139.98199464204956 -36.11282259697021,139.98199228532238 -36.112817372390936,139.98198962713627 -36.11281228277183,139.9819870334092 -36.11280700340951,139.9819831107351 -36.11280281144379,139.98197927238627 -36.11279927626375,139.98197589583887 -36.112796293087186,139.9819726755405 -36.112792321789904,139.98196920751374 -36.11278795985887,139.98196534551658 -36.11278410944123,139.98196169938322 -36.11278017911464,139.98195798066527 -36.11277695092818,139.98195397026964 -36.11277282714266,139.98194968464603 -36.11276898596319,139.98194534296624 -36.1127650566302,139.98194037796821 -36.11276156321942,139.98193527996287 -36.11275797756727,139.98193009104352 -36.112754380393056,139.98192463319612 -36.11275112450434,139.98191932639526 -36.112747731604976,139.98191410000283 -36.11274466187199,139.98190910636512 -36.11274208725329,139.98190391235917 -36.11273926039839,139.98189904827487 -36.11273651218873,139.9818937967323 -36.112734147208165,139.98188821233782 -36.11273172919065,139.98188301488096 -36.11272944652909,139.9818775077418 -36.11272715705628,139.98187237124756 -36.11272526626835,139.98186763206897 -36.11272342572563,139.98186260769106 -36.11272179112292,139.98185734685026 -36.112720769342125,139.98185170222416 -36.11271968521485,139.98184651360256 -36.112718739041895,139.98184096331076 -36.11271784302972,139.9818349676224 -36.11271699064665,139.98182915552204 -36.11271618857826,139.98182254998554 -36.11271600398648,139.9818162575387 -36.1127157217475,139.98181001365919 -36.11271534673519,139.98180353367303 -36.11271542331416,139.98179757129395 -36.112715534428695,139.9817919240208 -36.112715910144004,139.98178628449594 -36.11271668307256,139.98178152268747 -36.11271724915524,139.98177649789176 -36.112718076543956,139.98177097832408 -36.112718620887776,139.9817656028344 -36.11271937319278,139.9817607002155 -36.11272019689611,139.9817553935228 -36.11272140886281,139.98175031960332 -36.11272273846767,139.9817451297462 -36.11272425625788,139.9817396788109 -36.112726105551936,139.9817351396983 -36.11272819998182,139.9817306232382 -36.11273021993922,139.9817261475512 -36.11273228214635,139.98172167582524 -36.11273431582137,139.9817172584792 -36.11273613452928,139.98171292601714 -36.112738174059345,139.98170840675567 -36.11274020680531,139.98170348081374 -36.11274252681591,139.98169903153794 -36.112744586157255,139.98169382986552 -36.11274689555202,139.98168793176794 -36.11274912132182,139.98168191873918 -36.11275163422772,139.9816755703306 -36.11275375975838,139.98166964191734 -36.11275559500965,139.98166417150773 -36.11275730258122,139.98165861872937 -36.11275919221704,139.9816532346853 -36.11276081254982,139.98164800272303 -36.1127625871688,139.98164260823307 -36.1127642897692,139.9816373864056 -36.11276597010004,139.98163194745973 -36.1127675826358,139.9816267684997 -36.112769111988655,139.98162150424912 -36.112770754226226,139.98161649416318 -36.11277239561109,139.9816113946337 -36.11277383121183,139.98160646339846 -36.112775449745094,139.98160206538347 -36.11277707462686,139.98159708579945 -36.112778722535595,139.9815918663091 -36.11278038601885,139.9815862559788 -36.11278201971852,139.98158039829778 -36.112783687097284,139.98157487990736 -36.112785596288724,139.9815692979393 -36.11278715936571,139.98156386098532 -36.11278881688151,139.98155868628703 -36.11279052877341,139.98155327032958 -36.11279194451088,139.981547722448 -36.112793402395916,139.9815425478673 -36.112794740341826,139.98153687906424 -36.112796142596835,139.98153129515669 -36.11279752159999,139.98152553883182 -36.11279870803353,139.98152006161175 -36.11279976143305,139.98151579840945 -36.11280073704562,139.9815113610214 -36.112801650303226,139.98150583796357 -36.11280321173497,139.98150004206556 -36.112804504567386,139.98149372983164 -36.11280573492295,139.98148791769333 -36.11280708432773,139.98148203846128 -36.11280842068323,139.9814766214537 -36.112809809001035,139.98147072235452 -36.11281112820557,139.98146485736874 -36.11281280232386,139.98145863903554 -36.11281449130758,139.9814525711385 -36.11281596423143,139.98144629635715 -36.11281771812552,139.98143976131195 -36.112819045948584,139.98143420161114 -36.11282080593602,139.98142822408582 -36.11282214435012,139.98142252716667 -36.11282356329227,139.98141732414558 -36.11282475883133,139.98141246820938 -36.11282608903804,139.98140774749822 -36.11282696081325,139.98140317282545 -36.11282809785707,139.98139807505365 -36.11282941255143,139.98139258412903 -36.11283080216126,139.98138662490788 -36.11283276552429,139.98138058644287 -36.11283412343623,139.98137493654053 -36.1128354310456,139.98136973652763 -36.112836801720434,139.98136471831526 -36.11283811089121,139.98135973711268 -36.1128393984219,139.9813546822907 -36.11284092813047,139.9813493696208 -36.112842292998074,139.98134384705213 -36.11284375341771,139.98133867383083 -36.11284474277956,139.98133348381847 -36.11284626488989,139.98132830308668 -36.11284729100948,139.98132381929344 -36.11284863258466,139.98131905567874 -36.11284998933604,139.98131427409172 -36.11285111977087,139.98130972174204 -36.11285230200988,139.98130506817384 -36.11285352668119,139.98130009777458 -36.11285491101602,139.98129458134676 -36.112856425514096,139.98128886778534 -36.11285744686053,139.9812830959611 -36.11285876771936,139.98127739343678 -36.1128600486427,139.98127186044653 -36.112861479773684,139.9812665337335 -36.112862771571976,139.98126191566746 -36.11286409355399,139.98125732892254 -36.11286534682177,139.98125286939165 -36.11286672250897,139.98124788427 -36.11286796279771,139.98124281110142 -36.112869138579875,139.9812376178472 -36.112870626544066,139.98123250410242 -36.112872498360325,139.9812270210756 -36.11287456663913,139.9812215044361 -36.11287657042233,139.98121628021002 -36.112879097233225,139.98121118946204 -36.11288141545739,139.98120616390213 -36.11288376019162,139.98120125723133 -36.11288618105172,139.98119629420466 -36.11288815539758,139.98119150596705 -36.11289063597138,139.98118666280052 -36.112893566359766,139.98118251199102 -36.11289649161873,139.98117800430538 -36.11289963886998,139.98117388409653 -36.11290265264805,139.9811702730612 -36.11290522175234,139.98116683329388 -36.11290778531446,139.9811636723704 -36.11291079608917,139.98116000814744 -36.112913770549895,139.981156387959 -36.11291732077647,139.98115283902158 -36.112921324987646,139.98114943187244 -36.11292528879754,139.9811458881807 -36.11292940212129,139.98114214498904 -36.11293301599083,139.9811384304345 -36.11293674186961,139.98113514094857 -36.11294043686322,139.9811322534564 -36.11294419101618,139.98112925919398 -36.11294791888956,139.98112677946486 -36.11295186067779,139.98112461182689 -36.11295604474383,139.98112276270479 -36.11296025267997,139.98112116976517 -36.112964048906754,139.9811196637157 -36.11296817317405,139.98111828739317 -36.11297193914835,139.98111707323986 -36.11297633767479,139.98111558505653 -36.11298046465929,139.98111454566993 -36.11298458046103,139.9811130725936 -36.112988318426076,139.98111193537468 -36.11299202246632,139.98111072518506 -36.11299583721229,139.98110984489261 -36.11300017615349,139.98110868163445 -36.11300412225679,139.9811081308214 -36.113008039459196,139.98110720054308 -36.11301174516185,139.98110637758774 -36.11301587884682,139.98110601332365 -36.11302007189911,139.98110503145438 -36.1130247794015,139.98110474802408 -36.11302899628535,139.9811039073169 -36.11303268829185,139.98110351713876 -36.113036507112994,139.98110279572003 -36.11304118894384,139.98110210776153 -36.11304590506853,139.98110163725062 -36.11305044468984,139.98110095841352 -36.11305507976856,139.9811001813752 -36.11305991929768,139.98109989067126 -36.11306501966496,139.98109950300014 -36.113069882042915,139.98109939993145 -36.1130747850554,139.98109930667115 -36.113078867196855,139.98109873391962 -36.113083554319765,139.98109788673432 -36.11308845621381,139.98109676142985 -36.11309381688049,139.98109566637476 -36.11309931927404,139.98109446048971 -36.11310471063109,139.9810929595592 -36.113109966477985,139.9810917682099 -36.11311534100327,139.98109080830181 -36.11312091577634,139.9810891142987 -36.11312568378249,139.98108817161628 -36.1131307117171,139.98108721947028 -36.11313552523848,139.98108601057788 -36.11313994766588,139.98108469922528 -36.11314454150844,139.98108361081407 -36.113149090691856,139.98108252936007 -36.11315382733601,139.98108164322093 -36.11315839150246,139.98108075256062 -36.11316317051246,139.98107962539734 -36.113168300174245,139.98107825921107 -36.11317364963162,139.9810772903714 -36.11317868506741,139.98107598446492 -36.113183662125785,139.9810751045376 -36.11318911613514,139.98107430673937 -36.11319465715495,139.9810735107084 -36.113200462154744,139.98107352282724 -36.113206219772835,139.9810731982369 -36.11321221999727,139.9810731107952 -36.11321727514795,139.98107333717383 -36.11322201448955,139.98107345579902 -36.11322681842219,139.98107379995992 -36.113232235581556,139.9810749468135 -36.113237937149215,139.9810760274382 -36.11324342835586,139.98107753743557 -36.11324899126644,139.98107907376198 -36.1132538118955,139.98108119508498 -36.11325903757917,139.9810837281234 -36.11326373142236,139.98108640750087 -36.11326875629548,139.98108943265407 -36.11327352803949,139.98109248827282 -36.11327822734262,139.98109487953457 -36.11328244920123,139.98109772353172 -36.11328639167813,139.98110040278627 -36.11329005261288,139.98110357578167 -36.113293621821676,139.98110635447645 -36.11329721397971,139.9811094341175 -36.11330084634446,139.98111264928656 -36.11330423740645,139.9811162920294 -36.11330743834276,139.98111983808224 -36.11331071150845,139.98112373399206 -36.11331416676118,139.98112752602734 -36.113317239560594,139.98113162260466 -36.11332028372578,139.9811357774539 -36.1133228177422,139.98114017826506 -36.11332562910044,139.9811448469221 -36.113328621539075,139.9811501145286 -36.11333202862881,139.9811555581749 -36.11333469150699,139.98116048286454 -36.11333681350939,139.9811652625921 -36.113338721644034,139.9811705262978 -36.113340633768004,139.98117506164542 -36.11334210536819,139.98117956406887 -36.113343919639085,139.98118424379632 -36.113346015030096,139.98119033841652 -36.113348171317895,139.98119655557696 -36.11335004109051,139.98120321804538 -36.11335170797013,139.98120951393295 -36.11335349152559,139.98121616733317 -36.11335482839615,139.98122248366056 -36.11335640364131,139.98122825930994 -36.11335748509323,139.98123442288295 -36.113358826156784,139.9812412052418 -36.113360040598906,139.98124772444973 -36.11336144266777,139.98125435440267 -36.113362537945676,139.98126097573714 -36.113363605436014,139.9812667038517 -36.113364608318946,139.98127231770027 -36.11336509466546,139.98127699475998 -36.11336577567698,139.98128227011625 -36.113366518837054,139.98128714979916 -36.11336748107888,139.98129226415008 -36.11336873627354,139.98129846835195 -36.11337027243834,139.98130487410367 -36.11337127531239,139.98131132653063 -36.11337156794864,139.98131721792907 -36.11337218777341,139.98132333415145 -36.11337253571091,139.9813287155521 -36.11337307865982,139.98133429718493 -36.11337338928878,139.9813389996489 -36.1133738347019,139.98134402572242 -36.11337435414085,139.98134919253982 -36.1133746145523,139.9813547601653 -36.11337501217214,139.98136006598196 -36.11337562047516,139.98136560513558 -36.1133760268566,139.981370928015 -36.11337601655965,139.9813762688006 -36.11337645861446,139.98138204860086 -36.11337646557271,139.98138763209315 -36.113376716751844,139.9813927442731 -36.113377287694796,139.9813985621011 -36.113377731449866,139.98140358706513 -36.11337818011226,139.98140797935795 -36.113378373964885,139.98141222371393 -36.11337888419066,139.98141497520027 -36.113378852868586,139.98141595958398 -36.11337880236079,139.98141620548938 -36.11337879883847,139.98141618559336 -36.11337890948188,139.9814164665844 -36.11337877064369,139.98141635524667 -36.11337879112548,139.98141647293815 -36.113378710914844,139.9814165087002 -36.113378525845356,139.98141621434095 diff --git a/gnssnav/config/course_data/pre_line.csv b/gnssnav/config/course_data/pre_line.csv deleted file mode 100644 index 2d066608..00000000 --- a/gnssnav/config/course_data/pre_line.csv +++ /dev/null @@ -1,411 +0,0 @@ -36.113143771908774,139.9810963796176 -36.11314330047328,139.98109640171893 -36.11314151148857,139.98109628692825 -36.113138954094154,139.98109651309554 -36.11313674227604,139.98109659641239 -36.11313336856847,139.98109676246028 -36.11312853296092,139.98109682407028 -36.11312341169616,139.98109698824138 -36.11311635768789,139.98109710833205 -36.11310807076071,139.98109723817507 -36.11309893930732,139.9810976565442 -36.11308940233398,139.98109851176338 -36.11307964120883,139.98109948104636 -36.113070799652284,139.98110031344717 -36.11306271627457,139.9811010990845 -36.113055834091746,139.98110150224983 -36.11304925217149,139.98110217720446 -36.113043011171165,139.98110307581615 -36.1130374392318,139.98110383517357 -36.1130315788392,139.98110448076156 -36.11302533725603,139.9811050306084 -36.11301986036793,139.98110535740526 -36.11301501956698,139.9811061940279 -36.1130101403378,139.98110733624765 -36.11300449192975,139.9811090284236 -36.11299805194952,139.98111013729337 -36.11299170008031,139.98111076130718 -36.11298706337613,139.98111142922576 -36.11298253094329,139.98111249130864 -36.11297768794784,139.98111501107533 -36.11297402309899,139.9811171000929 -36.11296982996585,139.98111883059818 -36.11296492219704,139.9811201692932 -36.11296033669829,139.98112112374784 -36.11295555071313,139.98112280935163 -36.112950426449466,139.98112503229962 -36.112944745278995,139.9811283521069 -36.11293909916913,139.9811315127579 -36.11293441689813,139.98113426936476 -36.11293018269976,139.98113692023378 -36.112925929933255,139.9811398644101 -36.11292120754851,139.98114320750395 -36.11291698711681,139.98114643632888 -36.11291263371082,139.98115022948753 -36.112908645274,139.9811542245084 -36.11290424112188,139.98115889675833 -36.11289992042469,139.98116351235785 -36.1128954424592,139.9811685178701 -36.112891636812904,139.98117302255284 -36.112888085588665,139.98117759579628 -36.112883808847684,139.98118331843395 -36.11287981298896,139.98118877480943 -36.11287600419227,139.9811946416166 -36.11287273841116,139.98120093760838 -36.112869390967305,139.98120765896698 -36.11286597147413,139.98121482705025 -36.112862405651114,139.98122229936072 -36.112859264745914,139.9812295710748 -36.11285716660135,139.98123646060506 -36.11285510398639,139.98124263693376 -36.11285339917281,139.98124834330875 -36.112851907614704,139.9812543860798 -36.11285032370222,139.98126145594085 -36.11284826271586,139.98126855735387 -36.11284660328345,139.98127564177832 -36.11284473074559,139.98128295603343 -36.11284331230957,139.9812899199349 -36.1128413884797,139.98129790285728 -36.11283965727459,139.98130622470927 -36.1128382351082,139.98131362011688 -36.112836616387284,139.98132114572616 -36.11283467906318,139.98132765523627 -36.11283297854257,139.98133294603713 -36.112831656978905,139.98133711785422 -36.1128305940642,139.9813414466395 -36.11282918270681,139.98134773131304 -36.11282795935294,139.98135431425175 -36.11282668370019,139.9813612272636 -36.11282553404192,139.98136814329487 -36.11282437116756,139.9813734985894 -36.112822990802194,139.98137918678873 -36.11282141739235,139.9813856874787 -36.11282000682611,139.98139198253122 -36.112818556920274,139.9813976499693 -36.11281709213524,139.98140265038785 -36.112816221126714,139.98140737994242 -36.11281559314735,139.981412800582 -36.112813960409504,139.9814195060797 -36.112812309809364,139.98142672182578 -36.11281092874796,139.9814334709931 -36.11280951071575,139.9814410444878 -36.11280809200132,139.98144859377416 -36.11280634307476,139.98145591421562 -36.11280436426195,139.98146320560775 -36.11280277905575,139.9814705085141 -36.112800963028455,139.9814780950379 -36.112799058361226,139.98148511489447 -36.112797858422134,139.98149021701218 -36.112796698216904,139.98149534267912 -36.11279558459377,139.9815007514418 -36.11279416025427,139.9815073582449 -36.112792340394975,139.98151493988757 -36.11279050788206,139.98152249311596 -36.11278863214227,139.981529915657 -36.11278673171756,139.98153794838936 -36.11278463425826,139.9815460558771 -36.112782701467204,139.98155392564516 -36.11278068273825,139.98156171542217 -36.11277861023285,139.9815692441754 -36.11277660700564,139.98157658126547 -36.11277464740808,139.98158388203632 -36.112772447033024,139.98159143707997 -36.112770246585214,139.98160004869186 -36.112768313299135,139.98160805971864 -36.11276675738923,139.98161558088233 -36.112765228464866,139.98162395305255 -36.112762869265026,139.98163359546044 -36.11276041845139,139.98164266007618 -36.11275785316091,139.98165095594848 -36.112756039795194,139.98165827481907 -36.11275416509366,139.98166572986239 -36.11275247053915,139.98167393729952 -36.11275048954687,139.98168170983948 -36.11274859614727,139.98168952286926 -36.11274654780384,139.9816973172084 -36.11274466686289,139.9817050391457 -36.1127425677145,139.9817130439771 -36.11274051163663,139.9817207756811 -36.11273826405517,139.98172813879603 -36.11273623270293,139.98173524910572 -36.11273421692846,139.98174287376995 -36.11273239978461,139.98175053179588 -36.11273055864648,139.98175783773988 -36.112728606856024,139.98176507786152 -36.11272697068902,139.98177234487773 -36.11272563456443,139.98177924435853 -36.11272458029622,139.98178623144724 -36.112723375383375,139.98179333029543 -36.11272233139981,139.9817997269069 -36.11272162362686,139.98180580268397 -36.11272085220546,139.98181236742175 -36.11272050043552,139.98181931712074 -36.11271994194882,139.98182651734422 -36.11271960324722,139.9818333872015 -36.11271993041295,139.9818404583678 -36.11272028187452,139.98184839723783 -36.112720495873496,139.98185634060857 -36.11272122350383,139.98186462116044 -36.11272204745927,139.98187241279624 -36.11272305397839,139.98187942573756 -36.1127239921623,139.98188568360612 -36.11272527331489,139.98189142970202 -36.11272737786044,139.9818987699249 -36.11272953700687,139.9819061917625 -36.112732121316505,139.98191356120913 -36.11273502499898,139.98192205215923 -36.112738031754255,139.98193028805798 -36.11274099888277,139.9819377564473 -36.11274330278236,139.98194333783974 -36.11274526410814,139.98194739870348 -36.11274741267446,139.98195235308359 -36.11274981262626,139.98195735952143 -36.1127526511845,139.9819624890589 -36.112756377297124,139.98196802888464 -36.11276068686583,139.9819739527467 -36.11276459896098,139.98197929141526 -36.112768357957826,139.98198445964135 -36.11277231020467,139.98198973908768 -36.1127764407071,139.98199462135085 -36.11278103746424,139.98199943798664 -36.11278597778651,139.98200426679583 -36.11279159510322,139.9820091897493 -36.112797497943475,139.98201416284738 -36.112802833529685,139.9820178238173 -36.11280741218508,139.98202086811796 -36.11281212798768,139.98202388368986 -36.112816983234474,139.9820269804482 -36.11282139950288,139.9820298451714 -36.11282602458856,139.98203222530702 -36.11283139969262,139.98203443037022 -36.11283683781961,139.9820364291621 -36.11284283551912,139.98203796440683 -36.11284963151291,139.98203971029577 -36.112855900129155,139.98204124555426 -36.11286224906519,139.98204201683444 -36.112868106108444,139.98204267166435 -36.112873369143784,139.98204275321004 -36.11287936023144,139.98204275419255 -36.11288581779687,139.98204264996096 -36.11289175167926,139.98204212087398 -36.11289767456651,139.98204129242413 -36.112903215717814,139.98204046070657 -36.11290928443387,139.9820396753275 -36.11291530199711,139.9820391490902 -36.1129215732303,139.9820385276778 -36.11292735095192,139.98203807036833 -36.11293354564947,139.9820368952451 -36.11293983519595,139.98203596321605 -36.11294627347709,139.982035114024 -36.11295190373007,139.9820344757395 -36.112957426740515,139.9820339800051 -36.11296269288931,139.98203322766534 -36.11296820362081,139.98203231400407 -36.112972493068206,139.98203185934707 -36.112976625097915,139.98203166177413 -36.112981288213966,139.98203117293076 -36.11298673988333,139.98203071293887 -36.11299241932613,139.98203006098004 -36.11299833408506,139.9820290484892 -36.11300404123103,139.9820284177059 -36.11300908011844,139.98202804768576 -36.11301505708774,139.9820272088281 -36.11302127172512,139.9820266261748 -36.11302808686148,139.98202554181054 -36.113035307665754,139.98202445963744 -36.113043254835134,139.98202365836013 -36.1130507564664,139.98202310911518 -36.11305716458209,139.9820225445787 -36.11306365501182,139.98202161816104 -36.11307037859858,139.9820206397478 -36.113077188486756,139.982019955189 -36.11308345228016,139.98201935962356 -36.113089318634295,139.98201869666565 -36.11309454402291,139.98201810629743 -36.113099415644356,139.98201768343253 -36.11310455389802,139.98201700695378 -36.11310999823141,139.9820166231348 -36.11311546022184,139.98201618994275 -36.11312099952475,139.9820150067595 -36.11312687033546,139.9820139866859 -36.1131315422286,139.98201295272005 -36.11313661639903,139.9820125449276 -36.1131432056287,139.98201141309823 -36.11315032887234,139.98201046557355 -36.1131578168992,139.982009176353 -36.11316537384945,139.98200804850347 -36.11317336575803,139.982007068703 -36.113180659078324,139.98200617328683 -36.113187925910395,139.98200546398814 -36.113194390877446,139.98200512792548 -36.11320084670531,139.98200462276714 -36.11320701927851,139.98200399818703 -36.11321362943228,139.98200323450982 -36.11322049131405,139.98200211573632 -36.11322777785743,139.982001263463 -36.11323513852612,139.98200054488103 -36.11324193198919,139.98199991388108 -36.11324770037867,139.98199922264726 -36.113251973891494,139.9819991530078 -36.11325551342783,139.98199885816067 -36.113258384774866,139.9819989162447 -36.11326369701134,139.98199803046793 -36.11326932524168,139.98199646412954 -36.113275233920994,139.98199476010424 -36.11328152997928,139.98199300153837 -36.113287750547336,139.98199100773735 -36.11329285584683,139.98198893132474 -36.11329729486345,139.98198712046624 -36.113302048439344,139.98198487293078 -36.11330617608818,139.98198199649207 -36.11331096501717,139.98197855221014 -36.113316054471575,139.9819752559116 -36.11332113427644,139.9819716274133 -36.11332619076477,139.98196773513158 -36.11333128159888,139.98196409834287 -36.11333602951746,139.98196044279325 -36.113340889023554,139.98195643621278 -36.113346124920284,139.9819514763635 -36.113351279445325,139.9819460624527 -36.11335617701815,139.981940731468 -36.11336091702453,139.9819351530513 -36.11336416768612,139.98193086859143 -36.11336800535189,139.98192569837454 -36.1133722150284,139.98191967953562 -36.113376384386925,139.9819126568351 -36.113380615863484,139.9819049605909 -36.11338447204076,139.98189713115957 -36.113388258630245,139.98188903038718 -36.11339171950888,139.981880976461 -36.113394696108394,139.98187235247366 -36.113397708735796,139.98186344869953 -36.1134002786781,139.98185460820878 -36.11340268100139,139.98184641619997 -36.113404136152326,139.98183814163278 -36.11340456374244,139.98182901195477 -36.11340592657195,139.98181908856517 -36.11340718432204,139.98180993167784 -36.113407081389255,139.98180111006494 -36.113406842751985,139.98179115077392 -36.11340685038641,139.98178162541677 -36.11340730080855,139.9817733170171 -36.11340717925431,139.9817653772332 -36.11340703135245,139.98175764169093 -36.11340623554198,139.9817499150629 -36.11340590807142,139.98174201090336 -36.113405260925965,139.98173381621805 -36.113404401802136,139.9817257816921 -36.11340367616556,139.9817180056964 -36.11340320814968,139.98170975151203 -36.113402773849295,139.98170111455948 -36.11340210958849,139.98169215124682 -36.113401625339954,139.98168278661842 -36.11340120940249,139.98167303206458 -36.113400907014146,139.98166322597123 -36.11340063898277,139.98165501959957 -36.1134002908072,139.98164794491595 -36.113399968774566,139.98164059540193 -36.11339952213211,139.98163415111176 -36.11339891386003,139.9816273655258 -36.11339817225475,139.9816198718604 -36.113397438074905,139.98161152496462 -36.11339665402643,139.98160349556073 -36.113396228620225,139.98159593550002 -36.11339563698366,139.98158943801278 -36.113395306725266,139.98158289074223 -36.11339479632223,139.98157653176014 -36.113394792026945,139.98157084573808 -36.11339468705786,139.98156460276618 -36.113394515239555,139.98155717553536 -36.11339423719889,139.98154942701822 -36.113394170894125,139.98154188035977 -36.113393905153295,139.98153452385097 -36.113393738934725,139.98152731756224 -36.11339340317181,139.98151999245272 -36.11339321094161,139.9815124158146 -36.113392666131354,139.98150483952347 -36.11339203800407,139.98149744980705 -36.11339164129632,139.981489959826 -36.11339107046285,139.9814821058258 -36.11339049529946,139.98147351433488 -36.11339047505907,139.98146457907714 -36.11339010202835,139.98145529941556 -36.113389123081895,139.98144542022297 -36.11338843333755,139.98143458772202 -36.11338795101999,139.98142544989875 -36.113387241864686,139.9814181836598 -36.113387214453546,139.98141155491948 -36.113386618466066,139.98140438800826 -36.113386338678765,139.98139755368464 -36.11338624609271,139.98139068885988 -36.113386159504756,139.98138437221715 -36.11338622831192,139.98137797077658 -36.113386374123436,139.98137135522083 -36.11338626588561,139.98136399199848 -36.11338627083855,139.98135712085437 -36.11338588820389,139.98135090792238 -36.11338515012889,139.98134471222215 -36.1133849402836,139.98133737308075 -36.11338483969362,139.98133066108457 -36.113384504357725,139.98132368892902 -36.113384433596416,139.98131721326592 -36.11338390913052,139.98130982003588 -36.113383267777216,139.98130178595412 -36.11338261188205,139.9812937354804 -36.113382110685365,139.98128492492046 -36.113381759716795,139.98127610567806 -36.11338168651903,139.98126845366912 -36.113380981177215,139.9812609128353 -36.113380277132855,139.98125287907962 -36.11337890480401,139.98124505210663 -36.11337767066986,139.98123748895497 -36.11337618543478,139.98123009040376 -36.11337472417706,139.9812225558907 -36.113372591964435,139.98121547182163 -36.11337024128102,139.98120758163125 -36.1133673436918,139.98119892932377 -36.11336234039901,139.98119099949878 -36.11335948863125,139.98118411347667 -36.113357154555175,139.9811779094549 -36.11335487883755,139.98117213376173 -36.1133519178286,139.9811660554128 -36.113347995482435,139.98115926086956 -36.11334409230416,139.9811529764618 -36.11334031187566,139.98114757294903 -36.113336567139235,139.98114264495402 -36.11333272385954,139.98113768765384 -36.1133282798425,139.98113281456517 -36.11332335444858,139.981127543009 -36.11331832247565,139.9811219019428 -36.11331359701272,139.9811172853659 -36.1133090418738,139.9811127638852 -36.11330369178246,139.98110822261552 -36.11329843623418,139.9811038945087 -36.11329321624371,139.98110042620814 -36.113288723345484,139.9810975031778 -36.11328311241038,139.98109431489627 -36.113277321415815,139.98109146409686 -36.11327095765377,139.98108957173727 -36.1132639237564,139.98108740222608 -36.11325695667706,139.98108503984292 -36.113251050206145,139.98108330064701 -36.11324596670652,139.98108168042367 -36.113241522642745,139.98108046494713 -36.11323685432782,139.98107994651744 -36.11323117716669,139.98107964597858 -36.113225649702464,139.98107911873896 -36.113220902955725,139.9810783644446 -36.113216786728735,139.98107781448178 -36.113212978416655,139.98107756159402 -36.113210746368665,139.98107763083036 -36.11321005865316,139.98107801617735 -36.11321001350784,139.9810781260831 -36.113209624395495,139.981078449884 -36.11320962376039,139.98107858476607 -36.11320925565184,139.98107875839548 -36.1132093456865,139.981078798085 -36.11320931463475,139.98107900225867 -36.11320927950982,139.98107887881443 -36.113209245372246,139.98107881617514 -36.11320938337019,139.98107882897182 -36.11320918562513,139.98107880986282 diff --git a/gnssnav/config/course_data/precourse.csv b/gnssnav/config/course_data/precourse.csv deleted file mode 100644 index df550919..00000000 --- a/gnssnav/config/course_data/precourse.csv +++ /dev/null @@ -1,399 +0,0 @@ -36.11317102125407,139.98105915573356 -36.11317092108219,139.98105906009548 -36.11317115339315,139.98105903834562 -36.11317139836712,139.98105903726898 -36.113171673267225,139.98105919492278 -36.11317182803843,139.98105950858977 -36.11317185967462,139.98105950465666 -36.11317174384307,139.98105971273606 -36.11317008061029,139.9810599454943 -36.11316691970899,139.98106037350115 -36.1131635699875,139.9810608794211 -36.113159498337524,139.98106116948512 -36.11315441108667,139.9810614169245 -36.11314926932107,139.98106193082654 -36.1131448989619,139.9810622220839 -36.113139683148084,139.9810627499734 -36.11313477791616,139.98106318084737 -36.11312968909013,139.98106392430404 -36.1131240586043,139.981064756829 -36.113118399994285,139.98106537741646 -36.11311288771157,139.9810663405994 -36.113107217092676,139.98106705616797 -36.113102098889435,139.98106767354574 -36.113096539370616,139.98106829412643 -36.11309073637073,139.98106896517416 -36.11308556924795,139.9810695235294 -36.11308039429926,139.9810702259969 -36.1130753763797,139.98107091264734 -36.11307056763586,139.98107136842455 -36.11306535225549,139.98107191293002 -36.11305955833772,139.98107223927144 -36.11305344962385,139.98107264084595 -36.11304728763996,139.98107314869245 -36.113041867394976,139.98107350853172 -36.11303579324257,139.98107419136434 -36.11302907162396,139.9810753402663 -36.11302221480933,139.9810762252667 -36.11301525687994,139.9810769755283 -36.11300852188473,139.98107807721962 -36.11300235503563,139.9810793950053 -36.11299628883927,139.98108168063146 -36.11298960058311,139.98108476669086 -36.11298298617527,139.98108768433667 -36.112975934119376,139.9810902854268 -36.11296950339503,139.98109258910054 -36.11296456370438,139.98109469164606 -36.11296007445793,139.9810970015866 -36.11295488557761,139.9810999901617 -36.11295005424944,139.98110358701186 -36.11294480239594,139.981107178439 -36.11293973505186,139.9811110524239 -36.11293504056416,139.98111531685362 -36.112930303189856,139.98111968208462 -36.11292544543992,139.981124267149 -36.112920661335,139.98112932542676 -36.11291615273681,139.98113588061 -36.11291124175976,139.98114253372202 -36.11290712614562,139.98114934443691 -36.11290329014529,139.9811578632716 -36.11289973095021,139.9811675776047 -36.11289649463387,139.9811777576875 -36.112893261673996,139.98118779729026 -36.11289040616964,139.98119902329788 -36.112888116968755,139.98120972869702 -36.11288819853158,139.98122102866253 -36.11288853419132,139.98123252922147 -36.11288765046592,139.98124408966115 -36.1128867992945,139.9812550480541 -36.1128855442814,139.98126676783247 -36.11288397688149,139.9812778636452 -36.11288196883545,139.981289218704 -36.11287990678162,139.98129932578954 -36.11287804056146,139.98130876738713 -36.11287606123689,139.98131723902262 -36.11287411006955,139.9813256800661 -36.11287214257376,139.98133346783163 -36.1128701508243,139.98134185746213 -36.112868398734314,139.98134998605565 -36.112867610153884,139.98135875577202 -36.11286617858049,139.98136768500095 -36.112864603939606,139.9813769067443 -36.11286257607839,139.98138629644876 -36.112860338235414,139.98139630000682 -36.11285780119229,139.98140555356912 -36.11285521884594,139.98141450413473 -36.11285292748236,139.98142264566312 -36.11285116384645,139.9814301335569 -36.11284928951538,139.98143713415317 -36.112847328784284,139.9814438640664 -36.11284533845656,139.98145050576483 -36.11284302530232,139.98145715072494 -36.112841070088884,139.98146319603453 -36.11283937555661,139.981469325851 -36.1128376065247,139.98147610481908 -36.112835659522574,139.98148287409066 -36.11283374420658,139.98148976437858 -36.11283148425876,139.9814968215302 -36.11282922636638,139.98150380630722 -36.11282682799763,139.98151034398376 -36.11282461598724,139.981516919433 -36.11282202288199,139.981523438586 -36.11281971670252,139.98152951253337 -36.11281770580383,139.98153588378523 -36.11281527610526,139.98154285598892 -36.11281248968274,139.98154979427727 -36.11280950289723,139.98155684403093 -36.112806448431456,139.9815640372001 -36.112803558866,139.9815709921735 -36.11280047234937,139.98157818028633 -36.11279738478427,139.98158518672122 -36.11279455970849,139.98159162645106 -36.112792381572454,139.98159802914776 -36.11279086779892,139.9816040468099 -36.112788717594015,139.98161050906137 -36.112786106637465,139.98161686107008 -36.11278390268374,139.98162285150894 -36.112781543433705,139.9816289260278 -36.112779278768606,139.9816348306498 -36.11277694253065,139.98164065181615 -36.112774760365355,139.9816461463299 -36.11277242973458,139.98165245039058 -36.1127700396269,139.98165899197076 -36.11276850069698,139.9816658035655 -36.112767016518944,139.98167176290508 -36.112765902106624,139.98167787170223 -36.11276416202087,139.98168450419786 -36.11276246753595,139.98169115167008 -36.11276110241377,139.98169770834298 -36.11275952517344,139.98170452958502 -36.11275817009509,139.98171119560087 -36.1127566511169,139.98171825200276 -36.11275514971259,139.98172451401692 -36.112753492089865,139.98173123628877 -36.11275192611621,139.98173811452915 -36.11275013646435,139.9817452322822 -36.11274883654999,139.9817519056259 -36.112747608675,139.98175902837983 -36.11274705682938,139.98176715523616 -36.11274644912797,139.9817759395516 -36.11274596905787,139.9817849405654 -36.11274516933066,139.98179391671962 -36.11274466186728,139.9818013949022 -36.112744878800214,139.98180958669133 -36.11274507248215,139.9818175856246 -36.112745448622114,139.98182570596916 -36.112745767827214,139.98183281743033 -36.112746436588594,139.98183960448287 -36.11274740279874,139.9818459054134 -36.112748802197004,139.98185229249395 -36.112751090977206,139.98185918556325 -36.112753364249244,139.9818667109458 -36.112755895805364,139.98187477701282 -36.11275909816365,139.98188347662906 -36.11276202218657,139.9818920391091 -36.11276528344247,139.98190029695027 -36.11276846856668,139.98190807690796 -36.11277206991457,139.9819154831206 -36.11277591882479,139.9819226488547 -36.112780181779435,139.98192991406302 -36.11278439671248,139.98193645377856 -36.11278895377055,139.98194295604495 -36.11279332557808,139.98194882658186 -36.11279813744797,139.98195419506862 -36.112803121418224,139.98195900319323 -36.112808910872324,139.98196383087335 -36.11281453952077,139.9819686203808 -36.112820416590374,139.9819734094042 -36.11282571780897,139.98197811170363 -36.112831211816506,139.98198258076718 -36.112836288701764,139.98198610948657 -36.11284234373136,139.98198921243954 -36.11284839291922,139.98199248903225 -36.11285471110292,139.98199571098982 -36.11286128931805,139.98199880207238 -36.11286795623583,139.9820022022522 -36.1128738488246,139.98200403869484 -36.11287916457055,139.982005760404 -36.11288423789362,139.98200673787576 -36.112889542075585,139.98200762177848 -36.11289555740218,139.98200866821477 -36.1129026270162,139.9820095015536 -36.11291025400602,139.98201041601914 -36.11291837386676,139.98201157954395 -36.112926087990736,139.98201227187894 -36.11293425892387,139.98201245474 -36.112942755491986,139.98201190978654 -36.11295179642243,139.98201053105646 -36.11296084504002,139.9820086177681 -36.112970140108295,139.98200647585915 -36.112979175143494,139.98200434463706 -36.11298837135177,139.98200188936806 -36.112997250537454,139.98199937807158 -36.113006519975656,139.98199659158328 -36.11301549278021,139.9819934946607 -36.11302414072362,139.9819903060879 -36.11303192385456,139.98198774099865 -36.113039449406585,139.9819848602818 -36.113046262946895,139.98198221738306 -36.11305358567917,139.98197959946793 -36.11306068446973,139.981976779868 -36.11306801793339,139.98197404386917 -36.11307507215479,139.9819717911276 -36.11308213163858,139.98196966281907 -36.113088590316494,139.98196772868445 -36.11309533510521,139.98196560706933 -36.113102178583205,139.98196399894184 -36.11310924581644,139.98196240237579 -36.1131164856596,139.98196065835026 -36.11312357198825,139.98195925710502 -36.113130536257735,139.98195756115192 -36.11313751896406,139.98195618947565 -36.11314418435813,139.98195484480627 -36.11315086684402,139.98195367285754 -36.11315756124333,139.9819523041294 -36.11316408480531,139.98195099573678 -36.11317064823048,139.9819498793506 -36.113177151870644,139.98194907780305 -36.11318399887683,139.9819478199857 -36.11319059803616,139.9819469336651 -36.11319664446173,139.98194597416747 -36.11320282120367,139.98194541835727 -36.11320889347001,139.98194438063217 -36.11321514506063,139.98194386269802 -36.11322132679585,139.98194294916794 -36.113227383948725,139.98194228528413 -36.11323338239445,139.9819417057569 -36.11323916061249,139.9819412583616 -36.11324487980869,139.98194069040642 -36.113250579476315,139.98194002591856 -36.11325607098248,139.98193968577547 -36.11326145250758,139.98193929257076 -36.113266575472046,139.98193855260672 -36.11327165207749,139.98193781897578 -36.11327703548768,139.98193717715267 -36.11328244006017,139.98193625649634 -36.113287647846235,139.98193526157 -36.11329275116972,139.98193458222073 -36.11329785585324,139.98193365286963 -36.1133030160037,139.98193240255307 -36.11330831041147,139.98193088788517 -36.1133131885905,139.9819297834376 -36.113318634500324,139.98192815255746 -36.11332375993935,139.98192636426506 -36.11332895146735,139.98192442754436 -36.11333414107644,139.9819221000646 -36.11333952266912,139.9819198720817 -36.11334498479067,139.98191760571925 -36.113350017536284,139.98191511783375 -36.113354774439344,139.98191241958932 -36.113359198611064,139.98190944682204 -36.11336385584482,139.98190623773544 -36.11336857544291,139.98190347395982 -36.11337313484437,139.98190054013423 -36.1133777596827,139.9818969567079 -36.113382647177865,139.98189337281985 -36.11338745512564,139.98188969447628 -36.11339152717961,139.98188625425587 -36.11339523727457,139.98188238087735 -36.11339844972979,139.98187816372814 -36.113402150178,139.98187368229668 -36.113405512514476,139.98186912525793 -36.11340930713366,139.98186373275456 -36.11341262718805,139.98185719156388 -36.11341558336067,139.98185048643072 -36.11341888789598,139.98184383262824 -36.113421778974974,139.9818372380252 -36.113424024148216,139.9818305271744 -36.11342574760426,139.98182443022196 -36.11342716487692,139.9818187052731 -36.113428292286805,139.98181361273404 -36.11342971189981,139.98180739652307 -36.113430993407704,139.98180034592446 -36.11343225071714,139.98179231566644 -36.113433056202666,139.98178483159302 -36.11343340904,139.98177719842263 -36.1134335353495,139.98177012303913 -36.113433800426236,139.98176279003758 -36.113433890516745,139.98175559774322 -36.11343402599789,139.98174904228355 -36.1134338427951,139.98174223668852 -36.11343263485345,139.98173469315802 -36.113432111561714,139.98172606335766 -36.113431001153415,139.98171738085975 -36.113429888348726,139.9817088872023 -36.1134287080321,139.98170066273596 -36.11342796339546,139.98169301858005 -36.113426897939064,139.98168503017257 -36.11342579455431,139.98167722493875 -36.113425271100134,139.98166956016794 -36.11342460468306,139.98166218535007 -36.11342363652834,139.98165510801329 -36.11342327365452,139.98164873093413 -36.11342273036771,139.98164186684787 -36.11342201499148,139.9816353083656 -36.113421652611585,139.98162780294297 -36.11342137396328,139.98162008696872 -36.11342127681293,139.98161231738877 -36.11342133640774,139.98160455778003 -36.113421374089356,139.9815974885217 -36.11342159068347,139.98159026293413 -36.113421451217384,139.98158364512338 -36.11342175222521,139.98157787772408 -36.1134220525445,139.98157204545748 -36.113422123661266,139.98156516818304 -36.11342245551576,139.98155764295691 -36.11342297490187,139.98155019593221 -36.113423412358806,139.981543102961 -36.113423020817926,139.98153594947408 -36.11342279989269,139.98152795885682 -36.11342254110989,139.98152002940847 -36.11342212240004,139.98151232072982 -36.11342169754031,139.9815051451793 -36.11342121701399,139.9814978809862 -36.113420784123576,139.98149016725355 -36.1134200979799,139.98148262431718 -36.1134193530103,139.98147561478797 -36.11341836710851,139.98146907106963 -36.1134172688828,139.98146238766017 -36.113415767723865,139.98145535178438 -36.11341455600297,139.9814484187971 -36.11341318310244,139.98144118319811 -36.1134116972789,139.98143390568882 -36.11341028248541,139.98142704984411 -36.113408986050345,139.98142021219752 -36.11340792626351,139.98141353619127 -36.11340645738086,139.98140677470468 -36.11340512246942,139.98139997129672 -36.11340381207056,139.9813926230586 -36.11340390187327,139.98138504984013 -36.11340355746915,139.98137775783567 -36.113403358466165,139.98137086879723 -36.11340300016599,139.98136420707732 -36.113402723528644,139.9813579443481 -36.113402373348364,139.98135094496809 -36.113402061256366,139.9813433056301 -36.113401427251844,139.98133566549254 -36.11340105308123,139.98132794620906 -36.11340040307421,139.98132016461113 -36.11339949957495,139.98131227105597 -36.11339862535798,139.9813042106252 -36.11339801256413,139.981296101832 -36.11339717479135,139.98128791779672 -36.113396562857396,139.98127941117366 -36.113395618797114,139.98127107109278 -36.113394898645915,139.98126231837608 -36.11339416504949,139.9812535200286 -36.11339345765344,139.98124480666408 -36.11339269006575,139.98123649728842 -36.11339175018155,139.98122789725397 -36.1133903876268,139.9812194798685 -36.11338911268429,139.98121171943095 -36.11338737143515,139.98120395761762 -36.11338583508861,139.9811962133605 -36.11338393955234,139.98118907537963 -36.113381678863895,139.98118160303153 -36.11337955739533,139.9811740844769 -36.11337658887371,139.98116755825973 -36.11337373747988,139.9811611679626 -36.11337095016132,139.98115471186003 -36.11336809375855,139.98114855043815 -36.113365180053215,139.9811417098688 -36.113361761657686,139.9811355242045 -36.113357666149504,139.98112995436256 -36.11335296899543,139.9811244495364 -36.11334799397826,139.9811187403584 -36.11334321505984,139.98111329028856 -36.11333842986145,139.98110777303344 -36.11333372255142,139.98110223680732 -36.113328808085015,139.9810972074582 -36.11332393771882,139.98109211001324 -36.113318763217805,139.98108676246187 -36.11331325950484,139.98108189709632 -36.1133074688299,139.98107702050498 -36.11330238697022,139.981072676189 -36.11329680027543,139.98106905852435 -36.113291705606336,139.9810659560804 -36.11328598904608,139.98106370298035 -36.113280362760065,139.98106129078838 -36.113274047677514,139.9810590033022 -36.11326799176451,139.9810568307383 -36.11326118132677,139.98105455863907 -36.113254801648495,139.98105375068437 -36.11324863094771,139.98105277160516 -36.11324298429363,139.98105171516502 -36.113238242492905,139.98105129028352 -36.11323468018488,139.98105077688197 -36.11323087713881,139.98105039877458 -36.11322781587648,139.98105005944691 -36.11322628015831,139.98105005230693 -36.113226433516175,139.9810500701497 -36.11322594601749,139.9810500580591 -36.113226053250315,139.98105015048702 -36.11322589316939,139.9810499330521 -36.11322624724891,139.98104983988867 -36.1132260712394,139.98104972754248 -36.11322629559934,139.9810497400483 -36.11322643967453,139.98104987913442 -36.11322654998123,139.98104997103786 -36.113226658659244,139.98104966266473 diff --git a/gnssnav/config/course_data/precourse_250128.csv b/gnssnav/config/course_data/precourse_250128.csv deleted file mode 100644 index 335e755a..00000000 --- a/gnssnav/config/course_data/precourse_250128.csv +++ /dev/null @@ -1,476 +0,0 @@ -36.11314517039175,139.9810618772794 -36.11314504718486,139.98106191754403 -36.11314506131763,139.98106207974055 -36.11314506310504,139.9810621170177 -36.11314416799243,139.98106240835284 -36.11314116723725,139.9810629102194 -36.11313762535467,139.98106360702377 -36.11313336317823,139.98106413594434 -36.113129130674274,139.98106493557276 -36.11312522121771,139.98106540638534 -36.113120930946714,139.98106590243387 -36.11311711239682,139.9810664571229 -36.11311283821832,139.9810667710723 -36.11310887715124,139.9810670650348 -36.11310480030446,139.98106728529976 -36.113099412974385,139.9810676118376 -36.11309362312619,139.9810678485226 -36.113087715393185,139.9810686085345 -36.11308152806356,139.98106920405363 -36.11307559482438,139.98107016334157 -36.11307072293381,139.98107046844873 -36.113066409009754,139.9810707645184 -36.11306183340245,139.98107126146365 -36.1130571841108,139.9810718376549 -36.113052849706946,139.9810724834302 -36.113047460805106,139.9810729351066 -36.11304265961474,139.98107357544848 -36.11303852388833,139.98107403867738 -36.113034496748,139.98107424387123 -36.11303082068318,139.98107464827976 -36.11302690898228,139.9810752196369 -36.113021516480536,139.98107559097028 -36.11301577865004,139.98107601947103 -36.113010165371534,139.98107655726417 -36.11300421473452,139.98107727980175 -36.11299827122023,139.98107807058375 -36.11299262890773,139.9810790213442 -36.11298722051285,139.981080096193 -36.112981829651446,139.98108140507262 -36.11297653681463,139.9810826656673 -36.11297098578655,139.98108396613864 -36.11296575177827,139.98108587459623 -36.11296038168885,139.98108798822625 -36.112954977015235,139.98109022151243 -36.11294956563053,139.98109281459097 -36.112944195478235,139.98109540347167 -36.11293898841937,139.9810985552439 -36.11293407673426,139.98110177473333 -36.11292921051073,139.98110552943953 -36.11292425009087,139.9811093013037 -36.11291893832827,139.98111348337983 -36.11291371816382,139.98111759106118 -36.11290888695532,139.9811220701277 -36.11290457847842,139.9811257512265 -36.11290089060131,139.98112981969342 -36.11289676086477,139.9811340841912 -36.112892653909945,139.9811389094797 -36.1128883011816,139.9811440897027 -36.112884197305455,139.98114999290684 -36.112880115285826,139.9811560493574 -36.11287606258973,139.9811625554911 -36.11287253969596,139.98116838296497 -36.11286973025626,139.98117354930315 -36.1128668336646,139.98117875766457 -36.112864450465686,139.98118445918362 -36.11286193127711,139.98119034505424 -36.11285953693274,139.9811966752628 -36.11285712587694,139.9812031565754 -36.112855120746985,139.98120948109647 -36.1128530953577,139.9812156569928 -36.11285115280009,139.98122257026296 -36.11284916980676,139.98122922951634 -36.11284761098924,139.98123537985472 -36.11284629847108,139.98124142163502 -36.112844917312366,139.98124781350205 -36.11284311399332,139.98125367924624 -36.11284081621112,139.98125942368227 -36.11283877516491,139.98126519590008 -36.11283697191049,139.98127149202992 -36.112835228934934,139.9812776746554 -36.112833736440386,139.9812839248176 -36.11283224734625,139.98129010707723 -36.112831269930375,139.98129654963589 -36.112830088417475,139.98130278736687 -36.11282895364683,139.98130946758135 -36.112827556588584,139.98131557721322 -36.11282598124467,139.98132121297073 -36.11282472409569,139.98132687974666 -36.11282304934247,139.98133280797094 -36.11282153561246,139.98133865103858 -36.112820483925105,139.98134481434812 -36.11281905433182,139.98135118186855 -36.1128179352435,139.98135761098158 -36.112816338493715,139.9813638330807 -36.112814925414185,139.98137016097695 -36.112813513898054,139.9813763704132 -36.11281178101587,139.9813822670958 -36.11280993710253,139.98138857863276 -36.11280777091678,139.9813950382284 -36.11280581748456,139.9814020385354 -36.11280386485651,139.9814091699606 -36.11280220049153,139.98141601747125 -36.11280076688263,139.98142292761992 -36.11279882719266,139.98142940317643 -36.11279733493617,139.98143579666177 -36.11279582803396,139.98144207997916 -36.112794342558,139.98144826367187 -36.11279254221224,139.98145428686325 -36.11279087201134,139.98146041814687 -36.11278896885364,139.9814663119603 -36.11278709327441,139.98147217676285 -36.11278536726284,139.98147847219388 -36.11278380419389,139.9814843684971 -36.112782499058596,139.98149066105876 -36.11278093912482,139.98149702817312 -36.11277933110741,139.98150352401566 -36.112777890356384,139.98150932902237 -36.11277628030781,139.98151522698782 -36.1127745890334,139.98152129445165 -36.112773029496964,139.98152718056022 -36.11277187701142,139.9815334249522 -36.11277051993785,139.98153968939476 -36.11276967302462,139.98154625922464 -36.11276810468113,139.9815523716991 -36.112766435427716,139.98155816212292 -36.11276500004031,139.98156418340722 -36.11276343350043,139.98156971346677 -36.112761902472506,139.9815755324672 -36.11276018516882,139.98158181695044 -36.11275868680388,139.9815881290886 -36.112757228249976,139.9815941948121 -36.11275560897549,139.98160042421588 -36.11275395636002,139.98160664433652 -36.11275246233742,139.98161295680396 -36.11275062665829,139.981619069877 -36.11274888682233,139.98162519737446 -36.112746990847064,139.98163124055847 -36.11274554294608,139.9816374372256 -36.112744082277295,139.98164408057977 -36.11274243246825,139.98165042612325 -36.112740650943145,139.98165712615688 -36.112738837893,139.98166367423596 -36.11273709791256,139.9816701580114 -36.11273554389636,139.9816763726738 -36.11273374387474,139.98168252750318 -36.11273196804996,139.98168860590005 -36.11273017046291,139.98169449087843 -36.11272883317214,139.9817005690799 -36.11272752366747,139.98170662297773 -36.11272633552221,139.98171310142695 -36.11272500036588,139.98171936813867 -36.11272360673602,139.98172534711046 -36.112722345736124,139.98173124568535 -36.11272119621711,139.98173675434074 -36.11271971690505,139.98174217582414 -36.11271830049367,139.98174804216288 -36.11271702300808,139.981754013788 -36.112715993412934,139.98176062462505 -36.11271505628,139.98176712897603 -36.112713809993686,139.9817736380302 -36.11271306010765,139.98178017976252 -36.112712334732,139.98178672674123 -36.11271181385469,139.98179326491774 -36.112711784725334,139.98179987985398 -36.11271216440338,139.98180619750386 -36.112712392252426,139.9818127414768 -36.1127129261629,139.98181905485626 -36.112713387392375,139.98182536052502 -36.11271392362966,139.98183152676063 -36.11271452132513,139.981837794418 -36.11271556598587,139.98184424967184 -36.112716573343214,139.9818508506937 -36.11271750595063,139.98185706019015 -36.11271911071431,139.98186314607435 -36.11272096718462,139.98186943888203 -36.112722775027734,139.98187574839162 -36.11272474614812,139.9818817360476 -36.11272683150544,139.98188788878124 -36.11272916607705,139.98189405871028 -36.112731534802904,139.98189976162473 -36.112734255277005,139.98190536256922 -36.112736951933165,139.98191079862892 -36.11273989549111,139.98191609123293 -36.11274257387623,139.98192122460463 -36.112745648377455,139.98192674046288 -36.112748686923005,139.9819319340133 -36.11275219602018,139.98193716286787 -36.112755797128045,139.98194232442472 -36.11275974345726,139.9819469921765 -36.112763336283926,139.9819516234612 -36.112767278346325,139.98195593642933 -36.11277117141767,139.98196007416627 -36.11277514718342,139.98196413211392 -36.11277929248462,139.9819681397188 -36.112782958020624,139.9819715510004 -36.11278645981948,139.98197494440743 -36.112789956323695,139.98197834546568 -36.112793960424,139.9819817101448 -36.11279862271272,139.98198499330903 -36.11280343194209,139.9819879303912 -36.11280829564962,139.9819905433976 -36.112813071098664,139.98199313580173 -36.11281796413249,139.9819955794142 -36.112822857951215,139.9819979491887 -36.112827740371905,139.98200013401885 -36.11283267980406,139.9820019332882 -36.11283763072627,139.98200363423473 -36.11284239799918,139.98200519413803 -36.11284756364576,139.9820069874488 -36.11285258155672,139.9820082704142 -36.112857686046574,139.98200914608017 -36.112862921027684,139.9820099373301 -36.112868283396644,139.98201062774126 -36.112873597168054,139.9820108818692 -36.11287878357453,139.9820109212389 -36.11288384756631,139.9820108578457 -36.11288898927866,139.98201060428585 -36.112894155803346,139.982010245228 -36.11289910773691,139.9820098908717 -36.11290412048173,139.9820094489664 -36.112909063230305,139.98200864728818 -36.11291384781521,139.98200825106952 -36.11291895820967,139.98200773397036 -36.11292400663993,139.9820069659856 -36.112929670721066,139.98200613160927 -36.11293515431142,139.9820054432187 -36.11294057707798,139.98200470504813 -36.11294592259035,139.98200404188253 -36.11295126603823,139.98200338196864 -36.11295651137127,139.98200319978875 -36.112961868019035,139.9820029115999 -36.11296752388512,139.98200245856077 -36.11297298708388,139.98200213550416 -36.11297846953093,139.9820013967959 -36.11298375940207,139.98200070564116 -36.112989605285065,139.98199982286548 -36.112995727909805,139.98199875093442 -36.11299973880484,139.98199833916667 -36.113003550534856,139.98199787940828 -36.11300798946895,139.98199750700695 -36.11301287906807,139.98199686793342 -36.11301830277606,139.9819964458278 -36.11302393227783,139.98199602599206 -36.11302993747208,139.98199531419073 -36.11303621010389,139.98199469476933 -36.113042341478476,139.9819938654195 -36.113048582929096,139.98199342562816 -36.113054557677515,139.98199300242652 -36.11306038939222,139.98199252064023 -36.113066625398766,139.98199178164973 -36.113073372907,139.98199120172995 -36.11307980236831,139.98199047695664 -36.11308326097387,139.98198990783035 -36.11308701030812,139.98198916123667 -36.11309187927364,139.98198845926595 -36.11309672764879,139.98198801019265 -36.11310139871355,139.98198787215932 -36.11310610491166,139.98198747443323 -36.11311052362811,139.98198721290336 -36.113115460254875,139.98198648583323 -36.11312089668347,139.9819856799285 -36.113127256966685,139.98198453309135 -36.113133607063816,139.9819836321278 -36.11313949636347,139.98198300436763 -36.113144631813704,139.98198254417338 -36.11314924385239,139.98198206241156 -36.11315418064632,139.98198175855484 -36.113160015251964,139.98198092693858 -36.11316569254063,139.98197990910626 -36.11317110192185,139.98197911804985 -36.11317740059094,139.98197857171974 -36.11318287964115,139.98197774580333 -36.11318850927161,139.98197734934027 -36.1131937475528,139.98197676100742 -36.113199271661756,139.9819759227362 -36.11320449889643,139.98197566093154 -36.11320993976942,139.98197494753444 -36.113215521901004,139.98197441990814 -36.11322141472745,139.98197374189644 -36.113227732971524,139.9819727834382 -36.11323447341472,139.98197215263278 -36.11324105319011,139.9819714528347 -36.11324675490393,139.98197067046988 -36.113250731538265,139.98197036597765 -36.11325457897099,139.98197009508715 -36.11325884785585,139.98196943551042 -36.113264207151396,139.98196833192225 -36.11327031341479,139.9819671906909 -36.11327577240681,139.9819662075947 -36.113280919445636,139.98196474209294 -36.11328658066432,139.98196309025715 -36.11329208392111,139.98196074723793 -36.11329742782996,139.98195811516945 -36.11330280754392,139.9819557509383 -36.11330809723463,139.9819528595665 -36.113313416770694,139.98194997186218 -36.11331872738934,139.98194665066768 -36.11332348545702,139.98194376734563 -36.113327657329265,139.98194072299435 -36.113331797706955,139.98193766809297 -36.113335879050524,139.9819337776114 -36.11333920244145,139.98193054977136 -36.11334312201525,139.98192673862422 -36.11334741124611,139.98192252300828 -36.11335191442108,139.9819175606789 -36.11335682753148,139.98191202624992 -36.11336104804435,139.9819063936768 -36.11336455614349,139.98190226187012 -36.11336750857712,139.98189814913954 -36.113370449928006,139.98189380366674 -36.113373546581414,139.98188874273413 -36.113377077231995,139.98188299613466 -36.11338053005991,139.98187675834464 -36.11338337985231,139.98187109032025 -36.1133863445789,139.98186526147649 -36.11338915273221,139.98185954212084 -36.113391684839705,139.98185344015215 -36.11339405782853,139.98184727034626 -36.11339602705116,139.9818405788514 -36.113397867787846,139.98183350293593 -36.1133998772047,139.98182646289177 -36.11340161704897,139.98181943263387 -36.113402618687594,139.98181235190432 -36.11340379787875,139.98180540090274 -36.11340459272233,139.98179799433257 -36.11340526843247,139.9817912290262 -36.113406064953644,139.98178464599084 -36.113406452116514,139.9817780823146 -36.1134068082701,139.98177127965585 -36.11340689072972,139.9817644842909 -36.11340682832327,139.98175676319818 -36.11340626863795,139.98174903024582 -36.11340556375144,139.98174350204528 -36.11340509461754,139.98174005479942 -36.11340476177139,139.9817355628786 -36.113404183276195,139.98173077621428 -36.11340364463029,139.98172546384802 -36.113403263683566,139.98172021607556 -36.113402725327624,139.98171441656737 -36.113402335468884,139.98170937306142 -36.11340220137106,139.9817048240594 -36.11340208234956,139.98169920966512 -36.113401808703244,139.98169294552264 -36.11340146075638,139.98168633756504 -36.11340103051714,139.98167994456392 -36.11340065664068,139.98167350157948 -36.11340033168664,139.98166667446014 -36.11339985825403,139.9816599364366 -36.11339961797366,139.98165335063493 -36.113399194476145,139.98164693638466 -36.11339865483251,139.98164016159336 -36.113398016766936,139.98163300331208 -36.113397634267685,139.98162604658066 -36.11339735816594,139.9816192980026 -36.113396989123466,139.98161199048312 -36.11339615791644,139.98160496655493 -36.11339535590944,139.98159793813443 -36.113394882391006,139.98159061190128 -36.11339462765512,139.9815834440646 -36.113394208363886,139.981576705079 -36.11339388680381,139.98156972194798 -36.113393539445454,139.98156358041788 -36.11339302490466,139.98155667462814 -36.11339286256772,139.98154937520658 -36.113392447036176,139.98154237530252 -36.1133921096103,139.98153576453407 -36.11339175454684,139.9815295141916 -36.11339132346536,139.98152350220784 -36.11339069962289,139.9815165834053 -36.113390232936425,139.98150965540236 -36.113389765377306,139.98150390586918 -36.11338925630887,139.98149946050796 -36.11338898519098,139.98149427962042 -36.1133886602541,139.98148855055962 -36.11338823687211,139.98148200019332 -36.11338799332507,139.98147505466915 -36.11338745586901,139.98146822229367 -36.11338703582214,139.98146121376615 -36.1133864200935,139.98145442900272 -36.11338593747628,139.98144767421866 -36.113385267937396,139.981440927847 -36.113384698033485,139.98143373619757 -36.11338411426454,139.98142674493195 -36.113383500151436,139.98141997474315 -36.113383058293856,139.98141318075213 -36.11338260669843,139.9814065603889 -36.11338212903004,139.98139993385237 -36.11338141380485,139.98139350054913 -36.11338094161771,139.9813868218076 -36.11338041436803,139.98137956906436 -36.11337972223648,139.9813721569012 -36.11337909583526,139.98136467040644 -36.11337862693562,139.9813576019425 -36.11337839637816,139.98135098527283 -36.1133780482014,139.98134448057954 -36.11337750105006,139.98133798394358 -36.11337718285075,139.98133161592756 -36.11337697695911,139.9813251479901 -36.11337627912013,139.98131848343905 -36.11337561232168,139.98131164910671 -36.113375178429365,139.98130457462062 -36.113374642311506,139.98129766303148 -36.113373986062655,139.98129100735284 -36.1133734075339,139.98128455005593 -36.113372990204304,139.9812783942012 -36.11337251872906,139.98127295672109 -36.11337188992101,139.98126708471386 -36.1133712598156,139.98126083039205 -36.1133708989346,139.9812548890428 -36.11337034498172,139.98124839070368 -36.113369808989965,139.98124178404092 -36.11336903954558,139.98123519236665 -36.113368437479046,139.98122837409946 -36.11336746489546,139.9812220932163 -36.113366579793976,139.98121606554662 -36.113365347229184,139.98120959744102 -36.11336371529073,139.98120318352494 -36.113362233065494,139.98119686564297 -36.113360368981525,139.98119070176125 -36.11335851181689,139.98118515888498 -36.11335643205356,139.9811793652614 -36.11335416144507,139.9811737628475 -36.113351688994236,139.9811679254471 -36.11334917873312,139.98116211638077 -36.11334635169108,139.98115620497362 -36.113343028360674,139.98114993387955 -36.11333957884174,139.9811435754964 -36.11333606186388,139.98113770482666 -36.11333245472215,139.98113242902662 -36.11332902436094,139.9811275444642 -36.113325180077226,139.98112267069018 -36.113322091492506,139.98111876499516 -36.11331921007814,139.98111516955092 -36.113315682553505,139.98111092499786 -36.11331195957323,139.9811065117385 -36.113308029229685,139.98110204704147 -36.11330441879494,139.9810982279101 -36.11330016120152,139.98109478548886 -36.11329526710181,139.9810909938269 -36.113290198002964,139.98108721662766 -36.113285456648704,139.9810839836239 -36.113280780775476,139.98108102035718 -36.113276041604244,139.98107814870997 -36.11327096721077,139.98107499927528 -36.113266279961586,139.98107221497767 -36.1132610219959,139.98106939192795 -36.11325564246147,139.98106675452414 -36.11325046593698,139.98106468456544 -36.11324544624388,139.9810628020561 -36.11324032390639,139.98106131663243 -36.11323541961244,139.98105995369022 -36.11323055256044,139.98105885706207 -36.11322533169856,139.98105807932896 -36.113220288043514,139.9810571149815 -36.11321527228458,139.981056462077 -36.113210948582214,139.9810561011583 -36.11320628540937,139.9810557831685 -36.11320166471999,139.981055911848 -36.113196987241494,139.98105622755932 -36.11319251599929,139.98105634697927 -36.1131883166578,139.98105648619438 -36.113184283380185,139.98105685408584 -36.113180282612895,139.98105746656 -36.113178422718946,139.98105773405118 -36.11317726947851,139.98105790249983 -36.113177251817525,139.9810579881588 -36.113177190858046,139.98105789294024 -36.113177133300006,139.98105785487436 -36.11317699795839,139.98105795222182 -36.11317683816162,139.9810578613462 -36.11317663719404,139.98105790883136 -36.11317654162725,139.9810578613662 -36.11317634751979,139.98105783607946 -36.113176344688775,139.98105775300542 -36.113176208794926,139.98105765877582 -36.113176035819635,139.98105759740963 diff --git a/gnssnav/config/course_data/precourse_obstacle.csv b/gnssnav/config/course_data/precourse_obstacle.csv deleted file mode 100644 index bf5358ad..00000000 --- a/gnssnav/config/course_data/precourse_obstacle.csv +++ /dev/null @@ -1,33 +0,0 @@ -36.11314659022032,139.98107711987782 -36.11314633111294,139.98107697800333 -36.113146202276425,139.98107707920505 -36.11314495542709,139.98107470172442 -36.11310799308068,139.9810743451087 -36.11301731467879,139.98108263976812 -36.11292299766594,139.98111226408622 -36.112857086503624,139.98119961130587 -36.11282205977104,139.98132254525387 -36.11280176292454,139.9814325410555 -36.112777453595,139.98151353662487 -36.11275426052966,139.9816151272352 -36.11272947226728,139.98171390601775 -36.112714433494276,139.98181128368785 -36.11274095567964,139.98191808766967 -36.11280550124105,139.98199328430152 -36.112886741821214,139.98201535646814 -36.1129645172432,139.98200501185502 -36.11305222641808,139.98199139929238 -36.113145702845806,139.9819971497557 -36.11324628852881,139.9819812418534 -36.11332420084349,139.9819554500169 -36.1133884836614,139.98187103500808 -36.11340848602004,139.9817618269985 -36.11340026373205,139.9816523943878 -36.1133879313032,139.9815520653129 -36.11337408733011,139.98145892261545 -36.11338159964266,139.98135014995376 -36.11337294700847,139.9812477402738 -36.11334797398464,139.98115945923948 -36.113298500672165,139.9810975626772 -36.11323834349179,139.98106618079623 -36.11320157558104,139.98106079265037 diff --git a/gnssnav/config/course_data/shihou_241030_dynamic.csv b/gnssnav/config/course_data/shihou_241030_dynamic.csv deleted file mode 100644 index ccc77858..00000000 --- a/gnssnav/config/course_data/shihou_241030_dynamic.csv +++ /dev/null @@ -1,380 +0,0 @@ -36.11339337570483,139.98143946196274 -36.11339343556239,139.9814396226223 -36.113393547746846,139.9814396088537 -36.11339370087445,139.98143956352118 -36.113393846853754,139.9814402619585 -36.11339425280016,139.9814432780538 -36.11339461735385,139.98144859807337 -36.113394749488506,139.98145552179568 -36.11339457060114,139.9814636511254 -36.1133945418506,139.98147251444982 -36.11339426495831,139.98148137195713 -36.11339396209961,139.98149071785463 -36.11339440010436,139.98149968348278 -36.1133954992758,139.98150849394713 -36.11339688361488,139.98151734783474 -36.113397636101155,139.9815261441965 -36.11339806300583,139.98153500765193 -36.11339879741368,139.98154355470763 -36.1133994163471,139.98155227015096 -36.113400340758005,139.98156068230873 -36.11340105266164,139.98156908768738 -36.113401996802025,139.98157760229253 -36.1134028752747,139.98158594704125 -36.113403514565356,139.98159446599286 -36.11340417351901,139.9816030335024 -36.11340467367221,139.98161133312828 -36.11340481970947,139.98161968684033 -36.11340505889632,139.98162781313692 -36.113405218885546,139.98163595158888 -36.11340566481388,139.98164391975988 -36.113406097982526,139.98165192614184 -36.11340678749893,139.98166003630698 -36.11340730411305,139.98166814950298 -36.11340802651209,139.9816761801058 -36.11340857756294,139.9816848297848 -36.11340916348559,139.98169341781224 -36.11340942263562,139.98170174032637 -36.11341015103491,139.98171027958406 -36.113411054915886,139.98171892821964 -36.11341154336681,139.98172752592396 -36.11341208226169,139.98173601790467 -36.113412810592884,139.98174425881132 -36.11341311271445,139.98175271502905 -36.113413417908845,139.98176110925937 -36.11341276268591,139.981769497136 -36.11341237236628,139.98177788034644 -36.11341199974222,139.9817861318378 -36.113411006726636,139.98179392321316 -36.11340961979511,139.98180193508335 -36.1134078073717,139.98180975210178 -36.113406688765174,139.98181775966594 -36.113405481353745,139.98182570661973 -36.113403801378126,139.98183368203993 -36.11340197198586,139.9818420634655 -36.11339996252829,139.98184999365125 -36.11339691997587,139.98185740775037 -36.1133936567931,139.9818642586321 -36.113390427901344,139.98187148544747 -36.113387022688215,139.98187885691513 -36.11338320758591,139.98188603670422 -36.11337920746867,139.98189328039666 -36.113375067491894,139.9819002716886 -36.11337065412785,139.98190695202928 -36.11336595370521,139.98191339704832 -36.11336101274116,139.98191938548987 -36.11335591959685,139.98192485663006 -36.1133508200607,139.98193015426833 -36.11334526927184,139.98193500595968 -36.113339723500545,139.9819394469698 -36.113333909137815,139.98194354799918 -36.113328102228415,139.9819475687574 -36.11332183143248,139.98195135933517 -36.11331547892336,139.98195499788642 -36.113308856475875,139.98195849803733 -36.11330189591397,139.98196138222116 -36.11329507252084,139.98196401261217 -36.11328841941623,139.98196632751151 -36.113281483339335,139.98196820819365 -36.113274811549694,139.9819698053282 -36.113267802113256,139.9819713742343 -36.11326084009825,139.98197230479173 -36.11325397654707,139.98197320858537 -36.11324739348491,139.98197422206232 -36.1132403294715,139.98197496905667 -36.113233441695904,139.9819754631446 -36.11322646882612,139.9819759740332 -36.11321962930939,139.98197623159618 -36.11321275149766,139.98197667045716 -36.11320577870985,139.98197725271942 -36.11319870552192,139.981977988808 -36.113191714239946,139.98197865681394 -36.113184603505665,139.98197888568387 -36.113177645619984,139.9819792817127 -36.1131704236072,139.98198015916495 -36.11316335064786,139.98198064320113 -36.113156452382235,139.9819815013728 -36.113149525392664,139.98198213676625 -36.11314252040962,139.9819827536934 -36.11313578937618,139.98198359973586 -36.113128969969445,139.98198454099276 -36.11312217349569,139.98198539969678 -36.11311523303832,139.9819857681588 -36.11310848466298,139.98198614200805 -36.11310185505491,139.98198704978878 -36.11309500484848,139.98198749935653 -36.1130881281953,139.98198794873966 -36.11308123814011,139.9819885794427 -36.113074536414494,139.98198916144742 -36.11306777162609,139.98198999330924 -36.113060980919855,139.9819907080641 -36.113054044723235,139.9819912760711 -36.113047286850524,139.98199178389447 -36.11304116425231,139.98199263892408 -36.113035156148094,139.9819940409648 -36.11302982661118,139.98199524440653 -36.11302451963442,139.98199623435184 -36.11301947440522,139.9819965363197 -36.11301404400977,139.98199700087514 -36.11300835755897,139.98199736960245 -36.11300321212048,139.98199770225278 -36.11299886569422,139.9819982048006 -36.11299480627621,139.98199868203446 -36.11299082438407,139.98199920456796 -36.112986570106045,139.98199948808562 -36.11298149705557,139.9820002443659 -36.1129755990006,139.98200076157877 -36.112970532676165,139.98200130282694 -36.11296563086046,139.98200202618423 -36.11296142596362,139.9820027602146 -36.11295664957918,139.98200335161982 -36.11295130642731,139.98200428842378 -36.11294479632847,139.9820049375978 -36.11293863466259,139.9820057740584 -36.11293342752237,139.98200648998733 -36.11292876447125,139.98200705034546 -36.112923747763844,139.98200715258193 -36.11291782196567,139.98200742891092 -36.11291136447932,139.9820079452561 -36.11290475455832,139.98200839216832 -36.11289858556986,139.9820084336441 -36.11289343195328,139.98200856422554 -36.112887878995146,139.98200848008426 -36.11288202693831,139.98200817524156 -36.11287587449338,139.98200761726667 -36.11286937873205,139.9820063529195 -36.11286259264897,139.98200487998707 -36.11285614562304,139.98200373025514 -36.112849730938564,139.98200228164447 -36.112843210159724,139.98199988431293 -36.112836619296026,139.98199699426323 -36.11282953172386,139.98199465697127 -36.1128230822988,139.98199181876714 -36.112816895099826,139.9819881544222 -36.11281079126785,139.98198432325412 -36.112804638996074,139.98198053662324 -36.11279877149624,139.981976865315 -36.11279323621612,139.9819723270248 -36.11278837241413,139.9819670146862 -36.11278330255765,139.98196117801373 -36.112777840719325,139.98195586632966 -36.11277212920287,139.98195024773398 -36.11276682536206,139.9819448753286 -36.11276202688865,139.98193907040792 -36.112758177508766,139.98193276223722 -36.11275435148874,139.9819261408997 -36.112750011646625,139.98191973424272 -36.112745567388444,139.98191318375103 -36.11274168564766,139.98190647067258 -36.11273850754768,139.98189904997074 -36.112735363489286,139.9818917896541 -36.11273193314878,139.98188473564107 -36.112728593946244,139.9818776686047 -36.11272584967021,139.98187007214037 -36.112724090163674,139.98186216823237 -36.1127230180699,139.98185397659222 -36.11272129913713,139.98184563618113 -36.11271924447107,139.98183729829148 -36.11271762865345,139.98182922613128 -36.11271603869977,139.98182083614654 -36.11271438922451,139.98181252531546 -36.112713001028965,139.98180437290284 -36.11271341003205,139.98179654132744 -36.11271409691943,139.9817883728315 -36.11271492699244,139.98178020644673 -36.11271601860551,139.98177184630177 -36.11271744594432,139.98176369703393 -36.11271886506028,139.9817554160536 -36.112720187681646,139.98174718136107 -36.11272158236341,139.98173878847783 -36.112723830428166,139.98173097645738 -36.11272678994285,139.98172386349768 -36.11273010852327,139.9817165296529 -36.11273244707848,139.98170818841612 -36.11273441412244,139.98170011370428 -36.11273632878424,139.98169171506285 -36.112738068708865,139.98168334558213 -36.11273955751972,139.9816750162188 -36.11274121624222,139.98166698276432 -36.11274352985111,139.9816594567102 -36.11274586455596,139.98165202844226 -36.112747447018556,139.98164356404104 -36.11274915434459,139.98163519737827 -36.11275077268982,139.9816270360502 -36.112752656904526,139.9816190388664 -36.11275453225891,139.9816111765358 -36.11275633251628,139.98160300902512 -36.11275808873941,139.9815950057154 -36.11276003253435,139.98158725405082 -36.11276227642796,139.98157966248533 -36.11276478710598,139.98157184119614 -36.112766701857815,139.9815635598254 -36.1127683732319,139.98155545066317 -36.11276999341837,139.98154742476586 -36.11277185395249,139.98153926208286 -36.11277347566857,139.98153113824574 -36.11277517212201,139.98152342451422 -36.112777216610624,139.98151566689873 -36.11277900247517,139.98150721471973 -36.112780591511076,139.98149905387115 -36.112782139309985,139.9814910134079 -36.11278427993158,139.9814830329023 -36.11278637876236,139.98147495179376 -36.11278815431831,139.98146681398097 -36.11279015896012,139.98145903017488 -36.11279201968981,139.9814508419001 -36.11279363352559,139.98144282317313 -36.112795537300315,139.98143496508115 -36.112797467072696,139.98142730091735 -36.11279924425,139.98141934863781 -36.112800897517324,139.98141110354223 -36.11280264781712,139.98140291571778 -36.112804701303816,139.9813957531404 -36.11280771126153,139.98138881478965 -36.11281088457133,139.9813813320268 -36.11281364308539,139.98137322701066 -36.112816242297626,139.98136475375685 -36.11281821603391,139.9813561319222 -36.11281998112407,139.98134756035245 -36.11282140668236,139.98133916059535 -36.11282291644261,139.98133104104656 -36.11282425080891,139.98132318651548 -36.11282577519621,139.9813153619297 -36.11282752088201,139.98130765982214 -36.11282901048259,139.9812997780327 -36.11283053571003,139.9812920485797 -36.11283231756975,139.98128460454063 -36.11283468038809,139.9812769846269 -36.11283638113204,139.9812690642898 -36.11283815734547,139.98126087324633 -36.11283977912312,139.98125252534788 -36.112841749084396,139.9812445636026 -36.112844280921024,139.98123700165598 -36.11284680209126,139.98122914451838 -36.112849001731995,139.9812210000257 -36.11285158717576,139.98121326170184 -36.11285476342753,139.98120567577038 -36.11285788240995,139.98119832735065 -36.11286073777351,139.9811908878675 -36.112863718820186,139.9811835735019 -36.11286708484629,139.9811765089799 -36.112870345198715,139.98116961086092 -36.112874297634065,139.98116323345306 -36.112878551839444,139.98115678383607 -36.11288291117738,139.98115030867663 -36.11288718238454,139.9811434338635 -36.112891263967,139.98113716275526 -36.11289560430248,139.98113220359562 -36.11290085955682,139.9811282039576 -36.112906314929134,139.9811228025359 -36.11291137201696,139.98111747939336 -36.11291646133267,139.9811129100716 -36.112922069963695,139.98110964283808 -36.11292826602496,139.98110554769292 -36.112934352459995,139.98110075534873 -36.112940430972245,139.9810972827584 -36.11294669868043,139.98109410150133 -36.11295321622581,139.98109091886386 -36.11295950928155,139.98108736098652 -36.1129659278711,139.98108465204763 -36.112972085027124,139.98108184646745 -36.11297852717597,139.9810795995941 -36.11298452701673,139.9810786509944 -36.112991349707734,139.98107786297703 -36.11299833494606,139.9810764038821 -36.11300500526015,139.98107580525053 -36.1130119480571,139.98107538312803 -36.11301894935558,139.9810742350771 -36.11302600252904,139.98107399759445 -36.113032715174235,139.98107399346407 -36.11303921811132,139.98107400869435 -36.11304534316554,139.98107316061768 -36.11305139738399,139.98107235188294 -36.11305662393675,139.9810713382793 -36.11306096784295,139.98107091038145 -36.11306402964757,139.98107083280127 -36.113067087461914,139.9810710451307 -36.1130708700804,139.9810713008065 -36.11307589242931,139.98107108522834 -36.1130811603213,139.98107039661062 -36.11308612676697,139.98106989207307 -36.11309066528067,139.98106964631233 -36.113095010808955,139.98106953218303 -36.11309978923,139.9810691390038 -36.11310526042958,139.9810688183006 -36.11311097635551,139.98106825919967 -36.113116648569616,139.9810678738425 -36.113121943740786,139.9810672567473 -36.11312684970705,139.98106654652975 -36.113131575493526,139.9810656474421 -36.113136980607734,139.981064790197 -36.113142836955696,139.98106363523547 -36.11314897989751,139.98106280749994 -36.113154665135944,139.98106259492832 -36.11315967164963,139.98106353864554 -36.11316468173308,139.98106419107728 -36.11317015206473,139.9810643919295 -36.11317623115699,139.9810642488078 -36.11318277547638,139.9810639196864 -36.1131894017138,139.981064039929 -36.11319578336529,139.98106448613052 -36.11320208317019,139.98106477436116 -36.11320840239323,139.98106499679452 -36.113214926833194,139.9810652367 -36.11322121040469,139.9810665234674 -36.11322763681729,139.98106815003482 -36.11323447346776,139.98106989536694 -36.11324130644907,139.98107152772084 -36.113247975221256,139.9810735804461 -36.11325455878063,139.98107599317197 -36.11326093680304,139.981078917043 -36.11326726922999,139.9810822400627 -36.1132734399972,139.98108568148615 -36.113279483099745,139.98108952768038 -36.11328546390861,139.9810934232373 -36.11329160579368,139.98109741381217 -36.11329741383084,139.98110128869047 -36.113302788880134,139.9811058110412 -36.113307535076366,139.981111149242 -36.11331235607836,139.98111650711746 -36.11331734357299,139.98112192962435 -36.113322261328264,139.9811275820783 -36.11332678826484,139.98113370153396 -36.11333114936093,139.98114005208086 -36.113335040416665,139.98114651694354 -36.11333876031981,139.9811529732778 -36.113342992816605,139.98115979206253 -36.11334679934671,139.98116671555252 -36.11334952528892,139.9811744955322 -36.113351927764604,139.98118195660533 -36.11335425329588,139.9811893548042 -36.11335711268771,139.9811970711971 -36.113359196702454,139.9812044469707 -36.11336109930831,139.98121200669402 -36.113363050312735,139.98121962690522 -36.11336450741939,139.98122735985174 -36.113365475332685,139.98123520652845 -36.113366594181834,139.98124294750818 -36.113367210948034,139.9812503529097 -36.113367498242006,139.98125812073346 -36.11336789261007,139.98126626344472 -36.11336853649106,139.9812742465445 -36.11336924033999,139.9812824402399 -36.11336961935749,139.98129078747 -36.11337043923872,139.98129925788905 -36.11337110224454,139.98130765084963 -36.11337178342701,139.98131601583756 -36.11337239020979,139.98132431074188 -36.113373056979505,139.98133277048385 -36.11337377563001,139.98134086518402 -36.1133742848791,139.98134894091 -36.1133747616305,139.9813571394803 -36.1133753184955,139.98136530855527 -36.11337572653677,139.98137342844495 -36.113376653251144,139.9813815937018 -36.1133770008605,139.9813893750949 -36.113376895381805,139.98139580547183 -36.11337673697438,139.98140035729256 -36.113377025014906,139.98140240983216 -36.11337708033923,139.9814025787229 -36.1133772452504,139.98140263935267 diff --git a/gnssnav/config/course_data/shihou_241030_static.csv b/gnssnav/config/course_data/shihou_241030_static.csv deleted file mode 100644 index c332bddb..00000000 --- a/gnssnav/config/course_data/shihou_241030_static.csv +++ /dev/null @@ -1,500 +0,0 @@ -36.11339969534548,139.9814452716108 -36.11339955441279,139.98144549301284 -36.113399542504624,139.9814454318339 -36.11339986990634,139.98144556753687 -36.11339980613785,139.98144548526184 -36.113399836278425,139.98144555779137 -36.113399868515025,139.98144558339706 -36.11339986220851,139.9814456827704 -36.113399992273834,139.98144560695127 -36.11340002184469,139.98144566745404 -36.11340008502246,139.98144579312162 -36.11339988151564,139.98144615309835 -36.11340013621556,139.9814488770341 -36.11340032324087,139.98145338138414 -36.113400653742566,139.98145886856864 -36.11340095042237,139.9814643982618 -36.11340107067198,139.9814694502327 -36.11340152372787,139.98147491968032 -36.11340219613061,139.981480685909 -36.113402816014904,139.98148796463255 -36.11340300062762,139.98149515678463 -36.11340339789472,139.9815012276109 -36.11340367279505,139.9815061422194 -36.11340400666228,139.98151212441627 -36.1134043803462,139.98151917273955 -36.1134047533659,139.98152710187514 -36.113405291514944,139.98153494809347 -36.11340533628564,139.98154239206303 -36.11340588820678,139.98154864210727 -36.11340605523054,139.98155411476134 -36.11340643544081,139.98155994341482 -36.11340699186747,139.9815661808708 -36.11340744643439,139.9815727305157 -36.11340780898565,139.9815797685458 -36.11340850455028,139.9815866570382 -36.113409339949754,139.98159337080307 -36.113410257388814,139.98160062266325 -36.113410805101836,139.9816076990074 -36.11341103759442,139.98161466864994 -36.113411455457864,139.9816210024929 -36.11341192071882,139.98162708917047 -36.11341274474393,139.98163295965657 -36.11341326530518,139.98163961172926 -36.11341380721071,139.98164716963228 -36.11341414033755,139.98165440649143 -36.11341458556232,139.98166134177245 -36.11341524837441,139.9816682382905 -36.11341588525347,139.9816752693271 -36.11341634555051,139.98168259127192 -36.11341668755741,139.98168982879844 -36.113416707082045,139.98169676856938 -36.11341716456462,139.98170357020047 -36.11341759146093,139.98171057954116 -36.11341810104909,139.9817175613102 -36.11341835281184,139.9817245534534 -36.113418849908115,139.98173167110235 -36.11341918916625,139.98173882368465 -36.11341957532011,139.98174606003667 -36.11341981579287,139.98175334035247 -36.11341974708919,139.9817603734174 -36.11341955257722,139.98176739319365 -36.113419382926374,139.9817739078317 -36.11341923393117,139.98178041612613 -36.113419002332314,139.98178681654852 -36.11341871152632,139.98179371416373 -36.11341841914566,139.98180115176845 -36.11341789775436,139.98180880881029 -36.11341698275311,139.98181598935003 -36.11341564121514,139.98182226613903 -36.11341447765708,139.98182797953373 -36.11341292932591,139.98183358182112 -36.11341141149125,139.98183925710072 -36.113409274813016,139.9818452032197 -36.113407179958806,139.98185115125102 -36.11340482300243,139.9818566976557 -36.11340260399542,139.98186220122264 -36.11340000400886,139.9818676024816 -36.11339701389344,139.98187345018331 -36.11339375827229,139.9818798553919 -36.113390264404806,139.98188616547938 -36.11338681171033,139.98189201231955 -36.11338302399094,139.98189767063153 -36.11337943037137,139.98190309755023 -36.11337568400289,139.9819083683758 -36.1133717530551,139.98191317830586 -36.11336755839584,139.98191792383884 -36.113363609196995,139.98192210483555 -36.11335950682611,139.98192600622423 -36.11335485626694,139.98193014548696 -36.11334991871004,139.98193471243 -36.11334457896068,139.98193897284153 -36.113339459582456,139.981942926125 -36.11333363842599,139.98194673115037 -36.113327958027476,139.9819501896312 -36.11332239845058,139.98195335922102 -36.113316567012205,139.98195635607772 -36.11331092103239,139.98195923994606 -36.11330505939749,139.9819615835679 -36.113299029730925,139.98196358499433 -36.113293981528734,139.98196535682197 -36.11328940702316,139.981966931293 -36.11328480687011,139.9819681434149 -36.11328023790331,139.98196933711867 -36.11327484535795,139.98197029632487 -36.113269843959166,139.98197153984495 -36.11326525853122,139.98197263147642 -36.11326144978971,139.9819738243465 -36.11325741213226,139.98197471491576 -36.11325226549981,139.98197586418607 -36.11324681901004,139.98197671944203 -36.11324139101683,139.98197783545183 -36.11323654716628,139.9819793601944 -36.113232051365415,139.98198038982383 -36.11322714088177,139.98198149014357 -36.11322121652932,139.98198238728784 -36.11321472130911,139.98198359632747 -36.11320925497508,139.98198516673096 -36.11320401517442,139.98198602021017 -36.1131982562648,139.98198687666124 -36.1131918051228,139.9819876057213 -36.11318530845725,139.98198879018838 -36.11317841777192,139.98198928166605 -36.11317169722414,139.98199046742428 -36.11316491948613,139.98199123597786 -36.113158598568994,139.98199203909303 -36.11315231590934,139.98199300342554 -36.11314661295936,139.98199311555172 -36.113141127299194,139.9819929142144 -36.11313565196098,139.98199301675115 -36.11312996009154,139.98199286429755 -36.11312433274496,139.98199322189853 -36.113119418917364,139.98199346392695 -36.11311496551671,139.98199351373302 -36.11311079578292,139.98199385356855 -36.11310702868955,139.98199423662197 -36.11310281182121,139.98199452078057 -36.11309831344166,139.98199461553264 -36.11309358198527,139.9819947641926 -36.11308830150398,139.9819950921057 -36.113083076726774,139.98199570736966 -36.113077943312064,139.98199613977107 -36.11307321236618,139.98199653973873 -36.11306851562197,139.9819967003578 -36.11306361324918,139.98199702772615 -36.11305856871388,139.981997633939 -36.11305336104521,139.9819981612066 -36.11304836454021,139.9819988817147 -36.11304346918506,139.98199925935984 -36.11303858616233,139.981999816098 -36.11303338695111,139.9820006041513 -36.11302836686947,139.98200115209352 -36.11302330266731,139.98200146205997 -36.113018368430076,139.9820019124383 -36.11301317797637,139.9820024451495 -36.113007920072704,139.98200283385464 -36.11300298130447,139.98200326616274 -36.11299800552579,139.98200387620588 -36.112993420764646,139.98200439513303 -36.11298856058259,139.9820043100458 -36.112983245447424,139.98200441365415 -36.112977901027136,139.98200458131635 -36.11297252221177,139.98200520204853 -36.112967055974934,139.98200560996992 -36.11296186177179,139.98200617322763 -36.11295656670764,139.98200676239273 -36.112951294260164,139.98200736879926 -36.112946144344946,139.98200789267256 -36.11294106794948,139.98200866774243 -36.11293631077834,139.9820090935085 -36.11293106007017,139.98200935845418 -36.112925854343615,139.98200986936652 -36.112920444423935,139.9820103577417 -36.11291533672415,139.98201055824308 -36.11291037135339,139.98201024955205 -36.1129052649962,139.98201007570998 -36.11290003720972,139.98201010849235 -36.11289518488308,139.98201023207565 -36.11289038855951,139.9820098175953 -36.11288568483382,139.98200970031337 -36.112881031101765,139.98200902503754 -36.11287639827088,139.98200865714844 -36.11287155816474,139.9820077794618 -36.11286660209253,139.9820066092474 -36.11286202339957,139.98200543233014 -36.11285720267759,139.98200358106783 -36.1128517383697,139.98200165104828 -36.11284646543489,139.98199974762343 -36.11284125439358,139.98199789572854 -36.112836744099326,139.98199515022486 -36.11283220576583,139.98199244507128 -36.11282748106842,139.98198946901175 -36.1128224618488,139.98198702339445 -36.11281759895729,139.98198461132694 -36.112813352575955,139.9819818134721 -36.11280887480949,139.98197912250043 -36.11280498819382,139.98197627501764 -36.112801582965155,139.98197306959187 -36.11279743890025,139.98196946115402 -36.112793358764016,139.9819659227337 -36.11278921162192,139.98196243799885 -36.11278559985453,139.98195908358585 -36.11278252608143,139.98195474138987 -36.11277928740375,139.98195037598418 -36.112775799887125,139.98194566721264 -36.11277196045658,139.9819410913091 -36.11276868955662,139.98193631420784 -36.112764834750024,139.981931690673 -36.11276201098256,139.98192683093694 -36.11275890488895,139.9819218211701 -36.112756445742896,139.98191683746316 -36.11275372688503,139.9819115771239 -36.112751369970255,139.9819060261926 -36.112749381677794,139.98190016559334 -36.112747130894874,139.9818946216634 -36.112744724654,139.9818888784352 -36.11274253497412,139.98188324367064 -36.11274090910935,139.98187736769975 -36.112738986703924,139.98187132593853 -36.11273713563569,139.9818654478479 -36.112735420403624,139.98185932263561 -36.11273384044659,139.98185322755867 -36.11273261189851,139.98184737516365 -36.112731579886216,139.98184140344696 -36.11273058069287,139.98183530519944 -36.11272939003991,139.9818291548682 -36.11272887534213,139.98182337544642 -36.11272876228141,139.9818175820425 -36.11272863406369,139.98181191777772 -36.11272862080994,139.9818059232143 -36.11272850792955,139.98179992988523 -36.11272853746602,139.981793863455 -36.112728471458325,139.98178761374757 -36.11272868345856,139.98178173271324 -36.11272891668726,139.98177617152157 -36.11272972422687,139.98177055328262 -36.11273019332584,139.98176488662864 -36.11273079543045,139.9817591654113 -36.11273159189406,139.98175355163113 -36.1127323432524,139.98174801605424 -36.11273338664294,139.98174226886454 -36.112734293415784,139.9817364992513 -36.11273567479507,139.98173133733368 -36.11273685676657,139.98172568185785 -36.11273803642216,139.98171990629172 -36.112739097571186,139.9817143657863 -36.11274025450522,139.981709596707 -36.112741884745276,139.98170563652027 -36.11274375811511,139.98170062206384 -36.1127454445785,139.9816942434273 -36.1127470505644,139.98168733925482 -36.11274867409612,139.98168043064607 -36.11275026337888,139.9816736179972 -36.11275201086722,139.98166740678968 -36.11275366679397,139.98166110738654 -36.11275532885812,139.98165566960705 -36.112756990602136,139.98165037935107 -36.112758529362836,139.9816448408181 -36.112759910958566,139.9816394101514 -36.112761222867846,139.98163393048773 -36.11276255264833,139.9816284222419 -36.11276393053233,139.9816232651234 -36.1127653138009,139.98161833708713 -36.112766613747816,139.98161304840127 -36.11276785558044,139.98160800553217 -36.11276964746507,139.98160287225275 -36.11277109596566,139.98159732950862 -36.112772890951845,139.98159175188647 -36.11277434082547,139.98158557367788 -36.112775781438074,139.98157957887636 -36.11277673523904,139.98157340389648 -36.112777465902724,139.98156832057435 -36.11277823055599,139.98156372358216 -36.112778957325716,139.98155947919966 -36.11278024571803,139.98155488902214 -36.112781661538435,139.98154893168552 -36.11278320339783,139.98154297778777 -36.112784581243275,139.98153693008788 -36.11278592343444,139.9815315264861 -36.112787339086196,139.98152565678575 -36.112788573076564,139.98151921229416 -36.11279072625196,139.981512709875 -36.11279257319722,139.98150685761422 -36.11279426721223,139.9815011426067 -36.11279579737412,139.98149589583792 -36.11279740191726,139.98148975623502 -36.112798965677676,139.98148280104462 -36.11280058404376,139.98147561932106 -36.11280190667736,139.98146913901695 -36.11280328884745,139.9814635134692 -36.11280451746715,139.9814569342744 -36.11280610814588,139.98145012622905 -36.112807504111196,139.98144249563015 -36.11280922071856,139.98143456762023 -36.112810933691456,139.9814274447724 -36.11281261346659,139.9814208330043 -36.11281399205257,139.98141393745402 -36.112815490038,139.98140771572577 -36.112817291748364,139.98140208095853 -36.11281895356605,139.98139632016594 -36.112820894235064,139.98139044291833 -36.11282249042599,139.98138456717902 -36.112824250282436,139.98137873570025 -36.11282579556999,139.981372622997 -36.112827153238,139.98136648407163 -36.11282875150795,139.98136014712168 -36.11283021590802,139.98135390056694 -36.112831628226594,139.98134769158787 -36.11283288025898,139.98134141796106 -36.1128341067205,139.98133506232057 -36.11283541389815,139.9813290956821 -36.112837134078426,139.9813231193969 -36.112839014973275,139.98131688976457 -36.11284092842421,139.9813105448088 -36.1128426502385,139.98130401711006 -36.112844332564826,139.9812977558333 -36.11284578591994,139.98129107563125 -36.11284720994146,139.98128448381115 -36.11284871059568,139.98127756503732 -36.112850513271155,139.98127122282932 -36.112852279011584,139.98126447142323 -36.112854287732354,139.98125780123786 -36.11285625726231,139.98125132833061 -36.11285829620134,139.98124446412606 -36.11286035616811,139.9812379883173 -36.11286216851205,139.98123167688178 -36.112863856628486,139.98122582228666 -36.11286529426096,139.98121993656477 -36.11286669900902,139.98121400587002 -36.112868293166656,139.9812081037987 -36.11287015276484,139.98120163277133 -36.11287221777998,139.98119524702133 -36.11287414234117,139.98118878671818 -36.11287650628579,139.98118276240632 -36.112879348046086,139.98117687581893 -36.11288205942048,139.98117113605204 -36.112884571209555,139.98116548830046 -36.11288689629161,139.98116004584736 -36.1128893127481,139.98115481301213 -36.11289184736632,139.98114990909198 -36.112894613872264,139.98114531080103 -36.112897974344556,139.98114142685708 -36.11290157846461,139.98113781971304 -36.11290510953187,139.9811336869391 -36.11290849563767,139.98112941678932 -36.11291206728276,139.9811245852576 -36.11291575260058,139.98111967965286 -36.11291978790249,139.98111550027014 -36.11292439079682,139.98111174905324 -36.11292916171937,139.98110758353752 -36.11293384386906,139.98110358919197 -36.112937954467526,139.9810998880586 -36.11294138842303,139.98109683601422 -36.11294535070814,139.98109449220382 -36.11295026969887,139.98109162930209 -36.11295552585832,139.98108875582176 -36.1129605211561,139.98108564351654 -36.112965231979985,139.98108281557612 -36.11296958527927,139.9810806003886 -36.112973718733,139.98107934733136 -36.11297864609349,139.98107822885956 -36.112984218049576,139.98107667545295 -36.11298996462192,139.98107485859563 -36.112995440040585,139.9810729309007 -36.113000181908575,139.98107160207573 -36.1130046873176,139.98107102278613 -36.113009915964014,139.98107076767545 -36.1130155782548,139.98107012385003 -36.113021360126936,139.9810696914324 -36.113026453951015,139.9810688326005 -36.11303127726048,139.98106802667715 -36.11303556925016,139.98106727453683 -36.11304021667709,139.9810667895451 -36.11304524356784,139.98106640239936 -36.1130506381512,139.98106579015374 -36.11305573167695,139.98106498709382 -36.11306071050317,139.98106426551925 -36.11306553109315,139.9810634351579 -36.11307041636022,139.9810630498828 -36.11307551795922,139.98106277676553 -36.11308078180492,139.9810625366602 -36.11308578453486,139.98106198528095 -36.11309067356819,139.9810617892338 -36.113095475937456,139.98106144650566 -36.113100716871315,139.98106082943337 -36.11310598673013,139.98106051203465 -36.113111126973514,139.98105994917663 -36.1131163999737,139.98105941096492 -36.11312137241333,139.98105866255685 -36.11312676003241,139.98105817602885 -36.113131758804755,139.98105772970246 -36.113136849680025,139.9810571333959 -36.113141678937865,139.981056654824 -36.11314633363482,139.9810561258627 -36.1131507230963,139.9810554745256 -36.113155512827234,139.98105496846975 -36.11316032932447,139.98105413543038 -36.11316521607757,139.9810536646749 -36.11317005114689,139.98105316700196 -36.113174643871666,139.98105268634794 -36.11317941578564,139.98105199853208 -36.1131841642434,139.98105132414017 -36.113189307012874,139.9810509889981 -36.113194236168624,139.98105066241604 -36.113199098047794,139.9810507425895 -36.1132038869562,139.9810507395763 -36.11320872777035,139.98105059023248 -36.11321361762657,139.9810505963863 -36.11321853776287,139.9810507361459 -36.11322369885304,139.9810512899877 -36.11322879971235,139.9810519123288 -36.11323400594384,139.98105251007576 -36.11323907732383,139.9810534298537 -36.11324426432695,139.98105473937667 -36.11324945114671,139.98105600087848 -36.11325451478308,139.98105703925873 -36.113259516626435,139.9810588128758 -36.11326423790861,139.98106106823158 -36.113269145043,139.98106317707274 -36.1132734354703,139.98106505076427 -36.11327761758674,139.98106688499587 -36.11328171642708,139.98106874923894 -36.11328581061857,139.98107124750885 -36.11329032646793,139.9810737968632 -36.11329514617915,139.98107638325138 -36.11329958182444,139.98107979662367 -36.11330378711552,139.981083787755 -36.113308053494706,139.9810873119294 -36.113311815167535,139.98109042840366 -36.11331524627982,139.98109320379916 -36.11331865865548,139.98109603489152 -36.113322549781785,139.98109948691808 -36.11332635694573,139.98110300720572 -36.113329962323775,139.9811068619511 -36.113332762544104,139.98111033303425 -36.11333558565347,139.98111417507627 -36.1133388118909,139.9811180911283 -36.1133418099386,139.9811219543485 -36.113344386029375,139.98112561335168 -36.113347070426336,139.9811296444486 -36.11334957088273,139.98113417907132 -36.11335223548521,139.9811383851468 -36.11335501336632,139.9811430575269 -36.1133579631822,139.9811481299947 -36.11336053659746,139.9811525796268 -36.1133620006541,139.98115671427797 -36.1133632805604,139.98116025908803 -36.113364850348034,139.98116427601911 -36.11336716330763,139.98116937127853 -36.113369660435126,139.98117503498622 -36.11337158249916,139.98118128007883 -36.11337377925425,139.98118652665764 -36.113374842225625,139.98119117815392 -36.11337577723499,139.98119628102788 -36.113377007643926,139.98120294994615 -36.11337797805206,139.98120964448614 -36.113378331076206,139.98121608837462 -36.11337900874597,139.9812218023786 -36.113379613653066,139.9812270058581 -36.113380411222785,139.98123231365128 -36.113380637038205,139.98123858839597 -36.11338188839199,139.98124549448187 -36.11338299952913,139.98125218115345 -36.11338338834936,139.98125771924538 -36.11338351743458,139.981262587676 -36.113384010724715,139.98126864324476 -36.11338476506431,139.98127556128085 -36.11338499991583,139.98128293508216 -36.11338560878089,139.98129055039303 -36.113385956794524,139.98129653840076 -36.1133857154753,139.98130119293216 -36.11338516637628,139.98130619992787 -36.11338491160851,139.98131303538614 -36.1133850567706,139.981320091393 -36.11338560469693,139.98132727495474 -36.11338648207039,139.9813336422079 -36.1133870760591,139.9813394013736 -36.1133874315776,139.9813438765794 -36.11338797664614,139.98134728049132 -36.113388039770605,139.98134955944994 -36.113388258218436,139.98135301867708 -36.11338854084122,139.98135715847806 -36.11338899299939,139.98136177100102 -36.11338958998365,139.98136610611954 -36.1133899154148,139.98136945829688 -36.1133899389475,139.98137214199818 -36.11339018778628,139.9813759091209 -36.11339048453963,139.98138052601212 -36.11339100420496,139.98138481964415 -36.113391592421124,139.9813885703512 -36.11339223929336,139.9813912935611 -36.113392744022775,139.98139340655845 -36.11339323126692,139.98139529214225 -36.11339390866611,139.98139725017944 -36.113394330829145,139.98139920943348 -36.113394856586496,139.9814018074847 -36.11339579320245,139.9814047765134 -36.11339640971288,139.98140634961098 -36.11339655223892,139.98140705016738 -36.11339658344054,139.98140694121946 -36.11339656671726,139.9814070835533 diff --git a/gnssnav/config/course_data/shihou_gnssnav.csv b/gnssnav/config/course_data/shihou_gnssnav.csv deleted file mode 100644 index 06d154df..00000000 --- a/gnssnav/config/course_data/shihou_gnssnav.csv +++ /dev/null @@ -1,294 +0,0 @@ -36.11339126569408,139.98145533892483 -36.113391389305185,139.98145543546926 -36.11339144955584,139.9814553508664 -36.11339161498881,139.98145520471292 -36.11339170879556,139.98145504497592 -36.11339172743383,139.9814548015158 -36.11339177353364,139.9814546532021 -36.113391872050954,139.9814545370102 -36.113391711579006,139.98145427322928 -36.11339168107373,139.9814539235414 -36.11339164775816,139.98145371598252 -36.11339163153462,139.98145351969782 -36.11339164675337,139.98145347229678 -36.1133918733733,139.98145355543934 -36.11339185604933,139.98145342508425 -36.113392010349536,139.9814533522657 -36.11339205987316,139.98145325237184 -36.11339207496862,139.981453085517 -36.11339212816741,139.98145306679046 -36.11339218228707,139.98145305506443 -36.11339220314158,139.9814530282325 -36.11339233711361,139.98145298464485 -36.113392345482595,139.98145288089313 -36.11339239789637,139.98145277489033 -36.11339242161487,139.9814527736696 -36.11339244929931,139.98145276382158 -36.1133926764911,139.981452642983 -36.113392601935026,139.9814523059413 -36.11339235346438,139.9814526734891 -36.11339236431428,139.98145265899603 -36.113392320089254,139.98145268988407 -36.11339347699987,139.98145297708217 -36.113393701147906,139.98145306150212 -36.113393781409734,139.98145304107285 -36.11339368935786,139.9814528415238 -36.11339385126579,139.98145364542827 -36.11339392041104,139.98145772170403 -36.11339438168591,139.98146318397576 -36.113395240883214,139.98147049898333 -36.11339606278752,139.9814787059387 -36.11339693405276,139.9814876740646 -36.11339773605029,139.98149688287037 -36.113398542650266,139.98150619183332 -36.11339923432669,139.9815154773733 -36.11339991601097,139.9815248039219 -36.11340069782403,139.9815339734155 -36.113401243269465,139.98154290503757 -36.113401835412134,139.98155161011454 -36.11340361833585,139.9815898670199 -36.113406260973875,139.98162992517766 -36.11340778917736,139.98166773274048 -36.11340838439558,139.9816760521848 -36.11340898588299,139.9816843940894 -36.113409689391545,139.98169284911003 -36.11341030839775,139.98170111589636 -36.113410983629585,139.98170948786287 -36.113411347389736,139.98171796479232 -36.113411662095004,139.98172638056684 -36.11341196316747,139.98173485347772 -36.11341236543871,139.9817433313144 -36.113412640749836,139.98175158389722 -36.11341299875361,139.98176014973757 -36.113412949416734,139.98176854037254 -36.11341290623598,139.98177621408135 -36.113412624456046,139.98178393001479 -36.11341212335104,139.98179152432962 -36.11341069876311,139.98179916756732 -36.113409422925095,139.98180721120934 -36.11340792478426,139.98181558999119 -36.113406678506664,139.98182385978708 -36.11340465906691,139.98183176730652 -36.11340214686415,139.98183928850676 -36.113389404940826,139.9818725539318 -36.11338608069805,139.98188002815556 -36.11336553387295,139.98191310686457 -36.11333911614271,139.98194079327263 -36.113333640257075,139.98194539261297 -36.113327819993174,139.98194971382463 -36.11332170451706,139.98195343784008 -36.113315406602865,139.9819571059424 -36.11330898815079,139.98196059097205 -36.1133023149956,139.98196377569923 -36.11329560176037,139.98196682695993 -36.113288776684705,139.98196918622926 -36.11328200645467,139.9819708629925 -36.11327511286211,139.9819722467379 -36.11326818233542,139.981973341384 -36.11326122828443,139.98197399921307 -36.11325429264567,139.98197479812643 -36.11324725476825,139.98197520665985 -36.11324035718708,139.98197587343722 -36.11321112632988,139.9819788378175 -36.113176540128656,139.9819827334085 -36.113142423416726,139.9819863337849 -36.113135498333115,139.98198697633157 -36.113128642022446,139.98198774058596 -36.113121810675814,139.98198847542383 -36.113114837141474,139.98198932779738 -36.11310785924482,139.98198990825796 -36.11310105064378,139.9819905782552 -36.113094045196995,139.98199143603645 -36.11308715565227,139.98199211813537 -36.1130804224739,139.98199266421366 -36.11307355657242,139.98199349211788 -36.11306679646136,139.9819941342116 -36.11305981814803,139.98199477010405 -36.1130530015825,139.98199545880436 -36.113046160167514,139.98199605313616 -36.113039400499794,139.9819966971223 -36.1130129195168,139.98199936463118 -36.1129930106858,139.9820008612958 -36.11297312015705,139.98200306116712 -36.11296985798597,139.98200312518065 -36.112966487416195,139.98200288377203 -36.11296210136603,139.98200273001518 -36.11295670566066,139.98200345068375 -36.11295151160668,139.98200448134585 -36.112947224793054,139.98200500241012 -36.11294345085981,139.9820054804326 -36.11294054385965,139.98200592091106 -36.11293597121969,139.98200665247023 -36.11293025106822,139.98200746128828 -36.11292353800004,139.9820080640753 -36.112916596120165,139.98200876061523 -36.11290953201723,139.9820090755675 -36.112904258508806,139.9820093472262 -36.11289992497781,139.98200979356423 -36.11287288956992,139.9820078166817 -36.112839930067125,139.98200095658453 -36.11280722866488,139.98198554389245 -36.11280087764657,139.98198138432974 -36.112794904532244,139.9819769659073 -36.11278914366623,139.98197198549502 -36.112784624685176,139.98196733554488 -36.11278121257094,139.98196369679596 -36.11277839677,139.98196034401542 -36.112774950797544,139.9819557988587 -36.112770698929076,139.98195121992308 -36.11276607115095,139.98194579074777 -36.11276146946253,139.98193952453516 -36.11275689413709,139.98193318977573 -36.112752686956505,139.98192654623773 -36.11274856134091,139.9819194662533 -36.112744520027256,139.98191214614707 -36.11274082489782,139.9819046655774 -36.11272770581139,139.98186751002402 -36.112726292845565,139.9818611137689 -36.1127208732819,139.98183057384185 -36.11271857317226,139.9817868354694 -36.112719290929135,139.98177811911074 -36.11272032495842,139.98176976107564 -36.11272172275005,139.981761327357 -36.112723458285345,139.98175299979215 -36.11272500408954,139.981744723632 -36.11272641409833,139.98173639234423 -36.11272811348983,139.9817281657069 -36.11273049840684,139.9817201078991 -36.11273301200865,139.98171201360478 -36.112735614603366,139.98170415280038 -36.112737410946245,139.98169604223764 -36.112739719415664,139.9816881989788 -36.112742025016864,139.98168027010337 -36.11274427151436,139.98167234314337 -36.11274612586329,139.98166438515972 -36.112747869837456,139.98165613294546 -36.11274982514857,139.98164809881771 -36.11275208873602,139.981640165864 -36.11275421814108,139.98163234568884 -36.11275578373272,139.98162403918585 -36.11275763610959,139.9816160361132 -36.11276003074839,139.98160798449433 -36.11276221734405,139.981600106537 -36.11276438061994,139.9815923521059 -36.11277350296484,139.98155498082153 -36.11277572847529,139.9815468856168 -36.11278648877005,139.98150427821517 -36.112796764338334,139.98146606261525 -36.112798344561554,139.98145790884476 -36.11280020558192,139.98144967958703 -36.11280223754481,139.98144157038624 -36.11280444016419,139.981433639422 -36.112806497832196,139.9814256357122 -36.112808773483366,139.9814176508794 -36.1128106540866,139.98140951755389 -36.11281261321664,139.98140134098603 -36.112814367591795,139.98139280642212 -36.112816569252665,139.98138433966605 -36.11281878767204,139.98137596674712 -36.112821150415805,139.98136759942003 -36.112823572241744,139.98135958557305 -36.11282544985386,139.98135146175818 -36.1128273709626,139.98134332871192 -36.112829213953745,139.9813353919865 -36.112831133581814,139.98132716236995 -36.11283329338839,139.98131899233266 -36.112835538957825,139.98131080165103 -36.11283793006053,139.9813026724001 -36.11284009764755,139.9812946645546 -36.11284215160161,139.98128657364362 -36.112843874416384,139.98127835457154 -36.11284568802141,139.98126998342343 -36.11285485664203,139.9812339003799 -36.11286685301955,139.9811960408838 -36.11288508989191,139.98115904041208 -36.11288921610468,139.98115207366652 -36.11289341904385,139.98114523751053 -36.11289821543794,139.9811392607838 -36.11290315566799,139.9811335722599 -36.112908128171625,139.98112808264435 -36.112913312323236,139.98112271111077 -36.112918898012836,139.98111727883492 -36.112924160941546,139.98111215407187 -36.112930108900784,139.9811075266876 -36.112936165680985,139.98110394347034 -36.11294269399715,139.9811004781908 -36.11294909885099,139.9810973259901 -36.112955470554866,139.98109404628354 -36.11296187077614,139.981090874791 -36.11296829153141,139.981087887612 -36.112974676647156,139.98108516925885 -36.11298134706869,139.981083075802 -36.112987958805824,139.98108086971027 -36.11299461475846,139.98107937464937 -36.11300155611578,139.98107839245185 -36.11300855799703,139.98107811594699 -36.11301549937223,139.98107802575313 -36.11302234634715,139.9810771993462 -36.11302887235798,139.98107636381934 -36.1130555797088,139.98107514078856 -36.11306092715415,139.9810748406646 -36.11308128767401,139.98107147636983 -36.113102537434905,139.98106782907422 -36.113105468533405,139.98106746799917 -36.11310989323953,139.9810666959552 -36.11311520587198,139.98106578257395 -36.11312157453977,139.98106492579745 -36.11312783645265,139.9810641235425 -36.11313298105043,139.98106349270412 -36.11313687576671,139.98106295177126 -36.11314011906337,139.9810627377845 -36.11314417077804,139.9810627968929 -36.11314957675155,139.98106243607907 -36.11315606232163,139.98106188249056 -36.113162742865384,139.9810613842867 -36.113169987343845,139.98106133159433 -36.11317712192717,139.98106077885024 -36.11318336340363,139.98105985474183 -36.11318955892648,139.98105937997607 -36.113195964404404,139.9810590816995 -36.11320246827796,139.98105985758326 -36.1132092783055,139.98106077521277 -36.11321648780013,139.98106158609198 -36.113223485765914,139.98106233661508 -36.11325474212801,139.9810704413882 -36.113287921572265,139.9810879504468 -36.1133168213695,139.98111100312005 -36.11332187531526,139.9811167357324 -36.113327052635285,139.98112238683277 -36.11333219625374,139.98112845432323 -36.113337140481164,139.98113450170675 -36.1133416971893,139.9811405866063 -36.113345621459324,139.98114705046342 -36.11334912115809,139.98115423701088 -36.11335248366635,139.98116126263278 -36.11335560805999,139.98116870499464 -36.11335907142947,139.98117598163955 -36.11336206814401,139.98118382108254 -36.11336484178042,139.9811913763815 -36.11336718235388,139.98119935156325 -36.11337388637313,139.9812365620715 -36.113376221081246,139.9812805925687 -36.11337898983971,139.98132144288843 -36.11337962005235,139.98133012253393 -36.1133800871185,139.9813386874925 -36.11338073941308,139.9813474407054 -36.113381518518366,139.9813557826802 -36.11338205324506,139.98136397618953 -36.11338269350876,139.9813724447564 -36.11338330577966,139.98138115113662 -36.11338404458696,139.98138971006253 -36.11338443880956,139.98139790256434 -36.11338500364066,139.9814049945214 -36.11338513848484,139.98140887779056 -36.11338521691767,139.98141142517645 -36.11338503653369,139.98141151353576 -36.113384804808405,139.9814112542428 -36.11338474437926,139.98141115390686 -36.11338475674041,139.98141104088108 -36.11338460777009,139.98141083923568 -36.11338451335436,139.98141071432005 -36.113384406693335,139.98141058066113 -36.113384339645066,139.9814104160584 -36.11338418196263,139.98141019240708 -36.11338411486075,139.9814102165778 diff --git a/gnssnav/config/rviz/path.rviz b/gnssnav/config/rviz/path.rviz deleted file mode 100644 index e3a03316..00000000 --- a/gnssnav/config/rviz/path.rviz +++ /dev/null @@ -1,203 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /Pose1 - - /Pose2 - Splitter Ratio: 0.5 - Tree Height: 766 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /gnss_path - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Pose - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /current_ld - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Pose - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /current_pose - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 12.543999671936035 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 2.528569221496582 - Y: -1.5389280319213867 - Z: 2.228072166442871 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.0947964191436768 - Target Frame: - Value: Orbit (rviz) - Yaw: 3.2453980445861816 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1057 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000387fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000387000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000387000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000072e0000003efc0100000002fb0000000800540069006d006501000000000000072e0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004bd0000038700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1838 - X: 83 - Y: 23 diff --git a/gnssnav/include/gnssnav/follower_node.hpp b/gnssnav/include/gnssnav/follower_node.hpp deleted file mode 100644 index 21f3c3f7..00000000 --- a/gnssnav/include/gnssnav/follower_node.hpp +++ /dev/null @@ -1,103 +0,0 @@ -#pragma once - -#include "gnssnav/visibility_control.h" - -#include - -#include "nav_msgs/msg/path.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/vector3.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" - -#include -#include -#include -#include - -#include - -#include "utilities/position_pid.hpp" - -namespace gnssnav{ - -class Follower : public rclcpp::Node{ -public: - GNSSNAV_PUBLIC - explicit Follower(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - - GNSSNAV_PUBLIC - explicit Follower(const std::string& name_space, const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - - GNSSNAV_PUBLIC ~Follower(); - -private: - rclcpp::Subscription::SharedPtr vectornav_subscriber_; - rclcpp::Subscription::SharedPtr path_subscriber_; - rclcpp::Subscription::SharedPtr nav_start_subscriber_; - rclcpp::Subscription::SharedPtr autonomous_flag_subscriber_; - rclcpp::Subscription::SharedPtr restart_subscriber_; - - - rclcpp::Publisher::SharedPtr cmd_pub_; - rclcpp::Publisher::SharedPtr current_pose_pub_; - rclcpp::Publisher::SharedPtr current_ld_pub_; - - rclcpp::TimerBase::SharedPtr timer_; - - std::vector point_; - - void vectornavCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - void pathCallback(const nav_msgs::msg::Path::SharedPtr msg) { - point_ = msg->poses; - } - void navStartCallback(const std_msgs::msg::Empty::SharedPtr&); - void autonomousFlagCallback(const std_msgs::msg::Bool::SharedPtr msg); - void restartCallback(const std_msgs::msg::Empty::SharedPtr msg); - - void publishCurrentPose(void); - void publishLookahead(void); - void followPath(); - double findLookaheadDistance(); - void setBasePose(void); - double calculateYawFromQuaternion(const geometry_msgs::msg::Quaternion&); - double calculateCrossError(); - - std::pair convertECEFtoUTM(double x, double y, double z); - - controller::PositionPid pid; - - const int freq_ms; - const int is_debug; - const double ld_min_; - const double ld_gain_; - const double v_max_; - const double w_max_; - const double wheel_base_; - int laps; - - - bool autonomous_flag_=false; - bool nav_start_flag_=false; - bool init_base_flag_=false; - bool init_d=false; - - double v_=0; - double w_=0; - - double theta_error=0; - double theta_sum=0; - double prev_theta=0; - double current_position_x_=0; - double current_position_y_=0; - double current_yaw_=0; - size_t idx_=0; - size_t pre_point_idx=0; - double vectornav_base_x_=0; - double vectornav_base_y_=0; - double vectornav_base_yaw_=0; - - PJ_CONTEXT *C; - PJ *P; -}; - -} // namespace gnssnav diff --git a/gnssnav/include/gnssnav/path_publisher_node.hpp b/gnssnav/include/gnssnav/path_publisher_node.hpp deleted file mode 100644 index 33a885dc..00000000 --- a/gnssnav/include/gnssnav/path_publisher_node.hpp +++ /dev/null @@ -1,73 +0,0 @@ -#pragma once -#include "gnssnav/visibility_control.h" - -#include - -#include -#include -#include -#include - -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "nav_msgs/msg/path.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" - - -namespace gnssnav{ - -class Publisher : public rclcpp ::Node{ -public: - Publisher(); - - void loop(void); - - GNSSNAV_PUBLIC - explicit Publisher(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - GNSSNAV_PUBLIC - explicit Publisher(const std::string& name_space, const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - -private: - std::string file_path_; - std::string line_; - std::vector xs_; - std::vector ys_; - std::vector origin_xs_; - std::vector origin_ys_; - - std::string cell_; - std::vector tokens_; - - geometry_msgs::msg::PoseStamped pose_; - - geometry_msgs::msg::PoseStamped origin_pose_; - rclcpp::Publisher::SharedPtr publisher_; - rclcpp::Publisher::SharedPtr origin_publisher_; - nav_msgs::msg::Path path_msg_; - nav_msgs::msg::Path origin_path_msg_; - rclcpp::TimerBase::SharedPtr timer_; - - std::pair convertGPStoUTM(double lat, double lon); - std::vector interpolateSpline(const std::vector& xs, const std::vector& ys, int num_points); - std::vector result_; - - nav_msgs::msg::Path setMsg(const std::vector& xs, const std::vector& ys); - - void initCommunication(void); - void loadCSV(void); - void _publisher_callback(); - void setInitPose(double x, double y); - - bool init_flag_; - double base_x_; - double base_y_; - double step; - - const int freq; - const std::string path_file_name; - -}; - -} // namespace gnssnav diff --git a/gnssnav/include/gnssnav/visibility_control.h b/gnssnav/include/gnssnav/visibility_control.h deleted file mode 100644 index 6fa26acf..00000000 --- a/gnssnav/include/gnssnav/visibility_control.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef GNSSNAV__VISIBILITY_CONTROL_H_ -#define GNSSNAV__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define GNSSNAV_EXPORT __attribute__ ((dllexport)) - #define GNSSNAV_IMPORT __attribute__ ((dllimport)) - #else - #define GNSSNAV_EXPORT __declspec(dllexport) - #define GNSSNAV_IMPORT __declspec(dllimport) - #endif - #ifdef GNSSNAV_BUILDING_LIBRARY - #define GNSSNAV_PUBLIC GNSSNAV_EXPORT - #else - #define GNSSNAV_PUBLIC GNSSNAV_IMPORT - #endif - #define GNSSNAV_PUBLIC_TYPE GNSSNAV_PUBLIC - #define GNSSNAV_LOCAL -#else - #define GNSSNAV_EXPORT __attribute__ ((visibility("default"))) - #define GNSSNAV_IMPORT - #if __GNUC__ >= 4 - #define GNSSNAV_PUBLIC __attribute__ ((visibility("default"))) - #define GNSSNAV_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define GNSSNAV_PUBLIC - #define GNSSNAV_LOCAL - #endif - #define GNSSNAV_PUBLIC_TYPE -#endif - -#endif // GNSSNAV__VISIBILITY_CONTROL_H_ diff --git a/gnssnav/package.xml b/gnssnav/package.xml deleted file mode 100644 index a5b60b02..00000000 --- a/gnssnav/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - gnssnav - 0.0.0 - TODO: Package description - root - TODO: License declaration - - ament_cmake_ros - - rclcpp - geometry_msgs - nav_msgs - ros2launch - tf2 - tf2_ros - tf2_geometry_msgs - proj - utilities - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/gnssnav/src/follower_node.cpp b/gnssnav/src/follower_node.cpp deleted file mode 100644 index 638439b7..00000000 --- a/gnssnav/src/follower_node.cpp +++ /dev/null @@ -1,244 +0,0 @@ -#include "gnssnav/follower_node.hpp" - -#include "utilities/utils.hpp" -#include - -using namespace utils; - -namespace gnssnav{ - -Follower::Follower(const rclcpp::NodeOptions& options) : Follower("", options) {} - -// pub, sub, param -Follower::Follower(const std::string& name_space, const rclcpp::NodeOptions& options) -: rclcpp::Node("gnssnav_follower_node", name_space, options), -is_debug(get_parameter("debug_flag").as_bool()), -freq_ms(get_parameter("interval_ms").as_int()), -pid(get_parameter("interval_ms").as_int()), -laps(get_parameter("laps").as_int()), -ld_gain_(get_parameter("lookahead_gain").as_double()), -ld_min_(get_parameter("min_lookahead_distance").as_double()), -v_max_(get_parameter("max_linear_vel").as_double()), -w_max_(get_parameter("max_angular_vel").as_double()), -wheel_base_(get_parameter("wheelbase").as_double()) -{ - auto callback = [this](const std_msgs::msg::Empty::SharedPtr msg) { this->navStartCallback(msg); }; - - autonomous_flag_subscriber_ = this->create_subscription("/autonomous", 10, std::bind(&Follower::autonomousFlagCallback, this, std::placeholders::_1)); - nav_start_subscriber_ = this->create_subscription("/nav_start", 10, callback); - vectornav_subscriber_ = this->create_subscription("/vectornav/pose", 10, std::bind(&Follower::vectornavCallback, this, std::placeholders::_1)); - path_subscriber_ = this->create_subscription("/origin_gnss_path", 10, std::bind(&Follower::pathCallback, this, std::placeholders::_1)); - restart_subscriber_ = this->create_subscription("/restart", 10, std::bind(&Follower::restartCallback, this, std::placeholders::_1)); - - cmd_pub_ = this->create_publisher("/cmd_vel", 10); - current_pose_pub_ = this->create_publisher("/current_pose", 10); - current_ld_pub_ = this->create_publisher("/current_ld", 10); - - timer_ = this->create_wall_timer( - std::chrono::milliseconds(freq_ms), - std::bind(&Follower::followPath, this)); - - C = proj_context_create(); - P = proj_create_crs_to_crs(C, - "EPSG:4978", // ECEF - "EPSG:32654", // UTMゾーン54N - NULL); - - pid.gain(get_parameter("p_gain").as_double(), get_parameter("i_gain").as_double(), get_parameter("d_gain").as_double()); -} - -Follower::~Follower(){ - // リソースの解放 - proj_destroy(P); - proj_context_destroy(C); -} - -// vectornav/pose callback -void Follower::vectornavCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - if(point_.empty()) - return; - - auto [x, y] = convertECEFtoUTM(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z); - - current_position_x_ = x; - current_position_y_ = y; - - current_yaw_ = calculateYawFromQuaternion(msg->pose.pose.orientation) + (M_PI/2.0); - // RCLCPP_INFO(this->get_logger(), "current yaw:%lf°", rtod(current_yaw_)); - - if(!init_base_flag_) { - setBasePose(); - } else { - publishCurrentPose(); - } -} - -// gnssnav permit -void Follower::navStartCallback(const std_msgs::msg::Empty::SharedPtr&) { - idx_ = 0; - init_d = false; - RCLCPP_ERROR(this->get_logger(), "idx_がリセットされます"); -} - -// autonomous_flag_を更新 -void Follower::autonomousFlagCallback(const std_msgs::msg::Bool::SharedPtr msg) { - autonomous_flag_ = msg->data; - RCLCPP_INFO(this->get_logger(), "Autonomous flag updated to: %s", autonomous_flag_ ? "true" : "false"); -} - -// restart -void Follower::restartCallback(const std_msgs::msg::Empty::SharedPtr msg){ - pid.reset(); - RCLCPP_INFO(this->get_logger(), "再起動"); -} - -void Follower::setBasePose(){ - vectornav_base_x_ = current_position_x_; - vectornav_base_y_ = current_position_y_; - vectornav_base_yaw_ = current_yaw_; - - std::cerr << "set Base Pose" << std::endl; - init_base_flag_ = true; - - for(const auto &pose : point_){ - RCLCPP_INFO_EXPRESSION(this->get_logger(), is_debug, "path_x:%f , path_y:%f", pose.pose.position.x, pose.pose.position.y); - } -} - -// 現在地をパブリッシュ -void Follower::publishCurrentPose(){ - double dx = point_[1].pose.position.x - point_[0].pose.position.x; - double dy = point_[1].pose.position.y - point_[0].pose.position.y; - double path_direction_ = std::atan2(dy, dx); - - geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.header.stamp = this->now(); - pose_msg.header.frame_id = "map"; - - tf2::Quaternion initial_q, current_q, result_q; - initial_q.setRPY(0, 0, path_direction_ - vectornav_base_yaw_); - initial_q = initial_q.inverse(); - - tf2::Vector3 current_position( - current_position_x_ - vectornav_base_x_, - current_position_y_ - vectornav_base_y_, - 0.0 - ); - - tf2::Vector3 corrected_position = tf2::quatRotate(initial_q, current_position); - - pose_msg.pose.position.x = corrected_position.x(); - pose_msg.pose.position.y = corrected_position.y(); - pose_msg.pose.position.z = corrected_position.z(); - - current_q.setRPY(0, 0, current_yaw_); - result_q = initial_q * current_q; - - pose_msg.pose.orientation = tf2::toMsg(result_q); - current_pose_pub_->publish(pose_msg); -} - -void Follower::publishLookahead(){ - geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.header.stamp = this->now(); - pose_msg.header.frame_id = "map"; - - pose_msg.pose.position.x = point_[idx_].pose.position.x - vectornav_base_x_; - pose_msg.pose.position.y = point_[idx_].pose.position.y - vectornav_base_y_; - pose_msg.pose.position.z = 0.0; - - current_ld_pub_->publish(pose_msg); -} - -// 目標地点を探索する -double Follower::findLookaheadDistance(){ - double ld_ = ld_gain_ * v_ + ld_min_; - - for(idx_ = pre_point_idx;idx_ < point_.size(); idx_++){ - double dx = point_[idx_].pose.position.x - current_position_x_; - double dy = point_[idx_].pose.position.y - current_position_y_; - double distance_ = std::hypot(dx, dy); - - if(distance_ > ld_ && idx_ > pre_point_idx && point_[idx_].pose.position.x > 30000){ - pre_point_idx = idx_ - 1; - return distance_; - } - } -} - -double Follower::calculateCrossError(){ - double dx = point_[idx_].pose.position.x - current_position_x_; - double dy = point_[idx_].pose.position.y - current_position_y_; - - double target_angle = std::atan2(dy, dx); - - double angle = current_yaw_; - angle = std::atan2(std::sin(angle), std::cos(angle)); - - double theta = target_angle - angle; - theta = std::atan2(std::sin(theta), std::cos(theta)); - - RCLCPP_INFO_EXPRESSION(this->get_logger(), is_debug, "target:%lf° current:%lf°", rtod(target_angle), rtod(angle)); - return theta; -} - -void Follower::followPath(){ - if(point_.empty() || !autonomous_flag_) return; - - double distance = findLookaheadDistance(); - publishLookahead(); - - // 目標地点との角度のズレ - double theta = calculateCrossError(); - - v_ = v_max_; - w_ = pid.cycle(theta); - - geometry_msgs::msg::Twist cmd_vel; - cmd_vel.linear.x = constrain(v_, -v_max_, v_max_); - cmd_vel.angular.z = constrain(w_, -w_max_, w_max_); - - // 完走した判定 - if(idx_ >= point_.size()){ - if(laps == 0){ - cmd_vel.linear.x = 0.0; - cmd_vel.angular.z = 0.0; - RCLCPP_INFO(this->get_logger(), "Goal to reach"); - }else{ - idx_ = 0; - pre_point_idx = 0; - laps--; - RCLCPP_INFO(this->get_logger(), "laps : %d", laps); - } - } - - cmd_pub_->publish(cmd_vel); -} - -// クオータニオンからオイラーへ変換 -double Follower::calculateYawFromQuaternion(const geometry_msgs::msg::Quaternion& quat){ - - tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w); - tf2::Matrix3x3 m(q); - double roll, pitch, yaw; - m.getRPY(roll, pitch, yaw); - return yaw; -} - -std::pair Follower::convertECEFtoUTM(double x, double y, double z){ - if(P == NULL) { - std::cerr << "PROJ transformation creation failed." << std::endl; - } - PJ_COORD a, b; - - a.xyz.x = x; - a.xyz.y = y; - a.xyz.z = z; - - b = proj_trans(P, PJ_FWD, a); - - return {b.enu.e, b.enu.n}; -} - - -} // namespace gnssnav diff --git a/gnssnav/src/path_publisher_node.cpp b/gnssnav/src/path_publisher_node.cpp deleted file mode 100644 index 2b1daf97..00000000 --- a/gnssnav/src/path_publisher_node.cpp +++ /dev/null @@ -1,128 +0,0 @@ -#include "gnssnav/path_publisher_node.hpp" -#include - -namespace gnssnav{ - -Publisher::Publisher(const rclcpp::NodeOptions& options) : Publisher("", options) {} - -Publisher::Publisher(const std::string &name_space, const rclcpp::NodeOptions &options) -: rclcpp::Node("path_publisher_node", name_space, options), -init_flag_(true), -freq(get_parameter("interval_ms").as_int()), -path_file_name(get_parameter("path_file_name").as_string()) -{ - initCommunication(); - loadCSV(); - path_msg_ = setMsg(xs_, ys_); - origin_path_msg_ = setMsg(origin_xs_, origin_ys_); - - timer_ = this->create_wall_timer( - std::chrono::milliseconds(freq), - std::bind(&Publisher::loop, this)); -} - -// pub, param config -void Publisher::initCommunication(void){ - publisher_ = this->create_publisher("gnss_path", 10); - origin_publisher_ = this->create_publisher("origin_gnss_path", 10); - - //file_path_ = this->get_parameter("file_path").as_string(); - file_path_ = ament_index_cpp::get_package_share_directory("gnssnav")+"/config/"+"course_data/"+path_file_name+".csv"; -} - -// load CSV file -void Publisher::loadCSV(void){ - std::ifstream file(file_path_); - printf("loadCSV: file_path_ = %s\n", file_path_.c_str()); - - if (!file.is_open()) { - std::cerr << "loadCSV is failed to open file" << file_path_ << std::endl; - return; - }else { - std::cerr << "loadCSV is sucsess to open file" << std::endl; - } - - while(std::getline(file, line_)) { - std::stringstream ss(line_); - tokens_.clear(); - while(std::getline(ss, cell_, ',')) { - tokens_.push_back(cell_); - } - double lat = std::stod(tokens_[0]); - double lon = std::stod(tokens_[1]); - auto [x, y] = convertGPStoUTM(lat, lon); - - if(init_flag_) setInitPose(x, y); - - xs_.push_back(x - base_x_); - ys_.push_back(y - base_y_); - origin_xs_.push_back(x); - origin_ys_.push_back(y); - } -} - -// init pose -void Publisher::setInitPose(double x, double y){ - base_x_ = x; - base_y_ = y; - init_flag_ = false; -} - -// path create -nav_msgs::msg::Path Publisher::setMsg(const std::vector& xs, const std::vector& ys){ - std::vector spline_points = interpolateSpline(xs, ys, 100); - - nav_msgs::msg::Path path_msg; - path_msg.header.stamp = this->now(); - path_msg.header.frame_id = "map"; - for (const auto& coord : spline_points) { - geometry_msgs::msg::PoseStamped pose; - pose.header.stamp = this->now(); - pose.header.frame_id = "map"; - pose.pose.position.x = coord.x(); - pose.pose.position.y = coord.y(); - path_msg.poses.push_back(pose); - } - - return path_msg; -} - -// spline -std::vector Publisher::interpolateSpline(const std::vector& xs, const std::vector& ys, int num_points){ - Eigen::Matrix points(xs.size(), 2); - for (size_t i=0; i < xs.size(); ++i){ - points(i, 0) = xs[i]; - points(i, 1) = ys[i]; - } - - auto spline = Eigen::SplineFitting>::Interpolate(points.transpose(), 2); //2次のキュービックスプライン - - if(num_points > 1) - step = 1.0 / (num_points -1); - for (int i = 0; i < num_points; ++i) { - double u = i * step; - Eigen::Vector2d pt = spline(u); - result_.push_back(pt); - } - return result_; -} - -// WGS84系からUTM座標系へ変換 -std::pair Publisher::convertGPStoUTM(double lat, double lon) { - if (!(-90 <= lat) || !(lat <= 90) || !(-180 <= lon) || !(lon <= 180)) { - std::cerr << "Error: Latitude or longitude values are out of valid range." << std::endl; - return {std::numeric_limits::infinity(), std::numeric_limits::infinity()}; - } - PJ *P = proj_create_crs_to_crs(PJ_DEFAULT_CTX, "EPSG:4326", "EPSG:32654", nullptr); - PJ_COORD p = proj_coord(lat, lon, 0, 0); - p = proj_trans(P, PJ_FWD, p); - proj_destroy(P); - return {p.xy.x, p.xy.y}; -} - -void Publisher::loop(void){ - publisher_->publish(path_msg_); - origin_publisher_->publish(origin_path_msg_); -} - -} // namespace gnssnav diff --git a/path_tracker/CMakeLists.txt b/path_tracker/CMakeLists.txt deleted file mode 100644 index d3f8ef08..00000000 --- a/path_tracker/CMakeLists.txt +++ /dev/null @@ -1,49 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(path_tracker) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -ament_auto_add_library(pure_pursuit_node SHARED - src/pure_pursuit_node.cpp -) -ament_auto_add_library(pure_pursuit_global_node SHARED - src/pure_pursuit_global_node.cpp -) -target_compile_features(pure_pursuit_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_include_directories(pure_pursuit_node PUBLIC - $ - $ -) -target_compile_features(pure_pursuit_global_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_include_directories(pure_pursuit_global_node PUBLIC - $ - $ -) - -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(pure_pursuit_node PRIVATE "PATH_TRACKER_BUILDING_LIBRARY") -target_compile_definitions(pure_pursuit_global_node PRIVATE "PATH_TRACKER_BUILDING_LIBRARY") - -install( - DIRECTORY include/ - DESTINATION include -) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package() diff --git a/path_tracker/include/path_tracker/pure_pursuit_global_node.hpp b/path_tracker/include/path_tracker/pure_pursuit_global_node.hpp deleted file mode 100644 index 86114ee6..00000000 --- a/path_tracker/include/path_tracker/pure_pursuit_global_node.hpp +++ /dev/null @@ -1,68 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "path_tracker/visibility_control.h" - -namespace path_tracker { - -class PurePursuitGlobal : public rclcpp::Node { -public: - PATH_TRACKER_PUBLIC - explicit PurePursuitGlobal(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - - PATH_TRACKER_PUBLIC - explicit PurePursuitGlobal(const std::string& name_space, const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - -private: - rclcpp::Subscription::SharedPtr subscription_path_; - void path_callback(const nav_msgs::msg::Path::SharedPtr msg); - - rclcpp::Subscription::SharedPtr subscription_velocity_; - void velocity_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); - - rclcpp::Subscription::SharedPtr subscription_imu_; - void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg); - - rclcpp::Subscription::SharedPtr subscription_autonomous_; - void autonomous_callback(const std_msgs::msg::Bool::SharedPtr msg); - - rclcpp::Publisher::SharedPtr publisher_vel_; - rclcpp::Publisher::SharedPtr publisher_self_pose_; - rclcpp::Publisher::SharedPtr publisher_target_pose_; - - void timer_callback(); - static double yaw_from_quaternion(const geometry_msgs::msg::Quaternion& quat); - - rclcpp::QoS qos_ = rclcpp::QoS(10); - rclcpp::TimerBase::SharedPtr timer_; - - bool autonomous_flag_ = false; - bool has_velocity_ = false; - bool has_path_ = false; - bool has_imu_ = false; - bool has_yaw_reference_ = false; - double yaw_reference_ = 0.0; - geometry_msgs::msg::Pose current_pose_ = []{ - geometry_msgs::msg::Pose pose; - pose.orientation.w = 1.0; - return pose; - }(); - geometry_msgs::msg::Twist last_twist_; - nav_msgs::msg::Path::SharedPtr global_path_; - std::size_t last_target_index_ = 0; - - const double linear_max_vel; - const double lookahead_distance; - const double wheelbase_; - const double caster_max_angle_rad_; -}; - -} // namespace path_tracker diff --git a/path_tracker/include/path_tracker/pure_pursuit_node.hpp b/path_tracker/include/path_tracker/pure_pursuit_node.hpp deleted file mode 100644 index e3aa99ae..00000000 --- a/path_tracker/include/path_tracker/pure_pursuit_node.hpp +++ /dev/null @@ -1,40 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "path_tracker/visibility_control.h" - -namespace path_tracker{ - -class PurePursuit : public rclcpp::Node { -public: - PATH_TRACKER_PUBLIC - explicit PurePursuit(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - - PATH_TRACKER_PUBLIC - explicit PurePursuit(const std::string& name_space, const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - -private: - rclcpp::Subscription::SharedPtr _subscription_path; - void _subscriber_callback_path(const nav_msgs::msg::Path::SharedPtr msg); - - rclcpp::Subscription::SharedPtr _subscription_autonomous; - void autonomous_callback(const std_msgs::msg::Bool::SharedPtr msg); - - rclcpp::Publisher::SharedPtr publisher_vel; - - rclcpp::QoS _qos = rclcpp::QoS(10); - - bool autonomous_flag_ = false; - - const double linear_max_vel; - const double lookahead_distance; - const double steered_gain; - const double wheelbase_; - const double caster_max_angle_rad_; -}; - -} // namespace path_tracker diff --git a/path_tracker/include/path_tracker/visibility_control.h b/path_tracker/include/path_tracker/visibility_control.h deleted file mode 100644 index 06b2dcef..00000000 --- a/path_tracker/include/path_tracker/visibility_control.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef PATH_TRACKER__VISIBILITY_CONTROL_H_ -#define PATH_TRACKER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define PATH_TRACKER_EXPORT __attribute__ ((dllexport)) - #define PATH_TRACKER_IMPORT __attribute__ ((dllimport)) - #else - #define PATH_TRACKER_EXPORT __declspec(dllexport) - #define PATH_TRACKER_IMPORT __declspec(dllimport) - #endif - #ifdef PATH_TRACKER_BUILDING_LIBRARY - #define PATH_TRACKER_PUBLIC PATH_TRACKER_EXPORT - #else - #define PATH_TRACKER_PUBLIC PATH_TRACKER_IMPORT - #endif - #define PATH_TRACKER_PUBLIC_TYPE PATH_TRACKER_PUBLIC - #define PATH_TRACKER_LOCAL -#else - #define PATH_TRACKER_EXPORT __attribute__ ((visibility("default"))) - #define PATH_TRACKER_IMPORT - #if __GNUC__ >= 4 - #define PATH_TRACKER_PUBLIC __attribute__ ((visibility("default"))) - #define PATH_TRACKER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define PATH_TRACKER_PUBLIC - #define PATH_TRACKER_LOCAL - #endif - #define PATH_TRACKER_PUBLIC_TYPE -#endif - -#endif // PATH_TRACKER__VISIBILITY_CONTROL_H_ diff --git a/path_tracker/package.xml b/path_tracker/package.xml deleted file mode 100644 index 8e67f0d9..00000000 --- a/path_tracker/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - path_tracker - 0.0.0 - TODO: Package description - cmos - TODO: License declaration - - ament_cmake_ros - - rclcpp - std_msgs - steered_drive_msg - geometry_msgs - nav_msgs - sensor_msgs - utilities - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/path_tracker/src/pure_pursuit_global_node.cpp b/path_tracker/src/pure_pursuit_global_node.cpp deleted file mode 100644 index 2c28fb05..00000000 --- a/path_tracker/src/pure_pursuit_global_node.cpp +++ /dev/null @@ -1,188 +0,0 @@ -#include "path_tracker/pure_pursuit_global_node.hpp" - -#include -#include -#include - -namespace path_tracker { - -PurePursuitGlobal::PurePursuitGlobal(const rclcpp::NodeOptions& options) : PurePursuitGlobal("", options) {} - -PurePursuitGlobal::PurePursuitGlobal(const std::string& name_space, const rclcpp::NodeOptions& options) -: rclcpp::Node("pure_pursuit_global_node", name_space, options), - linear_max_vel(get_parameter("linear_max.vel").as_double()), - lookahead_distance(get_parameter("lookahead_distance").as_double()), - wheelbase_(get_parameter("wheelbase").as_double()), - caster_max_angle_rad_(get_parameter("steering_max.pos").as_double() * 0.017453292519943295) -{ - subscription_path_ = this->create_subscription( - "global_planner/path", - qos_, - std::bind(&PurePursuitGlobal::path_callback, this, std::placeholders::_1) - ); - subscription_velocity_ = this->create_subscription( - "/vectornav/velocity_body", - qos_, - std::bind(&PurePursuitGlobal::velocity_callback, this, std::placeholders::_1) - ); - subscription_imu_ = this->create_subscription( - "/vectornav/imu", - qos_, - std::bind(&PurePursuitGlobal::imu_callback, this, std::placeholders::_1) - ); - subscription_autonomous_ = this->create_subscription( - "/autonomous", - qos_, - std::bind(&PurePursuitGlobal::autonomous_callback, this, std::placeholders::_1) - ); - publisher_vel_ = this->create_publisher("cmd_vel", qos_); - publisher_self_pose_ = this->create_publisher( - "/path_tracker/self_pose", - qos_ - ); - publisher_target_pose_ = this->create_publisher( - "/path_tracker/target_pose", - qos_ - ); - timer_ = this->create_wall_timer( - std::chrono::milliseconds(100), - std::bind(&PurePursuitGlobal::timer_callback, this) - ); - - RCLCPP_INFO(this->get_logger(), "PurePursuitGlobal node has been initialized. lookahead_distance: %.2f", lookahead_distance); -} - -void PurePursuitGlobal::autonomous_callback(const std_msgs::msg::Bool::SharedPtr msg){ - autonomous_flag_ = msg->data; -} - -void PurePursuitGlobal::path_callback(const nav_msgs::msg::Path::SharedPtr msg){ - if (has_path_ || !msg || msg->poses.empty()) { - return; - } - - global_path_ = msg; - has_path_ = true; - RCLCPP_INFO(this->get_logger(), "PurePursuitGlobal node received global path"); - if (last_target_index_ >= global_path_->poses.size()) { - last_target_index_ = 0; - } - -} - -void PurePursuitGlobal::velocity_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg){ - if (!msg) { - return; - } - - last_twist_ = msg->twist.twist; - has_velocity_ = true; -} - -void PurePursuitGlobal::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg){ - if (!msg) { - return; - } - - current_pose_.orientation = msg->orientation; - has_yaw_reference_ = true; -} - -void PurePursuitGlobal::timer_callback(){ - if (!autonomous_flag_ || !global_path_ || global_path_->poses.empty() || !has_velocity_ || !has_yaw_reference_) { - return; - } - - constexpr double dt = 0.1; - const double yaw = yaw_from_quaternion(current_pose_.orientation) - yaw_reference_; - const double cos_yaw = std::cos(yaw); - const double sin_yaw = std::sin(yaw); - - const double vx_map = cos_yaw * last_twist_.linear.x - sin_yaw * last_twist_.linear.y; - const double vy_map = sin_yaw * last_twist_.linear.x + cos_yaw * last_twist_.linear.y; - - current_pose_.position.x += vx_map * dt; - current_pose_.position.y += vy_map * dt; - - const double current_x = current_pose_.position.x; - const double current_y = current_pose_.position.y; - - const auto begin_it = global_path_->poses.begin(); - const auto end_it = global_path_->poses.end(); - - auto closest_it = std::min_element( - begin_it, - end_it, - [current_x, current_y](const geometry_msgs::msg::PoseStamped& a, const geometry_msgs::msg::PoseStamped& b) { - const double dx_a = a.pose.position.x - current_x; - const double dy_a = a.pose.position.y - current_y; - const double dx_b = b.pose.position.x - current_x; - const double dy_b = b.pose.position.y - current_y; - return (dx_a * dx_a + dy_a * dy_a) < (dx_b * dx_b + dy_b * dy_b); - } - ); - - // Start searching from the closest point so std::find_if begins at the target anchor. - auto target_it = std::find_if( - closest_it, - end_it, - [this, current_x, current_y](const geometry_msgs::msg::PoseStamped& pose) { - const double dx = pose.pose.position.x - current_x; - const double dy = pose.pose.position.y - current_y; - return std::hypot(dx, dy) >= lookahead_distance; - } - ); - - if (target_it == end_it) { - target_it = std::prev(end_it); - } - - last_target_index_ = static_cast(std::distance(begin_it, target_it)); - - const double dx = target_it->pose.position.x - current_x; - const double dy = target_it->pose.position.y - current_y; - - const double target_x = cos_yaw * dx + sin_yaw * dy; - const double target_y = -sin_yaw * dx + cos_yaw * dy; - const double distance = std::hypot(target_x, target_y); - - geometry_msgs::msg::PoseStamped self_pose_msg; - self_pose_msg.header.stamp = this->now(); - self_pose_msg.header.frame_id = "base_link"; - self_pose_msg.pose = closest_it->pose; - - geometry_msgs::msg::PoseStamped target_pose_msg; - target_pose_msg.header.stamp = self_pose_msg.header.stamp; - target_pose_msg.header.frame_id = "base_link"; - target_pose_msg.pose = target_it->pose; - - publisher_self_pose_->publish(self_pose_msg); - publisher_target_pose_->publish(target_pose_msg); - - steered_drive_msg::msg::SteeredDrive command; - - if (distance < 1e-6) { - publisher_vel_->publish(command); - return; - } - - const double safe_lookahead = std::max(lookahead_distance, 1e-3); - const double linear_scale = std::clamp(distance / safe_lookahead, 0.0, 1.0); - const double linear_velocity = std::clamp(linear_max_vel * linear_scale, 0.0, linear_max_vel); - - const double alpha = std::atan2(target_y, target_x); - const double steer_angle = std::atan2(2.0 * wheelbase_ * std::sin(alpha), lookahead_distance); - const double steer_angle_clamped = std::clamp(steer_angle, -caster_max_angle_rad_, caster_max_angle_rad_); - - command.velocity = linear_velocity; - command.steering_angle = steer_angle_clamped; - publisher_vel_->publish(command); -} - -double PurePursuitGlobal::yaw_from_quaternion(const geometry_msgs::msg::Quaternion& quat){ - const double siny_cosp = 2.0 * (quat.w * quat.z + quat.x * quat.y); - const double cosy_cosp = 1.0 - 2.0 * (quat.y * quat.y + quat.z * quat.z); - return std::atan2(siny_cosp, cosy_cosp); -} - -} // namespace path_tracker diff --git a/path_tracker/src/pure_pursuit_node.cpp b/path_tracker/src/pure_pursuit_node.cpp deleted file mode 100644 index 0a065f4b..00000000 --- a/path_tracker/src/pure_pursuit_node.cpp +++ /dev/null @@ -1,95 +0,0 @@ -#include "path_tracker/pure_pursuit_node.hpp" - -#include "utilities/data_utils.hpp" -#include "utilities/utils.hpp" - -using namespace utils; - -namespace path_tracker{ - -PurePursuit::PurePursuit(const rclcpp::NodeOptions& options) : PurePursuit("", options) {} - -PurePursuit::PurePursuit(const std::string& name_space, const rclcpp::NodeOptions& options) -: rclcpp::Node("pure_pursuit_node", name_space, options), -linear_max_vel(get_parameter("linear_max.vel").as_double()), -lookahead_distance(get_parameter("lookahead_distance").as_double()), -steered_gain(get_parameter("steered_gain").as_double()), -wheelbase_(get_parameter("wheelbase").as_double()), -caster_max_angle_rad_(get_parameter("steering_max.pos").as_double() * 0.017453292519943295) -{ - _subscription_path = this->create_subscription( - "/frenet_planner/path", - _qos, - std::bind(&PurePursuit::_subscriber_callback_path, this, std::placeholders::_1) - ); - _subscription_autonomous = this->create_subscription( - "/autonomous", - _qos, - std::bind(&PurePursuit::autonomous_callback, this, std::placeholders::_1) - ); - publisher_vel = this->create_publisher("cmd_vel", _qos); - - RCLCPP_INFO(this->get_logger(), "PurePursuit node has been initialized. lookahead_distance: %.2f", lookahead_distance); -} - -void PurePursuit::autonomous_callback(const std_msgs::msg::Bool::SharedPtr msg){ - autonomous_flag_ = msg->data; -} - -void PurePursuit::_subscriber_callback_path(const nav_msgs::msg::Path::SharedPtr msg){ - if (!autonomous_flag_) { - return; - } - - steered_drive_msg::msg::SteeredDrive command; - - if (!msg || msg->poses.empty()) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), - *this->get_clock(), - 2000, - "Received empty path. Publishing zero velocity." - ); - publisher_vel->publish(command); - return; - } - - // Select a lookahead pose measured in the robot base frame. - auto target_it = std::find_if( - msg->poses.begin(), - msg->poses.end(), - [this](const geometry_msgs::msg::PoseStamped& pose) { - const double dx = pose.pose.position.x; - const double dy = pose.pose.position.y; - return std::hypot(dx, dy) >= lookahead_distance; - } - ); - - if (target_it == msg->poses.end()) { - target_it = std::prev(msg->poses.end()); - } - - const double target_x = target_it->pose.position.x; - const double target_y = target_it->pose.position.y; - const double distance = std::hypot(target_x, target_y); - - if (distance < 1e-6) { - publisher_vel->publish(command); - return; - } - - const double safe_lookahead = std::max(lookahead_distance, 1e-3); - const double linear_scale = std::clamp(distance / safe_lookahead, 0.0, 1.0); - const double linear_velocity = std::clamp(linear_max_vel * linear_scale, 0.0, linear_max_vel); - - const double alpha = std::atan2(target_y, target_x); - const double steer_angle = std::atan2(2.0 * wheelbase_ * std::sin(alpha), lookahead_distance); - const double steer_angle_clamped = std::clamp(steer_angle * steered_gain, -caster_max_angle_rad_, caster_max_angle_rad_); - - command.velocity = linear_velocity; - command.steering_angle = steer_angle_clamped; - publisher_vel->publish(command); -} - - -} // namespace path_tracker From 7e1462e7245f2ed597db13f62003353ef4a3fa0e Mon Sep 17 00:00:00 2001 From: kyo0221 Date: Tue, 5 May 2026 20:52:24 +0900 Subject: [PATCH 02/13] =?UTF-8?q?=E2=9C=A8feat:=20add=20sensing?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitmodules | 2 +- vectornav => sensing/vectornav | 0 sensing/zed_wrapper/CMakeLists.txt | 44 ++++ .../include/zed_wrapper/visibility_control.h | 27 +++ .../include/zed_wrapper/zed_wrapper_node.hpp | 42 ++++ sensing/zed_wrapper/package.xml | 21 ++ sensing/zed_wrapper/src/zed_wrapper_node.cpp | 209 ++++++++++++++++++ 7 files changed, 344 insertions(+), 1 deletion(-) rename vectornav => sensing/vectornav (100%) create mode 100644 sensing/zed_wrapper/CMakeLists.txt create mode 100644 sensing/zed_wrapper/include/zed_wrapper/visibility_control.h create mode 100644 sensing/zed_wrapper/include/zed_wrapper/zed_wrapper_node.hpp create mode 100644 sensing/zed_wrapper/package.xml create mode 100644 sensing/zed_wrapper/src/zed_wrapper_node.cpp diff --git a/.gitmodules b/.gitmodules index cb5566db..0a1a47b0 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,5 +1,5 @@ [submodule "vectornav"] - path = vectornav + path = sensing/vectornav url = https://github.com/open-rdc/vectornav [submodule "socketcan_interface"] path = socketcan_interface diff --git a/vectornav b/sensing/vectornav similarity index 100% rename from vectornav rename to sensing/vectornav diff --git a/sensing/zed_wrapper/CMakeLists.txt b/sensing/zed_wrapper/CMakeLists.txt new file mode 100644 index 00000000..5159a9f5 --- /dev/null +++ b/sensing/zed_wrapper/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.8) +project(zed_wrapper) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# ZED SDK is a system library — not a ROS package +set(ZED_DIR "/usr/local/zed/") +find_package(zed REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) + +ament_auto_add_library(zed_wrapper_node SHARED + src/zed_wrapper_node.cpp +) +target_compile_features(zed_wrapper_node PUBLIC c_std_99 cxx_std_17) +target_compile_definitions(zed_wrapper_node PRIVATE "ZED_WRAPPER_BUILDING_LIBRARY") +target_include_directories(zed_wrapper_node PUBLIC + $ + $ + ${ZED_INCLUDE_DIRS} + ${CUDA_INCLUDE_DIRS} +) +target_link_libraries(zed_wrapper_node + ${ZED_LIBRARIES} + ${CUDA_DEP_LIBRARIES_ZED} +) + +install( + DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/sensing/zed_wrapper/include/zed_wrapper/visibility_control.h b/sensing/zed_wrapper/include/zed_wrapper/visibility_control.h new file mode 100644 index 00000000..2341e659 --- /dev/null +++ b/sensing/zed_wrapper/include/zed_wrapper/visibility_control.h @@ -0,0 +1,27 @@ +#pragma once + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ZED_WRAPPER_EXPORT __attribute__((dllexport)) + #define ZED_WRAPPER_IMPORT __attribute__((dllimport)) + #else + #define ZED_WRAPPER_EXPORT __declspec(dllexport) + #define ZED_WRAPPER_IMPORT __declspec(dllimport) + #endif + #ifdef ZED_WRAPPER_BUILDING_LIBRARY + #define ZED_WRAPPER_PUBLIC ZED_WRAPPER_EXPORT + #else + #define ZED_WRAPPER_PUBLIC ZED_WRAPPER_IMPORT + #endif + #define ZED_WRAPPER_LOCAL +#else + #if __GNUC__ >= 4 + #define ZED_WRAPPER_PUBLIC __attribute__((visibility("default"))) + #define ZED_WRAPPER_LOCAL __attribute__((visibility("hidden"))) + #else + #define ZED_WRAPPER_PUBLIC + #define ZED_WRAPPER_LOCAL + #endif + #define ZED_WRAPPER_EXPORT ZED_WRAPPER_PUBLIC + #define ZED_WRAPPER_IMPORT +#endif diff --git a/sensing/zed_wrapper/include/zed_wrapper/zed_wrapper_node.hpp b/sensing/zed_wrapper/include/zed_wrapper/zed_wrapper_node.hpp new file mode 100644 index 00000000..abaf0f64 --- /dev/null +++ b/sensing/zed_wrapper/include/zed_wrapper/zed_wrapper_node.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include "zed_wrapper/visibility_control.h" + +#include + +#include +#include +#include +#include + +namespace zed_wrapper +{ + +class ZED_WRAPPER_PUBLIC ZedWrapperNode : public rclcpp::Node +{ +public: + explicit ZedWrapperNode( + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + explicit ZedWrapperNode( + const std::string & name_space, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~ZedWrapperNode(); + +private: + void grab_callback(); + sensor_msgs::msg::CameraInfo build_camera_info(); + + // PIMPL: ZED SDK types are hidden from the public header + struct Impl; + std::unique_ptr impl_; + + std::string camera_frame_id_; + sensor_msgs::msg::CameraInfo camera_info_cache_; + + rclcpp::Publisher::SharedPtr image_publisher_; + rclcpp::Publisher::SharedPtr pointcloud_publisher_; + rclcpp::Publisher::SharedPtr camera_info_publisher_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace zed_wrapper diff --git a/sensing/zed_wrapper/package.xml b/sensing/zed_wrapper/package.xml new file mode 100644 index 00000000..8ed338a5 --- /dev/null +++ b/sensing/zed_wrapper/package.xml @@ -0,0 +1,21 @@ + + + + zed_wrapper + 0.0.0 + ZED SDK wrapper node — publishes image and point cloud as ROS 2 msgs. + Kyo Yamashita + MIT + + ament_cmake_auto + + rclcpp + sensor_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/sensing/zed_wrapper/src/zed_wrapper_node.cpp b/sensing/zed_wrapper/src/zed_wrapper_node.cpp new file mode 100644 index 00000000..8d80786d --- /dev/null +++ b/sensing/zed_wrapper/src/zed_wrapper_node.cpp @@ -0,0 +1,209 @@ +#include "zed_wrapper/zed_wrapper_node.hpp" + +#include +#include + +#include +#include + +namespace zed_wrapper +{ + +// --- PIMPL body ----------------------------------------------------------- +struct ZedWrapperNode::Impl +{ + sl::Camera zed; + sl::RuntimeParameters runtime_params; +}; + +// --- helpers -------------------------------------------------------------- +namespace +{ + +// ZED X が出力対応するセンサ解像度（HD720/VGA/HD2K は非対応） +sl::RESOLUTION parse_resolution(const std::string & s) +{ + if (s == "HD1200") { return sl::RESOLUTION::HD1200; } + if (s == "HD1080") { return sl::RESOLUTION::HD1080; } + if (s == "SVGA") { return sl::RESOLUTION::SVGA; } + throw std::invalid_argument( + "ZedWrapperNode: unsupported resolution for ZED X: " + s); +} + +// ZED SDK 4.X の DEPTH_MODE 全選択肢 +sl::DEPTH_MODE parse_depth_mode(const std::string & s) +{ + if (s == "NONE") { return sl::DEPTH_MODE::NONE; } + if (s == "PERFORMANCE") { return sl::DEPTH_MODE::PERFORMANCE; } + if (s == "QUALITY") { return sl::DEPTH_MODE::QUALITY; } + if (s == "ULTRA") { return sl::DEPTH_MODE::ULTRA; } + if (s == "NEURAL") { return sl::DEPTH_MODE::NEURAL; } + if (s == "NEURAL_PLUS") { return sl::DEPTH_MODE::NEURAL_PLUS; } + throw std::invalid_argument( + "ZedWrapperNode: unsupported depth_mode: " + s); +} + +// publish 解像度（ZED X 想定: 640x360 へリサイズ） +const sl::Resolution kPublishResolution(640, 360); + +} // namespace + +// --- constructors / destructor -------------------------------------------- +ZedWrapperNode::ZedWrapperNode(const rclcpp::NodeOptions & options) +: ZedWrapperNode("", options) {} + +ZedWrapperNode::ZedWrapperNode( + const std::string & name_space, + const rclcpp::NodeOptions & options) +: rclcpp::Node("zed_wrapper_node", name_space, options), + impl_(std::make_unique()) +{ + const int grab_fps = get_parameter("grab_fps").as_int(); + const auto resolution = get_parameter("resolution").as_string(); + const auto depth_mode = get_parameter("depth_mode").as_string(); + const int confidence = get_parameter("confidence_threshold").as_int(); + const int serial_num = get_parameter("serial_number").as_int(); + camera_frame_id_ = get_parameter("camera_frame_id").as_string(); + + if (grab_fps <= 0) { + throw std::invalid_argument("ZedWrapperNode: grab_fps must be > 0"); + } + + sl::InitParameters init_params; + init_params.camera_resolution = parse_resolution(resolution); + init_params.camera_fps = grab_fps; + init_params.depth_mode = parse_depth_mode(depth_mode); + init_params.coordinate_units = sl::UNIT::METER; + init_params.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; + if (serial_num != 0) { + init_params.input.setFromSerialNumber(serial_num); + } + + const sl::ERROR_CODE ec = impl_->zed.open(init_params); + if (ec != sl::ERROR_CODE::SUCCESS) { + throw std::runtime_error( + std::string("ZedWrapperNode: camera open failed: ") + sl::toString(ec).c_str()); + } + + impl_->runtime_params.confidence_threshold = confidence; + camera_info_cache_ = build_camera_info(); + + image_publisher_ = create_publisher( + "/zed/zed_node/rgb/image_rect_color", rclcpp::QoS(10)); + pointcloud_publisher_ = create_publisher( + "/zed/zed_node/point_cloud", rclcpp::QoS(10)); + camera_info_publisher_ = create_publisher( + "/zed/zed_node/rgb/camera_info", rclcpp::QoS(10)); + + const auto period = std::chrono::milliseconds(1000 / grab_fps); + timer_ = create_wall_timer(period, [this]() { grab_callback(); }); + + RCLCPP_INFO(get_logger(), "ZedWrapperNode initialized (fps=%d, res=%s, depth=%s)", + grab_fps, resolution.c_str(), depth_mode.c_str()); +} + +ZedWrapperNode::~ZedWrapperNode() +{ + impl_->zed.close(); +} + +// --- timer callback ------------------------------------------------------- +void ZedWrapperNode::grab_callback() +{ + const sl::ERROR_CODE ec = impl_->zed.grab(impl_->runtime_params); + if (ec != sl::ERROR_CODE::SUCCESS) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, + "ZedWrapperNode: grab failed: %s", sl::toString(ec).c_str()); + return; + } + + const rclcpp::Time stamp = now(); + + sl::Mat left_image; + impl_->zed.retrieveImage(left_image, sl::VIEW::LEFT, sl::MEM::CPU, kPublishResolution); + { + sensor_msgs::msg::Image msg; + msg.header.stamp = stamp; + msg.header.frame_id = camera_frame_id_; + msg.width = static_cast(left_image.getWidth()); + msg.height = static_cast(left_image.getHeight()); + msg.encoding = "bgra8"; + msg.step = static_cast(left_image.getStepBytes()); + const size_t nbytes = msg.height * msg.step; + msg.data.resize(nbytes); + std::memcpy(msg.data.data(), left_image.getPtr(), nbytes); + image_publisher_->publish(msg); + } + + // XYZRGBA point cloud + sl::Mat pc_mat; + impl_->zed.retrieveMeasure(pc_mat, sl::MEASURE::XYZRGBA, sl::MEM::CPU); + { + sensor_msgs::msg::PointCloud2 msg; + msg.header.stamp = stamp; + msg.header.frame_id = camera_frame_id_; + msg.height = static_cast(pc_mat.getHeight()); + msg.width = static_cast(pc_mat.getWidth()); + msg.is_dense = false; + msg.point_step = 16; // 4 x float32: x, y, z, rgba + msg.row_step = msg.point_step * msg.width; + + msg.fields.resize(4); + msg.fields[0].name = "x"; msg.fields[0].offset = 0; + msg.fields[1].name = "y"; msg.fields[1].offset = 4; + msg.fields[2].name = "z"; msg.fields[2].offset = 8; + msg.fields[3].name = "rgba"; msg.fields[3].offset = 12; + for (auto & f : msg.fields) { + f.datatype = sensor_msgs::msg::PointField::FLOAT32; + f.count = 1; + } + + const size_t nbytes = static_cast(msg.height) * msg.row_step; + msg.data.resize(nbytes); + std::memcpy(msg.data.data(), pc_mat.getPtr(), nbytes); + pointcloud_publisher_->publish(msg); + } + + // Camera info (constant intrinsics, only stamp changes each frame) + camera_info_cache_.header.stamp = stamp; + camera_info_publisher_->publish(camera_info_cache_); +} + +// --- camera info construction (called once after open) -------------------- +sensor_msgs::msg::CameraInfo ZedWrapperNode::build_camera_info() +{ + // 取得サイズ（=publish サイズ）に合わせて intrinsics をスケール済みで取得 + const sl::CameraInformation cam_info = impl_->zed.getCameraInformation(kPublishResolution); + const auto & calib = cam_info.camera_configuration.calibration_parameters.left_cam; + const auto & res = kPublishResolution; + + sensor_msgs::msg::CameraInfo msg; + msg.header.frame_id = camera_frame_id_; + msg.width = static_cast(res.width); + msg.height = static_cast(res.height); + msg.distortion_model = "plumb_bob"; + + // D: k1, k2, p1, p2, k3 + msg.d = {calib.disto[0], calib.disto[1], calib.disto[2], calib.disto[3], calib.disto[4]}; + + // K: row-major 3x3 + msg.k = { + calib.fx, 0.0, calib.cx, + 0.0, calib.fy, calib.cy, + 0.0, 0.0, 1.0 + }; + + // R: identity (images are already rectified by ZED SDK) + msg.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + + // P: row-major 3x4 + msg.p = { + calib.fx, 0.0, calib.cx, 0.0, + 0.0, calib.fy, calib.cy, 0.0, + 0.0, 0.0, 1.0, 0.0 + }; + + return msg; +} + +} // namespace zed_wrapper From cb6c0afc54c084ccfd72bbf12ae15f68fe5a7c69 Mon Sep 17 00:00:00 2001 From: kyo0221 Date: Tue, 5 May 2026 20:54:44 +0900 Subject: [PATCH 03/13] =?UTF-8?q?=E2=9C=A8feat:=20mapless=20perception?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../road_detector/road_detector_node.py | 34 ++- .../CMakeLists.txt | 25 +- .../include/road_segments_provider/ipm.hpp | 31 ++ .../road_segments_provider_node.hpp | 101 +++++++ .../visibility_control.h | 32 ++ .../package.xml | 12 +- perception/road_segments_provider/src/ipm.cpp | 56 ++++ .../src/road_segments_provider_node.cpp | 282 ++++++++++++++++++ .../include/zed_wrapper/visibility_control.h | 27 -- .../include/zed_wrapper/zed_wrapper_node.hpp | 42 --- .../zed_wrapper/src/zed_wrapper_node.cpp | 209 ------------- 11 files changed, 544 insertions(+), 307 deletions(-) rename perception/{zed_wrapper => road_segments_provider}/CMakeLists.txt (50%) create mode 100644 perception/road_segments_provider/include/road_segments_provider/ipm.hpp create mode 100644 perception/road_segments_provider/include/road_segments_provider/road_segments_provider_node.hpp create mode 100644 perception/road_segments_provider/include/road_segments_provider/visibility_control.h rename perception/{zed_wrapper => road_segments_provider}/package.xml (55%) create mode 100644 perception/road_segments_provider/src/ipm.cpp create mode 100644 perception/road_segments_provider/src/road_segments_provider_node.cpp delete mode 100644 perception/zed_wrapper/include/zed_wrapper/visibility_control.h delete mode 100644 perception/zed_wrapper/include/zed_wrapper/zed_wrapper_node.hpp delete mode 100644 perception/zed_wrapper/src/zed_wrapper_node.cpp diff --git a/perception/road_detector/road_detector/road_detector_node.py b/perception/road_detector/road_detector/road_detector_node.py index 96fe6580..2d78fd00 100644 --- a/perception/road_detector/road_detector/road_detector_node.py +++ b/perception/road_detector/road_detector/road_detector_node.py @@ -9,7 +9,7 @@ from rclpy.qos import qos_profile_system_default from ament_index_python.packages import get_package_share_directory -from .utils.utils import letterbox, lane_line_mask +from .utils.utils import letterbox, lane_line_mask, driving_area_mask INPUT_SHAPE = (640, 640) DEVICE = torch.device('cuda' if torch.cuda.is_available() else 'cpu') @@ -22,12 +22,14 @@ def __init__(self, context=None): self.declare_parameter('input_image_topic', '/zed/zed_node/rgb/image_rect_color') self.declare_parameter('output_mask_topic', '/perception/lane_mask') + self.declare_parameter('output_drivable_area_topic', '/perception/drivable_area_mask') self.declare_parameter('capture_width', 640) self.declare_parameter('capture_height', 360) self.declare_parameter('visualize', False) self.input_image_topic = self.get_parameter('input_image_topic').value self.output_mask_topic = self.get_parameter('output_mask_topic').value + self.output_drivable_area_topic = self.get_parameter('output_drivable_area_topic').value self.capture_size = ( int(self.get_parameter('capture_width').value), int(self.get_parameter('capture_height').value), @@ -38,6 +40,7 @@ def __init__(self, context=None): raise ValueError('capture_width and capture_height must be greater than 0') self.ll_seg_publisher = self.create_publisher(Image, self.output_mask_topic, qos_profile_system_default) + self.da_seg_publisher = self.create_publisher(Image, self.output_drivable_area_topic, qos_profile_system_default) self.image_subscription = self.create_subscription( Image, self.input_image_topic, @@ -55,7 +58,8 @@ def __init__(self, context=None): self.model.eval() self.logger.info('PyTorch model loaded successfully!') self.logger.info(f'Subscribed image topic: {self.input_image_topic}') - self.logger.info(f'Publishing mask topic: {self.output_mask_topic}') + self.logger.info(f'Publishing lane mask topic: {self.output_mask_topic}') + self.logger.info(f'Publishing drivable area mask topic: {self.output_drivable_area_topic}') def image_callback(self, msg): cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') @@ -72,27 +76,37 @@ def image_callback(self, msg): outputs = self.model(img) [pred, anchor_grid], seg, ll = outputs - ll_seg_mask = lane_line_mask(ll) - ll_seg_mask = ll_seg_mask.astype(np.uint8) + ll_seg_mask = lane_line_mask(ll).astype(np.uint8) + da_seg_mask = driving_area_mask(seg).astype(np.uint8) ll_seg_resize_mask = cv2.resize(ll_seg_mask, (origin_shape[1], origin_shape[0]), interpolation=cv2.INTER_NEAREST) + da_seg_resize_mask = cv2.resize(da_seg_mask, (origin_shape[1], origin_shape[0]), interpolation=cv2.INTER_NEAREST) - self.ll_seg_publish(ll_seg_resize_mask) + stamp = msg.header.stamp + self.ll_seg_publish(ll_seg_resize_mask, stamp) + self.da_seg_publish(da_seg_resize_mask, stamp) if self.visualize_flag: - self.visualize(cv_img, ll_seg_resize_mask) + self.visualize(cv_img, ll_seg_resize_mask, da_seg_resize_mask) - def visualize(self, img, mask): + def visualize(self, img, ll_mask, da_mask): vis = img.copy() - vis[mask == 1] = [0, 0, 255] + vis[da_mask == 1] = (vis[da_mask == 1] * 0.5 + np.array([0, 255, 0]) * 0.5).astype(np.uint8) + vis[ll_mask == 1] = [0, 0, 255] cv2.imshow('road_detector', vis) cv2.waitKey(1) - def ll_seg_publish(self, ll_seg_mask): + def ll_seg_publish(self, ll_seg_mask, stamp): ll_seg_mask = (ll_seg_mask * 255).astype(np.uint8) ll_seg_msg = self.bridge.cv2_to_imgmsg(ll_seg_mask, encoding="mono8") - ll_seg_msg.header.stamp = self.get_clock().now().to_msg() + ll_seg_msg.header.stamp = stamp self.ll_seg_publisher.publish(ll_seg_msg) + def da_seg_publish(self, da_seg_mask, stamp): + da_seg_mask = (da_seg_mask * 255).astype(np.uint8) + da_seg_msg = self.bridge.cv2_to_imgmsg(da_seg_mask, encoding="mono8") + da_seg_msg.header.stamp = stamp + self.da_seg_publisher.publish(da_seg_msg) + def main(args=None): rclpy.init(args=args) road_detector_node = RoadDetectorNode() diff --git a/perception/zed_wrapper/CMakeLists.txt b/perception/road_segments_provider/CMakeLists.txt similarity index 50% rename from perception/zed_wrapper/CMakeLists.txt rename to perception/road_segments_provider/CMakeLists.txt index 5159a9f5..94c39192 100644 --- a/perception/zed_wrapper/CMakeLists.txt +++ b/perception/road_segments_provider/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(zed_wrapper) +project(road_segments_provider) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -8,26 +8,19 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -# ZED SDK is a system library — not a ROS package -set(ZED_DIR "/usr/local/zed/") -find_package(zed REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) +find_package(OpenCV REQUIRED) -ament_auto_add_library(zed_wrapper_node SHARED - src/zed_wrapper_node.cpp +ament_auto_add_library(road_segments_provider_node SHARED + src/ipm.cpp + src/road_segments_provider_node.cpp ) -target_compile_features(zed_wrapper_node PUBLIC c_std_99 cxx_std_17) -target_compile_definitions(zed_wrapper_node PRIVATE "ZED_WRAPPER_BUILDING_LIBRARY") -target_include_directories(zed_wrapper_node PUBLIC +target_compile_features(road_segments_provider_node PUBLIC c_std_99 cxx_std_17) +target_compile_definitions(road_segments_provider_node PRIVATE "ROAD_SEGMENTS_PROVIDER_BUILDING_LIBRARY") +target_include_directories(road_segments_provider_node PUBLIC $ $ - ${ZED_INCLUDE_DIRS} - ${CUDA_INCLUDE_DIRS} -) -target_link_libraries(zed_wrapper_node - ${ZED_LIBRARIES} - ${CUDA_DEP_LIBRARIES_ZED} ) +target_link_libraries(road_segments_provider_node ${OpenCV_LIBS}) install( DIRECTORY include/ diff --git a/perception/road_segments_provider/include/road_segments_provider/ipm.hpp b/perception/road_segments_provider/include/road_segments_provider/ipm.hpp new file mode 100644 index 00000000..a33c67a7 --- /dev/null +++ b/perception/road_segments_provider/include/road_segments_provider/ipm.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include +#include + +#include + +namespace road_segments_provider +{ + +struct CameraParams +{ + double fx{0.0}; + double fy{0.0}; + double cx{0.0}; + double cy{0.0}; + // Rotation matrix: camera_frame -> base_link, row-major [r00,r01,r02, r10,...,r22] + std::array R{}; + // Translation: camera origin expressed in base_link frame [tx, ty, tz] + std::array t{}; +}; + +// Converts a quaternion (qx, qy, qz, qw) to a row-major 3x3 rotation matrix. +std::array quat_to_rotation_matrix(double qx, double qy, double qz, double qw); + +// Projects pixel (u, v) onto the ground plane (z = 0 in base_link frame). +// Returns nullopt when the ray points away from the ground or is parallel to it. +std::optional project_pixel_to_ground( + int u, int v, const CameraParams & params); + +} // namespace road_segments_provider diff --git a/perception/road_segments_provider/include/road_segments_provider/road_segments_provider_node.hpp b/perception/road_segments_provider/include/road_segments_provider/road_segments_provider_node.hpp new file mode 100644 index 00000000..8a8a65f8 --- /dev/null +++ b/perception/road_segments_provider/include/road_segments_provider/road_segments_provider_node.hpp @@ -0,0 +1,101 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "road_segments_provider/ipm.hpp" +#include "road_segments_provider/visibility_control.h" + +namespace road_segments_provider +{ + +class RoadSegmentsProviderNode : public rclcpp::Node +{ +public: + ROAD_SEGMENTS_PROVIDER_PUBLIC + explicit RoadSegmentsProviderNode( + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + ROAD_SEGMENTS_PROVIDER_PUBLIC + explicit RoadSegmentsProviderNode( + const std::string & name_space, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + void sync_callback( + const sensor_msgs::msg::Image::ConstSharedPtr & lane_mask, + const sensor_msgs::msg::Image::ConstSharedPtr & da_mask); + + void camera_info_callback(const sensor_msgs::msg::CameraInfo::ConstSharedPtr & msg); + + // Builds CameraParams from stored intrinsics and TF lookup result. + // Returns nullopt if TF is unavailable. + std::optional build_camera_params(); + + // Extracts ego lane left/right boundaries from the lane mask image. + // Left boundary: rightmost lane pixels in left half (u < width/2), row by row. + // Right boundary: leftmost lane pixels in right half (u >= width/2), row by row. + // Both are returned sorted by increasing ground-x (near to far). + std::pair, std::vector> + extract_lane_boundaries( + const cv::Mat & lane_img, + const CameraParams & params) const; + + visualization_msgs::msg::MarkerArray build_markers( + const std::vector & left, + const std::vector & right, + const std::string & frame_id, + const rclcpp::Time & stamp) const; + + // Parameters + const std::string camera_info_topic_; + const std::string lane_mask_topic_; + const std::string da_mask_topic_; + const std::string road_segments_topic_; + const std::string camera_frame_id_; + const std::string base_frame_id_; + const double min_ground_x_m_; + const double max_ground_x_m_; + const bool publish_markers_; + + // Subscribers / sync + rclcpp::Subscription::SharedPtr camera_info_sub_; + std::shared_ptr> lane_mask_sub_; + std::shared_ptr> da_mask_sub_; + + using SyncPolicy = message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::Image, sensor_msgs::msg::Image>; + std::shared_ptr> sync_; + + // Publishers + rclcpp::Publisher::SharedPtr road_segments_pub_; + rclcpp::Publisher::SharedPtr marker_pub_; + + // TF2 + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // Camera intrinsics (set once from CameraInfo) + mutable std::mutex camera_info_mutex_; + std::optional> camera_intrinsics_; // [fx, fy, cx, cy] +}; + +} // namespace road_segments_provider diff --git a/perception/road_segments_provider/include/road_segments_provider/visibility_control.h b/perception/road_segments_provider/include/road_segments_provider/visibility_control.h new file mode 100644 index 00000000..46f63827 --- /dev/null +++ b/perception/road_segments_provider/include/road_segments_provider/visibility_control.h @@ -0,0 +1,32 @@ +#ifndef ROAD_SEGMENTS_PROVIDER__VISIBILITY_CONTROL_H_ +#define ROAD_SEGMENTS_PROVIDER__VISIBILITY_CONTROL_H_ + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ROAD_SEGMENTS_PROVIDER_EXPORT __attribute__ ((dllexport)) + #define ROAD_SEGMENTS_PROVIDER_IMPORT __attribute__ ((dllimport)) + #else + #define ROAD_SEGMENTS_PROVIDER_EXPORT __declspec(dllexport) + #define ROAD_SEGMENTS_PROVIDER_IMPORT __declspec(dllimport) + #endif + #ifdef ROAD_SEGMENTS_PROVIDER_BUILDING_LIBRARY + #define ROAD_SEGMENTS_PROVIDER_PUBLIC ROAD_SEGMENTS_PROVIDER_EXPORT + #else + #define ROAD_SEGMENTS_PROVIDER_PUBLIC ROAD_SEGMENTS_PROVIDER_IMPORT + #endif + #define ROAD_SEGMENTS_PROVIDER_PUBLIC_TYPE ROAD_SEGMENTS_PROVIDER_PUBLIC + #define ROAD_SEGMENTS_PROVIDER_LOCAL +#else + #define ROAD_SEGMENTS_PROVIDER_EXPORT __attribute__ ((visibility("default"))) + #define ROAD_SEGMENTS_PROVIDER_IMPORT + #if __GNUC__ >= 4 + #define ROAD_SEGMENTS_PROVIDER_PUBLIC __attribute__ ((visibility("default"))) + #define ROAD_SEGMENTS_PROVIDER_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define ROAD_SEGMENTS_PROVIDER_PUBLIC + #define ROAD_SEGMENTS_PROVIDER_LOCAL + #endif + #define ROAD_SEGMENTS_PROVIDER_PUBLIC_TYPE +#endif + +#endif // ROAD_SEGMENTS_PROVIDER__VISIBILITY_CONTROL_H_ diff --git a/perception/zed_wrapper/package.xml b/perception/road_segments_provider/package.xml similarity index 55% rename from perception/zed_wrapper/package.xml rename to perception/road_segments_provider/package.xml index 8ed338a5..2fbf3611 100644 --- a/perception/zed_wrapper/package.xml +++ b/perception/road_segments_provider/package.xml @@ -1,16 +1,22 @@ - zed_wrapper + road_segments_provider 0.0.0 - ZED SDK wrapper node — publishes image and point cloud as ROS 2 msgs. - Kyo Yamashita + Converts YOLOPv2 segmentation masks to RoadSegments via IPM + Kyo Yamashita MIT ament_cmake_auto + cv_bridge + geometry_msgs + mapless_planning_msgs + message_filters rclcpp sensor_msgs + tf2_ros + visualization_msgs ament_lint_auto ament_lint_common diff --git a/perception/road_segments_provider/src/ipm.cpp b/perception/road_segments_provider/src/ipm.cpp new file mode 100644 index 00000000..32e16a4f --- /dev/null +++ b/perception/road_segments_provider/src/ipm.cpp @@ -0,0 +1,56 @@ +#include "road_segments_provider/ipm.hpp" + +#include + +namespace road_segments_provider +{ + +std::array quat_to_rotation_matrix(double qx, double qy, double qz, double qw) +{ + std::array R{}; + R[0] = 1.0 - 2.0 * (qy * qy + qz * qz); + R[1] = 2.0 * (qx * qy - qz * qw); + R[2] = 2.0 * (qx * qz + qy * qw); + R[3] = 2.0 * (qx * qy + qz * qw); + R[4] = 1.0 - 2.0 * (qx * qx + qz * qz); + R[5] = 2.0 * (qy * qz - qx * qw); + R[6] = 2.0 * (qx * qz - qy * qw); + R[7] = 2.0 * (qy * qz + qx * qw); + R[8] = 1.0 - 2.0 * (qx * qx + qy * qy); + return R; +} + +std::optional project_pixel_to_ground( + int u, int v, const CameraParams & params) +{ + // Ray direction in camera frame (unnormalized) + const double dx = (static_cast(u) - params.cx) / params.fx; + const double dy = (static_cast(v) - params.cy) / params.fy; + const double dz = 1.0; + + // Transform ray direction to base_link frame: dir_base = R * [dx, dy, dz] + const auto & R = params.R; + const double bx = R[0] * dx + R[1] * dy + R[2] * dz; + const double by = R[3] * dx + R[4] * dy + R[5] * dz; + const double bz = R[6] * dx + R[7] * dy + R[8] * dz; + + // Ground intersection: t + lambda * [bx, by, bz], z-component = 0 + // params.t[2] + lambda * bz = 0 => lambda = -t[2] / bz + if (std::abs(bz) < 1e-9) { + return std::nullopt; + } + + const double lambda = -params.t[2] / bz; + if (lambda < 0.0) { + // Intersection is behind the camera + return std::nullopt; + } + + geometry_msgs::msg::Point p; + p.x = params.t[0] + lambda * bx; + p.y = params.t[1] + lambda * by; + p.z = 0.0; + return p; +} + +} // namespace road_segments_provider diff --git a/perception/road_segments_provider/src/road_segments_provider_node.cpp b/perception/road_segments_provider/src/road_segments_provider_node.cpp new file mode 100644 index 00000000..a7cd9aff --- /dev/null +++ b/perception/road_segments_provider/src/road_segments_provider_node.cpp @@ -0,0 +1,282 @@ +#include "road_segments_provider/road_segments_provider_node.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace road_segments_provider +{ + +RoadSegmentsProviderNode::RoadSegmentsProviderNode(const rclcpp::NodeOptions & options) +: RoadSegmentsProviderNode("", options) +{ +} + +RoadSegmentsProviderNode::RoadSegmentsProviderNode( + const std::string & name_space, + const rclcpp::NodeOptions & options) +: Node("road_segments_provider_node", name_space, options), + camera_info_topic_(declare_parameter( + "camera_info_topic", "/zed/zed_node/rgb/camera_info")), + lane_mask_topic_( + declare_parameter("lane_mask_topic", "/perception/lane_mask")), + da_mask_topic_( + declare_parameter("da_mask_topic", "/perception/drivable_area_mask")), + road_segments_topic_( + declare_parameter("road_segments_topic", "/perception/road_segments")), + camera_frame_id_( + declare_parameter("camera_frame_id", "camera_link")), + base_frame_id_( + declare_parameter("base_frame_id", "base_link")), + min_ground_x_m_(declare_parameter("min_ground_x_m", 0.5)), + max_ground_x_m_(declare_parameter("max_ground_x_m", 20.0)), + publish_markers_(declare_parameter("publish_markers", false)), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_) +{ + if (min_ground_x_m_ < 0.0 || max_ground_x_m_ <= min_ground_x_m_) { + throw std::invalid_argument( + "Invalid ground range: min_ground_x_m must be >= 0 and < max_ground_x_m"); + } + + camera_info_sub_ = create_subscription( + camera_info_topic_, + rclcpp::QoS(1).transient_local(), + [this](const sensor_msgs::msg::CameraInfo::ConstSharedPtr & msg) { + camera_info_callback(msg); + }); + + lane_mask_sub_ = std::make_shared>( + this, lane_mask_topic_, rclcpp::QoS(10).get_rmw_qos_profile()); + da_mask_sub_ = std::make_shared>( + this, da_mask_topic_, rclcpp::QoS(10).get_rmw_qos_profile()); + + sync_ = std::make_shared>( + SyncPolicy(10), *lane_mask_sub_, *da_mask_sub_); + sync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(0.1)); + sync_->registerCallback( + std::bind( + &RoadSegmentsProviderNode::sync_callback, this, + std::placeholders::_1, std::placeholders::_2)); + + road_segments_pub_ = create_publisher( + road_segments_topic_, rclcpp::QoS(10)); + + if (publish_markers_) { + marker_pub_ = create_publisher( + road_segments_topic_ + "/markers", rclcpp::QoS(10)); + } + + RCLCPP_INFO(get_logger(), "RoadSegmentsProviderNode initialized"); + RCLCPP_INFO(get_logger(), " lane_mask: %s", lane_mask_topic_.c_str()); + RCLCPP_INFO(get_logger(), " da_mask: %s", da_mask_topic_.c_str()); + RCLCPP_INFO(get_logger(), " output: %s", road_segments_topic_.c_str()); +} + +void RoadSegmentsProviderNode::camera_info_callback( + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & msg) +{ + std::lock_guard lock(camera_info_mutex_); + if (camera_intrinsics_) { + return; // already set + } + // K = [fx, 0, cx, 0, fy, cy, 0, 0, 1] (row-major) + camera_intrinsics_ = {msg->k[0], msg->k[4], msg->k[2], msg->k[5]}; + RCLCPP_INFO(get_logger(), "Camera intrinsics received: fx=%.1f fy=%.1f cx=%.1f cy=%.1f", + (*camera_intrinsics_)[0], (*camera_intrinsics_)[1], + (*camera_intrinsics_)[2], (*camera_intrinsics_)[3]); +} + +std::optional RoadSegmentsProviderNode::build_camera_params() +{ + std::lock_guard lock(camera_info_mutex_); + if (!camera_intrinsics_) { + return std::nullopt; + } + + geometry_msgs::msg::TransformStamped tf_stamped; + try { + tf_stamped = tf_buffer_.lookupTransform( + base_frame_id_, camera_frame_id_, tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, + "TF lookup %s -> %s failed: %s", + camera_frame_id_.c_str(), base_frame_id_.c_str(), ex.what()); + return std::nullopt; + } + + const auto & q = tf_stamped.transform.rotation; + const auto & tr = tf_stamped.transform.translation; + + CameraParams params; + params.fx = (*camera_intrinsics_)[0]; + params.fy = (*camera_intrinsics_)[1]; + params.cx = (*camera_intrinsics_)[2]; + params.cy = (*camera_intrinsics_)[3]; + params.R = quat_to_rotation_matrix(q.x, q.y, q.z, q.w); + params.t = {tr.x, tr.y, tr.z}; + return params; +} + +std::pair, std::vector> +RoadSegmentsProviderNode::extract_lane_boundaries( + const cv::Mat & lane_img, + const CameraParams & params) const +{ + std::vector left_pts; + std::vector right_pts; + + const int width = lane_img.cols; + const int height = lane_img.rows; + const int cx = width / 2; + + // Scan rows from bottom (nearest) to top (farthest) to build ordered boundaries + for (int v = height - 1; v >= 0; --v) { + const uchar * row = lane_img.ptr(v); + + int left_u = -1; // rightmost non-zero pixel in left half + int right_u = -1; // leftmost non-zero pixel in right half + + for (int u = cx - 1; u >= 0; --u) { + if (row[u] != 0) { + left_u = u; + break; + } + } + for (int u = cx; u < width; ++u) { + if (row[u] != 0) { + right_u = u; + break; + } + } + + if (left_u >= 0) { + auto pt = project_pixel_to_ground(left_u, v, params); + if (pt && pt->x >= min_ground_x_m_ && pt->x <= max_ground_x_m_) { + left_pts.push_back(*pt); + } + } + if (right_u >= 0) { + auto pt = project_pixel_to_ground(right_u, v, params); + if (pt && pt->x >= min_ground_x_m_ && pt->x <= max_ground_x_m_) { + right_pts.push_back(*pt); + } + } + } + + // Already ordered near to far (bottom row projected first, smallest x first in base_link) + // Sort by x ascending just in case projection re-orders due to camera tilt + auto by_x = [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) { + return a.x < b.x; + }; + std::sort(left_pts.begin(), left_pts.end(), by_x); + std::sort(right_pts.begin(), right_pts.end(), by_x); + + return {left_pts, right_pts}; +} + +visualization_msgs::msg::MarkerArray RoadSegmentsProviderNode::build_markers( + const std::vector & left, + const std::vector & right, + const std::string & frame_id, + const rclcpp::Time & stamp) const +{ + visualization_msgs::msg::MarkerArray arr; + + auto make_line_marker = [&]( + const std::vector & pts, + int id, float r, float g, float b) { + visualization_msgs::msg::Marker m; + m.header.frame_id = frame_id; + m.header.stamp = stamp; + m.ns = "lane_boundary"; + m.id = id; + m.type = visualization_msgs::msg::Marker::LINE_STRIP; + m.action = visualization_msgs::msg::Marker::ADD; + m.scale.x = 0.05; + m.color.r = r; + m.color.g = g; + m.color.b = b; + m.color.a = 1.0f; + m.points = pts; + return m; + }; + + if (!left.empty()) { + arr.markers.push_back(make_line_marker(left, 0, 1.0f, 1.0f, 0.0f)); + } + if (!right.empty()) { + arr.markers.push_back(make_line_marker(right, 1, 0.0f, 1.0f, 1.0f)); + } + return arr; +} + +void RoadSegmentsProviderNode::sync_callback( + const sensor_msgs::msg::Image::ConstSharedPtr & lane_msg, + const sensor_msgs::msg::Image::ConstSharedPtr & /*da_msg*/) +{ + const auto params_opt = build_camera_params(); + if (!params_opt) { + return; + } + const auto & params = *params_opt; + + cv_bridge::CvImageConstPtr lane_cv; + try { + lane_cv = cv_bridge::toCvShare(lane_msg, "mono8"); + } catch (const cv_bridge::Exception & ex) { + RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", ex.what()); + return; + } + + const cv::Mat & lane_img = lane_cv->image; + if (lane_img.empty()) { + return; + } + + auto [left_pts, right_pts] = extract_lane_boundaries(lane_img, params); + + if (left_pts.empty() && right_pts.empty()) { + RCLCPP_DEBUG(get_logger(), "No lane boundaries detected"); + return; + } + + // Build ego segment + mapless_planning_msgs::msg::Segment segment; + segment.id = 0; + + // linestrings[0] = left boundary + for (const auto & pt : left_pts) { + geometry_msgs::msg::Pose pose; + pose.position = pt; + pose.orientation.w = 1.0; + segment.linestrings[0].poses.push_back(pose); + } + // linestrings[1] = right boundary + for (const auto & pt : right_pts) { + geometry_msgs::msg::Pose pose; + pose.position = pt; + pose.orientation.w = 1.0; + segment.linestrings[1].poses.push_back(pose); + } + + mapless_planning_msgs::msg::RoadSegments msg; + msg.header.stamp = lane_msg->header.stamp; + msg.header.frame_id = base_frame_id_; + msg.segments.push_back(segment); + // pose = identity: segments are expressed in base_link frame, ego is at origin + msg.pose.orientation.w = 1.0; + + road_segments_pub_->publish(msg); + + if (publish_markers_ && marker_pub_) { + marker_pub_->publish( + build_markers(left_pts, right_pts, base_frame_id_, lane_msg->header.stamp)); + } +} + +} // namespace road_segments_provider diff --git a/perception/zed_wrapper/include/zed_wrapper/visibility_control.h b/perception/zed_wrapper/include/zed_wrapper/visibility_control.h deleted file mode 100644 index 2341e659..00000000 --- a/perception/zed_wrapper/include/zed_wrapper/visibility_control.h +++ /dev/null @@ -1,27 +0,0 @@ -#pragma once - -#if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define ZED_WRAPPER_EXPORT __attribute__((dllexport)) - #define ZED_WRAPPER_IMPORT __attribute__((dllimport)) - #else - #define ZED_WRAPPER_EXPORT __declspec(dllexport) - #define ZED_WRAPPER_IMPORT __declspec(dllimport) - #endif - #ifdef ZED_WRAPPER_BUILDING_LIBRARY - #define ZED_WRAPPER_PUBLIC ZED_WRAPPER_EXPORT - #else - #define ZED_WRAPPER_PUBLIC ZED_WRAPPER_IMPORT - #endif - #define ZED_WRAPPER_LOCAL -#else - #if __GNUC__ >= 4 - #define ZED_WRAPPER_PUBLIC __attribute__((visibility("default"))) - #define ZED_WRAPPER_LOCAL __attribute__((visibility("hidden"))) - #else - #define ZED_WRAPPER_PUBLIC - #define ZED_WRAPPER_LOCAL - #endif - #define ZED_WRAPPER_EXPORT ZED_WRAPPER_PUBLIC - #define ZED_WRAPPER_IMPORT -#endif diff --git a/perception/zed_wrapper/include/zed_wrapper/zed_wrapper_node.hpp b/perception/zed_wrapper/include/zed_wrapper/zed_wrapper_node.hpp deleted file mode 100644 index abaf0f64..00000000 --- a/perception/zed_wrapper/include/zed_wrapper/zed_wrapper_node.hpp +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once - -#include "zed_wrapper/visibility_control.h" - -#include - -#include -#include -#include -#include - -namespace zed_wrapper -{ - -class ZED_WRAPPER_PUBLIC ZedWrapperNode : public rclcpp::Node -{ -public: - explicit ZedWrapperNode( - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - explicit ZedWrapperNode( - const std::string & name_space, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - ~ZedWrapperNode(); - -private: - void grab_callback(); - sensor_msgs::msg::CameraInfo build_camera_info(); - - // PIMPL: ZED SDK types are hidden from the public header - struct Impl; - std::unique_ptr impl_; - - std::string camera_frame_id_; - sensor_msgs::msg::CameraInfo camera_info_cache_; - - rclcpp::Publisher::SharedPtr image_publisher_; - rclcpp::Publisher::SharedPtr pointcloud_publisher_; - rclcpp::Publisher::SharedPtr camera_info_publisher_; - rclcpp::TimerBase::SharedPtr timer_; -}; - -} // namespace zed_wrapper diff --git a/perception/zed_wrapper/src/zed_wrapper_node.cpp b/perception/zed_wrapper/src/zed_wrapper_node.cpp deleted file mode 100644 index 8d80786d..00000000 --- a/perception/zed_wrapper/src/zed_wrapper_node.cpp +++ /dev/null @@ -1,209 +0,0 @@ -#include "zed_wrapper/zed_wrapper_node.hpp" - -#include -#include - -#include -#include - -namespace zed_wrapper -{ - -// --- PIMPL body ----------------------------------------------------------- -struct ZedWrapperNode::Impl -{ - sl::Camera zed; - sl::RuntimeParameters runtime_params; -}; - -// --- helpers -------------------------------------------------------------- -namespace -{ - -// ZED X が出力対応するセンサ解像度（HD720/VGA/HD2K は非対応） -sl::RESOLUTION parse_resolution(const std::string & s) -{ - if (s == "HD1200") { return sl::RESOLUTION::HD1200; } - if (s == "HD1080") { return sl::RESOLUTION::HD1080; } - if (s == "SVGA") { return sl::RESOLUTION::SVGA; } - throw std::invalid_argument( - "ZedWrapperNode: unsupported resolution for ZED X: " + s); -} - -// ZED SDK 4.X の DEPTH_MODE 全選択肢 -sl::DEPTH_MODE parse_depth_mode(const std::string & s) -{ - if (s == "NONE") { return sl::DEPTH_MODE::NONE; } - if (s == "PERFORMANCE") { return sl::DEPTH_MODE::PERFORMANCE; } - if (s == "QUALITY") { return sl::DEPTH_MODE::QUALITY; } - if (s == "ULTRA") { return sl::DEPTH_MODE::ULTRA; } - if (s == "NEURAL") { return sl::DEPTH_MODE::NEURAL; } - if (s == "NEURAL_PLUS") { return sl::DEPTH_MODE::NEURAL_PLUS; } - throw std::invalid_argument( - "ZedWrapperNode: unsupported depth_mode: " + s); -} - -// publish 解像度（ZED X 想定: 640x360 へリサイズ） -const sl::Resolution kPublishResolution(640, 360); - -} // namespace - -// --- constructors / destructor -------------------------------------------- -ZedWrapperNode::ZedWrapperNode(const rclcpp::NodeOptions & options) -: ZedWrapperNode("", options) {} - -ZedWrapperNode::ZedWrapperNode( - const std::string & name_space, - const rclcpp::NodeOptions & options) -: rclcpp::Node("zed_wrapper_node", name_space, options), - impl_(std::make_unique()) -{ - const int grab_fps = get_parameter("grab_fps").as_int(); - const auto resolution = get_parameter("resolution").as_string(); - const auto depth_mode = get_parameter("depth_mode").as_string(); - const int confidence = get_parameter("confidence_threshold").as_int(); - const int serial_num = get_parameter("serial_number").as_int(); - camera_frame_id_ = get_parameter("camera_frame_id").as_string(); - - if (grab_fps <= 0) { - throw std::invalid_argument("ZedWrapperNode: grab_fps must be > 0"); - } - - sl::InitParameters init_params; - init_params.camera_resolution = parse_resolution(resolution); - init_params.camera_fps = grab_fps; - init_params.depth_mode = parse_depth_mode(depth_mode); - init_params.coordinate_units = sl::UNIT::METER; - init_params.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; - if (serial_num != 0) { - init_params.input.setFromSerialNumber(serial_num); - } - - const sl::ERROR_CODE ec = impl_->zed.open(init_params); - if (ec != sl::ERROR_CODE::SUCCESS) { - throw std::runtime_error( - std::string("ZedWrapperNode: camera open failed: ") + sl::toString(ec).c_str()); - } - - impl_->runtime_params.confidence_threshold = confidence; - camera_info_cache_ = build_camera_info(); - - image_publisher_ = create_publisher( - "/zed/zed_node/rgb/image_rect_color", rclcpp::QoS(10)); - pointcloud_publisher_ = create_publisher( - "/zed/zed_node/point_cloud", rclcpp::QoS(10)); - camera_info_publisher_ = create_publisher( - "/zed/zed_node/rgb/camera_info", rclcpp::QoS(10)); - - const auto period = std::chrono::milliseconds(1000 / grab_fps); - timer_ = create_wall_timer(period, [this]() { grab_callback(); }); - - RCLCPP_INFO(get_logger(), "ZedWrapperNode initialized (fps=%d, res=%s, depth=%s)", - grab_fps, resolution.c_str(), depth_mode.c_str()); -} - -ZedWrapperNode::~ZedWrapperNode() -{ - impl_->zed.close(); -} - -// --- timer callback ------------------------------------------------------- -void ZedWrapperNode::grab_callback() -{ - const sl::ERROR_CODE ec = impl_->zed.grab(impl_->runtime_params); - if (ec != sl::ERROR_CODE::SUCCESS) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, - "ZedWrapperNode: grab failed: %s", sl::toString(ec).c_str()); - return; - } - - const rclcpp::Time stamp = now(); - - sl::Mat left_image; - impl_->zed.retrieveImage(left_image, sl::VIEW::LEFT, sl::MEM::CPU, kPublishResolution); - { - sensor_msgs::msg::Image msg; - msg.header.stamp = stamp; - msg.header.frame_id = camera_frame_id_; - msg.width = static_cast(left_image.getWidth()); - msg.height = static_cast(left_image.getHeight()); - msg.encoding = "bgra8"; - msg.step = static_cast(left_image.getStepBytes()); - const size_t nbytes = msg.height * msg.step; - msg.data.resize(nbytes); - std::memcpy(msg.data.data(), left_image.getPtr(), nbytes); - image_publisher_->publish(msg); - } - - // XYZRGBA point cloud - sl::Mat pc_mat; - impl_->zed.retrieveMeasure(pc_mat, sl::MEASURE::XYZRGBA, sl::MEM::CPU); - { - sensor_msgs::msg::PointCloud2 msg; - msg.header.stamp = stamp; - msg.header.frame_id = camera_frame_id_; - msg.height = static_cast(pc_mat.getHeight()); - msg.width = static_cast(pc_mat.getWidth()); - msg.is_dense = false; - msg.point_step = 16; // 4 x float32: x, y, z, rgba - msg.row_step = msg.point_step * msg.width; - - msg.fields.resize(4); - msg.fields[0].name = "x"; msg.fields[0].offset = 0; - msg.fields[1].name = "y"; msg.fields[1].offset = 4; - msg.fields[2].name = "z"; msg.fields[2].offset = 8; - msg.fields[3].name = "rgba"; msg.fields[3].offset = 12; - for (auto & f : msg.fields) { - f.datatype = sensor_msgs::msg::PointField::FLOAT32; - f.count = 1; - } - - const size_t nbytes = static_cast(msg.height) * msg.row_step; - msg.data.resize(nbytes); - std::memcpy(msg.data.data(), pc_mat.getPtr(), nbytes); - pointcloud_publisher_->publish(msg); - } - - // Camera info (constant intrinsics, only stamp changes each frame) - camera_info_cache_.header.stamp = stamp; - camera_info_publisher_->publish(camera_info_cache_); -} - -// --- camera info construction (called once after open) -------------------- -sensor_msgs::msg::CameraInfo ZedWrapperNode::build_camera_info() -{ - // 取得サイズ（=publish サイズ）に合わせて intrinsics をスケール済みで取得 - const sl::CameraInformation cam_info = impl_->zed.getCameraInformation(kPublishResolution); - const auto & calib = cam_info.camera_configuration.calibration_parameters.left_cam; - const auto & res = kPublishResolution; - - sensor_msgs::msg::CameraInfo msg; - msg.header.frame_id = camera_frame_id_; - msg.width = static_cast(res.width); - msg.height = static_cast(res.height); - msg.distortion_model = "plumb_bob"; - - // D: k1, k2, p1, p2, k3 - msg.d = {calib.disto[0], calib.disto[1], calib.disto[2], calib.disto[3], calib.disto[4]}; - - // K: row-major 3x3 - msg.k = { - calib.fx, 0.0, calib.cx, - 0.0, calib.fy, calib.cy, - 0.0, 0.0, 1.0 - }; - - // R: identity (images are already rectified by ZED SDK) - msg.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; - - // P: row-major 3x4 - msg.p = { - calib.fx, 0.0, calib.cx, 0.0, - 0.0, calib.fy, calib.cy, 0.0, - 0.0, 0.0, 1.0, 0.0 - }; - - return msg; -} - -} // namespace zed_wrapper From f4a9ea676fd8520e65111401f2700942fae885ec Mon Sep 17 00:00:00 2001 From: kyo0221 Date: Tue, 5 May 2026 20:55:20 +0900 Subject: [PATCH 04/13] =?UTF-8?q?=E2=9C=A8feat:=20mapless=20lane=5Fplanner?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- planner/lane_planner/CMakeLists.txt | 5 +- .../lane_planner/lane_planner_plugin.hpp | 39 ++ .../lane_planner/lane_planner_server.hpp | 54 ++ .../mapless_lane_planner_plugin.hpp | 72 ++ .../vectormap_lane_planner_plugin.hpp | 115 ++++ planner/lane_planner/package.xml | 1 + ...ner_node.cpp => lane_planner_node.cpp.bak} | 0 .../lane_planner/src/lane_planner_server.cpp | 119 ++++ .../plugins/mapless_lane_planner_plugin.cpp | 263 +++++++ .../plugins/vectormap_lane_planner_plugin.cpp | 644 ++++++++++++++++++ ...oute_builder.cpp => route_builder.cpp.bak} | 0 .../local_planner/local_planner_server.hpp | 2 + .../src/local_planner_server.cpp | 20 + 13 files changed, 1332 insertions(+), 2 deletions(-) create mode 100644 planner/lane_planner/include/lane_planner/lane_planner_plugin.hpp create mode 100644 planner/lane_planner/include/lane_planner/lane_planner_server.hpp create mode 100644 planner/lane_planner/include/lane_planner/mapless_lane_planner_plugin.hpp create mode 100644 planner/lane_planner/include/lane_planner/vectormap_lane_planner_plugin.hpp rename planner/lane_planner/src/{lane_planner_node.cpp => lane_planner_node.cpp.bak} (100%) create mode 100644 planner/lane_planner/src/lane_planner_server.cpp create mode 100644 planner/lane_planner/src/plugins/mapless_lane_planner_plugin.cpp create mode 100644 planner/lane_planner/src/plugins/vectormap_lane_planner_plugin.cpp rename planner/lane_planner/src/{route_builder.cpp => route_builder.cpp.bak} (100%) diff --git a/planner/lane_planner/CMakeLists.txt b/planner/lane_planner/CMakeLists.txt index 5f7e572e..8db4c2b4 100644 --- a/planner/lane_planner/CMakeLists.txt +++ b/planner/lane_planner/CMakeLists.txt @@ -9,8 +9,9 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() ament_auto_add_library(lane_planner_node SHARED - src/lane_planner_node.cpp - src/route_builder.cpp + src/lane_planner_server.cpp + src/plugins/vectormap_lane_planner_plugin.cpp + src/plugins/mapless_lane_planner_plugin.cpp ) target_compile_features(lane_planner_node PUBLIC c_std_99 cxx_std_17) target_compile_definitions(lane_planner_node PRIVATE "LANE_PLANNER_BUILDING_LIBRARY") diff --git a/planner/lane_planner/include/lane_planner/lane_planner_plugin.hpp b/planner/lane_planner/include/lane_planner/lane_planner_plugin.hpp new file mode 100644 index 00000000..24744e25 --- /dev/null +++ b/planner/lane_planner/include/lane_planner/lane_planner_plugin.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include + +namespace lane_planner +{ + +class LanePlannerPlugin +{ +public: + virtual ~LanePlannerPlugin() = default; + + virtual void initialize(rclcpp::Node & node) = 0; + + virtual void set_pose( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose) = 0; + + virtual void set_nav_command(const std::string & command) = 0; + + virtual std::optional compute_path( + const rclcpp::Time & stamp) = 0; + + // Optional setter — only used by mapless plugin; default is no-op + virtual void set_road_segments( + const mapless_planning_msgs::msg::RoadSegments & /*segments*/) {} + + // Optional — only used by mapless plugin; default returns nullopt + virtual std::optional + get_mission_lanes() const { return std::nullopt; } +}; + +} // namespace lane_planner diff --git a/planner/lane_planner/include/lane_planner/lane_planner_server.hpp b/planner/lane_planner/include/lane_planner/lane_planner_server.hpp new file mode 100644 index 00000000..e3691f10 --- /dev/null +++ b/planner/lane_planner/include/lane_planner/lane_planner_server.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "lane_planner/lane_planner_plugin.hpp" +#include "lane_planner/visibility_control.h" + +namespace lane_planner +{ + +class LanePlannerServer : public rclcpp::Node +{ +public: + LANE_PLANNER_PUBLIC + explicit LanePlannerServer( + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + LANE_PLANNER_PUBLIC + explicit LanePlannerServer( + const std::string & name_space, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + void nav_cmd_callback(const std_msgs::msg::String::SharedPtr msg); + void road_segments_callback( + const mapless_planning_msgs::msg::RoadSegments::SharedPtr msg); + void timer_callback(); + + const int update_period_ms_; + const rclcpp::QoS qos_; + + std::unique_ptr plugin_; + mutable std::mutex data_mutex_; + + rclcpp::Subscription::SharedPtr pose_sub_; + rclcpp::Subscription::SharedPtr nav_cmd_sub_; + rclcpp::Subscription::SharedPtr road_segments_sub_; + rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Publisher::SharedPtr mission_lanes_pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace lane_planner diff --git a/planner/lane_planner/include/lane_planner/mapless_lane_planner_plugin.hpp b/planner/lane_planner/include/lane_planner/mapless_lane_planner_plugin.hpp new file mode 100644 index 00000000..8c2c175b --- /dev/null +++ b/planner/lane_planner/include/lane_planner/mapless_lane_planner_plugin.hpp @@ -0,0 +1,72 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "lane_planner/lane_planner_plugin.hpp" + +namespace lane_planner +{ + +class MaplessLanePlannerPlugin : public LanePlannerPlugin +{ +public: + void initialize(rclcpp::Node & node) override; + void set_pose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) override; + void set_nav_command(const std::string & command) override; + void set_road_segments(const mapless_planning_msgs::msg::RoadSegments & segments) override; + std::optional compute_path(const rclcpp::Time & stamp) override; + std::optional + get_mission_lanes() const override; + +private: + using Point = geometry_msgs::msg::Point; + + struct Pose2D + { + double x{0.0}; + double y{0.0}; + double yaw{0.0}; + }; + + std::vector compute_centerline( + const mapless_planning_msgs::msg::RoadSegments & segments) const; + + mapless_planning_msgs::msg::DrivingCorridor build_ego_corridor( + const mapless_planning_msgs::msg::RoadSegments & segments) const; + + nav_msgs::msg::Path corridor_to_path( + const mapless_planning_msgs::msg::DrivingCorridor & corridor, + const geometry_msgs::msg::Pose & ego_in_map, + const rclcpp::Time & stamp) const; + + static geometry_msgs::msg::Quaternion yaw_to_quaternion(double yaw); + + // Parameters + std::string map_frame_id_{"map"}; + std::string base_frame_id_{"base_link"}; + double max_corridor_length_m_{20.0}; + double path_resample_interval_m_{0.5}; + int target_lane_{0}; // 0=LANE_KEEP, -1=LEFT, +1=RIGHT + + // Runtime state + mutable std::mutex data_mutex_; + std::optional latest_segments_; + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr latest_pose_; + std::optional latest_mission_lanes_; + + rclcpp::Logger logger_{rclcpp::get_logger("mapless_lane_planner_plugin")}; + rclcpp::Clock::SharedPtr clock_; +}; + +} // namespace lane_planner diff --git a/planner/lane_planner/include/lane_planner/vectormap_lane_planner_plugin.hpp b/planner/lane_planner/include/lane_planner/vectormap_lane_planner_plugin.hpp new file mode 100644 index 00000000..57679b7f --- /dev/null +++ b/planner/lane_planner/include/lane_planner/vectormap_lane_planner_plugin.hpp @@ -0,0 +1,115 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "lane_planner/lane_planner_plugin.hpp" + +namespace lane_planner +{ + +class VectormapLanePlannerPlugin : public LanePlannerPlugin +{ +public: + struct Point2D + { + double x; + double y; + }; + + struct PathPoint + { + double s; + double x; + double y; + double yaw; + uint64_t lanelet_id; + }; + + struct LaneletRange + { + uint64_t lanelet_id; + double start_s; + double end_s; + }; + + struct RouteEdge + { + uint64_t to_lanelet_id; + uint8_t turn_direction; + double cost; + }; + + void initialize(rclcpp::Node & node) override; + void set_pose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) override; + void set_nav_command(const std::string & command) override; + std::optional compute_path(const rclcpp::Time & stamp) override; + +private: + void vector_map_callback(const vectormap_msgs::msg::VectorMap::SharedPtr msg); + + void build_global_path_once(const vectormap_msgs::msg::VectorMap & map_msg); + void build_route_from_lanelet_ids(const std::vector & route_lanelet_ids); + void rebuild_route_from_lanelet(uint64_t start_lanelet_id, const std::string & reason); + bool rebuild_route_from_pose(const Point2D & ego, const std::string & reason); + std::vector build_route_sequence_from_graph( + uint64_t start_lanelet_id, std::size_t & fallback_count) const; + uint64_t select_next_lanelet( + uint64_t from_lanelet_id, uint8_t requested_turn, + uint8_t & selected_turn, bool & used_fallback) const; + uint64_t find_nearest_lanelet_from_pose(const Point2D & point) const; + std::pair find_nearest_lanelet_within_route(const Point2D & point) const; + uint64_t lanelet_at_s(double s) const; + double normalize_path_s(double s) const; + uint8_t parse_nav_cmd(const std::string & command) const; + std::string turn_direction_to_string(uint8_t turn_direction) const; + nav_msgs::msg::Path make_path_message(const rclcpp::Time & stamp) const; + static geometry_msgs::msg::Quaternion yaw_to_quaternion(double yaw); + + // Parameters (read during initialize()) + std::string map_frame_id_; + std::string vector_map_topic_; + std::string default_nav_cmd_; + std::vector route_lanelet_ids_param_; + std::vector nav_cmd_fallback_order_param_; + double global_path_resample_interval_m_{0.5}; + double max_centerline_connection_gap_m_{0.5}; + double off_route_distance_threshold_m_{3.0}; + int route_lookahead_lanelet_count_{10}; + + // Runtime state + bool global_path_ready_{false}; + bool route_is_loop_{false}; + bool pending_route_rebuild_{false}; + std::string pending_route_rebuild_reason_; + std::vector current_route_lanelet_ids_; + uint8_t last_nav_cmd_turn_{0}; + std::vector nav_cmd_fallback_order_; + std::string path_frame_id_; + std::vector global_samples_; + std::vector lanelet_ranges_; + std::unordered_map lanelet_by_id_; + std::unordered_map> lanelet_centerline_points_by_id_; + std::unordered_map> connection_edges_by_from_lanelet_id_; + + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr latest_pose_; + mutable std::mutex data_mutex_; + + rclcpp::Subscription::SharedPtr vector_map_sub_; + rclcpp::Logger logger_{rclcpp::get_logger("vectormap_lane_planner_plugin")}; + rclcpp::Clock::SharedPtr clock_; +}; + +} // namespace lane_planner diff --git a/planner/lane_planner/package.xml b/planner/lane_planner/package.xml index eef12029..bc77d28b 100644 --- a/planner/lane_planner/package.xml +++ b/planner/lane_planner/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto geometry_msgs + mapless_planning_msgs nav_msgs rclcpp std_msgs diff --git a/planner/lane_planner/src/lane_planner_node.cpp b/planner/lane_planner/src/lane_planner_node.cpp.bak similarity index 100% rename from planner/lane_planner/src/lane_planner_node.cpp rename to planner/lane_planner/src/lane_planner_node.cpp.bak diff --git a/planner/lane_planner/src/lane_planner_server.cpp b/planner/lane_planner/src/lane_planner_server.cpp new file mode 100644 index 00000000..7d523da7 --- /dev/null +++ b/planner/lane_planner/src/lane_planner_server.cpp @@ -0,0 +1,119 @@ +#include "lane_planner/lane_planner_server.hpp" + +#include + +#include "lane_planner/mapless_lane_planner_plugin.hpp" +#include "lane_planner/vectormap_lane_planner_plugin.hpp" + +namespace lane_planner +{ + +LanePlannerServer::LanePlannerServer(const rclcpp::NodeOptions & options) +: LanePlannerServer("", options) +{ +} + +LanePlannerServer::LanePlannerServer( + const std::string & name_space, + const rclcpp::NodeOptions & options) +: rclcpp::Node("lane_planner_node", name_space, options), + update_period_ms_(get_parameter("update_period_ms").as_int()), + qos_(rclcpp::QoS(10)) +{ + if (update_period_ms_ <= 0) { + throw std::invalid_argument("update_period_ms must be greater than 0"); + } + + const std::string mode = get_parameter("lane_planner_mode").as_string(); + + if (mode == "vectormap") { + plugin_ = std::make_unique(); + } else if (mode == "mapless") { + plugin_ = std::make_unique(); + } else { + throw std::invalid_argument("lane_planner_mode must be 'vectormap' or 'mapless': " + mode); + } + + plugin_->initialize(*this); + + pose_sub_ = create_subscription( + get_parameter("localization_pose_topic").as_string(), + qos_, + [this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + pose_callback(msg); + }); + + nav_cmd_sub_ = create_subscription( + get_parameter("nav_cmd_topic").as_string(), + qos_, + [this](const std_msgs::msg::String::SharedPtr msg) { + nav_cmd_callback(msg); + }); + + if (mode == "mapless") { + road_segments_sub_ = create_subscription( + get_parameter("road_segments_topic").as_string(), + qos_, + [this](const mapless_planning_msgs::msg::RoadSegments::SharedPtr msg) { + road_segments_callback(msg); + }); + mission_lanes_pub_ = create_publisher( + get_parameter("mission_lanes_topic").as_string(), qos_); + } + + path_pub_ = create_publisher( + get_parameter("global_path_topic").as_string(), qos_); + + timer_ = create_wall_timer( + std::chrono::milliseconds(update_period_ms_), + std::bind(&LanePlannerServer::timer_callback, this)); + + RCLCPP_INFO(get_logger(), "LanePlannerServer started in '%s' mode", mode.c_str()); +} + +void LanePlannerServer::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + std::lock_guard lock(data_mutex_); + plugin_->set_pose(*msg); +} + +void LanePlannerServer::nav_cmd_callback(const std_msgs::msg::String::SharedPtr msg) +{ + if (!msg) { + RCLCPP_ERROR(get_logger(), "nav_cmd message must not be null"); + return; + } + std::lock_guard lock(data_mutex_); + plugin_->set_nav_command(msg->data); +} + +void LanePlannerServer::road_segments_callback( + const mapless_planning_msgs::msg::RoadSegments::SharedPtr msg) +{ + if (!msg) { + RCLCPP_ERROR(get_logger(), "RoadSegments message must not be null"); + return; + } + std::lock_guard lock(data_mutex_); + plugin_->set_road_segments(*msg); +} + +void LanePlannerServer::timer_callback() +{ + std::lock_guard lock(data_mutex_); + const auto path = plugin_->compute_path(now()); + if (!path) { + return; + } + path_pub_->publish(*path); + + if (mission_lanes_pub_) { + const auto ml = plugin_->get_mission_lanes(); + if (ml) { + mission_lanes_pub_->publish(*ml); + } + } +} + +} // namespace lane_planner diff --git a/planner/lane_planner/src/plugins/mapless_lane_planner_plugin.cpp b/planner/lane_planner/src/plugins/mapless_lane_planner_plugin.cpp new file mode 100644 index 00000000..3a405070 --- /dev/null +++ b/planner/lane_planner/src/plugins/mapless_lane_planner_plugin.cpp @@ -0,0 +1,263 @@ +#include "lane_planner/mapless_lane_planner_plugin.hpp" + +#include +#include + +namespace lane_planner +{ +namespace +{ + +constexpr double EPSILON = 1.0e-9; + +// 3x3 rotation matrix rows from quaternion (x, y, z, w) +struct RotMat3 +{ + double r[3][3]; +}; + +RotMat3 quat_to_rotmat(double qx, double qy, double qz, double qw) +{ + RotMat3 R{}; + R.r[0][0] = 1.0 - 2.0 * (qy * qy + qz * qz); + R.r[0][1] = 2.0 * (qx * qy - qz * qw); + R.r[0][2] = 2.0 * (qx * qz + qy * qw); + R.r[1][0] = 2.0 * (qx * qy + qz * qw); + R.r[1][1] = 1.0 - 2.0 * (qx * qx + qz * qz); + R.r[1][2] = 2.0 * (qy * qz - qx * qw); + R.r[2][0] = 2.0 * (qx * qz - qy * qw); + R.r[2][1] = 2.0 * (qy * qz + qx * qw); + R.r[2][2] = 1.0 - 2.0 * (qx * qx + qy * qy); + return R; +} + +geometry_msgs::msg::Point transform_point( + const geometry_msgs::msg::Point & p_base, + const geometry_msgs::msg::Pose & ego_in_map) +{ + const auto & o = ego_in_map.orientation; + const auto R = quat_to_rotmat(o.x, o.y, o.z, o.w); + geometry_msgs::msg::Point p_map; + p_map.x = R.r[0][0] * p_base.x + R.r[0][1] * p_base.y + R.r[0][2] * p_base.z + + ego_in_map.position.x; + p_map.y = R.r[1][0] * p_base.x + R.r[1][1] * p_base.y + R.r[1][2] * p_base.z + + ego_in_map.position.y; + p_map.z = R.r[2][0] * p_base.x + R.r[2][1] * p_base.y + R.r[2][2] * p_base.z + + ego_in_map.position.z; + return p_map; +} + +} // namespace + +// ---------- LanePlannerPlugin interface ---------- + +void MaplessLanePlannerPlugin::initialize(rclcpp::Node & node) +{ + clock_ = node.get_clock(); + logger_ = node.get_logger(); + + map_frame_id_ = node.get_parameter("map_frame_id").as_string(); + max_corridor_length_m_ = node.get_parameter("max_corridor_length_m").as_double(); + path_resample_interval_m_ = node.get_parameter("path_resample_interval_m").as_double(); + + if (max_corridor_length_m_ <= 0.0) { + throw std::invalid_argument("max_corridor_length_m must be greater than 0"); + } + if (path_resample_interval_m_ <= 0.0) { + throw std::invalid_argument("path_resample_interval_m must be greater than 0"); + } + + RCLCPP_INFO(logger_, "MaplessLanePlannerPlugin initialized"); +} + +void MaplessLanePlannerPlugin::set_pose( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +{ + std::lock_guard lock(data_mutex_); + latest_pose_ = std::make_shared(pose); +} + +void MaplessLanePlannerPlugin::set_nav_command(const std::string & command) +{ + int16_t target = mapless_planning_msgs::msg::MissionLanesStamped::LANE_KEEP; + if (command == "left") { + target = mapless_planning_msgs::msg::MissionLanesStamped::LANE_CHANGE_LEFT; + } else if (command == "right") { + target = mapless_planning_msgs::msg::MissionLanesStamped::LANE_CHANGE_RIGHT; + } else if (command != "straight" && command != "keep") { + RCLCPP_ERROR(logger_, "unknown nav_cmd for mapless: '%s'", command.c_str()); + return; + } + std::lock_guard lock(data_mutex_); + target_lane_ = static_cast(target); +} + +void MaplessLanePlannerPlugin::set_road_segments( + const mapless_planning_msgs::msg::RoadSegments & segments) +{ + std::lock_guard lock(data_mutex_); + latest_segments_ = segments; +} + +std::optional MaplessLanePlannerPlugin::compute_path( + const rclcpp::Time & stamp) +{ + std::lock_guard lock(data_mutex_); + if (!latest_segments_) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 2000, "no RoadSegments received yet"); + return std::nullopt; + } + if (!latest_pose_) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 2000, "no localization pose received yet"); + return std::nullopt; + } + + if (latest_segments_->segments.empty()) { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 2000, "RoadSegments has no segments"); + return std::nullopt; + } + + if (target_lane_ != mapless_planning_msgs::msg::MissionLanesStamped::LANE_KEEP) { + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 2000, + "lane change requested (target_lane=%d) but not implemented in initial version; " + "no path published", + target_lane_); + return std::nullopt; + } + + const auto ego_corridor = build_ego_corridor(*latest_segments_); + + // Build MissionLanesStamped for external use + mapless_planning_msgs::msg::MissionLanesStamped ml; + ml.header.stamp = stamp; + ml.header.frame_id = latest_segments_->header.frame_id; + ml.ego_lane = ego_corridor; + ml.lane_with_goal_point = ego_corridor; + ml.target_lane = static_cast(target_lane_); + latest_mission_lanes_ = ml; + + if (ego_corridor.centerline.empty()) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 2000, "ego corridor centerline is empty"); + return std::nullopt; + } + + return corridor_to_path(ego_corridor, latest_pose_->pose.pose, stamp); +} + +std::optional +MaplessLanePlannerPlugin::get_mission_lanes() const +{ + std::lock_guard lock(data_mutex_); + return latest_mission_lanes_; +} + +// ---------- Internal helpers ---------- + +mapless_planning_msgs::msg::DrivingCorridor MaplessLanePlannerPlugin::build_ego_corridor( + const mapless_planning_msgs::msg::RoadSegments & segments) const +{ + // Initial version: use segment 0 as ego lane + const auto & seg = segments.segments[0]; + + if (seg.linestrings.size() < 2 || + seg.linestrings[0].poses.empty() || + seg.linestrings[1].poses.empty()) + { + RCLCPP_WARN(logger_, "segment 0 has insufficient linestrings"); + return mapless_planning_msgs::msg::DrivingCorridor{}; + } + + const auto & left_ls = seg.linestrings[0]; + const auto & right_ls = seg.linestrings[1]; + + mapless_planning_msgs::msg::DrivingCorridor corridor; + + for (const auto & pose : left_ls.poses) { + corridor.bound_left.push_back(pose.position); + } + for (const auto & pose : right_ls.poses) { + corridor.bound_right.push_back(pose.position); + } + + // Compute centerline: interpolate between left and right at same x samples + const std::size_t n_left = left_ls.poses.size(); + const std::size_t n_right = right_ls.poses.size(); + const std::size_t n_pts = std::max(n_left, n_right); + + for (std::size_t i = 0; i < n_pts; ++i) { + const double tl = static_cast(i) / std::max(1, n_pts - 1); + const double tr = tl; + + const auto idx_l = static_cast(tl * static_cast(n_left - 1)); + const auto idx_r = static_cast(tr * static_cast(n_right - 1)); + + geometry_msgs::msg::Point c; + c.x = 0.5 * (left_ls.poses[idx_l].position.x + right_ls.poses[idx_r].position.x); + c.y = 0.5 * (left_ls.poses[idx_l].position.y + right_ls.poses[idx_r].position.y); + c.z = 0.0; + corridor.centerline.push_back(c); + } + + return corridor; +} + +nav_msgs::msg::Path MaplessLanePlannerPlugin::corridor_to_path( + const mapless_planning_msgs::msg::DrivingCorridor & corridor, + const geometry_msgs::msg::Pose & ego_in_map, + const rclcpp::Time & stamp) const +{ + nav_msgs::msg::Path path; + path.header.stamp = stamp; + path.header.frame_id = map_frame_id_; + + const auto & cl = corridor.centerline; + if (cl.size() < 2) { + return path; + } + + path.poses.reserve(cl.size()); + for (std::size_t i = 0; i < cl.size(); ++i) { + const auto p_map = transform_point(cl[i], ego_in_map); + + double yaw = 0.0; + if (i + 1 < cl.size()) { + const auto p_next = transform_point(cl[i + 1], ego_in_map); + const double dx = p_next.x - p_map.x; + const double dy = p_next.y - p_map.y; + if (std::hypot(dx, dy) > EPSILON) { + yaw = std::atan2(dy, dx); + } + } else if (i > 0) { + const auto p_prev = transform_point(cl[i - 1], ego_in_map); + const double dx = p_map.x - p_prev.x; + const double dy = p_map.y - p_prev.y; + if (std::hypot(dx, dy) > EPSILON) { + yaw = std::atan2(dy, dx); + } + } + + geometry_msgs::msg::PoseStamped ps; + ps.header = path.header; + ps.pose.position = p_map; + ps.pose.orientation = yaw_to_quaternion(yaw); + path.poses.push_back(ps); + } + + return path; +} + +geometry_msgs::msg::Quaternion MaplessLanePlannerPlugin::yaw_to_quaternion(const double yaw) +{ + geometry_msgs::msg::Quaternion q; + q.x = 0.0; + q.y = 0.0; + q.z = std::sin(yaw * 0.5); + q.w = std::cos(yaw * 0.5); + return q; +} + +} // namespace lane_planner diff --git a/planner/lane_planner/src/plugins/vectormap_lane_planner_plugin.cpp b/planner/lane_planner/src/plugins/vectormap_lane_planner_plugin.cpp new file mode 100644 index 00000000..e121f52c --- /dev/null +++ b/planner/lane_planner/src/plugins/vectormap_lane_planner_plugin.cpp @@ -0,0 +1,644 @@ +#include "lane_planner/vectormap_lane_planner_plugin.hpp" + +#include +#include +#include +#include + +namespace lane_planner +{ +namespace +{ + +constexpr double EPSILON = 1.0e-6; + +double distance_2d( + const VectormapLanePlannerPlugin::Point2D & a, + const VectormapLanePlannerPlugin::Point2D & b) +{ + return std::hypot(a.x - b.x, a.y - b.y); +} + +double point_segment_distance_sq( + const VectormapLanePlannerPlugin::Point2D & point, + const VectormapLanePlannerPlugin::Point2D & start, + const VectormapLanePlannerPlugin::Point2D & end) +{ + const double vx = end.x - start.x; + const double vy = end.y - start.y; + const double length_sq = vx * vx + vy * vy; + if (length_sq <= EPSILON) { + const double dx = point.x - start.x; + const double dy = point.y - start.y; + return dx * dx + dy * dy; + } + const double wx = point.x - start.x; + const double wy = point.y - start.y; + const double t = std::clamp((wx * vx + wy * vy) / length_sq, 0.0, 1.0); + const double px = start.x + t * vx; + const double py = start.y + t * vy; + const double dx = point.x - px; + const double dy = point.y - py; + return dx * dx + dy * dy; +} + +VectormapLanePlannerPlugin::PathPoint interpolate_raw_path( + const std::vector & points, + const std::vector & s_values, + const double target_s, + const uint64_t lanelet_id) +{ + if (points.size() != s_values.size() || points.size() < 2U) { + throw std::runtime_error("raw path is invalid"); + } + const double clamped_s = std::clamp(target_s, s_values.front(), s_values.back()); + const auto upper = std::upper_bound(s_values.begin(), s_values.end(), clamped_s); + std::size_t index = 0U; + if (upper == s_values.end()) { + index = s_values.size() - 2U; + } else if (upper != s_values.begin()) { + index = static_cast(std::distance(s_values.begin(), upper) - 1); + } + const double segment_length = s_values[index + 1U] - s_values[index]; + const double ratio = segment_length > EPSILON + ? (clamped_s - s_values[index]) / segment_length : 0.0; + const auto & p0 = points[index]; + const auto & p1 = points[index + 1U]; + const double x = p0.x + ratio * (p1.x - p0.x); + const double y = p0.y + ratio * (p1.y - p0.y); + const double yaw = std::atan2(p1.y - p0.y, p1.x - p0.x); + return {clamped_s, x, y, yaw, lanelet_id}; +} + +} // namespace + +// ---------- LanePlannerPlugin interface ---------- + +void VectormapLanePlannerPlugin::initialize(rclcpp::Node & node) +{ + clock_ = node.get_clock(); + logger_ = node.get_logger(); + + map_frame_id_ = node.get_parameter("map_frame_id").as_string(); + vector_map_topic_ = node.get_parameter("vector_map_topic").as_string(); + default_nav_cmd_ = node.get_parameter("default_nav_cmd").as_string(); + route_lanelet_ids_param_ = node.get_parameter("route_lanelet_ids").as_integer_array(); + nav_cmd_fallback_order_param_ = node.get_parameter("nav_cmd_fallback_order").as_string_array(); + global_path_resample_interval_m_ = + node.get_parameter("global_path_resample_interval_m").as_double(); + max_centerline_connection_gap_m_ = + node.get_parameter("max_centerline_connection_gap_m").as_double(); + off_route_distance_threshold_m_ = + node.get_parameter("off_route_distance_threshold_m").as_double(); + route_lookahead_lanelet_count_ = + node.get_parameter("route_lookahead_lanelet_count").as_int(); + + if (route_lanelet_ids_param_.empty()) { + throw std::invalid_argument("route_lanelet_ids must not be empty"); + } + for (const auto id : route_lanelet_ids_param_) { + if (id <= 0) { + throw std::invalid_argument("route_lanelet_ids must contain positive lanelet ids"); + } + } + if (nav_cmd_fallback_order_param_.empty()) { + throw std::invalid_argument("nav_cmd_fallback_order must not be empty"); + } + if (global_path_resample_interval_m_ <= 0.0) { + throw std::invalid_argument("global_path_resample_interval_m must be greater than 0"); + } + if (off_route_distance_threshold_m_ <= 0.0) { + throw std::invalid_argument("off_route_distance_threshold_m must be greater than 0"); + } + if (route_lookahead_lanelet_count_ < 3) { + throw std::invalid_argument("route_lookahead_lanelet_count must be at least 3"); + } + + last_nav_cmd_turn_ = parse_nav_cmd(default_nav_cmd_); + nav_cmd_fallback_order_.reserve(nav_cmd_fallback_order_param_.size()); + for (const auto & cmd : nav_cmd_fallback_order_param_) { + nav_cmd_fallback_order_.push_back(parse_nav_cmd(cmd)); + } + + vector_map_sub_ = node.create_subscription( + vector_map_topic_, + rclcpp::QoS(10), + [this](const vectormap_msgs::msg::VectorMap::SharedPtr msg) { + vector_map_callback(msg); + }); + + RCLCPP_INFO(logger_, "VectormapLanePlannerPlugin initialized"); +} + +void VectormapLanePlannerPlugin::set_pose( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +{ + std::lock_guard lock(data_mutex_); + latest_pose_ = std::make_shared(pose); +} + +void VectormapLanePlannerPlugin::set_nav_command(const std::string & command) +{ + uint8_t turn = 0; + try { + turn = parse_nav_cmd(command); + } catch (const std::invalid_argument & e) { + RCLCPP_ERROR(logger_, "%s", e.what()); + return; + } + std::lock_guard lock(data_mutex_); + last_nav_cmd_turn_ = turn; + if (global_path_ready_ && latest_pose_) { + const Point2D ego{ + latest_pose_->pose.pose.position.x, + latest_pose_->pose.pose.position.y}; + rebuild_route_from_pose(ego, "nav_cmd"); + } +} + +std::optional VectormapLanePlannerPlugin::compute_path( + const rclcpp::Time & stamp) +{ + std::lock_guard lock(data_mutex_); + if (!global_path_ready_ || !latest_pose_) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 2000, + "waiting for vector map and localization pose"); + return std::nullopt; + } + + const Point2D ego{ + latest_pose_->pose.pose.position.x, + latest_pose_->pose.pose.position.y}; + + if (pending_route_rebuild_) { + rebuild_route_from_pose(ego, pending_route_rebuild_reason_); + pending_route_rebuild_ = false; + pending_route_rebuild_reason_.clear(); + } else { + const auto [current_lanelet_id, distance] = find_nearest_lanelet_within_route(ego); + if (current_lanelet_id != 0U) { + if (distance > off_route_distance_threshold_m_) { + rebuild_route_from_pose(ego, "out_of_route"); + } else { + const auto route_it = std::find( + current_route_lanelet_ids_.begin(), + current_route_lanelet_ids_.end(), + current_lanelet_id); + const std::size_t remaining = static_cast( + std::distance(route_it, current_route_lanelet_ids_.end())); + if (remaining == 1U) { + rebuild_route_from_lanelet(current_lanelet_id, "lookahead_extension"); + } + } + } + } + + return make_path_message(stamp); +} + +// ---------- Internal methods ---------- + +void VectormapLanePlannerPlugin::vector_map_callback( + const vectormap_msgs::msg::VectorMap::SharedPtr msg) +{ + if (!msg) { + throw std::runtime_error("VectorMap message must not be null"); + } + std::lock_guard lock(data_mutex_); + if (global_path_ready_) { + return; + } + build_global_path_once(*msg); +} + +void VectormapLanePlannerPlugin::build_global_path_once( + const vectormap_msgs::msg::VectorMap & map_msg) +{ + if (map_msg.header.frame_id != map_frame_id_) { + throw std::runtime_error( + "VectorMap frame_id must be " + map_frame_id_ + + ", got " + map_msg.header.frame_id); + } + + lanelet_by_id_.clear(); + lanelet_by_id_.reserve(map_msg.lanelets.size()); + for (const auto & lanelet : map_msg.lanelets) { + lanelet_by_id_.emplace(lanelet.id, lanelet); + } + + std::unordered_map line_string_by_id; + line_string_by_id.reserve(map_msg.line_strings.size()); + for (const auto & ls : map_msg.line_strings) { + line_string_by_id.emplace(ls.id, ls); + } + + connection_edges_by_from_lanelet_id_.clear(); + connection_edges_by_from_lanelet_id_.reserve(map_msg.lane_connections.size()); + for (const auto & conn : map_msg.lane_connections) { + connection_edges_by_from_lanelet_id_[conn.from_lanelet_id].push_back( + RouteEdge{conn.to_lanelet_id, conn.turn_direction, conn.cost}); + } + + lanelet_centerline_points_by_id_.clear(); + lanelet_centerline_points_by_id_.reserve(lanelet_by_id_.size()); + for (const auto & [lanelet_id, lanelet] : lanelet_by_id_) { + const auto ls_it = line_string_by_id.find(lanelet.centerline_id); + if (ls_it == line_string_by_id.end() || ls_it->second.points.size() < 2U) { + continue; + } + if (ls_it->second.line_type != vectormap_msgs::msg::LineString::TYPE_VIRTUAL_LINE || + ls_it->second.marking_type != vectormap_msgs::msg::LineString::MARKING_VIRTUAL) + { + throw std::runtime_error("route centerline must be virtual line"); + } + std::vector pts; + pts.reserve(ls_it->second.points.size()); + for (const auto & p : ls_it->second.points) { + pts.push_back({p.x, p.y}); + } + lanelet_centerline_points_by_id_.emplace(lanelet_id, std::move(pts)); + } + + std::size_t fallback_count = 0U; + const auto route_ids = build_route_sequence_from_graph( + static_cast(route_lanelet_ids_param_.front()), fallback_count); + if (fallback_count > 0U) { + RCLCPP_WARN( + logger_, + "initial route used nav_cmd fallback %zu times: requested=%s", + fallback_count, + turn_direction_to_string(last_nav_cmd_turn_).c_str()); + } + build_route_from_lanelet_ids(route_ids); + path_frame_id_ = map_msg.header.frame_id; + global_path_ready_ = true; + RCLCPP_INFO( + logger_, + "built vector map global path: %zu points, %zu lanelets, loop=%s", + global_samples_.size(), + route_ids.size(), + route_is_loop_ ? "true" : "false"); +} + +void VectormapLanePlannerPlugin::build_route_from_lanelet_ids( + const std::vector & route_lanelet_ids) +{ + if (route_lanelet_ids.empty()) { + throw std::runtime_error("route lanelet sequence must not be empty"); + } + + std::vector route_points; + std::vector raw_s; + lanelet_ranges_.clear(); + route_is_loop_ = false; + route_points.reserve(route_lanelet_ids.size() * 16U); + + double accumulated_s = 0.0; + for (std::size_t i = 0U; i < route_lanelet_ids.size(); ++i) { + const uint64_t lanelet_id = route_lanelet_ids[i]; + if (lanelet_by_id_.find(lanelet_id) == lanelet_by_id_.end()) { + throw std::runtime_error( + "route lanelet id not found: " + std::to_string(lanelet_id)); + } + + if (i + 1U < route_lanelet_ids.size()) { + const uint64_t next_id = route_lanelet_ids[i + 1U]; + const auto conn_it = connection_edges_by_from_lanelet_id_.find(lanelet_id); + const bool connected = conn_it != connection_edges_by_from_lanelet_id_.end() && + std::any_of( + conn_it->second.begin(), conn_it->second.end(), + [next_id](const RouteEdge & e) { return e.to_lanelet_id == next_id; }); + if (!connected) { + throw std::runtime_error( + "missing LaneConnection from " + std::to_string(lanelet_id) + + " to " + std::to_string(next_id)); + } + } + + const auto cl_it = lanelet_centerline_points_by_id_.find(lanelet_id); + if (cl_it == lanelet_centerline_points_by_id_.end()) { + throw std::runtime_error("route centerline points are not available"); + } + const auto & pts = cl_it->second; + if (!route_points.empty()) { + const double gap = distance_2d(route_points.back(), pts.front()); + if (gap > max_centerline_connection_gap_m_) { + throw std::runtime_error( + "centerline connection gap too large: " + std::to_string(gap)); + } + } + + const double start_s = accumulated_s; + for (const auto & p : pts) { + if (!route_points.empty()) { + const double ds = distance_2d(route_points.back(), p); + if (ds <= EPSILON) { + continue; + } + accumulated_s += ds; + } + route_points.push_back(p); + raw_s.push_back(accumulated_s); + } + lanelet_ranges_.push_back({lanelet_id, start_s, accumulated_s}); + } + + if (route_points.size() < 2U) { + throw std::runtime_error("route must contain at least two centerline points"); + } + + const uint64_t first_id = route_lanelet_ids.front(); + const uint64_t last_id = route_lanelet_ids.back(); + const auto last_conn_it = connection_edges_by_from_lanelet_id_.find(last_id); + route_is_loop_ = last_conn_it != connection_edges_by_from_lanelet_id_.end() && + std::any_of( + last_conn_it->second.begin(), last_conn_it->second.end(), + [first_id](const RouteEdge & e) { return e.to_lanelet_id == first_id; }); + + if (route_is_loop_) { + const double closing_gap = distance_2d(route_points.back(), route_points.front()); + if (closing_gap > max_centerline_connection_gap_m_) { + throw std::runtime_error( + "loop closing gap too large: " + std::to_string(closing_gap)); + } + if (closing_gap > EPSILON) { + accumulated_s += closing_gap; + route_points.push_back(route_points.front()); + raw_s.push_back(accumulated_s); + lanelet_ranges_.back().end_s = accumulated_s; + } + } + + global_samples_.clear(); + global_samples_.reserve( + static_cast( + std::ceil(raw_s.back() / global_path_resample_interval_m_)) + 2U); + for (double s = 0.0; s < raw_s.back(); s += global_path_resample_interval_m_) { + global_samples_.push_back( + interpolate_raw_path(route_points, raw_s, s, lanelet_at_s(s))); + } + global_samples_.push_back( + interpolate_raw_path(route_points, raw_s, raw_s.back(), lanelet_at_s(raw_s.back()))); + current_route_lanelet_ids_ = route_lanelet_ids; +} + +void VectormapLanePlannerPlugin::rebuild_route_from_lanelet( + const uint64_t start_lanelet_id, const std::string & reason) +{ + std::size_t fallback_count = 0U; + const auto route_ids = build_route_sequence_from_graph(start_lanelet_id, fallback_count); + build_route_from_lanelet_ids(route_ids); + RCLCPP_INFO( + logger_, + "rebuilt global path: reason=%s start=%lu lanelets=%zu points=%zu", + reason.c_str(), start_lanelet_id, route_ids.size(), global_samples_.size()); + if (fallback_count > 0U) { + RCLCPP_WARN( + logger_, "route rebuild used fallback %zu times", fallback_count); + } +} + +bool VectormapLanePlannerPlugin::rebuild_route_from_pose( + const Point2D & ego, const std::string & reason) +{ + const uint64_t start_id = find_nearest_lanelet_from_pose(ego); + if (start_id == 0U) { + throw std::runtime_error("failed to find nearest lanelet for route rebuild"); + } + rebuild_route_from_lanelet(start_id, reason); + return true; +} + +std::vector VectormapLanePlannerPlugin::build_route_sequence_from_graph( + const uint64_t start_lanelet_id, std::size_t & fallback_count) const +{ + if (lanelet_centerline_points_by_id_.find(start_lanelet_id) == + lanelet_centerline_points_by_id_.end()) + { + throw std::runtime_error( + "route start lanelet centerline not available: " + + std::to_string(start_lanelet_id)); + } + + fallback_count = 0U; + std::vector ids; + ids.reserve(static_cast(route_lookahead_lanelet_count_)); + ids.push_back(start_lanelet_id); + + while (ids.size() < static_cast(route_lookahead_lanelet_count_)) { + uint8_t selected_turn = 0; + bool used_fallback = false; + const uint64_t next_id = select_next_lanelet( + ids.back(), last_nav_cmd_turn_, selected_turn, used_fallback); + if (next_id == 0U || next_id == start_lanelet_id) { + break; + } + if (std::find(ids.begin(), ids.end(), next_id) != ids.end()) { + RCLCPP_WARN( + logger_, + "route cycle detected at lanelet %lu; stopping lookahead", next_id); + break; + } + if (lanelet_centerline_points_by_id_.find(next_id) == + lanelet_centerline_points_by_id_.end()) + { + throw std::runtime_error( + "selected lanelet centerline not available: " + std::to_string(next_id)); + } + ids.push_back(next_id); + if (used_fallback) { + ++fallback_count; + } + } + return ids; +} + +uint64_t VectormapLanePlannerPlugin::select_next_lanelet( + const uint64_t from_lanelet_id, + const uint8_t requested_turn, + uint8_t & selected_turn, + bool & used_fallback) const +{ + const auto edges_it = connection_edges_by_from_lanelet_id_.find(from_lanelet_id); + if (edges_it == connection_edges_by_from_lanelet_id_.end() || + edges_it->second.empty()) + { + return 0U; + } + + const auto find_min_cost = [&edges = edges_it->second](uint8_t turn) { + auto best = edges.end(); + for (auto it = edges.begin(); it != edges.end(); ++it) { + if (it->turn_direction != turn) { + continue; + } + if (best == edges.end() || it->cost < best->cost) { + best = it; + } + } + return best; + }; + + const auto req_it = find_min_cost(requested_turn); + if (req_it != edges_it->second.end() && + req_it->turn_direction == requested_turn) + { + selected_turn = requested_turn; + used_fallback = false; + return req_it->to_lanelet_id; + } + + for (const auto fallback_turn : nav_cmd_fallback_order_) { + if (fallback_turn == requested_turn) { + continue; + } + const auto fb_it = find_min_cost(fallback_turn); + if (fb_it != edges_it->second.end() && + fb_it->turn_direction == fallback_turn) + { + selected_turn = fallback_turn; + used_fallback = true; + return fb_it->to_lanelet_id; + } + } + return 0U; +} + +uint64_t VectormapLanePlannerPlugin::find_nearest_lanelet_from_pose( + const Point2D & point) const +{ + uint64_t best_id = 0U; + double best_dist_sq = std::numeric_limits::max(); + for (const auto & [lanelet_id, pts] : lanelet_centerline_points_by_id_) { + if (pts.size() < 2U) { + continue; + } + for (std::size_t i = 1U; i < pts.size(); ++i) { + const double d = point_segment_distance_sq(point, pts[i - 1U], pts[i]); + if (d < best_dist_sq) { + best_dist_sq = d; + best_id = lanelet_id; + } + } + } + return best_id; +} + +std::pair VectormapLanePlannerPlugin::find_nearest_lanelet_within_route( + const Point2D & point) const +{ + uint64_t best_id = 0U; + double best_dist_sq = std::numeric_limits::max(); + for (const uint64_t lanelet_id : current_route_lanelet_ids_) { + const auto cl_it = lanelet_centerline_points_by_id_.find(lanelet_id); + if (cl_it == lanelet_centerline_points_by_id_.end() || + cl_it->second.size() < 2U) + { + continue; + } + const auto & pts = cl_it->second; + for (std::size_t i = 1U; i < pts.size(); ++i) { + const double d = point_segment_distance_sq(point, pts[i - 1U], pts[i]); + if (d < best_dist_sq) { + best_dist_sq = d; + best_id = lanelet_id; + } + } + } + const double dist = best_id != 0U + ? std::sqrt(best_dist_sq) + : std::numeric_limits::max(); + return {best_id, dist}; +} + +uint64_t VectormapLanePlannerPlugin::lanelet_at_s(const double s) const +{ + if (lanelet_ranges_.empty()) { + return 0U; + } + const double ns = normalize_path_s(s); + const auto it = std::find_if( + lanelet_ranges_.begin(), lanelet_ranges_.end(), + [ns](const LaneletRange & r) { + return ns >= r.start_s && ns <= r.end_s; + }); + return it != lanelet_ranges_.end() + ? it->lanelet_id + : lanelet_ranges_.back().lanelet_id; +} + +double VectormapLanePlannerPlugin::normalize_path_s(const double s) const +{ + double path_length = 0.0; + if (!global_samples_.empty()) { + path_length = global_samples_.back().s; + } else if (!lanelet_ranges_.empty()) { + path_length = lanelet_ranges_.back().end_s; + } + if (path_length <= EPSILON) { + return s; + } + if (!route_is_loop_) { + return std::clamp(s, 0.0, path_length); + } + double n = std::fmod(s, path_length); + if (n < 0.0) { + n += path_length; + } + return n; +} + +uint8_t VectormapLanePlannerPlugin::parse_nav_cmd(const std::string & command) const +{ + if (command == "straight") { + return vectormap_msgs::msg::LaneConnection::TURN_STRAIGHT; + } + if (command == "left") { + return vectormap_msgs::msg::LaneConnection::TURN_LEFT; + } + if (command == "right") { + return vectormap_msgs::msg::LaneConnection::TURN_RIGHT; + } + throw std::invalid_argument("nav_cmd must be straight, left, or right: " + command); +} + +std::string VectormapLanePlannerPlugin::turn_direction_to_string( + const uint8_t turn_direction) const +{ + if (turn_direction == vectormap_msgs::msg::LaneConnection::TURN_STRAIGHT) return "straight"; + if (turn_direction == vectormap_msgs::msg::LaneConnection::TURN_LEFT) return "left"; + if (turn_direction == vectormap_msgs::msg::LaneConnection::TURN_RIGHT) return "right"; + return "unknown"; +} + +nav_msgs::msg::Path VectormapLanePlannerPlugin::make_path_message( + const rclcpp::Time & stamp) const +{ + nav_msgs::msg::Path path; + path.header.stamp = stamp; + path.header.frame_id = path_frame_id_.empty() ? map_frame_id_ : path_frame_id_; + path.poses.reserve(global_samples_.size()); + for (const auto & pt : global_samples_) { + geometry_msgs::msg::PoseStamped pose; + pose.header = path.header; + pose.pose.position.x = pt.x; + pose.pose.position.y = pt.y; + pose.pose.position.z = 0.0; + pose.pose.orientation = yaw_to_quaternion(pt.yaw); + path.poses.push_back(pose); + } + return path; +} + +geometry_msgs::msg::Quaternion VectormapLanePlannerPlugin::yaw_to_quaternion(const double yaw) +{ + geometry_msgs::msg::Quaternion q; + q.x = 0.0; + q.y = 0.0; + q.z = std::sin(yaw * 0.5); + q.w = std::cos(yaw * 0.5); + return q; +} + +} // namespace lane_planner diff --git a/planner/lane_planner/src/route_builder.cpp b/planner/lane_planner/src/route_builder.cpp.bak similarity index 100% rename from planner/lane_planner/src/route_builder.cpp rename to planner/lane_planner/src/route_builder.cpp.bak diff --git a/planner/local_planner/include/local_planner/local_planner_server.hpp b/planner/local_planner/include/local_planner/local_planner_server.hpp index afad8d1f..b10188e1 100644 --- a/planner/local_planner/include/local_planner/local_planner_server.hpp +++ b/planner/local_planner/include/local_planner/local_planner_server.hpp @@ -43,6 +43,7 @@ class LocalPlannerServer : public rclcpp::Node LocalPlannerPlugin::SharedPtr plugin_; const int update_period_ms_; + const int global_path_timeout_ms_; const std::string global_path_topic_; const std::string local_path_topic_; const std::string vector_map_topic_; @@ -55,6 +56,7 @@ class LocalPlannerServer : public rclcpp::Node geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr latest_pose_; geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr latest_velocity_; object_detection_msgs::msg::ObjectInfoArray::SharedPtr latest_objects_; + rclcpp::Time last_global_path_stamp_{0, 0, RCL_ROS_TIME}; mutable std::mutex data_mutex_; rclcpp::Subscription::SharedPtr global_path_subscription_; diff --git a/planner/local_planner/src/local_planner_server.cpp b/planner/local_planner/src/local_planner_server.cpp index c2bf348f..fb9160c3 100644 --- a/planner/local_planner/src/local_planner_server.cpp +++ b/planner/local_planner/src/local_planner_server.cpp @@ -16,6 +16,7 @@ LocalPlannerServer::LocalPlannerServer( : rclcpp::Node("local_planner_server_node", name_space, options), plugin_loader_("local_planner", "local_planner::LocalPlannerPlugin"), update_period_ms_(get_parameter("update_period_ms").as_int()), + global_path_timeout_ms_(get_parameter("global_path_timeout_ms").as_int()), global_path_topic_(get_parameter("global_path_topic").as_string()), local_path_topic_(get_parameter("local_path_topic").as_string()), vector_map_topic_(get_parameter("vector_map_topic").as_string()), @@ -28,6 +29,9 @@ LocalPlannerServer::LocalPlannerServer( if (update_period_ms_ <= 0) { throw std::invalid_argument("update_period_ms must be greater than 0"); } + if (global_path_timeout_ms_ <= 0) { + throw std::invalid_argument("global_path_timeout_ms must be greater than 0"); + } if (global_path_topic_.empty() || local_path_topic_.empty() || vector_map_topic_.empty() || @@ -88,6 +92,7 @@ void LocalPlannerServer::global_path_callback(const nav_msgs::msg::Path::SharedP } std::lock_guard lock(data_mutex_); plugin_->setGlobalPath(*msg); + last_global_path_stamp_ = now(); } void LocalPlannerServer::vector_map_callback( @@ -141,6 +146,21 @@ void LocalPlannerServer::timer_callback() return; } + // Freshness gate: suppress local path when global path is stale + if (last_global_path_stamp_.nanoseconds() == 0) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 2000, "waiting for global path"); + return; + } + const auto age_ms = static_cast( + (now() - last_global_path_stamp_).nanoseconds() / 1'000'000LL); + if (age_ms > global_path_timeout_ms_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, + "global path is stale (%d ms); local path suppressed", age_ms); + return; + } + geometry_msgs::msg::TwistWithCovarianceStamped velocity; if (latest_velocity_) { velocity = *latest_velocity_; From e4647c680da324bf09aa27a0673f3ece165a0050 Mon Sep 17 00:00:00 2001 From: kyo0221 Date: Tue, 5 May 2026 20:56:18 +0900 Subject: [PATCH 05/13] =?UTF-8?q?=E2=9C=A8feat:=20mapless=E7=94=A8=20?= =?UTF-8?q?=E7=8B=AC=E8=87=AAmsg?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mapless_planning_msgs/CMakeLists.txt | 16 ++++++++++++ mapless_planning_msgs/msg/DrivingCorridor.msg | 3 +++ mapless_planning_msgs/msg/Linestring.msg | 1 + mapless_planning_msgs/msg/LocalMap.msg | 1 + mapless_planning_msgs/msg/Mission.msg | 8 ++++++ .../msg/MissionLanesStamped.msg | 13 ++++++++++ mapless_planning_msgs/msg/RoadSegments.msg | 3 +++ mapless_planning_msgs/msg/Segment.msg | 4 +++ .../msg/SegmentConnection.msg | 9 +++++++ mapless_planning_msgs/package.xml | 25 +++++++++++++++++++ 10 files changed, 83 insertions(+) create mode 100644 mapless_planning_msgs/CMakeLists.txt create mode 100644 mapless_planning_msgs/msg/DrivingCorridor.msg create mode 100644 mapless_planning_msgs/msg/Linestring.msg create mode 100644 mapless_planning_msgs/msg/LocalMap.msg create mode 100644 mapless_planning_msgs/msg/Mission.msg create mode 100644 mapless_planning_msgs/msg/MissionLanesStamped.msg create mode 100644 mapless_planning_msgs/msg/RoadSegments.msg create mode 100644 mapless_planning_msgs/msg/Segment.msg create mode 100644 mapless_planning_msgs/msg/SegmentConnection.msg create mode 100644 mapless_planning_msgs/package.xml diff --git a/mapless_planning_msgs/CMakeLists.txt b/mapless_planning_msgs/CMakeLists.txt new file mode 100644 index 00000000..9455e709 --- /dev/null +++ b/mapless_planning_msgs/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(mapless_planning_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) + +file(GLOB msg_sources RELATIVE ${PROJECT_SOURCE_DIR} msg/*.msg) +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_sources} + DEPENDENCIES geometry_msgs std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/mapless_planning_msgs/msg/DrivingCorridor.msg b/mapless_planning_msgs/msg/DrivingCorridor.msg new file mode 100644 index 00000000..f171949b --- /dev/null +++ b/mapless_planning_msgs/msg/DrivingCorridor.msg @@ -0,0 +1,3 @@ +geometry_msgs/Point[] centerline +geometry_msgs/Point[] bound_left +geometry_msgs/Point[] bound_right diff --git a/mapless_planning_msgs/msg/Linestring.msg b/mapless_planning_msgs/msg/Linestring.msg new file mode 100644 index 00000000..9c58030a --- /dev/null +++ b/mapless_planning_msgs/msg/Linestring.msg @@ -0,0 +1 @@ +geometry_msgs/Pose[] poses diff --git a/mapless_planning_msgs/msg/LocalMap.msg b/mapless_planning_msgs/msg/LocalMap.msg new file mode 100644 index 00000000..e2d872ec --- /dev/null +++ b/mapless_planning_msgs/msg/LocalMap.msg @@ -0,0 +1 @@ +RoadSegments road_segments diff --git a/mapless_planning_msgs/msg/Mission.msg b/mapless_planning_msgs/msg/Mission.msg new file mode 100644 index 00000000..c27ed704 --- /dev/null +++ b/mapless_planning_msgs/msg/Mission.msg @@ -0,0 +1,8 @@ +uint8 mission_type +uint8 LANE_KEEP=0 +uint8 LANE_CHANGE_LEFT=1 +uint8 LANE_CHANGE_RIGHT=2 +uint8 TAKE_NEXT_EXIT_LEFT=3 +uint8 TAKE_NEXT_EXIT_RIGHT=4 + +float32 deadline diff --git a/mapless_planning_msgs/msg/MissionLanesStamped.msg b/mapless_planning_msgs/msg/MissionLanesStamped.msg new file mode 100644 index 00000000..03efde24 --- /dev/null +++ b/mapless_planning_msgs/msg/MissionLanesStamped.msg @@ -0,0 +1,13 @@ +std_msgs/Header header +DrivingCorridor lane_with_goal_point +DrivingCorridor ego_lane +DrivingCorridor[] drivable_lanes_left +DrivingCorridor[] drivable_lanes_right +float32 deadline_target_lane +int16 target_lane + +int16 LANE_KEEP=0 +int16 LANE_CHANGE_LEFT=-1 +int16 LANE_CHANGE_RIGHT=1 +int16 TAKE_NEXT_EXIT_LEFT=-2 +int16 TAKE_NEXT_EXIT_RIGHT=2 diff --git a/mapless_planning_msgs/msg/RoadSegments.msg b/mapless_planning_msgs/msg/RoadSegments.msg new file mode 100644 index 00000000..e9b2f07d --- /dev/null +++ b/mapless_planning_msgs/msg/RoadSegments.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +Segment[] segments +geometry_msgs/Pose pose diff --git a/mapless_planning_msgs/msg/Segment.msg b/mapless_planning_msgs/msg/Segment.msg new file mode 100644 index 00000000..be184a03 --- /dev/null +++ b/mapless_planning_msgs/msg/Segment.msg @@ -0,0 +1,4 @@ +uint16 id +Linestring[2] linestrings +int32[] successor_segment_id +int32[] neighboring_segment_id diff --git a/mapless_planning_msgs/msg/SegmentConnection.msg b/mapless_planning_msgs/msg/SegmentConnection.msg new file mode 100644 index 00000000..2ad68c68 --- /dev/null +++ b/mapless_planning_msgs/msg/SegmentConnection.msg @@ -0,0 +1,9 @@ +uint16 to_segment_id +uint8 turn_direction +float32 cost +float32 confidence + +uint8 TURN_UNKNOWN=0 +uint8 TURN_STRAIGHT=1 +uint8 TURN_LEFT=2 +uint8 TURN_RIGHT=3 diff --git a/mapless_planning_msgs/package.xml b/mapless_planning_msgs/package.xml new file mode 100644 index 00000000..a4d38b69 --- /dev/null +++ b/mapless_planning_msgs/package.xml @@ -0,0 +1,25 @@ + + + + mapless_planning_msgs + 0.0.0 + Mapless planning message definitions. + Kyo Yamashita + MIT + + ament_cmake + + rosidl_default_generators + geometry_msgs + std_msgs + + rosidl_default_runtime + geometry_msgs + std_msgs + + rosidl_interface_packages + + + ament_cmake + + From 9cbc8e6ae0884563f4789e42cb3f4e49d11ad977 Mon Sep 17 00:00:00 2001 From: kyo0221 Date: Tue, 5 May 2026 20:56:43 +0900 Subject: [PATCH 06/13] =?UTF-8?q?=E2=9C=A8feat:=20mapless?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- main_executor/config/main_params.yaml | 26 ++++++++++++++++++++++++-- main_executor/package.xml | 2 ++ main_executor/src/main.cpp | 18 ++++++++++++++---- 3 files changed, 40 insertions(+), 6 deletions(-) diff --git a/main_executor/config/main_params.yaml b/main_executor/config/main_params.yaml index 3b4a891e..6e3bbbad 100644 --- a/main_executor/config/main_params.yaml +++ b/main_executor/config/main_params.yaml @@ -6,6 +6,7 @@ launch: #起動パラメータ socketcan : false zed : false sim : false + mapless : false # true: mapless planner, false: vectormap planner /**: #ワイルドカード(ここのパラメータは全ノードから読める: <名前に注意>) ros__parameters: @@ -162,28 +163,49 @@ map_odom_tf_node: base_frame_id : "base_link" localized_pose_topic : "/localization/pose" +road_segments_provider_node: + ros__parameters: + camera_info_topic : "/zed/zed_node/rgb/camera_info" + lane_mask_topic : "/perception/lane_mask" + da_mask_topic : "/perception/drivable_area_mask" + road_segments_topic : "/perception/road_segments" + camera_frame_id : "camera_link" + base_frame_id : "base_link" + min_ground_x_m : 0.5 # [m] 最低前方距離 + max_ground_x_m : 20.0 # [m] 最大前方距離 + publish_markers : true + lane_planner_node: ros__parameters: update_period_ms : 100 map_frame_id : "map" base_frame_id : "base_link" - vector_map_topic : "/vector_map" localization_pose_topic : "/localization/pose" nav_cmd_topic : "/planning/nav_cmd" + global_path_topic : "/planner/global_path" + # lane_planner_mode: "vectormap" or "mapless" + lane_planner_mode : "vectormap" + # --- vectormap mode --- + vector_map_topic : "/vector_map" default_nav_cmd : "straight" nav_cmd_fallback_order : ["straight", "left", "right"] - global_path_topic : "/planner/global_path" route_lanelet_ids : [301, 302, 303, 304, 305, 311, 313, 312, 314, 315, 309, 310] # aiformula 2026 course: 301, 302, 303, 304, 305, 311, 313, 312, 314, 315, 309, 310 global_path_resample_interval_m : 0.2 max_centerline_connection_gap_m : 0.5 off_route_distance_threshold_m : 3.0 route_lookahead_lanelet_count : 3 + # --- mapless mode --- + road_segments_topic : "/perception/road_segments" + mission_lanes_topic : "/planner/mission_lanes" + max_corridor_length_m : 20.0 + path_resample_interval_m : 0.2 local_planner_server_node: ros__parameters: local_planner_plugin : "local_planner::VectormapFrenetPlugin" update_period_ms : 100 + global_path_timeout_ms : 1000 # [ms] global path が stale になるまでの時間 map_frame_id : "map" base_frame_id : "base_link" global_path_topic : "/planner/global_path" diff --git a/main_executor/package.xml b/main_executor/package.xml index f0a4e16c..a7707d25 100644 --- a/main_executor/package.xml +++ b/main_executor/package.xml @@ -20,6 +20,8 @@ localization motion_control lane_planner + mapless_planning_msgs + road_segments_provider local_planner object_detector zed_wrapper diff --git a/main_executor/src/main.cpp b/main_executor/src/main.cpp index 2467bc1d..5ffe7ba0 100644 --- a/main_executor/src/main.cpp +++ b/main_executor/src/main.cpp @@ -7,8 +7,9 @@ #include "localization/odom_tf_node.hpp" #include "localization/localization_node.hpp" #include "motion_control/controller_server.hpp" -#include "lane_planner/lane_planner_node.hpp" +#include "lane_planner/lane_planner_server.hpp" #include "local_planner/local_planner_server.hpp" +#include "road_segments_provider/road_segments_provider_node.hpp" #include "object_detector/object_detector_node.hpp" #include "vectormap_server/vectormap_server_node.hpp" @@ -21,8 +22,10 @@ int main(int argc, char * argv[]){ nodes_option.automatically_declare_parameters_from_overrides(true); // Read launch flags from the 'launch' node's parameters - const bool use_zed = rclcpp::Node("launch", nodes_option).get_parameter("zed").as_bool(); - const bool use_sim = rclcpp::Node("launch", nodes_option).get_parameter("sim").as_bool(); + auto launch_node = rclcpp::Node("launch", nodes_option); + const bool use_zed = launch_node.get_parameter("zed").as_bool(); + const bool use_sim = launch_node.get_parameter("sim").as_bool(); + const bool use_mapless = launch_node.get_parameter("mapless").as_bool(); if (use_sim) { nodes_option.parameter_overrides({rclcpp::Parameter("use_sim_time", true)}); @@ -34,7 +37,7 @@ int main(int argc, char * argv[]){ auto localization_node = std::make_shared(nodes_option); auto odom_tf_node = std::make_shared(nodes_option); auto map_odom_tf_node = std::make_shared(nodes_option); - auto lane_planner_node = std::make_shared(nodes_option); + auto lane_planner_node = std::make_shared(nodes_option); auto local_planner_server_node = std::make_shared(nodes_option); auto controller_server_node = std::make_shared(nodes_option); auto object_detector_node = std::make_shared(nodes_option); @@ -44,6 +47,13 @@ int main(int argc, char * argv[]){ zed_wrapper_node = std::make_shared(nodes_option); exec.add_node(zed_wrapper_node); } + + std::shared_ptr road_segments_provider_node; + if (use_mapless) { + road_segments_provider_node = + std::make_shared(nodes_option); + exec.add_node(road_segments_provider_node); + } exec.add_node(controller_node); exec.add_node(chassis_driver_node); exec.add_node(vectormap_server_node); From 61eda58065f4149a470f81f30e5a7ba862b90c69 Mon Sep 17 00:00:00 2001 From: kyo0221 Date: Wed, 6 May 2026 08:40:03 +0900 Subject: [PATCH 07/13] =?UTF-8?q?=E2=9C=A8feat:=20=E3=82=AB=E3=83=A1?= =?UTF-8?q?=E3=83=A9=E5=BA=A7=E6=A8=99=E7=B3=BBlink=E5=AE=9A=E7=BE=A9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- simulator/models/ai_car1/model.urdf | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/simulator/models/ai_car1/model.urdf b/simulator/models/ai_car1/model.urdf index b650083e..18f2cb0c 100644 --- a/simulator/models/ai_car1/model.urdf +++ b/simulator/models/ai_car1/model.urdf @@ -169,6 +169,15 @@ + + + + + + + + + From c75dc2def033b31cd0a77f1ce28104909daa6743 Mon Sep 17 00:00:00 2001 From: kyo0221 Date: Wed, 6 May 2026 08:40:26 +0900 Subject: [PATCH 08/13] =?UTF-8?q?=E2=9C=A8feat:=20add=20camera=20info=20re?= =?UTF-8?q?mapping?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- simulator/launch/gazebo_ignition.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/simulator/launch/gazebo_ignition.launch.py b/simulator/launch/gazebo_ignition.launch.py index a5077be9..f6aaaf3b 100644 --- a/simulator/launch/gazebo_ignition.launch.py +++ b/simulator/launch/gazebo_ignition.launch.py @@ -43,6 +43,7 @@ def generate_launch_description(): output='screen', remappings=[ ('/image_raw', '/zed/zed_node/rgb/image_rect_color'), + ('/camera_info', '/zed/zed_node/rgb/camera_info'), ('/depth_image', '/zed/zed_node/depth/depth_registered'), ('/navsat', '/vectornav/gnss'), ('/depth_image_raw/points', '/zed/zed_node/point_cloud'), From 2574dbb9e116c467134ac3b95abb7871ce8ab5db Mon Sep 17 00:00:00 2001 From: kyo0221 Date: Wed, 6 May 2026 08:41:03 +0900 Subject: [PATCH 09/13] =?UTF-8?q?=F0=9F=90=9Bfix:=20get=5Fparam=20and=20qo?= =?UTF-8?q?s?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/road_segments_provider_node.cpp | 26 +++++++------------ 1 file changed, 10 insertions(+), 16 deletions(-) diff --git a/perception/road_segments_provider/src/road_segments_provider_node.cpp b/perception/road_segments_provider/src/road_segments_provider_node.cpp index a7cd9aff..d5e49caf 100644 --- a/perception/road_segments_provider/src/road_segments_provider_node.cpp +++ b/perception/road_segments_provider/src/road_segments_provider_node.cpp @@ -20,21 +20,15 @@ RoadSegmentsProviderNode::RoadSegmentsProviderNode( const std::string & name_space, const rclcpp::NodeOptions & options) : Node("road_segments_provider_node", name_space, options), - camera_info_topic_(declare_parameter( - "camera_info_topic", "/zed/zed_node/rgb/camera_info")), - lane_mask_topic_( - declare_parameter("lane_mask_topic", "/perception/lane_mask")), - da_mask_topic_( - declare_parameter("da_mask_topic", "/perception/drivable_area_mask")), - road_segments_topic_( - declare_parameter("road_segments_topic", "/perception/road_segments")), - camera_frame_id_( - declare_parameter("camera_frame_id", "camera_link")), - base_frame_id_( - declare_parameter("base_frame_id", "base_link")), - min_ground_x_m_(declare_parameter("min_ground_x_m", 0.5)), - max_ground_x_m_(declare_parameter("max_ground_x_m", 20.0)), - publish_markers_(declare_parameter("publish_markers", false)), + camera_info_topic_(get_parameter("camera_info_topic").as_string()), + lane_mask_topic_(get_parameter("lane_mask_topic").as_string()), + da_mask_topic_(get_parameter("da_mask_topic").as_string()), + road_segments_topic_(get_parameter("road_segments_topic").as_string()), + camera_frame_id_(get_parameter("camera_frame_id").as_string()), + base_frame_id_(get_parameter("base_frame_id").as_string()), + min_ground_x_m_(get_parameter("min_ground_x_m").as_double()), + max_ground_x_m_(get_parameter("max_ground_x_m").as_double()), + publish_markers_(get_parameter("publish_markers").as_bool()), tf_buffer_(get_clock()), tf_listener_(tf_buffer_) { @@ -45,7 +39,7 @@ RoadSegmentsProviderNode::RoadSegmentsProviderNode( camera_info_sub_ = create_subscription( camera_info_topic_, - rclcpp::QoS(1).transient_local(), + rclcpp::QoS(10), [this](const sensor_msgs::msg::CameraInfo::ConstSharedPtr & msg) { camera_info_callback(msg); }); From e02e6e6ef40979edf06ba5f72a2ec6d8244a1207 Mon Sep 17 00:00:00 2001 From: kyo0221 Date: Wed, 6 May 2026 09:54:33 +0900 Subject: [PATCH 10/13] =?UTF-8?q?=E2=9C=A8feat:=20lanelet2=E4=BE=9D?= =?UTF-8?q?=E5=AD=98=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- planner/lane_planner/CMakeLists.txt | 1 + .../lane_planner/lane_planner_plugin.hpp | 5 + .../lane_planner/lane_planner_server.hpp | 2 + .../include/lane_planner/lanelet_utils.hpp | 48 +++++ .../mapless_lane_planner_plugin.hpp | 20 +- planner/lane_planner/package.xml | 2 + .../lane_planner/src/lane_planner_server.cpp | 9 + planner/lane_planner/src/lanelet_utils.cpp | 197 ++++++++++++++++++ .../plugins/mapless_lane_planner_plugin.cpp | 91 +++----- 9 files changed, 305 insertions(+), 70 deletions(-) create mode 100644 planner/lane_planner/include/lane_planner/lanelet_utils.hpp create mode 100644 planner/lane_planner/src/lanelet_utils.cpp diff --git a/planner/lane_planner/CMakeLists.txt b/planner/lane_planner/CMakeLists.txt index 8db4c2b4..a974f7a0 100644 --- a/planner/lane_planner/CMakeLists.txt +++ b/planner/lane_planner/CMakeLists.txt @@ -10,6 +10,7 @@ ament_auto_find_build_dependencies() ament_auto_add_library(lane_planner_node SHARED src/lane_planner_server.cpp + src/lanelet_utils.cpp src/plugins/vectormap_lane_planner_plugin.cpp src/plugins/mapless_lane_planner_plugin.cpp ) diff --git a/planner/lane_planner/include/lane_planner/lane_planner_plugin.hpp b/planner/lane_planner/include/lane_planner/lane_planner_plugin.hpp index 24744e25..e418b5d8 100644 --- a/planner/lane_planner/include/lane_planner/lane_planner_plugin.hpp +++ b/planner/lane_planner/include/lane_planner/lane_planner_plugin.hpp @@ -8,6 +8,7 @@ #include #include #include +#include namespace lane_planner { @@ -34,6 +35,10 @@ class LanePlannerPlugin // Optional — only used by mapless plugin; default returns nullopt virtual std::optional get_mission_lanes() const { return std::nullopt; } + + // Optional — road model visualization; default returns nullopt + virtual std::optional + get_markers() const { return std::nullopt; } }; } // namespace lane_planner diff --git a/planner/lane_planner/include/lane_planner/lane_planner_server.hpp b/planner/lane_planner/include/lane_planner/lane_planner_server.hpp index e3691f10..bfd4976e 100644 --- a/planner/lane_planner/include/lane_planner/lane_planner_server.hpp +++ b/planner/lane_planner/include/lane_planner/lane_planner_server.hpp @@ -10,6 +10,7 @@ #include #include #include +#include #include "lane_planner/lane_planner_plugin.hpp" #include "lane_planner/visibility_control.h" @@ -48,6 +49,7 @@ class LanePlannerServer : public rclcpp::Node rclcpp::Subscription::SharedPtr road_segments_sub_; rclcpp::Publisher::SharedPtr path_pub_; rclcpp::Publisher::SharedPtr mission_lanes_pub_; + rclcpp::Publisher::SharedPtr marker_pub_; rclcpp::TimerBase::SharedPtr timer_; }; diff --git a/planner/lane_planner/include/lane_planner/lanelet_utils.hpp b/planner/lane_planner/include/lane_planner/lanelet_utils.hpp new file mode 100644 index 00000000..f0e9082c --- /dev/null +++ b/planner/lane_planner/include/lane_planner/lanelet_utils.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace lane_planner +{ + +struct LaneletConnection +{ + int original_lanelet_id{-1}; + std::vector predecessor_lanelet_ids; + std::vector successor_lanelet_ids; + std::vector neighbor_lanelet_ids; + bool goal_information{true}; +}; + +// RoadSegments の各 Segment を lanelet::Lanelet に変換し、接続情報を構築する。 +// out_lanelets[i] と out_connections[i] はインデックスベースで対応する。 +void convert_segments_to_lanelets( + const mapless_planning_msgs::msg::RoadSegments & msg, + std::vector & out_lanelets, + std::vector & out_connections); + +// ego 位置 (0,0) が含まれる lanelet のインデックスを返す。見つからない場合は -1。 +int find_ego_lanelet_id(const std::vector & lanelets); + +// lane_ids で指定した lanelet 群から DrivingCorridor を構築する。 +mapless_planning_msgs::msg::DrivingCorridor create_driving_corridor( + const std::vector & lane_ids, + const std::vector & lanelets); + +// 全 lanelet の centerline・左右境界を LINE_STRIP マーカーとして生成する。 +visualization_msgs::msg::MarkerArray create_road_model_markers( + const std::vector & lanelets, + const std::string & frame_id, + const rclcpp::Time & stamp); + +} // namespace lane_planner diff --git a/planner/lane_planner/include/lane_planner/mapless_lane_planner_plugin.hpp b/planner/lane_planner/include/lane_planner/mapless_lane_planner_plugin.hpp index 8c2c175b..dc314801 100644 --- a/planner/lane_planner/include/lane_planner/mapless_lane_planner_plugin.hpp +++ b/planner/lane_planner/include/lane_planner/mapless_lane_planner_plugin.hpp @@ -1,19 +1,22 @@ #pragma once -#include #include #include #include #include +#include + #include #include #include #include #include #include +#include #include "lane_planner/lane_planner_plugin.hpp" +#include "lane_planner/lanelet_utils.hpp" namespace lane_planner { @@ -28,22 +31,13 @@ class MaplessLanePlannerPlugin : public LanePlannerPlugin std::optional compute_path(const rclcpp::Time & stamp) override; std::optional get_mission_lanes() const override; + std::optional get_markers() const override; private: using Point = geometry_msgs::msg::Point; - struct Pose2D - { - double x{0.0}; - double y{0.0}; - double yaw{0.0}; - }; - - std::vector compute_centerline( - const mapless_planning_msgs::msg::RoadSegments & segments) const; - mapless_planning_msgs::msg::DrivingCorridor build_ego_corridor( - const mapless_planning_msgs::msg::RoadSegments & segments) const; + const mapless_planning_msgs::msg::RoadSegments & segments); nav_msgs::msg::Path corridor_to_path( const mapless_planning_msgs::msg::DrivingCorridor & corridor, @@ -54,7 +48,6 @@ class MaplessLanePlannerPlugin : public LanePlannerPlugin // Parameters std::string map_frame_id_{"map"}; - std::string base_frame_id_{"base_link"}; double max_corridor_length_m_{20.0}; double path_resample_interval_m_{0.5}; int target_lane_{0}; // 0=LANE_KEEP, -1=LEFT, +1=RIGHT @@ -64,6 +57,7 @@ class MaplessLanePlannerPlugin : public LanePlannerPlugin std::optional latest_segments_; geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr latest_pose_; std::optional latest_mission_lanes_; + std::vector latest_lanelets_; rclcpp::Logger logger_{rclcpp::get_logger("mapless_lane_planner_plugin")}; rclcpp::Clock::SharedPtr clock_; diff --git a/planner/lane_planner/package.xml b/planner/lane_planner/package.xml index bc77d28b..ea3a3a36 100644 --- a/planner/lane_planner/package.xml +++ b/planner/lane_planner/package.xml @@ -10,11 +10,13 @@ ament_cmake_auto geometry_msgs + lanelet2_core mapless_planning_msgs nav_msgs rclcpp std_msgs vectormap_msgs + visualization_msgs ament_lint_auto ament_lint_common diff --git a/planner/lane_planner/src/lane_planner_server.cpp b/planner/lane_planner/src/lane_planner_server.cpp index 7d523da7..46f5135d 100644 --- a/planner/lane_planner/src/lane_planner_server.cpp +++ b/planner/lane_planner/src/lane_planner_server.cpp @@ -59,6 +59,8 @@ LanePlannerServer::LanePlannerServer( }); mission_lanes_pub_ = create_publisher( get_parameter("mission_lanes_topic").as_string(), qos_); + marker_pub_ = create_publisher( + "/planning/mission_lanes/markers", qos_); } path_pub_ = create_publisher( @@ -114,6 +116,13 @@ void LanePlannerServer::timer_callback() mission_lanes_pub_->publish(*ml); } } + + if (marker_pub_) { + const auto markers = plugin_->get_markers(); + if (markers) { + marker_pub_->publish(*markers); + } + } } } // namespace lane_planner diff --git a/planner/lane_planner/src/lanelet_utils.cpp b/planner/lane_planner/src/lanelet_utils.cpp new file mode 100644 index 00000000..46135ddc --- /dev/null +++ b/planner/lane_planner/src/lanelet_utils.cpp @@ -0,0 +1,197 @@ +#include "lane_planner/lanelet_utils.hpp" + +#include +#include +#include +#include + +namespace lane_planner +{ + +void convert_segments_to_lanelets( + const mapless_planning_msgs::msg::RoadSegments & msg, + std::vector & out_lanelets, + std::vector & out_connections) +{ + out_lanelets.clear(); + out_connections.clear(); + out_lanelets.reserve(msg.segments.size()); + out_connections.reserve(msg.segments.size()); + + // original segment id → index へのマッピング + std::unordered_map id_map; + + for (std::size_t idx = 0; idx < msg.segments.size(); ++idx) { + const auto & seg = msg.segments[idx]; + + // linestrings[0] = 左境界、linestrings[1] = 右境界 + std::array ls; + for (std::size_t li = 0; li < 2; ++li) { + std::vector pts; + pts.reserve(seg.linestrings[li].poses.size()); + for (const auto & pose : seg.linestrings[li].poses) { + pts.emplace_back( + lanelet::utils::getId(), + pose.position.x, + pose.position.y, + pose.position.z); + } + ls[li] = lanelet::LineString3d(lanelet::utils::getId(), std::move(pts)); + } + + out_lanelets.emplace_back(lanelet::utils::getId(), ls[0], ls[1]); + + LaneletConnection conn; + conn.original_lanelet_id = static_cast(seg.id); + for (const auto id : seg.successor_segment_id) { + conn.successor_lanelet_ids.push_back(static_cast(id)); + } + for (const auto id : seg.neighboring_segment_id) { + conn.neighbor_lanelet_ids.push_back(static_cast(id)); + } + out_connections.push_back(conn); + + id_map[seg.id] = static_cast(idx); + } + + // original id をインデックスベース id に正規化 + auto remap = [&](std::vector & ids) { + for (auto & id : ids) { + auto it = id_map.find(static_cast(id)); + id = (it != id_map.end()) ? it->second : -1; + } + }; + for (auto & conn : out_connections) { + remap(conn.successor_lanelet_ids); + remap(conn.neighbor_lanelet_ids); + } + + // predecessor を successor の逆引きで補完 + for (int i = 0; i < static_cast(out_connections.size()); ++i) { + for (int succ : out_connections[i].successor_lanelet_ids) { + if (succ >= 0 && succ < static_cast(out_connections.size())) { + out_connections[succ].predecessor_lanelet_ids.push_back(i); + } + } + } +} + +int find_ego_lanelet_id(const std::vector & lanelets) +{ + const lanelet::BasicPoint2d ego{0.0, 0.0}; + for (int i = 0; i < static_cast(lanelets.size()); ++i) { + if (lanelet::geometry::inside(lanelets[i], ego)) { + return i; + } + } + return -1; +} + +mapless_planning_msgs::msg::DrivingCorridor create_driving_corridor( + const std::vector & lane_ids, + const std::vector & lanelets) +{ + mapless_planning_msgs::msg::DrivingCorridor corridor; + + for (const int id : lane_ids) { + if (id < 0 || id >= static_cast(lanelets.size())) { + continue; + } + const auto & ll = lanelets[id]; + + for (const auto & pt : ll.centerline()) { + geometry_msgs::msg::Point p; + p.x = pt.x(); + p.y = pt.y(); + p.z = pt.z(); + corridor.centerline.push_back(p); + } + for (const auto & pt : ll.leftBound()) { + geometry_msgs::msg::Point p; + p.x = pt.x(); + p.y = pt.y(); + p.z = pt.z(); + corridor.bound_left.push_back(p); + } + for (const auto & pt : ll.rightBound()) { + geometry_msgs::msg::Point p; + p.x = pt.x(); + p.y = pt.y(); + p.z = pt.z(); + corridor.bound_right.push_back(p); + } + } + + return corridor; +} + +visualization_msgs::msg::MarkerArray create_road_model_markers( + const std::vector & lanelets, + const std::string & frame_id, + const rclcpp::Time & stamp) +{ + visualization_msgs::msg::MarkerArray arr; + + auto make_marker = [&](int id, const std::string & ns, + float r, float g, float b, float width) + { + visualization_msgs::msg::Marker m; + m.header.frame_id = frame_id; + m.header.stamp = stamp; + m.ns = ns; + m.id = id; + m.type = visualization_msgs::msg::Marker::LINE_STRIP; + m.action = visualization_msgs::msg::Marker::ADD; + m.scale.x = static_cast(width); + m.color.r = r; + m.color.g = g; + m.color.b = b; + m.color.a = 1.0f; + m.pose.orientation.w = 1.0; + return m; + }; + + for (int i = 0; i < static_cast(lanelets.size()); ++i) { + const auto & ll = lanelets[i]; + + auto cl_marker = make_marker(i, "centerline", 0.0f, 1.0f, 0.0f, 0.05f); + for (const auto & pt : ll.centerline()) { + geometry_msgs::msg::Point p; + p.x = pt.x(); + p.y = pt.y(); + p.z = pt.z(); + cl_marker.points.push_back(p); + } + if (!cl_marker.points.empty()) { + arr.markers.push_back(cl_marker); + } + + auto lb_marker = make_marker(i + 1000, "boundary", 1.0f, 1.0f, 1.0f, 0.03f); + for (const auto & pt : ll.leftBound()) { + geometry_msgs::msg::Point p; + p.x = pt.x(); + p.y = pt.y(); + p.z = pt.z(); + lb_marker.points.push_back(p); + } + if (!lb_marker.points.empty()) { + arr.markers.push_back(lb_marker); + } + + auto rb_marker = make_marker(i + 2000, "boundary", 1.0f, 1.0f, 1.0f, 0.03f); + for (const auto & pt : ll.rightBound()) { + geometry_msgs::msg::Point p; + p.x = pt.x(); + p.y = pt.y(); + p.z = pt.z(); + rb_marker.points.push_back(p); + } + if (!rb_marker.points.empty()) { + arr.markers.push_back(rb_marker); + } + } + + return arr; +} + +} // namespace lane_planner diff --git a/planner/lane_planner/src/plugins/mapless_lane_planner_plugin.cpp b/planner/lane_planner/src/plugins/mapless_lane_planner_plugin.cpp index 3a405070..5d39e81d 100644 --- a/planner/lane_planner/src/plugins/mapless_lane_planner_plugin.cpp +++ b/planner/lane_planner/src/plugins/mapless_lane_planner_plugin.cpp @@ -10,7 +10,6 @@ namespace constexpr double EPSILON = 1.0e-9; -// 3x3 rotation matrix rows from quaternion (x, y, z, w) struct RotMat3 { double r[3][3]; @@ -104,33 +103,26 @@ std::optional MaplessLanePlannerPlugin::compute_path( { std::lock_guard lock(data_mutex_); if (!latest_segments_) { - RCLCPP_WARN_THROTTLE( - logger_, *clock_, 2000, "no RoadSegments received yet"); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 2000, "no RoadSegments received yet"); return std::nullopt; } if (!latest_pose_) { - RCLCPP_WARN_THROTTLE( - logger_, *clock_, 2000, "no localization pose received yet"); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 2000, "no localization pose received yet"); return std::nullopt; } - if (latest_segments_->segments.empty()) { RCLCPP_WARN_THROTTLE(logger_, *clock_, 2000, "RoadSegments has no segments"); return std::nullopt; } - if (target_lane_ != mapless_planning_msgs::msg::MissionLanesStamped::LANE_KEEP) { - RCLCPP_ERROR_THROTTLE( - logger_, *clock_, 2000, - "lane change requested (target_lane=%d) but not implemented in initial version; " - "no path published", + RCLCPP_ERROR_THROTTLE(logger_, *clock_, 2000, + "lane change requested (target_lane=%d) but not implemented; no path published", target_lane_); return std::nullopt; } const auto ego_corridor = build_ego_corridor(*latest_segments_); - // Build MissionLanesStamped for external use mapless_planning_msgs::msg::MissionLanesStamped ml; ml.header.stamp = stamp; ml.header.frame_id = latest_segments_->header.frame_id; @@ -140,8 +132,7 @@ std::optional MaplessLanePlannerPlugin::compute_path( latest_mission_lanes_ = ml; if (ego_corridor.centerline.empty()) { - RCLCPP_WARN_THROTTLE( - logger_, *clock_, 2000, "ego corridor centerline is empty"); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 2000, "ego corridor centerline is empty"); return std::nullopt; } @@ -155,54 +146,40 @@ MaplessLanePlannerPlugin::get_mission_lanes() const return latest_mission_lanes_; } -// ---------- Internal helpers ---------- - -mapless_planning_msgs::msg::DrivingCorridor MaplessLanePlannerPlugin::build_ego_corridor( - const mapless_planning_msgs::msg::RoadSegments & segments) const +std::optional +MaplessLanePlannerPlugin::get_markers() const { - // Initial version: use segment 0 as ego lane - const auto & seg = segments.segments[0]; - - if (seg.linestrings.size() < 2 || - seg.linestrings[0].poses.empty() || - seg.linestrings[1].poses.empty()) - { - RCLCPP_WARN(logger_, "segment 0 has insufficient linestrings"); - return mapless_planning_msgs::msg::DrivingCorridor{}; - } - - const auto & left_ls = seg.linestrings[0]; - const auto & right_ls = seg.linestrings[1]; - - mapless_planning_msgs::msg::DrivingCorridor corridor; - - for (const auto & pose : left_ls.poses) { - corridor.bound_left.push_back(pose.position); - } - for (const auto & pose : right_ls.poses) { - corridor.bound_right.push_back(pose.position); + std::lock_guard lock(data_mutex_); + if (latest_lanelets_.empty() || !latest_segments_) { + return std::nullopt; } + return create_road_model_markers( + latest_lanelets_, + latest_segments_->header.frame_id, + latest_segments_->header.stamp); +} - // Compute centerline: interpolate between left and right at same x samples - const std::size_t n_left = left_ls.poses.size(); - const std::size_t n_right = right_ls.poses.size(); - const std::size_t n_pts = std::max(n_left, n_right); - - for (std::size_t i = 0; i < n_pts; ++i) { - const double tl = static_cast(i) / std::max(1, n_pts - 1); - const double tr = tl; - - const auto idx_l = static_cast(tl * static_cast(n_left - 1)); - const auto idx_r = static_cast(tr * static_cast(n_right - 1)); +// ---------- Internal helpers ---------- - geometry_msgs::msg::Point c; - c.x = 0.5 * (left_ls.poses[idx_l].position.x + right_ls.poses[idx_r].position.x); - c.y = 0.5 * (left_ls.poses[idx_l].position.y + right_ls.poses[idx_r].position.y); - c.z = 0.0; - corridor.centerline.push_back(c); +mapless_planning_msgs::msg::DrivingCorridor MaplessLanePlannerPlugin::build_ego_corridor( + const mapless_planning_msgs::msg::RoadSegments & segments) +{ + std::vector connections; + convert_segments_to_lanelets(segments, latest_lanelets_, connections); + + const int ego_id = find_ego_lanelet_id(latest_lanelets_); + if (ego_id < 0) { + // 単一路では ego は常に segment 0 内にあるはずだが、境界線上では外れることがある + RCLCPP_WARN_THROTTLE(logger_, *clock_, 2000, + "ego lanelet not found among %zu lanelets; using index 0", + latest_lanelets_.size()); + if (latest_lanelets_.empty()) { + return mapless_planning_msgs::msg::DrivingCorridor{}; + } + return create_driving_corridor({0}, latest_lanelets_); } - return corridor; + return create_driving_corridor({ego_id}, latest_lanelets_); } nav_msgs::msg::Path MaplessLanePlannerPlugin::corridor_to_path( @@ -231,7 +208,7 @@ nav_msgs::msg::Path MaplessLanePlannerPlugin::corridor_to_path( if (std::hypot(dx, dy) > EPSILON) { yaw = std::atan2(dy, dx); } - } else if (i > 0) { + } else { const auto p_prev = transform_point(cl[i - 1], ego_in_map); const double dx = p_map.x - p_prev.x; const double dy = p_map.y - p_prev.y; From 24653953e4284409cb4ae97bf125b9580b2df84f Mon Sep 17 00:00:00 2001 From: kyo0221 Date: Wed, 6 May 2026 10:19:28 +0900 Subject: [PATCH 11/13] =?UTF-8?q?=E2=9C=A8feat:=20=E3=83=AC=E3=83=BC?= =?UTF-8?q?=E3=83=B3=E3=81=AE=E5=B9=BE=E4=BD=95=E5=AD=A6=E7=9A=84=E5=88=B6?= =?UTF-8?q?=E7=B4=84=E3=82=92=E8=80=83=E6=85=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../road_segments_provider_node.hpp | 32 +++- .../src/road_segments_provider_node.cpp | 170 +++++++++++++----- 2 files changed, 150 insertions(+), 52 deletions(-) diff --git a/perception/road_segments_provider/include/road_segments_provider/road_segments_provider_node.hpp b/perception/road_segments_provider/include/road_segments_provider/road_segments_provider_node.hpp index 8a8a65f8..2a6bb762 100644 --- a/perception/road_segments_provider/include/road_segments_provider/road_segments_provider_node.hpp +++ b/perception/road_segments_provider/include/road_segments_provider/road_segments_provider_node.hpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -27,6 +28,13 @@ namespace road_segments_provider { +struct LaneBoundaries +{ + std::vector left; + std::vector right; + float curvature{0.0f}; // [1/m] max |κ| in segment, left-curve positive +}; + class RoadSegmentsProviderNode : public rclcpp::Node { public: @@ -50,15 +58,22 @@ class RoadSegmentsProviderNode : public rclcpp::Node // Returns nullopt if TF is unavailable. std::optional build_camera_params(); - // Extracts ego lane left/right boundaries from the lane mask image. - // Left boundary: rightmost lane pixels in left half (u < width/2), row by row. - // Right boundary: leftmost lane pixels in right half (u >= width/2), row by row. - // Both are returned sorted by increasing ground-x (near to far). - std::pair, std::vector> - extract_lane_boundaries( + // Scans the lane mask row-by-row and collects IPM-projected center points + // (only for rows where both sides are detected above min_valid_pixel_width_). + // Returns empty vectors when no valid rows are found. + std::pair, std::vector> + collect_center_points( const cv::Mat & lane_img, const CameraParams & params) const; + // Fits a polynomial of degree poly_fit_order_ to the center points, + // resamples at uniform intervals, expands by ±lane_width_m_/2, + // and computes the max curvature scalar. + // Returns empty LaneBoundaries if fewer than min_fit_points_ are available. + LaneBoundaries fit_and_expand( + const std::vector & cx, + const std::vector & cy) const; + visualization_msgs::msg::MarkerArray build_markers( const std::vector & left, const std::vector & right, @@ -74,6 +89,11 @@ class RoadSegmentsProviderNode : public rclcpp::Node const std::string base_frame_id_; const double min_ground_x_m_; const double max_ground_x_m_; + const double lane_width_m_; // [m] 既知レーン幅 + const int min_valid_pixel_width_; // [px] 両側検出時の最小ピクセル幅（消失点フィルタ） + const int poly_fit_order_; // 多項式次数（2 または 3） + const int min_fit_points_; // フィット最少点数 + const double centerline_resample_interval_m_; // [m] 再サンプリング間隔 const bool publish_markers_; // Subscribers / sync diff --git a/perception/road_segments_provider/src/road_segments_provider_node.cpp b/perception/road_segments_provider/src/road_segments_provider_node.cpp index d5e49caf..e12821b6 100644 --- a/perception/road_segments_provider/src/road_segments_provider_node.cpp +++ b/perception/road_segments_provider/src/road_segments_provider_node.cpp @@ -1,8 +1,11 @@ #include "road_segments_provider/road_segments_provider_node.hpp" #include +#include #include +#include + #include #include #include @@ -28,6 +31,11 @@ RoadSegmentsProviderNode::RoadSegmentsProviderNode( base_frame_id_(get_parameter("base_frame_id").as_string()), min_ground_x_m_(get_parameter("min_ground_x_m").as_double()), max_ground_x_m_(get_parameter("max_ground_x_m").as_double()), + lane_width_m_(get_parameter("lane_width_m").as_double()), + min_valid_pixel_width_(get_parameter("min_valid_pixel_width").as_int()), + poly_fit_order_(get_parameter("poly_fit_order").as_int()), + min_fit_points_(get_parameter("min_fit_points").as_int()), + centerline_resample_interval_m_(get_parameter("centerline_resample_interval_m").as_double()), publish_markers_(get_parameter("publish_markers").as_bool()), tf_buffer_(get_clock()), tf_listener_(tf_buffer_) @@ -36,6 +44,18 @@ RoadSegmentsProviderNode::RoadSegmentsProviderNode( throw std::invalid_argument( "Invalid ground range: min_ground_x_m must be >= 0 and < max_ground_x_m"); } + if (lane_width_m_ <= 0.0) { + throw std::invalid_argument("lane_width_m must be greater than 0"); + } + if (poly_fit_order_ < 2 || poly_fit_order_ > 3) { + throw std::invalid_argument("poly_fit_order must be 2 or 3"); + } + if (min_fit_points_ < poly_fit_order_ + 1) { + throw std::invalid_argument("min_fit_points must be > poly_fit_order"); + } + if (centerline_resample_interval_m_ <= 0.0) { + throw std::invalid_argument("centerline_resample_interval_m must be > 0"); + } camera_info_sub_ = create_subscription( camera_info_topic_, @@ -69,6 +89,8 @@ RoadSegmentsProviderNode::RoadSegmentsProviderNode( RCLCPP_INFO(get_logger(), " lane_mask: %s", lane_mask_topic_.c_str()); RCLCPP_INFO(get_logger(), " da_mask: %s", da_mask_topic_.c_str()); RCLCPP_INFO(get_logger(), " output: %s", road_segments_topic_.c_str()); + RCLCPP_INFO(get_logger(), " poly_fit_order: %d, min_fit_points: %d, resample: %.2f m", + poly_fit_order_, min_fit_points_, centerline_resample_interval_m_); } void RoadSegmentsProviderNode::camera_info_callback( @@ -116,61 +138,117 @@ std::optional RoadSegmentsProviderNode::build_camera_params() return params; } -std::pair, std::vector> -RoadSegmentsProviderNode::extract_lane_boundaries( +std::pair, std::vector> +RoadSegmentsProviderNode::collect_center_points( const cv::Mat & lane_img, const CameraParams & params) const { - std::vector left_pts; - std::vector right_pts; + std::vector cx_out, cy_out; - const int width = lane_img.cols; + const int img_width = lane_img.cols; const int height = lane_img.rows; - const int cx = width / 2; + const int img_cx = img_width / 2; - // Scan rows from bottom (nearest) to top (farthest) to build ordered boundaries for (int v = height - 1; v >= 0; --v) { const uchar * row = lane_img.ptr(v); - int left_u = -1; // rightmost non-zero pixel in left half - int right_u = -1; // leftmost non-zero pixel in right half + int left_u = -1; + int right_u = -1; - for (int u = cx - 1; u >= 0; --u) { - if (row[u] != 0) { - left_u = u; - break; - } + for (int u = img_cx - 1; u >= 0; --u) { + if (row[u] != 0) { left_u = u; break; } } - for (int u = cx; u < width; ++u) { - if (row[u] != 0) { - right_u = u; - break; - } + for (int u = img_cx; u < img_width; ++u) { + if (row[u] != 0) { right_u = u; break; } } - if (left_u >= 0) { - auto pt = project_pixel_to_ground(left_u, v, params); - if (pt && pt->x >= min_ground_x_m_ && pt->x <= max_ground_x_m_) { - left_pts.push_back(*pt); - } + if (left_u < 0 || right_u < 0) { + continue; } - if (right_u >= 0) { - auto pt = project_pixel_to_ground(right_u, v, params); - if (pt && pt->x >= min_ground_x_m_ && pt->x <= max_ground_x_m_) { - right_pts.push_back(*pt); - } + if (right_u - left_u < min_valid_pixel_width_) { + continue; } + + const int u_center = (left_u + right_u) / 2; + auto pt = project_pixel_to_ground(u_center, v, params); + if (!pt || pt->x < min_ground_x_m_ || pt->x > max_ground_x_m_) { + continue; + } + + cx_out.push_back(pt->x); + cy_out.push_back(pt->y); } - // Already ordered near to far (bottom row projected first, smallest x first in base_link) - // Sort by x ascending just in case projection re-orders due to camera tilt - auto by_x = [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) { - return a.x < b.x; - }; - std::sort(left_pts.begin(), left_pts.end(), by_x); - std::sort(right_pts.begin(), right_pts.end(), by_x); + return {cx_out, cy_out}; +} + +LaneBoundaries RoadSegmentsProviderNode::fit_and_expand( + const std::vector & cx, + const std::vector & cy) const +{ + const int N = static_cast(cx.size()); + if (N < min_fit_points_) { + RCLCPP_DEBUG(get_logger(), "Too few center points (%d < %d), skipping segment", N, min_fit_points_); + return {}; + } + + // Build design matrix A (N × order+1) and RHS b (N × 1) + const int cols = poly_fit_order_ + 1; + cv::Mat A(N, cols, CV_64F); + cv::Mat b(N, 1, CV_64F); + for (int i = 0; i < N; ++i) { + double xi = 1.0; + for (int j = 0; j < cols; ++j) { + A.at(i, j) = xi; + xi *= cx[i]; + } + b.at(i, 0) = cy[i]; + } + + cv::Mat coeff; + if (!cv::solve(A, b, coeff, cv::DECOMP_SVD)) { + RCLCPP_WARN(get_logger(), "Polynomial fit failed (cv::solve returned false)"); + return {}; + } + + // Resample fitted centerline at uniform x intervals + const auto [x_min_it, x_max_it] = std::minmax_element(cx.begin(), cx.end()); + const double x_min = *x_min_it; + const double x_max = *x_max_it; + const int n_samples = std::max(2, + static_cast((x_max - x_min) / centerline_resample_interval_m_) + 1); + + const double half_width = lane_width_m_ * 0.5; + LaneBoundaries result; + result.left.reserve(n_samples); + result.right.reserve(n_samples); + + for (int i = 0; i < n_samples; ++i) { + const double x = x_min + (x_max - x_min) * i / (n_samples - 1); + double y = 0.0; + double xi = 1.0; + for (int j = 0; j < cols; ++j) { + y += coeff.at(j, 0) * xi; + xi *= x; + } + geometry_msgs::msg::Point lp, rp; + lp.x = rp.x = x; + lp.z = rp.z = 0.0; + lp.y = y + half_width; + rp.y = y - half_width; + result.left.push_back(lp); + result.right.push_back(rp); + } + + // κ(x) ≈ 2c₂ + 6c₃x; evaluate at near and far ends, take the larger magnitude + const double c2 = coeff.at(2, 0); + const double c3 = (poly_fit_order_ >= 3) ? coeff.at(3, 0) : 0.0; + const double kappa_near = 2.0 * c2 + 6.0 * c3 * x_min; + const double kappa_far = 2.0 * c2 + 6.0 * c3 * x_max; + result.curvature = static_cast( + std::abs(kappa_near) >= std::abs(kappa_far) ? kappa_near : kappa_far); - return {left_pts, right_pts}; + return result; } visualization_msgs::msg::MarkerArray RoadSegmentsProviderNode::build_markers( @@ -232,26 +310,26 @@ void RoadSegmentsProviderNode::sync_callback( return; } - auto [left_pts, right_pts] = extract_lane_boundaries(lane_img, params); + auto [cx, cy] = collect_center_points(lane_img, params); + const LaneBoundaries boundaries = fit_and_expand(cx, cy); - if (left_pts.empty() && right_pts.empty()) { - RCLCPP_DEBUG(get_logger(), "No lane boundaries detected"); + if (boundaries.left.empty()) { + RCLCPP_DEBUG(get_logger(), "No valid lane boundaries after polynomial fit"); return; } // Build ego segment mapless_planning_msgs::msg::Segment segment; segment.id = 0; + segment.curvature = boundaries.curvature; - // linestrings[0] = left boundary - for (const auto & pt : left_pts) { + for (const auto & pt : boundaries.left) { geometry_msgs::msg::Pose pose; pose.position = pt; pose.orientation.w = 1.0; segment.linestrings[0].poses.push_back(pose); } - // linestrings[1] = right boundary - for (const auto & pt : right_pts) { + for (const auto & pt : boundaries.right) { geometry_msgs::msg::Pose pose; pose.position = pt; pose.orientation.w = 1.0; @@ -262,14 +340,14 @@ void RoadSegmentsProviderNode::sync_callback( msg.header.stamp = lane_msg->header.stamp; msg.header.frame_id = base_frame_id_; msg.segments.push_back(segment); - // pose = identity: segments are expressed in base_link frame, ego is at origin msg.pose.orientation.w = 1.0; road_segments_pub_->publish(msg); if (publish_markers_ && marker_pub_) { marker_pub_->publish( - build_markers(left_pts, right_pts, base_frame_id_, lane_msg->header.stamp)); + build_markers(boundaries.left, boundaries.right, + base_frame_id_, lane_msg->header.stamp)); } } From 0f3b83a3e11d77645bb6f0b66ba6295987e56dd3 Mon Sep 17 00:00:00 2001 From: kyo0221 Date: Wed, 6 May 2026 10:19:50 +0900 Subject: [PATCH 12/13] =?UTF-8?q?=E2=9C=A8feat:=20=E6=9B=B2=E7=8E=87?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mapless_planning_msgs/msg/Segment.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/mapless_planning_msgs/msg/Segment.msg b/mapless_planning_msgs/msg/Segment.msg index be184a03..ff872265 100644 --- a/mapless_planning_msgs/msg/Segment.msg +++ b/mapless_planning_msgs/msg/Segment.msg @@ -2,3 +2,4 @@ uint16 id Linestring[2] linestrings int32[] successor_segment_id int32[] neighboring_segment_id +float32 curvature From ecfc01465382f73e6d323062ca7ca8512cde6a15 Mon Sep 17 00:00:00 2001 From: kyo0221 Date: Tue, 12 May 2026 13:43:33 +0900 Subject: [PATCH 13/13] =?UTF-8?q?=E2=9C=A8feat:=20add=20md=20ignore?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index dd331669..28971e4e 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,7 @@ *.log *.pyc *.pt +*.md #ディレクトリで指定 bin/ @@ -19,6 +20,5 @@ simulator_old/ AGENTS.md CLAUDE.md .codex -project.md #例外ファイルを指定