chore(agv_pro_navigation2): clean nav launch and example script

fix(agv_pro_bringup): add launch with RViz condition

agv_pro_base/include/agv_pro_base/agv_pro_driver.h

@@ -15,6 +15,8 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <agv_pro_msgs/srv/set_digital_output.hpp>
 #include <agv_pro_msgs/srv/get_digital_input.hpp>
+#include <agv_pro_msgs/srv/set_led_color.hpp>
+#include <agv_pro_msgs/srv/set_led_mode.hpp>
 
 #define SEND_DATA_SIZE 14                               // Total bytes in a command frame to ESP32(version>=V1.0.8)
 #define RECEIVE_FRAME_SIZE 31                           // Total bytes in a frame from ESP32(version>=V1.0.8)
@@ -23,6 +25,8 @@
 #define POWER_ON 0x10
 #define GET_POWER_STATE 0x12
 #define SET_AUTO_REPORT_STATE 0x23
+#define SET_LED_COLOR 0x34
+#define SET_LED_MODE 0x3A
 #define SET_OUTPUT_IO 0x40
 #define GET_INPUT_IO 0x41
 
@@ -196,6 +200,14 @@ private:
     const std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Request> request,
     std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> response);
 
+  void handleSetLedColor(
+    const std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Response> response);
+
+  void handleSetLedMode(
+    const std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Response> response);
+
   boost::asio::io_service io_;
   std::unique_ptr<boost::asio::serial_port> serial_port_;
 
@@ -262,6 +274,8 @@ private:
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub;
   rclcpp::Service<agv_pro_msgs::srv::SetDigitalOutput>::SharedPtr set_output_service;
   rclcpp::Service<agv_pro_msgs::srv::GetDigitalInput>::SharedPtr get_input_service;
+  rclcpp::Service<agv_pro_msgs::srv::SetLedColor>::SharedPtr set_led_service;
+  rclcpp::Service<agv_pro_msgs::srv::SetLedMode>::SharedPtr set_led_mode_service;
 
   sensor_msgs::msg::Imu imu_data;
   std::unique_ptr<tf2_ros::TransformBroadcaster> odomBroadcaster;

agv_pro_base/src/agv_pro_ros.cpp

@@ -280,7 +280,7 @@ void AGV_PRO::handleSetDigitalOutput(
 
   uint8_t status = response_frame[4];
   if (status == 0x01) {
-    RCLCPP_INFO(this->get_logger(), "SetDigitalOutput succeeded");
+    RCLCPP_DEBUG(this->get_logger(), "SetDigitalOutput succeeded");
     response->success = true;
     response->message = "Success";
   } else {
@@ -315,13 +315,96 @@ void AGV_PRO::handleGetDigitalInput(
     RCLCPP_ERROR(this->get_logger(), "GetDigitalInput failed with status: 0x%02X", status);
     response->success = false;
   } else {
-    RCLCPP_INFO(this->get_logger(), "GetDigitalInput succeeded, state: %u", status);
+    RCLCPP_DEBUG(this->get_logger(), "GetDigitalInput succeeded, state: %u", status);
     response->state = static_cast<int32_t>(status);
     response->success = true;
     response->message = "Success";
   }
 }
 
+void AGV_PRO::handleSetLedColor(
+  const std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Response> response)
+{
+  if (request->position < 0 || request->position > 1) {
+    RCLCPP_ERROR(this->get_logger(), "Invalid LED position: %d", request->position);
+    response->success = false;
+    response->message = "Invalid LED position";
+    return;
+  }
+
+  if (request->brightness < 0 || request->brightness > 255) {
+    RCLCPP_ERROR(this->get_logger(), "Invalid brightness: %d", request->brightness);
+    response->success = false;
+    response->message = "Invalid brightness";
+    return;
+  }
+
+  if (request->r < 0 || request->r > 255 ||
+      request->g < 0 || request->g > 255 ||
+      request->b < 0 || request->b > 255) {
+    RCLCPP_ERROR(
+      this->get_logger(),
+      "Invalid RGB value: r=%d g=%d b=%d",
+      request->r, request->g, request->b
+    );
+    response->success = false;
+    response->message = "Invalid RGB value";
+    return;
+  }
+
+  uint8_t position   = static_cast<uint8_t>(request->position);
+  uint8_t brightness = static_cast<uint8_t>(request->brightness);
+  uint8_t r          = static_cast<uint8_t>(request->r);
+  uint8_t g          = static_cast<uint8_t>(request->g);
+  uint8_t b          = static_cast<uint8_t>(request->b);
+
+  auto frame = build_serial_frame(SET_LED_COLOR, {position, brightness, r, g, b});
+  send_serial_frame(frame, true);
+
+  const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, SET_LED_COLOR};
+  auto response_frame = read_serial_response(expected_header, 8, 5.0);
+
+  // print_hex("recv_buf", response_frame); //debug
+
+  uint8_t status = response_frame[4];
+  if (status == 0x01) {
+    RCLCPP_DEBUG(this->get_logger(), "SetLedColor succeeded");
+    response->success = true;
+    response->message = "Success";
+  } else {
+    RCLCPP_ERROR(this->get_logger(), "SetLedColor failed with status: 0x%02X", status);
+    response->success = false;
+    response->message = "Failed with status code";
+  }
+}
+
+void AGV_PRO::handleSetLedMode(
+  const std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Response> response)
+{
+  uint8_t mode = request->mode ? 0x01 : 0x00;
+
+  auto frame = build_serial_frame(SET_LED_MODE, {mode});
+  send_serial_frame(frame, true);
+
+  const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, SET_LED_MODE};
+  auto response_frame = read_serial_response(expected_header, 8, 5.0);
+
+  // print_hex("recv_buf", response_frame); //debug
+
+  uint8_t status = response_frame[4];
+  if (status == 0x01) {
+    RCLCPP_DEBUG(this->get_logger(), "SetLedMode succeeded");
+    response->success = true;
+    response->message = "Success";
+  } else {
+    RCLCPP_ERROR(this->get_logger(), "SetLedMode failed with status: 0x%02X", status);
+    response->success = false;
+    response->message = "Failed with status code";
+  }
+}
+
 bool AGV_PRO::readData()
 {
   std::vector<uint8_t> buf_length(1);
@@ -569,6 +652,16 @@ AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name)
     std::bind(&AGV_PRO::handleGetDigitalInput, this, std::placeholders::_1, std::placeholders::_2)
   );
 
+  set_led_service = this->create_service<agv_pro_msgs::srv::SetLedColor>(
+    "set_led_color",
+    std::bind(&AGV_PRO::handleSetLedColor, this, std::placeholders::_1, std::placeholders::_2)
+  );
+
+  set_led_mode_service = this->create_service<agv_pro_msgs::srv::SetLedMode>(
+    "set_led_mode",
+    std::bind(&AGV_PRO::handleSetLedMode, this, std::placeholders::_1, std::placeholders::_2)
+  );
+
   lastTime = this->get_clock()->now();
       
   try{

agv_pro_bringup/launch/agv_pro_bringup.launch.py

@@ -1,12 +1,13 @@
 import os
 from launch import LaunchDescription
+from launch.conditions import IfCondition
 from launch_ros.actions import Node,PushRosNamespace
 from launch.actions import DeclareLaunchArgument,IncludeLaunchDescription
 from launch.substitutions import Command,LaunchConfiguration,PythonExpression
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from ament_index_python.packages import get_package_share_directory
 
-def include_lidar(pkg_name, launch_file, lidar_type, expected_type):
+def include_lidar(pkg_name, launch_file, enable_lidar, lidar_type, expected_type):
 
     return IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -16,16 +17,20 @@ def include_lidar(pkg_name, launch_file, lidar_type, expected_type):
                 launch_file
             )
         ),
-        condition=PythonExpression(
-            ["'", lidar_type, "' == '", expected_type, "'"]
+        condition=IfCondition(
+            PythonExpression([
+                "'", enable_lidar, "' == 'true' and '",
+                lidar_type, "' == '", expected_type, "'"
+            ])
         )
     )
 
 def generate_launch_description():
 
-    port_name_arg = LaunchConfiguration('port_name',default='/dev/agvpro_controller')
-    namespace = LaunchConfiguration('namespace', default='')
-    lidar_type = LaunchConfiguration('lidar_type', default='n10p')
+    port_name_arg = LaunchConfiguration('port_name')
+    namespace = LaunchConfiguration('namespace')
+    lidar_type = LaunchConfiguration('lidar_type')
+    enable_lidar = LaunchConfiguration('enable_lidar')
 
     urdf_file = os.path.join(
         get_package_share_directory('agv_pro_description'),
@@ -42,7 +47,7 @@ def generate_launch_description():
 
     declare_port_name_arg = DeclareLaunchArgument(
         'port_name', 
-        default_value=port_name_arg,
+        default_value='/dev/agvpro_controller',
         description='port name, e.g. /dev/ttyACM0'
     )
 
@@ -52,9 +57,15 @@ def generate_launch_description():
         description='Namespace for nodes'
     )
 
+    declare_enable_lidar_arg = DeclareLaunchArgument(
+        'enable_lidar',
+        default_value='true',
+        description='Whether to launch lidar drivers'
+    )
+
     declare_lidar_type_arg = DeclareLaunchArgument(
         'lidar_type',
-        default_value=lidar_type,
+        default_value='n10p',
         description='Lidar type: n10p | mid360 | l2'
     )
 
@@ -87,15 +98,16 @@ def generate_launch_description():
     )
 
     lidar_launchs = [
-        include_lidar('lslidar_driver', 'lsn10p_launch.py', lidar_type, 'n10p'),
-        include_lidar('livox_ros_driver2', 'msg_MID360_launch.py', lidar_type, 'mid360'),
-        include_lidar('unitree_lidar_ros2', 'launch.py', lidar_type, 'l2'),
+        include_lidar('lslidar_driver', 'lsn10p_launch.py', enable_lidar, lidar_type, 'n10p'),
+        include_lidar('livox_ros_driver2', 'MID360_launch.py',enable_lidar, lidar_type, 'mid360'),
+        include_lidar('unitree_lidar_ros2', 'launch.py', enable_lidar, lidar_type, 'l2'),
     ]
 
     return LaunchDescription(
         [
             declare_port_name_arg,
             declare_namespace_arg,
+            declare_enable_lidar_arg,
             declare_lidar_type_arg,
             ns_action,
             agv_pro_node,

agv_pro_calibration/agv_pro_calibration/final_pose_refiner.py

@@ -0,0 +1,388 @@
+#!/usr/bin/env python3
+"""Yaw-only final pose refinement helper for AGV Pro."""
+
+import math
+import os
+import shlex
+
+import rclpy
+from action_msgs.msg import GoalStatus, GoalStatusArray
+from geometry_msgs.msg import PoseStamped, Twist
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from std_msgs.msg import String
+from tf2_ros import Buffer, TransformException, TransformListener
+
+class FinalPoseRefiner(Node):
+    """Refine only the final map->base_footprint yaw with direct low-speed cmd_vel."""
+
+    def __init__(self):
+        super().__init__('final_pose_refiner')
+        self.param_prefix = 'final_pose_refiner_'
+        self.start_param = f'{self.param_prefix}start'
+        self.cancel_param = f'{self.param_prefix}cancel'
+        self.auto_start_param = f'{self.param_prefix}auto_start_on_nav_success'
+        self.log_separator = '------------------------------------------------------------'
+        self.cmd_vel_topic = '/cmd_vel'
+        self.goal_topic = '/goal_pose'
+        self.action_goal_topic = '/final_pose_refiner/goal_pose'
+        self.nav_status_topic = '/navigate_to_pose/_action/status'
+        self.nav2_status_topic = '/navigate_to_pose_nav2/_action/status'
+        self.global_frame = 'map'
+        self.base_frame = 'base_footprint'
+
+        self._declare_param('status_topic', '/final_pose_refiner/status')
+        self.declare_parameter(self.start_param, False)
+        self.declare_parameter(self.cancel_param, False)
+        self.declare_parameter(self.auto_start_param, False)
+        self._declare_param('handoff_distance', 0.20)
+        self._declare_param('yaw_tolerance', 0.04)
+        self._declare_param('settle_time', 0.5)
+        self._declare_param('timeout', 20.0)
+        self._declare_param('k_yaw', 0.5)
+        self._declare_param('max_wz', 0.35)
+        self._declare_param('min_cmd_w', 0.006)
+
+        self.cmd_vel_pub = self.create_publisher(Twist, self.cmd_vel_topic, 10)
+        self.status_pub = self.create_publisher(String, self._param('status_topic'), 10)
+        self.goal_sub = self.create_subscription(
+            PoseStamped,
+            self.goal_topic,
+            self._on_goal,
+            10,
+        )
+        self.action_goal_sub = self.create_subscription(
+            PoseStamped,
+            self.action_goal_topic,
+            self._on_goal,
+            10,
+        )
+        self.nav_status_sub = self.create_subscription(
+            GoalStatusArray,
+            self.nav_status_topic,
+            self._on_nav_status,
+            10,
+        )
+        self.nav2_status_sub = self.create_subscription(
+            GoalStatusArray,
+            self.nav2_status_topic,
+            self._on_nav_status,
+            10,
+        )
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        self.state = 'idle'
+        self.goal = None
+        self.target = None
+        self.waiting_for_nav_success = False
+        self.active_nav_goal_ids = set()
+        self.refined_nav_goal_ids = set()
+        self.start_time = None
+        self.settle_start_time = None
+        self.last_log_time = self.get_clock().now()
+
+        self.timer = self.create_timer(1.0 / 20.0, self.on_timer)
+
+        self.get_logger().info(
+            'final_pose_refiner ready in yaw-only mode. A navigation proxy may submit the '
+            f'target and set {self.start_param}:=true, or these may be provided manually.'
+        )
+
+    def _declare_param(self, name, value):
+        self.declare_parameter(f'{self.param_prefix}{name}', value)
+
+    def _param(self, name):
+        return self.get_parameter(f'{self.param_prefix}{name}').value
+
+    def _on_goal(self, msg):
+        if msg.header.frame_id and msg.header.frame_id != self.global_frame:
+            self.get_logger().warn(
+                f'Ignoring goal in frame "{msg.header.frame_id}". Expected "{self.global_frame}".'
+            )
+            return
+
+        q = msg.pose.orientation
+        target_yaw = self._yaw_from_quaternion(q.x, q.y, q.z, q.w)
+        self.target = (msg.pose.position.x, msg.pose.position.y, target_yaw)
+        self.waiting_for_nav_success = True
+        self.active_nav_goal_ids.clear()
+        self.get_logger().info(
+            f'Updated refine target from topic: x={msg.pose.position.x:.4f}, '
+            f'y={msg.pose.position.y:.4f}, yaw={math.degrees(target_yaw):.2f} deg'
+        )
+
+    def _on_nav_status(self, msg):
+        if not self.get_parameter(self.auto_start_param).value:
+            return
+
+        if self.state != 'idle' or self.target is None or not self.waiting_for_nav_success:
+            return
+
+        for status in msg.status_list:
+            goal_id = tuple(status.goal_info.goal_id.uuid)
+            if status.status in (GoalStatus.STATUS_ACCEPTED, GoalStatus.STATUS_EXECUTING):
+                self.active_nav_goal_ids.add(goal_id)
+            elif status.status == GoalStatus.STATUS_SUCCEEDED:
+                if (
+                    goal_id in self.active_nav_goal_ids and
+                    goal_id not in self.refined_nav_goal_ids
+                ):
+                    self.refined_nav_goal_ids.add(goal_id)
+                    self.get_logger().info(
+                        'Detected Nav2 goal succeeded; starting final yaw refinement.'
+                    )
+                    self._start_refine()
+                    return
+            elif status.status in (GoalStatus.STATUS_CANCELED, GoalStatus.STATUS_ABORTED):
+                if goal_id in self.active_nav_goal_ids:
+                    self.waiting_for_nav_success = False
+                    self.active_nav_goal_ids.discard(goal_id)
+
+    def on_timer(self):
+        if self.get_parameter(self.cancel_param).value:
+            if self.state == 'running':
+                self._finish_refine('canceled')
+            else:
+                self._reset_cancel_refine()
+            return
+
+        if self.state == 'running':
+            self._run_refine_step()
+            return
+
+        if self.get_parameter(self.start_param).value:
+            self._start_refine()
+
+    def _start_refine(self):
+        self._reset_cancel_refine()
+        if self.target is None:
+            self.get_logger().warn(
+                'Cannot start final refinement: no /goal_pose has been received yet.'
+            )
+            self._publish_status('no_goal')
+            self._reset_start_refine()
+            return
+
+        pose = self._lookup_pose()
+        if pose is None:
+            self.get_logger().warn('Cannot start final refinement: TF is not available.')
+            self._publish_status('failed_tf')
+            self._reset_start_refine()
+            return
+
+        target = self.target
+        distance, yaw_error = self._calculate_error(pose, target)
+        handoff_distance = max(self._param('handoff_distance'), 0.0)
+        if distance > handoff_distance:
+            self.get_logger().warn(
+                f'Cannot start final refinement: distance={distance:.3f} m exceeds '
+                f'handoff_distance={handoff_distance:.3f} m.'
+            )
+            self._publish_status('handoff_distance_exceeded')
+            self._reset_start_refine()
+            return
+
+        self.goal = target
+        self.waiting_for_nav_success = False
+        self.start_time = self.get_clock().now()
+        self.settle_start_time = None
+        self.last_log_time = self.get_clock().now()
+        self.state = 'running'
+        self._publish_status('running')
+        self.get_logger().info(
+            f'\n{self.log_separator}\n'
+            'FINAL YAW REFINE START\n'
+            f'target=({target[0]:.4f}, {target[1]:.4f}, {math.degrees(target[2]):.2f} deg)\n'
+            f'initial_distance={distance:.3f} m, '
+            f'initial_yaw_error={math.degrees(yaw_error):+.2f} deg\n'
+            f'{self.log_separator}'
+        )
+
+    def _run_refine_step(self):
+        pose = self._lookup_pose()
+        if pose is None:
+            self._finish_refine('failed_tf', warn=True)
+            return
+
+        distance, yaw_error = self._calculate_error(pose, self.goal)
+        elapsed = (self.get_clock().now() - self.start_time).nanoseconds / 1e9
+        yaw_tolerance = max(self._param('yaw_tolerance'), 0.0)
+        settle_time = max(self._param('settle_time'), 0.0)
+        timeout = self._param('timeout')
+
+        if timeout > 0.0 and elapsed > timeout:
+            self._finish_refine('timeout', pose, distance, yaw_error, warn=True)
+            return
+
+        if abs(yaw_error) <= yaw_tolerance:
+            now = self.get_clock().now()
+            if self.settle_start_time is None:
+                self.settle_start_time = now
+                self._publish_stop()
+            elif (now - self.settle_start_time).nanoseconds / 1e9 >= settle_time:
+                self._finish_refine('succeeded', pose, distance, yaw_error)
+                return
+            else:
+                self._publish_stop()
+            self._log_progress(pose, distance, yaw_error, elapsed, Twist())
+            return
+
+        self.settle_start_time = None
+        cmd = self._make_yaw_command(yaw_error)
+        self.cmd_vel_pub.publish(cmd)
+        self._log_progress(pose, distance, yaw_error, elapsed, cmd)
+
+    def _make_yaw_command(self, yaw_error):
+        cmd = Twist()
+        max_wz = max(abs(self._param('max_wz')), 0.0)
+        cmd.angular.z = self._clip(self._param('k_yaw') * yaw_error, -max_wz, max_wz)
+        cmd.angular.z = self._apply_min_abs(cmd.angular.z, self._param('min_cmd_w'))
+        return cmd
+
+    def _finish_refine(self, status, pose=None, distance=None, yaw_error=None, warn=False):
+        self._stop_robot()
+        self._reset_start_refine()
+        self._reset_cancel_refine()
+        self.state = 'idle'
+        self.settle_start_time = None
+        self._publish_status(status)
+
+        if pose is not None and distance is not None and yaw_error is not None:
+            msg = (
+                f'\n{self.log_separator}\n'
+                f'FINAL YAW REFINE END: {status}\n'
+                f'distance={distance:.4f} m, '
+                f'yaw_error={math.degrees(yaw_error):+.2f} deg, '
+                f'pose=({pose[0]:.4f}, {pose[1]:.4f}, {math.degrees(pose[2]):.2f} deg)\n'
+                f'{self.log_separator}'
+            )
+        else:
+            msg = (
+                f'\n{self.log_separator}\n'
+                f'FINAL YAW REFINE END: {status}\n'
+                f'{self.log_separator}'
+            )
+
+        if warn:
+            self.get_logger().warn(msg)
+        else:
+            self.get_logger().info(msg)
+
+    def _lookup_pose(self):
+        try:
+            trans = self.tf_buffer.lookup_transform(
+                self.global_frame,
+                self.base_frame,
+                rclpy.time.Time(),
+                timeout=Duration(seconds=0.3),
+            )
+        except TransformException as exc:
+            self.get_logger().warn(f'TF lookup failed: {exc}')
+            return None
+
+        translation = trans.transform.translation
+        rotation = trans.transform.rotation
+        return (
+            translation.x,
+            translation.y,
+            self._yaw_from_quaternion(rotation.x, rotation.y, rotation.z, rotation.w),
+        )
+
+    def _calculate_error(self, pose, target):
+        x, y, yaw = pose
+        target_x, target_y, target_yaw = target
+        distance = math.hypot(target_x - x, target_y - y)
+        yaw_error = self._normalize_angle(target_yaw - yaw)
+        return distance, yaw_error
+
+    def _log_progress(self, pose, distance, yaw_error, elapsed, cmd):
+        now = self.get_clock().now()
+        if (now - self.last_log_time).nanoseconds < 1e9:
+            return
+
+        self.get_logger().info(
+            f'[FINAL YAW REFINE RUNNING] '
+            f'distance={distance:.3f} m, yaw_error={math.degrees(yaw_error):+.2f} deg, '
+            f'elapsed={elapsed:.1f} s, cmd_wz={cmd.angular.z:+.3f}, '
+            f'pose=({pose[0]:.3f}, {pose[1]:.3f}, {math.degrees(pose[2]):.1f} deg)'
+        )
+        self.last_log_time = now
+
+    def _publish_status(self, status):
+        msg = String()
+        msg.data = status
+        self.status_pub.publish(msg)
+
+    def _publish_stop(self):
+        try:
+            self.cmd_vel_pub.publish(Twist())
+        except Exception:
+            pass
+
+    def _stop_robot(self):
+        for _ in range(5):
+            self._publish_stop()
+
+    def _stop_robot_with_ros_cli(self):
+        topic = shlex.quote(self.cmd_vel_topic)
+        zero_twist = (
+            '"{linear: {x: 0.0, y: 0.0, z: 0.0}, '
+            'angular: {x: 0.0, y: 0.0, z: 0.0}}"'
+        )
+        os.system(
+            f'timeout 2s ros2 topic pub --once {topic} '
+            f'geometry_msgs/msg/Twist {zero_twist} >/dev/null 2>&1'
+        )
+
+    def _reset_start_refine(self):
+        self.set_parameters([
+            Parameter(self.start_param, Parameter.Type.BOOL, False),
+        ])
+
+    def _reset_cancel_refine(self):
+        self.set_parameters([
+            Parameter(self.cancel_param, Parameter.Type.BOOL, False),
+        ])
+
+    @staticmethod
+    def _clip(value, low, high):
+        return max(low, min(high, value))
+
+    @staticmethod
+    def _apply_min_abs(value, min_abs):
+        min_abs = max(abs(min_abs), 0.0)
+        if value == 0.0 or abs(value) >= min_abs:
+            return value
+        return math.copysign(min_abs, value)
+
+    @staticmethod
+    def _normalize_angle(angle):
+        return math.atan2(math.sin(angle), math.cos(angle))
+
+    @staticmethod
+    def _yaw_from_quaternion(x, y, z, w):
+        siny_cosp = 2.0 * (w * z + x * y)
+        cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
+        return math.atan2(siny_cosp, cosy_cosp)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = FinalPoseRefiner()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node._stop_robot()
+        node._stop_robot_with_ros_cli()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

agv_pro_calibration/agv_pro_calibration/navigate_to_pose_refiner_proxy.py

@@ -0,0 +1,397 @@
+#!/usr/bin/env python3
+"""Transparent final-refinement proxy for Nav2 pose navigation actions."""
+
+import threading
+import time
+from copy import deepcopy
+
+import rclpy
+from action_msgs.msg import GoalStatus
+from geometry_msgs.msg import PoseStamped
+from nav2_msgs.action import NavigateThroughPoses, NavigateToPose
+from rcl_interfaces.msg import Parameter as ParameterMsg
+from rcl_interfaces.msg import ParameterType, ParameterValue
+from rcl_interfaces.srv import SetParameters
+from rclpy.action import ActionClient, ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from std_msgs.msg import String
+
+
+class NavigateToPoseRefinerProxy(Node):
+    """Forward pose-navigation actions and complete them after final yaw refinement."""
+
+    TERMINAL_REFINER_STATUSES = {
+        'succeeded',
+        'timeout',
+        'failed_tf',
+        'no_goal',
+        'handoff_distance_exceeded',
+        'canceled',
+    }
+    STARTLESS_REFINER_FAILURES = {
+        'failed_tf',
+        'no_goal',
+        'handoff_distance_exceeded',
+    }
+
+    def __init__(self):
+        super().__init__('navigate_to_pose_refiner_proxy')
+        self.public_goal_topic = '/goal_pose'
+        self.refiner_goal_topic = '/final_pose_refiner/goal_pose'
+        self.refiner_status_topic = '/final_pose_refiner/status'
+        self.refiner_param_service = '/final_pose_refiner/set_parameters'
+        self.start_param = 'final_pose_refiner_start'
+        self.cancel_param = 'final_pose_refiner_cancel'
+
+        self.declare_parameter('nav2_server_timeout_sec', 5.0)
+        self.declare_parameter('refiner_service_timeout_sec', 2.0)
+        self.declare_parameter('refiner_wait_timeout_sec', 25.0)
+        self.declare_parameter('require_refinement', True)
+        self.declare_parameter('debug_print', False)
+
+        self.callback_group = ReentrantCallbackGroup()
+        self.goal_pub = self.create_publisher(PoseStamped, self.refiner_goal_topic, 10)
+        self.refiner_status_sub = self.create_subscription(
+            String,
+            self.refiner_status_topic,
+            self._on_refiner_status,
+            10,
+            callback_group=self.callback_group,
+        )
+        self.refiner_param_client = self.create_client(
+            SetParameters,
+            self.refiner_param_service,
+            callback_group=self.callback_group,
+        )
+
+        self._refinement_lock = threading.Lock()
+        self._status_condition = threading.Condition()
+        self._status_sequence = 0
+        self._status_history = []
+        self.routes = []
+        self._add_route(
+            'NavigateToPose',
+            NavigateToPose,
+            '/navigate_to_pose',
+            '/navigate_to_pose_nav2',
+            lambda request: request.pose,
+        )
+        self._add_route(
+            'NavigateThroughPoses',
+            NavigateThroughPoses,
+            '/navigate_through_poses',
+            '/navigate_through_poses_nav2',
+            lambda request: request.poses[-1] if request.poses else None,
+        )
+        self.topic_nav_client = ActionClient(
+            self,
+            NavigateToPose,
+            '/navigate_to_pose',
+            callback_group=self.callback_group,
+        )
+        self.goal_topic_sub = self.create_subscription(
+            PoseStamped,
+            self.public_goal_topic,
+            self._on_goal_pose,
+            10,
+            callback_group=self.callback_group,
+        )
+
+        self.get_logger().info(
+            'Navigation refinement proxy ready for NavigateToPose, NavigateThroughPoses, '
+            'and /goal_pose; public tasks complete after final yaw refinement.'
+        )
+
+    def _add_route(self, label, action_type, public_name, nav2_name, final_pose_getter):
+        route = {
+            'label': label,
+            'action_type': action_type,
+            'public_name': public_name,
+            'nav2_name': nav2_name,
+            'final_pose_getter': final_pose_getter,
+        }
+        route['client'] = ActionClient(
+            self,
+            action_type,
+            nav2_name,
+            callback_group=self.callback_group,
+        )
+        route['server'] = ActionServer(
+            self,
+            action_type,
+            public_name,
+            execute_callback=lambda handle, current=route: self._execute_callback(current, handle),
+            goal_callback=lambda request, current=route: self._goal_callback(current, request),
+            cancel_callback=self._cancel_callback,
+            callback_group=self.callback_group,
+        )
+        self.routes.append(route)
+        self._debug(f'{label} route: {public_name} -> {nav2_name}')
+
+    def _goal_callback(self, route, goal_request):
+        if not route['client'].server_is_ready():
+            self.get_logger().warn(
+                f"{route['label']} Nav2 action server {route['nav2_name']} is not ready; "
+                'rejecting goal.'
+            )
+            return GoalResponse.REJECT
+
+        final_pose = route['final_pose_getter'](goal_request)
+        if final_pose is not None:
+            self._publish_refiner_goal(final_pose)
+        return GoalResponse.ACCEPT
+
+    @staticmethod
+    def _cancel_callback(_goal_handle):
+        return CancelResponse.ACCEPT
+
+    def _on_goal_pose(self, pose):
+        goal = NavigateToPose.Goal()
+        goal.pose = deepcopy(pose)
+        if not self.topic_nav_client.server_is_ready():
+            self.get_logger().error(
+                'Cannot forward /goal_pose: public NavigateToPose is unavailable.'
+            )
+            return
+
+        send_future = self.topic_nav_client.send_goal_async(goal)
+        send_future.add_done_callback(self._on_topic_goal_response)
+
+    def _on_topic_goal_response(self, future):
+        try:
+            goal_handle = future.result()
+        except Exception as exc:
+            self.get_logger().error(f'Failed to forward /goal_pose to NavigateToPose: {exc}')
+            return
+
+        if goal_handle is None or not goal_handle.accepted:
+            self.get_logger().error('/goal_pose navigation goal was rejected.')
+            return
+
+        result_future = goal_handle.get_result_async()
+        result_future.add_done_callback(self._on_topic_goal_result)
+
+    def _on_topic_goal_result(self, future):
+        try:
+            action_result = future.result()
+        except Exception as exc:
+            self.get_logger().error(f'Failed to receive /goal_pose navigation result: {exc}')
+            return
+
+        if action_result.status != GoalStatus.STATUS_SUCCEEDED:
+            self.get_logger().warn(
+                f'/goal_pose navigation ended with action status {action_result.status}.'
+            )
+
+    def _execute_callback(self, route, goal_handle):
+        nav_result = self._forward_to_nav2(route, goal_handle)
+        if nav_result is None:
+            return route['action_type'].Result()
+
+        result, status = nav_result
+        if status == GoalStatus.STATUS_CANCELED:
+            goal_handle.canceled()
+            return result
+        if status != GoalStatus.STATUS_SUCCEEDED:
+            goal_handle.abort()
+            return result
+
+        final_pose = route['final_pose_getter'](goal_handle.request)
+        refine_status = 'succeeded'
+        if final_pose is not None:
+            refine_status = self._refine_final_pose(goal_handle, final_pose)
+
+        if refine_status == 'succeeded':
+            goal_handle.succeed()
+        elif refine_status == 'canceled':
+            goal_handle.canceled()
+        else:
+            self.get_logger().error(
+                f"{route['label']} completed in Nav2 but final refinement ended with "
+                f"status '{refine_status}'."
+            )
+            goal_handle.abort()
+        return result
+
+    def _forward_to_nav2(self, route, goal_handle):
+        timeout = float(self.get_parameter('nav2_server_timeout_sec').value)
+        if not route['client'].wait_for_server(timeout_sec=timeout):
+            self.get_logger().error(f"Nav2 action server {route['nav2_name']} is not available.")
+            goal_handle.abort()
+            return None
+
+        send_future = route['client'].send_goal_async(
+            deepcopy(goal_handle.request),
+            feedback_callback=lambda message: self._relay_feedback(goal_handle, message),
+        )
+        if not self._wait_for_future(send_future, timeout):
+            self.get_logger().error(f"Timed out forwarding {route['label']} goal to Nav2.")
+            goal_handle.abort()
+            return None
+
+        try:
+            nav_goal_handle = send_future.result()
+        except Exception as exc:
+            self.get_logger().error(f"Failed to forward {route['label']} goal to Nav2: {exc}")
+            goal_handle.abort()
+            return None
+
+        if nav_goal_handle is None or not nav_goal_handle.accepted:
+            self.get_logger().error(f"Forwarded {route['label']} goal was rejected by Nav2.")
+            goal_handle.abort()
+            return None
+
+        result_future = nav_goal_handle.get_result_async()
+        while rclpy.ok() and not result_future.done():
+            if goal_handle.is_cancel_requested:
+                self._cancel_nav_goal(nav_goal_handle)
+                goal_handle.canceled()
+                return None
+            time.sleep(0.05)
+
+        if not result_future.done():
+            goal_handle.abort()
+            return None
+
+        try:
+            nav_result = result_future.result()
+        except Exception as exc:
+            self.get_logger().error(f"Failed to get Nav2 {route['label']} result: {exc}")
+            goal_handle.abort()
+            return None
+
+        result = (
+            nav_result.result
+            if nav_result and nav_result.result
+            else route['action_type'].Result()
+        )
+        return result, nav_result.status
+
+    def _refine_final_pose(self, goal_handle, pose):
+        if not self.get_parameter('require_refinement').value:
+            return 'succeeded'
+
+        with self._refinement_lock:
+            if goal_handle.is_cancel_requested:
+                return 'canceled'
+
+            self._publish_refiner_goal(pose)
+            start_sequence = self._status_snapshot()
+            if not self._set_refiner_parameter(self.start_param, True):
+                return 'unavailable'
+
+            wait_timeout = float(self.get_parameter('refiner_wait_timeout_sec').value)
+            deadline = time.monotonic() + max(wait_timeout, 0.0)
+            saw_running = False
+            sequence = start_sequence
+            while rclpy.ok():
+                if goal_handle.is_cancel_requested:
+                    self._set_refiner_parameter(self.cancel_param, True)
+                    return 'canceled'
+
+                updates = self._wait_for_status_updates(sequence, deadline)
+                if updates is None:
+                    self.get_logger().error('Timed out waiting for final pose refinement result.')
+                    self._set_refiner_parameter(self.cancel_param, True)
+                    return 'timeout'
+
+                for sequence, status in updates:
+                    if status == 'running':
+                        saw_running = True
+                    elif status in self.TERMINAL_REFINER_STATUSES:
+                        if saw_running or status in self.STARTLESS_REFINER_FAILURES:
+                            return status
+            return 'canceled'
+
+    def _set_refiner_parameter(self, name, value):
+        timeout = float(self.get_parameter('refiner_service_timeout_sec').value)
+        if not self.refiner_param_client.wait_for_service(timeout_sec=timeout):
+            self.get_logger().error(
+                f'Final pose refiner parameter service {self.refiner_param_service} '
+                'is unavailable.'
+            )
+            return False
+
+        parameter = ParameterMsg()
+        parameter.name = name
+        parameter.value = ParameterValue(type=ParameterType.PARAMETER_BOOL, bool_value=value)
+        request = SetParameters.Request()
+        request.parameters = [parameter]
+        future = self.refiner_param_client.call_async(request)
+        if not self._wait_for_future(future, timeout):
+            self.get_logger().error(f'Timed out setting final pose refiner parameter {name}.')
+            return False
+
+        response = future.result()
+        if response is None or not response.results or not response.results[0].successful:
+            reason = response.results[0].reason if response and response.results else ''
+            self.get_logger().error(f'Failed to set final pose refiner parameter {name}: {reason}')
+            return False
+        return True
+
+    def _on_refiner_status(self, message):
+        with self._status_condition:
+            self._status_sequence += 1
+            self._status_history.append((self._status_sequence, message.data))
+            self._status_history = self._status_history[-32:]
+            self._status_condition.notify_all()
+
+    def _status_snapshot(self):
+        with self._status_condition:
+            return self._status_sequence
+
+    def _wait_for_status_updates(self, sequence, deadline):
+        with self._status_condition:
+            while rclpy.ok():
+                updates = [item for item in self._status_history if item[0] > sequence]
+                if updates:
+                    return updates
+                remaining = deadline - time.monotonic()
+                if remaining <= 0.0:
+                    return None
+                self._status_condition.wait(timeout=min(remaining, 0.1))
+        return None
+
+    def _publish_refiner_goal(self, pose):
+        refiner_goal = deepcopy(pose)
+        refiner_goal.header.stamp = self.get_clock().now().to_msg()
+        self.goal_pub.publish(refiner_goal)
+
+    @staticmethod
+    def _relay_feedback(goal_handle, feedback_message):
+        if goal_handle.is_active:
+            goal_handle.publish_feedback(feedback_message.feedback)
+
+    def _debug(self, message):
+        if self.get_parameter('debug_print').value:
+            self.get_logger().info(message)
+
+    @staticmethod
+    def _wait_for_future(future, timeout_sec):
+        done = threading.Event()
+        future.add_done_callback(lambda _: done.set())
+        return done.wait(timeout_sec)
+
+    def _cancel_nav_goal(self, nav_goal_handle):
+        cancel_future = nav_goal_handle.cancel_goal_async()
+        self._wait_for_future(cancel_future, 2.0)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = NavigateToPoseRefinerProxy()
+    executor = MultiThreadedExecutor(num_threads=6)
+    try:
+        rclpy.spin(node, executor=executor)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

agv_pro_calibration/agv_pro_calibration/odom_linear_calib.py

@@ -0,0 +1,469 @@
+#!/usr/bin/env python3
+"""X-axis odometry scale calibration helper for AGV Pro."""
+
+import math
+import os
+import shlex
+import statistics
+import sys
+import threading
+
+import rclpy
+from geometry_msgs.msg import Twist
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from tf2_ros import Buffer, TransformException, TransformListener
+
+CONTROL_RATE_HZ = 20.0
+MAX_SPEED_LIMIT = 0.30
+TF_TIMEOUT_SEC = 0.5
+STOP_REPEAT_COUNT = 5
+
+
+class OdomLinearCalib(Node):
+    """Run repeated X-axis odom tests and compute the final scale from cached samples."""
+
+    def __init__(self):
+        super().__init__('odom_linear_calib')
+
+        self.declare_parameter('cmd_vel_topic', '/cmd_vel')
+        self.declare_parameter('odom_frame', 'odom')
+        self.declare_parameter('base_frame', 'base_footprint')
+        self.declare_parameter('start_test', False)
+        self.declare_parameter('test_distance', 1.0)
+        self.declare_parameter('speed', 0.10)
+        self.declare_parameter('tolerance', 0.01)
+        self.declare_parameter('odom_linear_scale_correction', 1.0)
+        self.declare_parameter('timeout', 30.0)
+
+        self.cmd_vel_topic = self.get_parameter('cmd_vel_topic').value
+        self.cmd_vel_pub = self.create_publisher(Twist, self.cmd_vel_topic, 10)
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        self.state = 'idle'
+        self.start_pose = None
+        self.direction_sign = 1.0
+        self.signed_target_distance = 1.0
+        self.target_distance = 1.0
+        self.command_speed = 0.10
+        self.tolerance = 0.01
+        self.odom_linear_scale_correction = 1.0
+        self.timeout = 30.0
+        self.start_time = None
+        self.last_odom_distance = 0.0
+        self.last_log_time = self.get_clock().now()
+
+        self.samples = []
+        self.samples_lock = threading.Lock()
+        self.pending_sample = None
+
+        self.timer = self.create_timer(1.0 / CONTROL_RATE_HZ, self.on_timer)
+        threading.Thread(target=self._stdin_loop, daemon=True).start()
+
+        self.get_logger().info(
+            'odom_linear_calib ready. Set params, set start_test:=true for each run, '
+            'use positive test_distance for forward and negative for backward, '
+            'then enter the measured ground error in cm after the robot stops. '
+            'Enter 0 to finish and print the cached scale summary; enter any text to skip a verification run.'
+        )
+
+    def _stdin_loop(self):
+        while True:
+            line = sys.stdin.readline()
+            if line == '':
+                return
+            self._handle_input_line(line.strip())
+
+    def _handle_input_line(self, text):
+        if not text:
+            self.get_logger().info(
+                'Input ignored. After a successful run enter ground error in cm '
+                '(+over target along motion direction, -short), or enter 0 to finish.'
+            )
+            return
+
+        try:
+            value = float(text)
+        except ValueError:
+            self._skip_pending_sample(text)
+            return
+
+        if value == 0.0 and not text.startswith(('+', '-')):
+            with self.samples_lock:
+                had_pending_sample = self.pending_sample is not None
+                self.pending_sample = None
+                if self.state == 'awaiting_input':
+                    self.state = 'idle'
+            if had_pending_sample:
+                self.get_logger().warn('Pending run was not recorded because finish input 0 was entered.')
+            self._print_summary()
+            return
+
+        self._record_pending_sample(value)
+
+    def on_timer(self):
+        if self.state == 'running':
+            self._run_test_step()
+            return
+
+        if self.state == 'awaiting_input':
+            if self.get_parameter('start_test').value:
+                self.get_logger().warn(
+                    'A finished run is waiting for ground-error input; record it before starting again.'
+                )
+                self._reset_start_test()
+            return
+
+        if self.get_parameter('start_test').value:
+            self._start_test()
+
+    def _start_test(self):
+        config = self._read_test_config()
+        if config is None:
+            self._reset_start_test()
+            return
+
+        pose = self._lookup_pose()
+        if pose is None:
+            self.get_logger().warn('Cannot start test: odom transform is not available.')
+            self._reset_start_test()
+            return
+
+        self.signed_target_distance = config['signed_test_distance']
+        self.target_distance = config['target_distance']
+        self.command_speed = config['speed']
+        self.tolerance = config['tolerance']
+        self.odom_linear_scale_correction = config['odom_linear_scale_correction']
+        self.timeout = config['timeout']
+        self.direction_sign = float(config['direction_sign'])
+        self.start_pose = pose
+        self.start_time = self.get_clock().now()
+        self.last_odom_distance = 0.0
+        self.state = 'running'
+
+        self.get_logger().info(
+            f'Start X odom calibration: direction={int(self.direction_sign)}, '
+            f'signed_target={self.signed_target_distance:.3f} m, '
+            f'target={self.target_distance:.3f} m, speed={self.command_speed:.3f} m/s, '
+            f'odom_linear_scale_correction={self.odom_linear_scale_correction:.6f}'
+        )
+
+    def _run_test_step(self):
+        pose = self._lookup_pose()
+        if pose is None:
+            self._finish_test('failed_tf', publish_warning=True)
+            return
+
+        raw_progress, lateral_drift = self._calculate_progress(pose)
+        corrected_progress = raw_progress * self.odom_linear_scale_correction
+        error = corrected_progress - self.target_distance
+        elapsed = (self.get_clock().now() - self.start_time).nanoseconds / 1e9
+        self.last_odom_distance = raw_progress
+
+        if corrected_progress >= self.target_distance - self.tolerance:
+            self._finish_test('succeeded', raw_progress, corrected_progress, lateral_drift, elapsed)
+            return
+
+        if elapsed > self.timeout:
+            self._finish_test('timeout', raw_progress, corrected_progress, lateral_drift, elapsed)
+            return
+
+        cmd = Twist()
+        cmd.linear.x = self.direction_sign * self.command_speed
+        self.cmd_vel_pub.publish(cmd)
+        self._log_progress(raw_progress, corrected_progress, error, lateral_drift, elapsed)
+
+    def _finish_test(
+        self,
+        status,
+        odom_distance=None,
+        corrected_distance=None,
+        lateral_drift=None,
+        elapsed=None,
+        publish_warning=False,
+    ):
+        self._stop_robot()
+        self._reset_start_test()
+
+        if odom_distance is None:
+            odom_distance = self.last_odom_distance
+        if corrected_distance is None:
+            corrected_distance = odom_distance * self.odom_linear_scale_correction
+        if lateral_drift is None:
+            lateral_drift = 0.0
+        if elapsed is None and self.start_time is not None:
+            elapsed = (self.get_clock().now() - self.start_time).nanoseconds / 1e9
+        if elapsed is None:
+            elapsed = 0.0
+
+        if status == 'succeeded' and odom_distance > 0.0:
+            pending_sample = {
+                'direction': int(self.direction_sign),
+                'signed_target_distance': self.signed_target_distance,
+                'target_distance': self.target_distance,
+                'odom_distance': odom_distance,
+                'corrected_distance': corrected_distance,
+                'lateral_drift': lateral_drift,
+                'elapsed': elapsed,
+                'used_correction': self.odom_linear_scale_correction,
+            }
+            with self.samples_lock:
+                self.pending_sample = pending_sample
+                self.state = 'awaiting_input'
+            self.get_logger().info(
+                'Run is waiting for measured ground error. '
+                'Enter cm error now: +over target along motion direction, -short of target, '
+                '+0/-0 for exact target, 0 to finish, or any text to skip this run.'
+            )
+        else:
+            self.state = 'idle'
+
+        msg = (
+            f'Calibration {status}: odom_distance={odom_distance:.4f} m, '
+            f'corrected_distance={corrected_distance:.4f} m, '
+            f'lateral_drift={lateral_drift:.4f} m, elapsed={elapsed:.2f} s, '
+            f'target={self.target_distance:.4f} m, '
+            f'used_correction={self.odom_linear_scale_correction:.6f}.'
+        )
+        if publish_warning:
+            self.get_logger().warn(msg)
+        else:
+            self.get_logger().info(msg)
+
+    def _read_test_config(self):
+        test_distance = self.get_parameter('test_distance').value
+        speed = abs(self.get_parameter('speed').value)
+        tolerance = max(self.get_parameter('tolerance').value, 0.0)
+        correction = self.get_parameter('odom_linear_scale_correction').value
+        timeout = self.get_parameter('timeout').value
+
+        if test_distance == 0.0:
+            self.get_logger().error(
+                'test_distance must not be 0.0 m. Use a positive value for forward, negative for backward.'
+            )
+            return None
+        if speed <= 0.0:
+            self.get_logger().error('speed must be greater than 0.0 m/s.')
+            return None
+        if timeout <= 0.0:
+            self.get_logger().error('timeout must be greater than 0.0 s.')
+            return None
+        if correction <= 0.0:
+            self.get_logger().error('odom_linear_scale_correction must be greater than 0.0.')
+            return None
+        if speed > MAX_SPEED_LIMIT:
+            self.get_logger().warn(
+                f'speed {speed:.3f} m/s exceeds internal safety limit '
+                f'{MAX_SPEED_LIMIT:.3f} m/s; clipping command speed.'
+            )
+            speed = MAX_SPEED_LIMIT
+
+        direction_sign = 1 if test_distance > 0.0 else -1
+        return {
+            'direction_sign': direction_sign,
+            'signed_test_distance': test_distance,
+            'target_distance': abs(test_distance),
+            'speed': speed,
+            'tolerance': tolerance,
+            'odom_linear_scale_correction': correction,
+            'timeout': timeout,
+        }
+
+    def _lookup_pose(self):
+        odom_frame = self.get_parameter('odom_frame').value
+        base_frame = self.get_parameter('base_frame').value
+        try:
+            trans = self.tf_buffer.lookup_transform(
+                odom_frame,
+                base_frame,
+                rclpy.time.Time(),
+                timeout=Duration(seconds=TF_TIMEOUT_SEC),
+            )
+        except TransformException as exc:
+            self.get_logger().warn(f'TF lookup failed: {exc}')
+            return None
+
+        translation = trans.transform.translation
+        rotation = trans.transform.rotation
+        return (
+            translation.x,
+            translation.y,
+            self._yaw_from_quaternion(rotation.x, rotation.y, rotation.z, rotation.w),
+        )
+
+    def _calculate_progress(self, pose):
+        x, y, _ = pose
+        start_x, start_y, start_yaw = self.start_pose
+        dx = x - start_x
+        dy = y - start_y
+        cos_yaw = math.cos(start_yaw)
+        sin_yaw = math.sin(start_yaw)
+
+        forward_delta = dx * cos_yaw + dy * sin_yaw
+        lateral_drift = -dx * sin_yaw + dy * cos_yaw
+        progress = self.direction_sign * forward_delta
+        return progress, lateral_drift
+
+    def _log_progress(self, raw_progress, corrected_progress, error, lateral_drift, elapsed):
+        now = self.get_clock().now()
+        if (now - self.last_log_time).nanoseconds < 1e9:
+            return
+        self.get_logger().info(
+            f'odom_distance={raw_progress:.3f} m, '
+            f'corrected_distance={corrected_progress:.3f} m, '
+            f'error={error:+.3f} m, lateral_drift={lateral_drift:.3f} m, '
+            f'elapsed={elapsed:.1f} s'
+        )
+        self.last_log_time = now
+
+    def _skip_pending_sample(self, reason):
+        with self.samples_lock:
+            if self.pending_sample is None:
+                self.get_logger().warn(
+                    f'Input "{reason}" ignored. No pending successful run is waiting for input.'
+                )
+                return
+
+            skipped_sample = self.pending_sample
+            self.pending_sample = None
+            self.state = 'idle'
+
+        self.get_logger().info(
+            f'Skipped pending run by input "{reason}": '
+            f'direction={skipped_sample["direction"]:+d}, '
+            f'signed_target={skipped_sample["signed_target_distance"]:.4f} m, '
+            f'odom={skipped_sample["odom_distance"]:.4f} m, '
+            f'corrected={skipped_sample["corrected_distance"]:.4f} m, '
+            f'used_correction={skipped_sample["used_correction"]:.6f}. '
+            'This run will not be used in the final scale summary.'
+        )
+
+    def _record_pending_sample(self, ground_error_cm):
+        with self.samples_lock:
+            if self.pending_sample is None:
+                self.get_logger().warn(
+                    'No pending successful run. Set start_test:=true first, wait for the robot to stop, '
+                    'then enter the measured cm error.'
+                )
+                return
+
+            actual_distance = self.pending_sample['target_distance'] + ground_error_cm / 100.0
+            if actual_distance <= 0.0:
+                self.get_logger().error(
+                    f'Invalid measured result: target + error = {actual_distance:.4f} m. '
+                    'Re-enter the cm error for this pending run.'
+                )
+                return
+
+            sample = dict(self.pending_sample)
+            sample['ground_error_cm'] = ground_error_cm
+            sample['actual_distance'] = actual_distance
+            sample['scale'] = actual_distance / sample['odom_distance']
+            self.samples.append(sample)
+            sample_index = len(self.samples)
+            direction_index = sum(
+                1 for recorded_sample in self.samples
+                if recorded_sample['direction'] == sample['direction']
+            )
+            self.pending_sample = None
+            self.state = 'idle'
+
+        self.get_logger().info(
+            f'Recorded sample #{sample_index} overall, direction {sample["direction"]:+d} #{direction_index}: '
+            f'actual={actual_distance:.4f} m, '
+            f'ground_error={ground_error_cm:+.2f} cm, odom={sample["odom_distance"]:.4f} m, '
+            f'scale={sample["scale"]:.6f}. Set start_test:=true for the next run, or enter 0 to finish.'
+        )
+
+    def _print_summary(self):
+        with self.samples_lock:
+            samples = list(self.samples)
+
+        if not samples:
+            self.get_logger().warn('No successful calibration samples have been recorded yet.')
+            return
+
+        self.get_logger().info('========== X ODOM SCALE SUMMARY ==========')
+        for index, sample in enumerate(samples, start=1):
+            self.get_logger().info(
+                f'#{index:02d} direction={sample["direction"]:+d}, '
+                f'signed_target={sample["signed_target_distance"]:.4f} m, '
+                f'target={sample["target_distance"]:.4f} m, '
+                f'actual={sample["actual_distance"]:.4f} m, '
+                f'ground_error={sample["ground_error_cm"]:+.2f} cm, '
+                f'odom={sample["odom_distance"]:.4f} m, '
+                f'corrected={sample["corrected_distance"]:.4f} m, '
+                f'lateral_drift={sample["lateral_drift"]:.4f} m, '
+                f'used_correction={sample["used_correction"]:.6f}, '
+                f'scale={sample["scale"]:.6f}'
+            )
+
+        self._print_scale_stats('all', samples)
+        for direction in (1, -1):
+            direction_samples = [sample for sample in samples if sample['direction'] == direction]
+            if direction_samples:
+                self._print_scale_stats(f'direction={direction:+d}', direction_samples)
+        self.get_logger().info('Restart this node to clear cached samples.')
+
+    def _print_scale_stats(self, label, samples):
+        scales = [sample['scale'] for sample in samples]
+        mean_scale = statistics.fmean(scales)
+        std_scale = statistics.pstdev(scales) if len(scales) > 1 else 0.0
+        self.get_logger().info(
+            f'{label}: samples={len(scales)}, recommended_odometry.scale_x={mean_scale:.6f}, '
+            f'std={std_scale:.6f}, min={min(scales):.6f}, max={max(scales):.6f}'
+        )
+
+    def _publish_stop(self):
+        try:
+            self.cmd_vel_pub.publish(Twist())
+        except Exception:
+            pass
+
+    def _stop_robot(self):
+        for _ in range(STOP_REPEAT_COUNT):
+            self._publish_stop()
+
+    def _stop_robot_with_ros_cli(self):
+        topic = shlex.quote(self.cmd_vel_topic)
+        zero_twist = (
+            '"{linear: {x: 0.0, y: 0.0, z: 0.0}, '
+            'angular: {x: 0.0, y: 0.0, z: 0.0}}"'
+        )
+        os.system(
+            f'timeout 2s ros2 topic pub --once {topic} '
+            f'geometry_msgs/msg/Twist {zero_twist} >/dev/null 2>&1'
+        )
+
+    def _reset_start_test(self):
+        self.set_parameters([
+            Parameter('start_test', Parameter.Type.BOOL, False),
+        ])
+
+    @staticmethod
+    def _yaw_from_quaternion(x, y, z, w):
+        siny_cosp = 2.0 * (w * z + x * y)
+        cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
+        return math.atan2(siny_cosp, cosy_cosp)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OdomLinearCalib()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node._stop_robot()
+        node._stop_robot_with_ros_cli()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

agv_pro_calibration/agv_pro_calibration/odom_yaw_calib.py

@@ -0,0 +1,466 @@
+#!/usr/bin/env python3
+"""Yaw odometry scale calibration helper for AGV Pro."""
+
+import math
+import os
+import shlex
+import statistics
+import sys
+import threading
+
+import rclpy
+from geometry_msgs.msg import Twist
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from tf2_ros import Buffer, TransformException, TransformListener
+
+CONTROL_RATE_HZ = 20.0
+MAX_ANGULAR_SPEED_LIMIT = 0.50
+TF_TIMEOUT_SEC = 0.5
+STOP_REPEAT_COUNT = 5
+
+
+class OdomYawCalib(Node):
+    """Run repeated yaw odom tests and compute the final scale from cached samples."""
+
+    def __init__(self):
+        super().__init__('odom_yaw_calib')
+
+        self.declare_parameter('cmd_vel_topic', '/cmd_vel')
+        self.declare_parameter('odom_frame', 'odom')
+        self.declare_parameter('base_frame', 'base_footprint')
+        self.declare_parameter('start_test', False)
+        self.declare_parameter('test_angle', 360.0)
+        self.declare_parameter('speed', 0.20)
+        self.declare_parameter('tolerance', 2.0)
+        self.declare_parameter('odom_yaw_scale_correction', 1.0)
+        self.declare_parameter('timeout', 60.0)
+
+        self.cmd_vel_topic = self.get_parameter('cmd_vel_topic').value
+        self.cmd_vel_pub = self.create_publisher(Twist, self.cmd_vel_topic, 10)
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        self.state = 'idle'
+        self.direction_sign = 1.0
+        self.signed_target_angle_deg = 360.0
+        self.target_angle_deg = 360.0
+        self.target_angle = math.radians(360.0)
+        self.command_speed = 0.20
+        self.tolerance_deg = 2.0
+        self.tolerance = math.radians(2.0)
+        self.odom_yaw_scale_correction = 1.0
+        self.timeout = 60.0
+        self.start_time = None
+        self.prev_yaw = None
+        self.accumulated_yaw = 0.0
+        self.last_odom_angle = 0.0
+        self.last_log_time = self.get_clock().now()
+
+        self.samples = []
+        self.samples_lock = threading.Lock()
+        self.pending_sample = None
+
+        self.timer = self.create_timer(1.0 / CONTROL_RATE_HZ, self.on_timer)
+        threading.Thread(target=self._stdin_loop, daemon=True).start()
+
+        self.get_logger().info(
+            'odom_yaw_calib ready. Set params, set start_test:=true for each run, '
+            'use positive test_angle for positive angular.z and negative for negative angular.z, '
+            'then enter the measured ground yaw error in deg after the robot stops. '
+            'Enter 0 to finish and print the cached scale summary; enter any text to skip a verification run.'
+        )
+
+    def _stdin_loop(self):
+        while True:
+            line = sys.stdin.readline()
+            if line == '':
+                return
+            self._handle_input_line(line.strip())
+
+    def _handle_input_line(self, text):
+        if not text:
+            self.get_logger().info(
+                'Input ignored. After a successful run enter ground yaw error in deg '
+                '(+over target along rotation direction, -short), or enter 0 to finish.'
+            )
+            return
+
+        try:
+            value = float(text)
+        except ValueError:
+            self._skip_pending_sample(text)
+            return
+
+        if value == 0.0 and not text.startswith(('+', '-')):
+            with self.samples_lock:
+                had_pending_sample = self.pending_sample is not None
+                self.pending_sample = None
+                if self.state == 'awaiting_input':
+                    self.state = 'idle'
+            if had_pending_sample:
+                self.get_logger().warn('Pending run was not recorded because finish input 0 was entered.')
+            self._print_summary()
+            return
+
+        self._record_pending_sample(value)
+
+    def on_timer(self):
+        if self.state == 'running':
+            self._run_test_step()
+            return
+
+        if self.state == 'awaiting_input':
+            if self.get_parameter('start_test').value:
+                self.get_logger().warn(
+                    'A finished run is waiting for ground-yaw-error input; record it before starting again.'
+                )
+                self._reset_start_test()
+            return
+
+        if self.get_parameter('start_test').value:
+            self._start_test()
+
+    def _start_test(self):
+        config = self._read_test_config()
+        if config is None:
+            self._reset_start_test()
+            return
+
+        pose = self._lookup_pose()
+        if pose is None:
+            self.get_logger().warn('Cannot start test: odom transform is not available.')
+            self._reset_start_test()
+            return
+
+        self.signed_target_angle_deg = config['signed_test_angle']
+        self.target_angle_deg = config['target_angle']
+        self.target_angle = math.radians(config['target_angle'])
+        self.command_speed = config['speed']
+        self.tolerance_deg = config['tolerance']
+        self.tolerance = math.radians(config['tolerance'])
+        self.odom_yaw_scale_correction = config['odom_yaw_scale_correction']
+        self.timeout = config['timeout']
+        self.direction_sign = float(config['direction_sign'])
+        self.prev_yaw = pose[2]
+        self.accumulated_yaw = 0.0
+        self.start_time = self.get_clock().now()
+        self.last_odom_angle = 0.0
+        self.state = 'running'
+
+        self.get_logger().info(
+            f'Start yaw odom calibration: direction={int(self.direction_sign)}, '
+            f'signed_target={self.signed_target_angle_deg:.1f} deg, '
+            f'target={self.target_angle_deg:.1f} deg, speed={self.command_speed:.3f} rad/s, '
+            f'odom_yaw_scale_correction={self.odom_yaw_scale_correction:.6f}'
+        )
+
+    def _run_test_step(self):
+        pose = self._lookup_pose()
+        if pose is None:
+            self._finish_test('failed_tf', publish_warning=True)
+            return
+
+        raw_progress = self._calculate_yaw_progress(pose)
+        raw_angle = max(raw_progress, 0.0)
+        corrected_angle = raw_angle * self.odom_yaw_scale_correction
+        error = corrected_angle - self.target_angle
+        elapsed = (self.get_clock().now() - self.start_time).nanoseconds / 1e9
+        self.last_odom_angle = raw_angle
+
+        if corrected_angle >= self.target_angle - self.tolerance:
+            self._finish_test('succeeded', raw_angle, corrected_angle, elapsed)
+            return
+
+        if elapsed > self.timeout:
+            self._finish_test('timeout', raw_angle, corrected_angle, elapsed)
+            return
+
+        cmd = Twist()
+        cmd.angular.z = self.direction_sign * self.command_speed
+        self.cmd_vel_pub.publish(cmd)
+        self._log_progress(raw_angle, corrected_angle, error, elapsed)
+
+    def _finish_test(
+        self,
+        status,
+        odom_angle=None,
+        corrected_angle=None,
+        elapsed=None,
+        publish_warning=False,
+    ):
+        self._stop_robot()
+        self._reset_start_test()
+
+        if odom_angle is None:
+            odom_angle = self.last_odom_angle
+        if corrected_angle is None:
+            corrected_angle = odom_angle * self.odom_yaw_scale_correction
+        if elapsed is None and self.start_time is not None:
+            elapsed = (self.get_clock().now() - self.start_time).nanoseconds / 1e9
+        if elapsed is None:
+            elapsed = 0.0
+
+        if status == 'succeeded' and odom_angle > 0.0:
+            pending_sample = {
+                'direction': int(self.direction_sign),
+                'signed_target_angle_deg': self.signed_target_angle_deg,
+                'target_angle_deg': self.target_angle_deg,
+                'odom_angle_deg': math.degrees(odom_angle),
+                'corrected_angle_deg': math.degrees(corrected_angle),
+                'elapsed': elapsed,
+                'used_correction': self.odom_yaw_scale_correction,
+            }
+            with self.samples_lock:
+                self.pending_sample = pending_sample
+                self.state = 'awaiting_input'
+            self.get_logger().info(
+                'Run is waiting for measured ground yaw error. '
+                'Enter deg error now: +over target along rotation direction, -short of target, '
+                '+0/-0 for exact target, 0 to finish, or any text to skip this run.'
+            )
+        else:
+            self.state = 'idle'
+
+        msg = (
+            f'Calibration {status}: odom_angle={math.degrees(odom_angle):.2f} deg, '
+            f'corrected_angle={math.degrees(corrected_angle):.2f} deg, '
+            f'elapsed={elapsed:.2f} s, target={self.target_angle_deg:.2f} deg, '
+            f'used_correction={self.odom_yaw_scale_correction:.6f}.'
+        )
+        if publish_warning:
+            self.get_logger().warn(msg)
+        else:
+            self.get_logger().info(msg)
+
+    def _read_test_config(self):
+        test_angle = self.get_parameter('test_angle').value
+        speed = abs(self.get_parameter('speed').value)
+        tolerance = max(self.get_parameter('tolerance').value, 0.0)
+        correction = self.get_parameter('odom_yaw_scale_correction').value
+        timeout = self.get_parameter('timeout').value
+
+        if test_angle == 0.0:
+            self.get_logger().error(
+                'test_angle must not be 0.0 deg. Use a positive value for one yaw direction, '
+                'negative for the opposite direction.'
+            )
+            return None
+        if speed <= 0.0:
+            self.get_logger().error('speed must be greater than 0.0 rad/s.')
+            return None
+        if timeout <= 0.0:
+            self.get_logger().error('timeout must be greater than 0.0 s.')
+            return None
+        if correction <= 0.0:
+            self.get_logger().error('odom_yaw_scale_correction must be greater than 0.0.')
+            return None
+        if speed > MAX_ANGULAR_SPEED_LIMIT:
+            self.get_logger().warn(
+                f'speed {speed:.3f} rad/s exceeds internal safety limit '
+                f'{MAX_ANGULAR_SPEED_LIMIT:.3f} rad/s; clipping command speed.'
+            )
+            speed = MAX_ANGULAR_SPEED_LIMIT
+
+        direction_sign = 1 if test_angle > 0.0 else -1
+        return {
+            'direction_sign': direction_sign,
+            'signed_test_angle': test_angle,
+            'target_angle': abs(test_angle),
+            'speed': speed,
+            'tolerance': tolerance,
+            'odom_yaw_scale_correction': correction,
+            'timeout': timeout,
+        }
+
+    def _lookup_pose(self):
+        odom_frame = self.get_parameter('odom_frame').value
+        base_frame = self.get_parameter('base_frame').value
+        try:
+            trans = self.tf_buffer.lookup_transform(
+                odom_frame,
+                base_frame,
+                rclpy.time.Time(),
+                timeout=Duration(seconds=TF_TIMEOUT_SEC),
+            )
+        except TransformException as exc:
+            self.get_logger().warn(f'TF lookup failed: {exc}')
+            return None
+
+        rotation = trans.transform.rotation
+        return (
+            trans.transform.translation.x,
+            trans.transform.translation.y,
+            self._yaw_from_quaternion(rotation.x, rotation.y, rotation.z, rotation.w),
+        )
+
+    def _calculate_yaw_progress(self, pose):
+        current_yaw = pose[2]
+        delta = math.atan2(
+            math.sin(current_yaw - self.prev_yaw),
+            math.cos(current_yaw - self.prev_yaw),
+        )
+        self.accumulated_yaw += delta
+        self.prev_yaw = current_yaw
+        return self.direction_sign * self.accumulated_yaw
+
+    def _log_progress(self, raw_angle, corrected_angle, error, elapsed):
+        now = self.get_clock().now()
+        if (now - self.last_log_time).nanoseconds < 1e9:
+            return
+        self.get_logger().info(
+            f'odom_angle={math.degrees(raw_angle):.1f} deg, '
+            f'corrected_angle={math.degrees(corrected_angle):.1f} deg, '
+            f'error={math.degrees(error):+.1f} deg, elapsed={elapsed:.1f} s'
+        )
+        self.last_log_time = now
+
+    def _skip_pending_sample(self, reason):
+        with self.samples_lock:
+            if self.pending_sample is None:
+                self.get_logger().warn(
+                    f'Input "{reason}" ignored. No pending successful run is waiting for input.'
+                )
+                return
+
+            skipped_sample = self.pending_sample
+            self.pending_sample = None
+            self.state = 'idle'
+
+        self.get_logger().info(
+            f'Skipped pending run by input "{reason}": '
+            f'direction={skipped_sample["direction"]:+d}, '
+            f'signed_target={skipped_sample["signed_target_angle_deg"]:.2f} deg, '
+            f'odom={skipped_sample["odom_angle_deg"]:.2f} deg, '
+            f'corrected={skipped_sample["corrected_angle_deg"]:.2f} deg, '
+            f'used_correction={skipped_sample["used_correction"]:.6f}. '
+            'This run will not be used in the final scale summary.'
+        )
+
+    def _record_pending_sample(self, ground_error_deg):
+        with self.samples_lock:
+            if self.pending_sample is None:
+                self.get_logger().warn(
+                    'No pending successful run. Set start_test:=true first, wait for the robot to stop, '
+                    'then enter the measured deg error.'
+                )
+                return
+
+            actual_angle_deg = self.pending_sample['target_angle_deg'] + ground_error_deg
+            if actual_angle_deg <= 0.0:
+                self.get_logger().error(
+                    f'Invalid measured result: target + error = {actual_angle_deg:.2f} deg. '
+                    'Re-enter the deg error for this pending run.'
+                )
+                return
+
+            sample = dict(self.pending_sample)
+            sample['ground_error_deg'] = ground_error_deg
+            sample['actual_angle_deg'] = actual_angle_deg
+            sample['scale'] = math.radians(actual_angle_deg) / math.radians(sample['odom_angle_deg'])
+            self.samples.append(sample)
+            sample_index = len(self.samples)
+            direction_index = sum(
+                1 for recorded_sample in self.samples
+                if recorded_sample['direction'] == sample['direction']
+            )
+            self.pending_sample = None
+            self.state = 'idle'
+
+        self.get_logger().info(
+            f'Recorded sample #{sample_index} overall, direction {sample["direction"]:+d} #{direction_index}: '
+            f'actual={actual_angle_deg:.2f} deg, '
+            f'ground_error={ground_error_deg:+.2f} deg, odom={sample["odom_angle_deg"]:.2f} deg, '
+            f'scale={sample["scale"]:.6f}. Set start_test:=true for the next run, or enter 0 to finish.'
+        )
+
+    def _print_summary(self):
+        with self.samples_lock:
+            samples = list(self.samples)
+
+        if not samples:
+            self.get_logger().warn('No successful calibration samples have been recorded yet.')
+            return
+
+        self.get_logger().info('========== YAW ODOM SCALE SUMMARY ==========')
+        for index, sample in enumerate(samples, start=1):
+            self.get_logger().info(
+                f'#{index:02d} direction={sample["direction"]:+d}, '
+                f'signed_target={sample["signed_target_angle_deg"]:.2f} deg, '
+                f'target={sample["target_angle_deg"]:.2f} deg, '
+                f'actual={sample["actual_angle_deg"]:.2f} deg, '
+                f'ground_error={sample["ground_error_deg"]:+.2f} deg, '
+                f'odom={sample["odom_angle_deg"]:.2f} deg, '
+                f'corrected={sample["corrected_angle_deg"]:.2f} deg, '
+                f'used_correction={sample["used_correction"]:.6f}, '
+                f'scale={sample["scale"]:.6f}'
+            )
+
+        self._print_scale_stats('all', samples)
+        for direction in (1, -1):
+            direction_samples = [sample for sample in samples if sample['direction'] == direction]
+            if direction_samples:
+                self._print_scale_stats(f'direction={direction:+d}', direction_samples)
+        self.get_logger().info('Restart this node to clear cached samples.')
+
+    def _print_scale_stats(self, label, samples):
+        scales = [sample['scale'] for sample in samples]
+        mean_scale = statistics.fmean(scales)
+        std_scale = statistics.pstdev(scales) if len(scales) > 1 else 0.0
+        self.get_logger().info(
+            f'{label}: samples={len(scales)}, recommended_odometry.scale_theta={mean_scale:.6f}, '
+            f'std={std_scale:.6f}, min={min(scales):.6f}, max={max(scales):.6f}'
+        )
+
+    def _publish_stop(self):
+        try:
+            self.cmd_vel_pub.publish(Twist())
+        except Exception:
+            pass
+
+    def _stop_robot(self):
+        for _ in range(STOP_REPEAT_COUNT):
+            self._publish_stop()
+
+    def _stop_robot_with_ros_cli(self):
+        topic = shlex.quote(self.cmd_vel_topic)
+        zero_twist = (
+            '"{linear: {x: 0.0, y: 0.0, z: 0.0}, '
+            'angular: {x: 0.0, y: 0.0, z: 0.0}}"'
+        )
+        os.system(
+            f'timeout 2s ros2 topic pub --once {topic} '
+            f'geometry_msgs/msg/Twist {zero_twist} >/dev/null 2>&1'
+        )
+
+    def _reset_start_test(self):
+        self.set_parameters([
+            Parameter('start_test', Parameter.Type.BOOL, False),
+        ])
+
+    @staticmethod
+    def _yaw_from_quaternion(x, y, z, w):
+        siny_cosp = 2.0 * (w * z + x * y)
+        cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
+        return math.atan2(siny_cosp, cosy_cosp)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OdomYawCalib()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node._stop_robot()
+        node._stop_robot_with_ros_cli()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

agv_pro_calibration/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>agv_pro_calibration</name>
+  <version>0.0.0</version>
+  <description>AGV Pro calibration tools for odom, IMU, and TF health check</description>
+  <maintainer email="elephant@todo.todo">elephant</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>std_msgs</depend>
+  <depend>action_msgs</depend>
+  <depend>nav2_msgs</depend>
+  <depend>rcl_interfaces</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

agv_pro_calibration/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/agv_pro_calibration
+[install]
+install_scripts=$base/lib/agv_pro_calibration

agv_pro_calibration/setup.py

@@ -0,0 +1,29 @@
+from setuptools import find_packages, setup
+
+package_name = 'agv_pro_calibration'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='elephant',
+    maintainer_email='elephant@todo.todo',
+    description='AGV Pro calibration tools for odom, IMU, and TF health check',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'odom_linear_calib = agv_pro_calibration.odom_linear_calib:main',
+            'odom_yaw_calib = agv_pro_calibration.odom_yaw_calib:main',
+            'final_pose_refiner = agv_pro_calibration.final_pose_refiner:main',
+            'navigate_to_pose_refiner_proxy = agv_pro_calibration.navigate_to_pose_refiner_proxy:main',
+        ],
+    },
+)

agv_pro_calibration/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

agv_pro_calibration/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

agv_pro_calibration/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

agv_pro_msgs/CMakeLists.txt

@@ -17,6 +17,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/AGVProStatus.msg"
   "srv/SetDigitalOutput.srv"
   "srv/GetDigitalInput.srv"
+  "srv/SetLedColor.srv"
+  "srv/SetLedMode.srv"
   DEPENDENCIES std_msgs
 )
 

agv_pro_msgs/scripts/ros_client.py

@@ -0,0 +1,133 @@
+#!/usr/bin/env python3
+import rclpy
+import time
+from rclpy.node import Node
+
+from agv_pro_msgs.srv import (
+    SetDigitalOutput,
+    GetDigitalInput,
+    SetLedColor,
+    SetLedMode
+)
+
+class AGVIOClient(Node):
+    def __init__(self):
+        super().__init__('agv_io_client')
+
+        # Create service client
+        self.cli_set_io = self.create_client(SetDigitalOutput, 'set_digital_output')
+        self.cli_get_io = self.create_client(GetDigitalInput, 'get_digital_input')
+        self.cli_led_output = self.create_client(SetLedColor, 'set_led_color')
+        self.cli_led_mode = self.create_client(SetLedMode, 'set_led_mode')
+
+        # Wait until all services are available
+        self._wait_for_services()
+
+    def _wait_for_services(self):
+        """Wait for all services to become available."""
+        clients = [
+            self.cli_set_io,
+            self.cli_get_io,
+            self.cli_led_output,
+            self.cli_led_mode
+        ]
+
+        for cli in clients:
+            while not cli.wait_for_service(timeout_sec=1.0):
+                pass
+
+    def _call_service(self, client, request):
+        future = client.call_async(request)
+        rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
+
+        if future.result() is not None:
+            return future.result()
+        else:
+            self.get_logger().error(f'Service call failed: {future.exception()}')
+            return None
+
+    # ------------------------------
+    # Set digital output
+    # ------------------------------
+    def set_digital_output(self, pin: int, state: int) -> bool:
+        """Set digital output pin state."""
+        req = SetDigitalOutput.Request()
+        req.pin = pin
+        req.state = state
+
+        res = self._call_service(self.cli_set_io, req)
+        return res.success if res else False
+
+    # ------------------------------
+    # Get digital input
+    # ------------------------------
+    def get_digital_input(self, pin: int):
+        """Read digital input pin state."""
+        req = GetDigitalInput.Request()
+        req.pin = pin
+
+        res = self._call_service(self.cli_get_io, req)
+        return res.state if (res and res.success) else None
+
+    # ------------------------------
+    # Set LED color
+    # ------------------------------
+    def set_led_color(self,
+                      position: int,
+                      brightness: int,
+                      r: int,
+                      g: int,
+                      b: int) -> bool:
+        """Set LED RGB color and brightness."""
+        req = SetLedColor.Request()
+        req.position = position
+        req.brightness = brightness
+        req.r = r
+        req.g = g
+        req.b = b
+
+        res = self._call_service(self.cli_led_output, req)
+        return res.success if res else False
+
+    # ------------------------------
+    # Set LED mode
+    # ------------------------------
+    def set_led_mode(self, mode: bool) -> bool:
+        """Set LED mode (True/False)."""
+        req = SetLedMode.Request()
+        req.mode = mode
+
+        res = self._call_service(self.cli_led_mode, req)
+        return res.success if res else False
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    client = AGVIOClient()
+
+    ################
+    # Example usage:
+    ################
+
+    # client.set_digital_output(pin=1, state=1)
+    # client.get_digital_input(pin=2)
+
+    # client.set_led_mode(True)
+    # for i in range(10):
+    #     client.set_led_color(0, 100, 255, 255, 0)
+    #     client.set_led_color(1, 100, 255, 255, 0)
+    #     time.sleep(0.5)
+    #     client.set_led_color(0, 0, 0, 0, 0)
+    #     client.set_led_color(1, 0, 0, 0, 0)
+    #     time.sleep(0.5)
+
+    # client.set_led_color(0, 100, 255, 255, 0)
+    # client.set_led_color(1, 100, 255, 255, 0)
+    # client.set_led_mode(True)
+
+    client.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

agv_pro_msgs/srv/SetLedColor.srv

@@ -0,0 +1,8 @@
+int32 position
+int32 brightness
+int32 r
+int32 g
+int32 b
+---
+bool success
+string message

agv_pro_msgs/srv/SetLedMode.srv

@@ -0,0 +1,4 @@
+bool mode
+---
+bool success
+string message

agv_pro_navigation2/launch/navigation2_active.launch.py

@@ -3,36 +3,28 @@ import os
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.substitutions import LaunchConfiguration
-from launch.actions import DeclareLaunchArgument,IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription
 from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node, SetRemap
+
 
 def generate_launch_description():
     use_sim_time = LaunchConfiguration('use_sim_time', default='false')
     use_rviz = LaunchConfiguration('use_rviz', default='true')
     map_dir = LaunchConfiguration(
         'map',
-        default=os.path.join(
-            get_package_share_directory('agv_pro_navigation2'),
-            'map',
-            'map.yaml'))
+        default=os.path.join(get_package_share_directory('agv_pro_navigation2'), 'map', 'map.yaml'))
 
     param_file_name = 'agvpro.yaml'
     param_dir = LaunchConfiguration(
         'params_file',
-        default=os.path.join(
-            get_package_share_directory('agv_pro_navigation2'),
-            'param',
-            param_file_name))
+        default=os.path.join(get_package_share_directory('agv_pro_navigation2'), 'param', param_file_name))
 
     nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch')
 
-    rviz_config_dir = os.path.join(
-        get_package_share_directory('agv_pro_navigation2'),
-        'rviz',
-        'agvpro_navigation2.rviz')
+    rviz_config_dir = os.path.join(get_package_share_directory('agv_pro_navigation2'), 'rviz', 'agvpro_navigation2.rviz')
 
     return LaunchDescription([
         DeclareLaunchArgument(
@@ -45,12 +37,41 @@ def generate_launch_description():
             default_value=param_dir,
             description='Full path to param file to load'),
 
+        GroupAction(actions=[SetRemap(
+            src='/goal_pose',
+            dst='/goal_pose_nav2',
+        )] + [
+            SetRemap(
+                src=f'/{action}/_action/{suffix}',
+                dst=f'/{action}_nav2/_action/{suffix}')
+            for action in ('navigate_to_pose', 'navigate_through_poses')
+            for suffix in ('send_goal', 'get_result', 'cancel_goal', 'feedback', 'status')
+        ] + [
             IncludeLaunchDescription(
-            PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']),
+                PythonLaunchDescriptionSource(
+                    [nav2_launch_file_dir, '/bringup_launch.py']),
                 launch_arguments={
                     'map': map_dir,
-                'params_file': param_dir}.items(),
-        ),
+                    'params_file': param_dir,
+                }.items()),
+        ], scoped=True),
+
+        Node(
+            package='agv_pro_calibration',
+            executable='navigate_to_pose_refiner_proxy',
+            name='navigate_to_pose_refiner_proxy',
+            output='screen',
+            parameters=[{'use_sim_time': use_sim_time}]),
+
+        Node(
+            package='agv_pro_calibration',
+            executable='final_pose_refiner',
+            name='final_pose_refiner',
+            output='screen',
+            parameters=[{
+                'use_sim_time': use_sim_time,
+                'final_pose_refiner_auto_start_on_nav_success': False,
+            }]),
 
         Node(
             package='rviz2',

agv_pro_navigation2/map/map.yaml

@@ -1,7 +1,7 @@
 image: map.pgm
 mode: trinary
 resolution: 0.05
-origin: [-10, -24.4, 0]
+origin: [-22.8, -10, 0]
 negate: 0
 occupied_thresh: 0.65
 free_thresh: 0.25

agv_pro_navigation2/map/map1.yaml

@@ -0,0 +1,7 @@
+image: map1.pgm
+mode: trinary
+resolution: 0.05
+origin: [-10, -10, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

agv_pro_navigation2/map/map2.yaml

@@ -0,0 +1,7 @@
+image: map.pgm
+mode: trinary
+resolution: 0.05
+origin: [-21.2, -22.8, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

agv_pro_navigation2/param/agvpro.yaml

@@ -14,10 +14,13 @@ amcl:
     global_frame_id: "map"
     lambda_short: 0.1
     laser_likelihood_max_dist: 2.0
-    laser_max_range: 100.0
-    laser_min_range: -1.0
+    # 激光匹配最大有效距离 / Maximum valid laser matching range, 限制远距离无效数据对粒子权重的影响 / limits the effect of invalid distant data on particle weights, 初始值 / Initial value: 100.0 m
+    laser_max_range: 10.0
+    # 激光匹配最小有效距离 / Minimum valid laser matching range, 过滤雷达近距离盲区数据 / filters data in the lidar near-field blind zone, 初始值 / Initial value: -1.0
+    laser_min_range: 0.2
     laser_model_type: "likelihood_field"
-    max_beams: 60
+    # 每次定位更新采样的激光束数量 / Number of laser beams sampled per localization update, 增加定位匹配所使用的观测信息 / increases observation information used for localization matching, 初始值 / Initial value: 60
+    max_beams: 90
     max_particles: 2000
     min_particles: 500
     odom_frame_id: "odom"
@@ -25,14 +28,19 @@ amcl:
     pf_z: 0.99
     recovery_alpha_fast: 0.0
     recovery_alpha_slow: 0.0
-    resample_interval: 2
-    robot_model_type: "nav2_amcl::OmniMotionModel"
+    # 粒子滤波重采样间隔 / Particle filter resampling interval, 控制定位重采样与收敛更新频率 / controls localization resampling and convergence update frequency, 初始值 / Initial value: 2
+    resample_interval: 1
+    # 里程计运动模型类型 / Odometry motion model type, 定义机器人运动噪声与粒子位姿预测模型 / defines robot motion noise and particle pose prediction model, 初始值 / Initial value: nav2_amcl::OmniMotionModel
+    robot_model_type: "nav2_amcl::DifferentialMotionModel"
     save_pose_rate: 0.5
-    sigma_hit: 0.02
+    # 激光命中模型标准差 / Laser hit model standard deviation, 调节激光观测偏差对粒子权重的敏感程度 / adjusts particle-weight sensitivity to laser observation error, 初始值 / Initial value: 0.02
+    sigma_hit: 0.04
     tf_broadcast: true
     transform_tolerance: 0.3
-    update_min_a: 0.06
-    update_min_d: 0.025
+    # 触发定位更新的最小旋转角度 / Minimum rotation angle triggering a localization update, 控制小角度运动时激光定位更新频率 / controls laser localization update frequency during small-angle motion, 初始值 / Initial value: 0.06 rad
+    update_min_a: 0.04
+    # 触发定位更新的最小平移距离 / Minimum translation distance triggering a localization update, 控制低速平移时激光定位更新频率 / controls laser localization update frequency during low-speed translation, 初始值 / Initial value: 0.025 m
+    update_min_d: 0.015
     z_hit: 0.7
     z_max: 0.001
     z_rand: 0.059
@@ -132,13 +140,15 @@ controller_server:
     general_goal_checker:
       stateful: True
       plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 0.25
-      yaw_goal_tolerance: 0.25
+      # 到达目标的位置容差 / Goal position tolerance, 判定机器人位置是否满足导航完成条件 / determines whether robot position satisfies navigation completion, 初始值 / Initial value: 0.25 m
+      xy_goal_tolerance: 0.05
+      # 到达目标的航向角容差 / Goal heading tolerance, 判定机器人姿态是否满足导航完成条件 / determines whether robot orientation satisfies navigation completion, 初始值 / Initial value: 0.25 rad
+      yaw_goal_tolerance: 0.8
     # DWB parameters
     FollowPath:
       plugin: "dwb_core::DWBLocalPlanner"
       debug_trajectory_details: True
-      min_vel_x: 0.0
+      min_vel_x: -0.03
       min_vel_y: 0.0
       max_vel_x: 0.26
       max_vel_y: 0.0
@@ -150,10 +160,10 @@ controller_server:
       # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
       acc_lim_x: 2.5
       acc_lim_y: 0.0
-      acc_lim_theta: 3.2
+      acc_lim_theta: 2.5
       decel_lim_x: -2.5
       decel_lim_y: 0.0
-      decel_lim_theta: -3.2
+      decel_lim_theta: -2.5
       vx_samples: 20
       vy_samples: 5
       vtheta_samples: 40
@@ -161,8 +171,10 @@ controller_server:
       linear_granularity: 0.05
       angular_granularity: 0.025
       transform_tolerance: 0.1
-      xy_goal_tolerance: 0.25
-      trans_stopped_velocity: 0.1
+      # DWB 进入目标姿态调整模式的位置容差 / Position tolerance for DWB goal-orientation adjustment mode, 控制路径跟踪切换到末端旋转控制的距离窗口 / controls the distance window for switching from path tracking to final rotation control, 初始值 / Initial value: 0.25 m
+      xy_goal_tolerance: 0.03
+      # 判定平移停止的速度阈值 / Velocity threshold for considering translation stopped, 控制进入仅旋转控制前的平移停止条件 / controls the translation-stop condition before rotate-only control, 初始值 / Initial value: 0.1 m/s
+      trans_stopped_velocity: 0.01
       short_circuit_trajectory_evaluation: True
       stateful: True
       critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
@@ -292,8 +304,10 @@ planner_server:
     planner_plugins: ["GridBased"]
     GridBased:
       plugin: "nav2_navfn_planner/NavfnPlanner"
-      tolerance: 2.0
+      # 全局规划终点替代容差 / Global planner substitute-goal tolerance, 目标点不可达时限定可接受替代终点的距离范围 / limits the acceptable substitute-goal distance when the goal is unreachable, 初始值 / Initial value: 2.0 m
+      tolerance: 0.05
       use_astar: false
+      # 是否允许路径经过未知区域 / Whether paths may traverse unknown space, 控制全局规划器能否使用未观测栅格 / controls whether the global planner may use unobserved cells, 初始值 / Initial value: true
       allow_unknown: true
 
 planner_server_rclcpp_node:

agv_pro_navigation2/scripts/example_nav_to_pose.py

@@ -1,17 +1,31 @@
 #! /usr/bin/env python3
 
+import argparse
+import sys
+
+import yaml
+import rclpy
 from geometry_msgs.msg import PoseStamped
 from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
-import rclpy
 from rclpy.duration import Duration
 
 """
 Basic navigation demo to go to pose.
 """
 
+
+def parse_arguments():
+    parser = argparse.ArgumentParser(description='Send navigation test goals.')
+    parser.add_argument(
+        'targets',
+        nargs='*',
+        help='Waypoint letters to execute once, for example AB. Omit to loop ABCDE.')
+    return parser.parse_args()
+
+
 def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float):
     """
-    Set the initial pose of the robot for AMCL localization.
+    Set the initial pose of the robot for the active localization backend.
 
     Args:
         navigator (BasicNavigator): The navigator instance controlling the robot.
@@ -30,21 +44,7 @@ def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, o
     navigator.setInitialPose(initial_pose)
 
 
-def navigate_to_goal(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float, verbose: bool = False) -> bool:
-    """
-    Navigate the robot to a target goal pose.
-
-    Args:
-        navigator (BasicNavigator): The navigator instance controlling the robot.
-        x (float): Goal X position in the map frame.
-        y (float): Goal Y position in the map frame.
-        oz (float): Orientation Z component (quaternion).
-        ow (float): Orientation W component (quaternion).
-        verbose (bool, optional): If True, prints navigation feedback such as estimated arrival time. Default is False.
-
-    Returns:
-        bool: True if navigation succeeded, False otherwise.
-    """
+def make_goal_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float) -> PoseStamped:
     goal_pose = PoseStamped()
     goal_pose.header.frame_id = 'map'
     goal_pose.header.stamp = navigator.get_clock().now().to_msg()
@@ -52,6 +52,22 @@ def navigate_to_goal(navigator: BasicNavigator, x: float, y: float, oz: float, o
     goal_pose.pose.position.y = y
     goal_pose.pose.orientation.z = oz
     goal_pose.pose.orientation.w = ow
+    return goal_pose
+
+
+def navigate_to_goal(navigator: BasicNavigator, goal_pose: PoseStamped, verbose: bool = False) -> bool:
+    """
+    Navigate the robot to a target goal pose.
+
+    Args:
+        navigator (BasicNavigator): The navigator instance controlling the robot.
+        goal_pose (PoseStamped): Goal pose in the map frame.
+        verbose (bool, optional): If True, prints navigation feedback such as estimated arrival time. Default is False.
+
+    Returns:
+        bool: True if navigation succeeded, False otherwise.
+    """
+    goal_pose.header.stamp = navigator.get_clock().now().to_msg()
 
     navigator.goToPose(goal_pose)
 
@@ -74,24 +90,71 @@ def navigate_to_goal(navigator: BasicNavigator, x: float, y: float, oz: float, o
     return False
 
 if __name__ == '__main__':
+    cli_args = parse_arguments()
     rclpy.init()
     navigator = BasicNavigator()
 
-    # Set robot initial pose
-    # set_initial_pose(navigator, x=-1.9248794317245483, y=-0.5366987586021423, oz=-1.8463129131030735e-06, ow=0.9999999999982956)
+    # AMCL obtains its initial origin pose from agvpro.yaml.
+    navigator.initial_pose_received = True
+    navigator.waitUntilNav2Active()
 
-    # Wait for navigation to fully activate, since autostarting nav2
-    # navigator.waitUntilNav2Active()
+    # Try to load waypoints from YAML, fallback to hardcoded defaults
+    waypoints = {}
+    try:
+        with open('waypoints.yaml', 'r') as f:
+            waypoints = (yaml.safe_load(f) or {}).get('waypoints', {})
+    except FileNotFoundError:
+        with open('waypoints.yaml', 'w') as f:
+            yaml.dump({'waypoints': {}}, f, default_flow_style=False)
 
-    goal_A = [1.6766083240509033,0.37930558800697327,-0.03491306994337919, 0.9993903529387947]
-    goal_B = [-0.5062443017959595,1.559376835823059,0.6869307039904945,0.7267229237578264]
+    goals = {
+        'A': waypoints.get('A', [4.89649,-0.617371,0.706899,0.707315]),
+        'B': waypoints.get('B', [0.90387,-0.446105,0.273676,0.961822]),
+        'C': waypoints.get('C', [4.46734,-0.532388,0.969886,-0.243558]),
+        'D': waypoints.get('D', [-0.0233348,0.00798563,0.999322,0.0368173]),
+        'E': waypoints.get('E', [2.33651,-0.440663,0.937234,-0.3487]),
+    }
 
-    x_goal, y_goal, orientation_z, orientation_w = goal_A
-    success = navigate_to_goal(navigator, x_goal, y_goal, orientation_z, orientation_w)
-    print("Navigation result:", success)
-
-    x_goal, y_goal, orientation_z, orientation_w = goal_B
-    success = navigate_to_goal(navigator, x_goal, y_goal, orientation_z, orientation_w)
-    print("Navigation result:", success)
+    args = cli_args.targets
+    loop_targets = False
+    if args:
+        targets = []
+        for arg in args:
+            for c in arg.upper():
+                if c in goals:
+                    targets.append(c)
+    else:
+        targets = ['A', 'B', 'C', 'D', 'E']
+        loop_targets = True
+        print('No target arguments provided; running A-B-C-D-E repeatedly. Press Ctrl+C to stop.')
 
+    if not targets:
+        print('No valid target names provided. Use names such as A, B, C, D, E or AB.')
+        rclpy.shutdown()
+        sys.exit(1)
+
+    try:
+        cycle_index = 1
+        while rclpy.ok():
+            if loop_targets:
+                print(f'============= cycle {cycle_index}: ABCDE =============')
+
+            for name in targets:
+                if not rclpy.ok():
+                    break
+
+                x_goal, y_goal, orientation_z, orientation_w = goals[name]
+                input(f'============={name}==================\n')
+                goal_pose = make_goal_pose(navigator, x_goal, y_goal, orientation_z, orientation_w)
+                success = navigate_to_goal(navigator, goal_pose)
+                print("Navigation result:", goals[name], success)
+
+            if not loop_targets:
+                break
+            cycle_index += 1
+    except KeyboardInterrupt:
+        print('Navigation loop interrupted by user.')
+        navigator.cancelTask()
+    finally:
+        if rclpy.ok():
             rclpy.shutdown()

agv_pro_navigation2/scripts/power_on.py

@@ -0,0 +1,3 @@
+from pymycobot import MyAGVPro
+m = MyAGVPro('/dev/agvpro_controller')
+m.power_on()

agv_pro_navigation2/scripts/record_waypoints.py

@@ -0,0 +1,119 @@
+#!/usr/bin/env python3
+"""Record waypoints by sampling stable map->base_footprint pose."""
+
+import sys
+import threading
+import time
+
+import rclpy
+from rclpy.node import Node
+from tf2_ros import Buffer, TransformListener
+import yaml
+
+
+class WaypointRecorder(Node):
+    def __init__(self):
+        super().__init__('waypoint_recorder')
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+        self.yaml_path = 'waypoints.yaml'
+        self.waypoints = {}
+        self._load_existing()
+
+    def _load_existing(self):
+        try:
+            with open(self.yaml_path, 'r') as f:
+                data = yaml.safe_load(f) or {}
+                self.waypoints = data.get('waypoints', {})
+                n = len(self.waypoints)
+                if n > 0:
+                    self.get_logger().info(f'Loaded {n} existing waypoints from {self.yaml_path}')
+        except FileNotFoundError:
+            self.waypoints = {}
+
+    def _save(self):
+        data = {'waypoints': self.waypoints}
+        with open(self.yaml_path, 'w') as f:
+            yaml.dump(data, f, default_flow_style=False, sort_keys=False)
+        self.get_logger().info(f'Saved waypoints to {self.yaml_path}')
+
+    def _sample_pose(self, duration_sec=3.0, rate_hz=20):
+        xs, ys, zs, ws = [], [], [], []
+        dt = 1.0 / rate_hz
+        start = time.time()
+        while time.time() - start < duration_sec:
+            try:
+                trans = self.tf_buffer.lookup_transform(
+                    'map', 'base_footprint', rclpy.time.Time()
+                )
+                t = trans.transform.translation
+                r = trans.transform.rotation
+                xs.append(t.x)
+                ys.append(t.y)
+                zs.append(r.z)
+                ws.append(r.w)
+            except Exception:
+                pass
+            time.sleep(dt)
+
+        if not xs:
+            return None
+
+        xs.sort()
+        ys.sort()
+        zs.sort()
+        ws.sort()
+        n = len(xs)
+        mid = n // 2
+        if n % 2 == 1:
+            return [xs[mid], ys[mid], zs[mid], ws[mid]]
+        return [
+            (xs[mid - 1] + xs[mid]) / 2,
+            (ys[mid - 1] + ys[mid]) / 2,
+            (zs[mid - 1] + zs[mid]) / 2,
+            (ws[mid - 1] + ws[mid]) / 2,
+        ]
+
+    def run(self):
+        self.get_logger().info('Waypoint recorder ready.')
+        self.get_logger().info('Enter A/B/C/D/E to record, q to quit.')
+        while rclpy.ok():
+            try:
+                cmd = input('> ').strip().upper()
+            except EOFError:
+                break
+            if cmd == 'Q':
+                break
+            if cmd in 'ABCDE':
+                self.get_logger().info(
+                    f'Sampling pose for {cmd} ({3}s, keep still)...'
+                )
+                pose = self._sample_pose()
+                if pose is None:
+                    self.get_logger().error(
+                        'Failed to sample pose. Is AMCL running?'
+                    )
+                    continue
+                self.waypoints[cmd] = [float(f'{v:.6f}') for v in pose]
+                self.get_logger().info(f'{cmd}: {self.waypoints[cmd]}')
+                self._save()
+            else:
+                self.get_logger().warn('Use A/B/C/D/E or q.')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = WaypointRecorder()
+    spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
+    spin_thread.start()
+    try:
+        node.run()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

agv_pro_navigation2/scripts/waypoints.yaml

@@ -0,0 +1,26 @@
+waypoints:
+  A:
+  - 4.24531
+  - 1.866955
+  - 0.584964
+  - 0.811059
+  B:
+  - 3.624886
+  - -2.221018
+  - -0.569358
+  - 0.82209
+  C:
+  - 1.900274
+  - 1.940145
+  - -0.737521
+  - 0.675324
+  D:
+  - 0.882323
+  - -0.456797
+  - 0.745742
+  - 0.666235
+  E:
+  - -1.63798
+  - -0.847909
+  - 0.978385
+  - 0.206792

livox_ros_driver2/.gitignore

@@ -1,4 +1,3 @@
 .vscode
 build
-package.xml
 __pycache__

livox_ros_driver2/CMakeLists.txt

@@ -1,264 +1,76 @@
-# judge which cmake codes to use 
-if(ROS_EDITION STREQUAL "ROS1")
+# Copyright(c) 2020 livoxtech limited.
 
-  # Copyright(c) 2019 livoxtech limited.
+cmake_minimum_required(VERSION 3.14)
+project(livox_ros_driver2)
 
-  cmake_minimum_required(VERSION 3.0)
-
-
-  #---------------------------------------------------------------------------------------
-  # Start livox_ros_driver2 project
-  #---------------------------------------------------------------------------------------
-  include(cmake/version.cmake)
-  project(livox_ros_driver2 VERSION ${LIVOX_ROS_DRIVER2_VERSION} LANGUAGES CXX)
-  message(STATUS "livox_ros_driver2 version: ${LIVOX_ROS_DRIVER2_VERSION}")
-
-  #---------------------------------------------------------------------------------------
-  # Add ROS Version MACRO
-  #---------------------------------------------------------------------------------------
-  add_definitions(-DBUILDING_ROS1)
-
-  #---------------------------------------------------------------------------------------
-  # find package and the dependecy
-  #---------------------------------------------------------------------------------------
-  find_package(Boost 1.54 REQUIRED COMPONENTS
-    system
-    thread
-    chrono
-  )
-
-  ## Find catkin macros and libraries
-  ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-  ## is used, also find other catkin packages
-  find_package(catkin REQUIRED COMPONENTS
-    roscpp
-    rospy
-    sensor_msgs
-    std_msgs
-    message_generation
-    rosbag
-    pcl_ros
-  )
-
-  ## Find pcl lib
-  find_package(PCL REQUIRED)
-
-  ## Generate messages in the 'msg' folder
-  add_message_files(FILES
-    CustomPoint.msg
-    CustomMsg.msg
-  #   Message2.msg
-  )
-
-  ## Generate added messages and services with any dependencies listed here
-  generate_messages(DEPENDENCIES
-    std_msgs
-  )
-
-  find_package(PkgConfig)
-  pkg_check_modules(APR apr-1)
-  if (APR_FOUND)
-    message(${APR_INCLUDE_DIRS})
-    message(${APR_LIBRARIES})
-  endif (APR_FOUND)
-
-  ###################################
-  ## catkin specific configuration ##
-  ###################################
-  ## The catkin_package macro generates cmake config files for your package
-  ## Declare things to be passed to dependent projects
-  ## INCLUDE_DIRS: uncomment this if your package contains header files
-  ## LIBRARIES: libraries you create in this project that dependent projects als    o need
-  ## CATKIN_DEPENDS: catkin_packages dependent projects also need
-  ## DEPENDS: system dependencies of this project that dependent projects also n    eed
-  catkin_package(CATKIN_DEPENDS
-    roscpp rospy std_msgs message_runtime
-    pcl_ros
-  )
-
-  #---------------------------------------------------------------------------------------
-  # Set default build to release
-  #---------------------------------------------------------------------------------------
-  if(NOT CMAKE_BUILD_TYPE)
-    set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose Release or Debug" FORCE)
-  endif()
-
-  #---------------------------------------------------------------------------------------
-  # Compiler config
-  #---------------------------------------------------------------------------------------
-  set(CMAKE_CXX_STANDARD 14)
-  set(CMAKE_CXX_STANDARD_REQUIRED ON)
-  set(CMAKE_CXX_EXTENSIONS OFF)
-
-  ## make sure the livox_lidar_sdk_static library is installed
-  find_library(LIVOX_LIDAR_SDK_LIBRARY  liblivox_lidar_sdk_static.a    /usr/local/lib)
-
-  ## PCL library
-  link_directories(${PCL_LIBRARY_DIRS})
-  add_definitions(${PCL_DEFINITIONS})
-
-  #---------------------------------------------------------------------------------------
-  # generate excutable and add libraries
-  #---------------------------------------------------------------------------------------
-  add_executable(${PROJECT_NAME}_node
-    ""
-  )
-
-  #---------------------------------------------------------------------------------------
-  # precompile macro and compile option
-  #---------------------------------------------------------------------------------------
-  target_compile_options(${PROJECT_NAME}_node
-    PRIVATE $<$<CXX_COMPILER_ID:GNU>:-Wall>
-  )
-
-  #---------------------------------------------------------------------------------------
-  # add projects that depend on
-  #---------------------------------------------------------------------------------------
-  add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_generate_messages_cpp)
-
-  #---------------------------------------------------------------------------------------
-  # source file
-  #---------------------------------------------------------------------------------------
-  target_sources(${PROJECT_NAME}_node
-    PRIVATE
-    src/driver_node.cpp
-    src/lds.cpp
-    src/lds_lidar.cpp
-    src/lddc.cpp
-    src/livox_ros_driver2.cpp
-
-    src/comm/comm.cpp
-    src/comm/ldq.cpp
-    src/comm/semaphore.cpp
-    src/comm/lidar_imu_data_queue.cpp
-    src/comm/cache_index.cpp
-    src/comm/pub_handler.cpp
-
-    src/parse_cfg_file/parse_cfg_file.cpp
-    src/parse_cfg_file/parse_livox_lidar_cfg.cpp
-
-    src/call_back/lidar_common_callback.cpp
-    src/call_back/livox_lidar_callback.cpp
-  )
-
-  #---------------------------------------------------------------------------------------
-  # include file
-  #---------------------------------------------------------------------------------------
-  target_include_directories(${PROJECT_NAME}_node
-    PUBLIC
-    ${catkin_INCLUDE_DIRS}
-    ${PCL_INCLUDE_DIRS}
-    ${APR_INCLUDE_DIRS}
-    3rdparty
-    src
-  )
-
-  #---------------------------------------------------------------------------------------
-  # link libraries
-  #---------------------------------------------------------------------------------------
-  target_link_libraries(${PROJECT_NAME}_node
-    ${LIVOX_LIDAR_SDK_LIBRARY}
-    ${Boost_LIBRARY}
-    ${catkin_LIBRARIES}
-    ${PCL_LIBRARIES}
-    ${APR_LIBRARIES}
-  )
-
-
-  #---------------------------------------------------------------------------------------
-  # Install
-  #---------------------------------------------------------------------------------------
-
-  install(TARGETS ${PROJECT_NAME}_node
-    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-  )
-
-  install(DIRECTORY launch_ROS1/
-    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch_ROS1
-  )
-
-  #---------------------------------------------------------------------------------------
-  # end of CMakeList.txt
-  #---------------------------------------------------------------------------------------
-
-
-else(ROS_EDITION STREQUAL "ROS2")
-
-  # Copyright(c) 2020 livoxtech limited.
-
-  cmake_minimum_required(VERSION 3.14)
-  project(livox_ros_driver2)
-
-  # Default to C99
-  if(NOT CMAKE_C_STANDARD)
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
   set(CMAKE_C_STANDARD 99)
-  endif()
+endif()
 
-  # Default to C++14
-  if(NOT CMAKE_CXX_STANDARD)
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
-  endif()
+endif()
 
-  list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake/modules")
+list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake/modules")
 
-  if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter)
-  endif()
+endif()
 
-  # Printf version info
-  include(cmake/version.cmake)
-  project(${PROJECT_NAME} VERSION ${LIVOX_ROS_DRIVER2_VERSION} LANGUAGES CXX)
-  message(STATUS "${PROJECT_NAME} version: ${LIVOX_ROS_DRIVER2_VERSION}")
+# Printf version info
+include(cmake/version.cmake)
+project(${PROJECT_NAME} VERSION ${LIVOX_ROS_DRIVER2_VERSION} LANGUAGES CXX)
+message(STATUS "${PROJECT_NAME} version: ${LIVOX_ROS_DRIVER2_VERSION}")
 
-  #---------------------------------------------------------------------------------------
-  # Add ROS Version MACRO
-  #---------------------------------------------------------------------------------------
-  add_definitions(-DBUILDING_ROS2)
+#---------------------------------------------------------------------------------------
+# Add ROS Version MACRO
+#---------------------------------------------------------------------------------------
+add_definitions(-DBUILDING_ROS2)
 
-  # find dependencies
-  # uncomment the following section in order to fill in
-  # further dependencies manually.
-  # find_package(<dependency> REQUIRED)
-  find_package(ament_cmake_auto REQUIRED)
-  ament_auto_find_build_dependencies()
-  find_package(PCL REQUIRED)
-  find_package(std_msgs REQUIRED)
-  find_package(builtin_interfaces REQUIRED)
-  find_package(rosidl_default_generators REQUIRED)
+# find dependencies
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+find_package(PCL REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 
-  # check apr
-  find_package(PkgConfig)
-  pkg_check_modules(APR apr-1)
-  if (APR_FOUND)
+# check apr
+find_package(PkgConfig)
+pkg_check_modules(APR apr-1)
+if (APR_FOUND)
   message(${APR_INCLUDE_DIRS})
   message(${APR_LIBRARIES})
-  endif (APR_FOUND)
+endif (APR_FOUND)
 
-  # generate custom msg headers
-  set(LIVOX_INTERFACES livox_interfaces2)
-  rosidl_generate_interfaces(${LIVOX_INTERFACES}
+# generate custom msg headers
+set(LIVOX_INTERFACES livox_interfaces2)
+rosidl_generate_interfaces(${LIVOX_INTERFACES}
   "msg/CustomPoint.msg"
   "msg/CustomMsg.msg"
   DEPENDENCIES builtin_interfaces std_msgs
   LIBRARY_NAME ${PROJECT_NAME}
-  )
+)
 
-  ## make sure the livox_lidar_sdk_shared library is installed
-  find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so /usr/local/lib REQUIRED)
+## make sure the livox_lidar_sdk_shared library is installed
+find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so /usr/local/lib REQUIRED)
 
-  ##
-  find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR
+##
+find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR
   NAMES "livox_lidar_api.h" "livox_lidar_def.h"
   REQUIRED)
 
-  ## PCL library
-  link_directories(${PCL_LIBRARY_DIRS})
-  add_definitions(${PCL_DEFINITIONS})
+## PCL library
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
 
-  # livox ros2 driver target
-  ament_auto_add_library(${PROJECT_NAME} SHARED
+# livox ros2 driver target
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/livox_ros_driver2.cpp
   src/lddc.cpp
   src/driver_node.cpp
@@ -277,47 +89,41 @@ else(ROS_EDITION STREQUAL "ROS2")
 
   src/call_back/lidar_common_callback.cpp
   src/call_back/livox_lidar_callback.cpp
-  )
+)
 
-  target_include_directories(${PROJECT_NAME} PRIVATE ${livox_sdk_INCLUDE_DIRS})
+target_include_directories(${PROJECT_NAME} PRIVATE ${livox_sdk_INCLUDE_DIRS})
 
-  # get include directories of custom msg headers
-  if(HUMBLE_ROS STREQUAL "humble")
-    rosidl_get_typesupport_target(cpp_typesupport_target
-    ${LIVOX_INTERFACES} "rosidl_typesupport_cpp")
-    target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}")
-  else()
-    set(LIVOX_INTERFACE_TARGET "${LIVOX_INTERFACES}__rosidl_typesupport_cpp")
-    add_dependencies(${PROJECT_NAME} ${LIVOX_INTERFACES})
-    get_target_property(LIVOX_INTERFACES_INCLUDE_DIRECTORIES ${LIVOX_INTERFACE_TARGET} INTERFACE_INCLUDE_DIRECTORIES)
-  endif()
+# livox ros2 driver target
+rosidl_get_typesupport_target(cpp_typesupport_target
+${LIVOX_INTERFACES} "rosidl_typesupport_cpp")
+target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}")
 
-  # include file direcotry
-  target_include_directories(${PROJECT_NAME} PUBLIC
+# include file direcotry
+target_include_directories(${PROJECT_NAME} PUBLIC
   ${PCL_INCLUDE_DIRS}
   ${APR_INCLUDE_DIRS}
   ${LIVOX_LIDAR_SDK_INCLUDE_DIR}
   ${LIVOX_INTERFACES_INCLUDE_DIRECTORIES}   # for custom msgs
   3rdparty
   src
-  )
+)
 
-  # link libraries
-  target_link_libraries(${PROJECT_NAME}
+# link libraries
+target_link_libraries(${PROJECT_NAME}
   ${LIVOX_LIDAR_SDK_LIBRARY}
   ${LIVOX_INTERFACE_TARGET}   # for custom msgs
   ${PPT_LIBRARY}
   ${Boost_LIBRARY}
   ${PCL_LIBRARIES}
   ${APR_LIBRARIES}
-  )
+)
 
-  rclcpp_components_register_node(${PROJECT_NAME}
+rclcpp_components_register_node(${PROJECT_NAME}
   PLUGIN "livox_ros::DriverNode"
   EXECUTABLE ${PROJECT_NAME}_node
-  )
+)
 
-  if(BUILD_TESTING)
+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
@@ -326,11 +132,9 @@ else(ROS_EDITION STREQUAL "ROS2")
   # 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_auto_package(INSTALL_TO_SHARE
-    config
-    launch_ROS2
-  )
-
 endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  config
+  launch
+)

livox_ros_driver2/launch/MID360_launch.py

@@ -0,0 +1,89 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import Node
+
+################### user configure parameters for ros2 start ###################
+# xfer_format   = 0    # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
+multi_topic   = 0    # 0-All LiDARs share the same topic, 1-One LiDAR one topic
+data_src      = 0    # 0-lidar, others-Invalid data src
+# publish_freq  = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
+output_type   = 0
+frame_id      = 'laser_link'
+lvx_file_path = '/home/livox/livox_test.lvx'
+cmdline_bd_code = 'livox0000000001'
+
+cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
+cur_config_path = cur_path + '../config'
+rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud_ROS2.rviz')
+user_config_path = os.path.join(cur_config_path, 'MID360_config.json')
+################### user configure parameters for ros2 end #####################
+
+def generate_launch_description():
+
+    use_mid360_rviz = LaunchConfiguration('use_mid360_rviz')
+    xfer_format = LaunchConfiguration('xfer_format')
+    publish_freq = LaunchConfiguration('publish_freq')
+
+    declare_rviz_arg = DeclareLaunchArgument(
+        'use_mid360_rviz',
+        default_value='false',
+        description='Enable RViz for MID360 LiDAR visualization'
+    )
+
+    declare_xfer_format_arg = DeclareLaunchArgument(
+        'xfer_format',
+        default_value='0',
+        description='MID360 format: 0=Pointcloud2(PointXYZRTL), 1=customized pointcloud format'
+    )
+
+    declare_publish_freq_arg = DeclareLaunchArgument(
+        'publish_freq',
+        default_value='10.0',
+        description='MID360 LiDAR publish frequency in Hz (e.g., 5.0, 10.0, 20.0, 50.0)'
+    )
+
+    livox_ros2_params = [
+        {"xfer_format": xfer_format},
+        {"multi_topic": multi_topic},
+        {"data_src": data_src},
+        {"publish_freq": publish_freq},
+        {"output_data_type": output_type},
+        {"frame_id": frame_id},
+        {"lvx_file_path": lvx_file_path},
+        {"user_config_path": user_config_path},
+        {"cmdline_input_bd_code": cmdline_bd_code}
+    ]
+
+    livox_driver = Node(
+        package='livox_ros_driver2',
+        executable='livox_ros_driver2_node',
+        name='livox_lidar_publisher',
+        output='screen',
+        parameters=livox_ros2_params
+        )
+
+    livox_rviz = Node(
+            package='rviz2',
+            executable='rviz2',
+            name='mid360_lidar_rviz',
+            output='screen',
+            arguments=['--display-config', rviz_config_path],
+            condition=IfCondition(
+                PythonExpression([
+                    "'", use_mid360_rviz, "' == 'true' and '",
+                    xfer_format, "' == '0'"
+                ])
+            )
+        )
+
+    return LaunchDescription([
+        declare_rviz_arg,
+        declare_xfer_format_arg,
+        declare_publish_freq_arg,
+        livox_driver,
+        livox_rviz,
+    ])

unitree_lidar_ros2/CMakeLists.txt

@@ -1,12 +1,13 @@
 cmake_minimum_required(VERSION 3.5)
+
+cmake_policy(SET CMP0074 NEW)
+
 project(unitree_lidar_ros2)
 
-# Default to C99
 if(NOT CMAKE_C_STANDARD)
   set(CMAKE_C_STANDARD 99)
 endif()
 
-# Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
@@ -24,30 +25,40 @@ find_package(PCL REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(sensor_msgs REQUIRED)
 
+# SDK include path
 include_directories(
-  ${PCL_INCLUDE_DIRS} 
   include
-  ../unitree_lidar_sdk/include
-)
-
-link_directories(
-  ${PCL_LIBRARY_DIRS}
-  ../unitree_lidar_sdk/lib/${CMAKE_SYSTEM_PROCESSOR}
+  /usr/local/include/unitree_lidar_sdk
+  ${PCL_INCLUDE_DIRS}
 )
 
 add_definitions(${PCL_DEFINITIONS})
 
-add_executable(unitree_lidar_ros2_node src/unitree_lidar_ros2_node.cpp)
+# SDK library path by architecture
+if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
+  set(UNITREE_LIDAR_SDK_LIB
+    /usr/local/lib/unitree_lidar_sdk/aarch64/libunilidar_sdk2.a
+  )
+else()
+  set(UNITREE_LIDAR_SDK_LIB
+    /usr/local/lib/unitree_lidar_sdk/x86_64/libunilidar_sdk2.a
+  )
+endif()
 
-target_link_libraries( unitree_lidar_ros2_node
-  ${Boost_SYSTEM_LIBRARY}
+add_executable(unitree_lidar_ros2_node
+  src/unitree_lidar_ros2_node.cpp
+)
+
+target_link_libraries(
+  unitree_lidar_ros2_node
+  ${UNITREE_LIDAR_SDK_LIB}
   ${PCL_LIBRARIES}
-  unilidar_sdk2
 )
 
 ament_target_dependencies(
   unitree_lidar_ros2_node
-  rclcpp std_msgs 
+  rclcpp
+  std_msgs
   sensor_msgs
   geometry_msgs
   tf2_ros
@@ -55,11 +66,10 @@ ament_target_dependencies(
 )
 
 install(TARGETS
-unitree_lidar_ros2_node
+  unitree_lidar_ros2_node
   DESTINATION lib/${PROJECT_NAME}
 )
 
-# install rviz file and launch file
 install(DIRECTORY launch/
   DESTINATION share/${PROJECT_NAME}/launch
 )
@@ -68,7 +78,6 @@ install(DIRECTORY rviz/
   DESTINATION share/${PROJECT_NAME}/rviz
 )
 
-# install config files if they exist
 if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/config")
   install(DIRECTORY config/
     DESTINATION share/${PROJECT_NAME}/config

unitree_lidar_ros2/launch/launch.py

@@ -9,12 +9,12 @@ from ament_index_python.packages import get_package_share_directory
 
 def generate_launch_description():
     
-    use_rviz = LaunchConfiguration('use_rviz')
+    use_l2_rviz = LaunchConfiguration('use_rviz')
 
-    declare_use_rviz = DeclareLaunchArgument(
-        'use_rviz',
+    declare_rviz_arg = DeclareLaunchArgument(
+        'use_l2_rviz',
         default_value='false',
-        description='Whether to start RViz'
+        description='Whether to launch RViz for Unitree L2 LiDAR visualization'
     )
 
     pkg_share = get_package_share_directory('unitree_lidar_ros2')
@@ -57,12 +57,12 @@ def generate_launch_description():
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_config_file],
-       condition=IfCondition(use_rviz),
+       condition=IfCondition(use_l2_rviz),
        output='log'
     )
     return LaunchDescription(
         [
-            declare_use_rviz,
+            declare_rviz_arg,
             node1, 
             rviz_node,
         ]