Integrate SANEHAL-2 Robot-side bringup

README.md

@@ -45,6 +45,14 @@ colcon build
 ```bash
 . install/setup.bash
 ros2 launch sanehal_bringup jt16.launch.py       # JT16 driverのみ
-ros2 launch sanehal_bringup sanehal.launch.py    # Raspberry Pi上の駆動系 + JT16
+ros2 launch sanehal_bringup sanehal.launch.py    # Raspberry Pi上のRobot + JT16 + 2D SLAM
 ros2 launch sanehal_bringup sanehal_rviz.launch.py # 母艦PC上での表示のみ
 ```
+
+Robot側の既定起動ではRVizを起動しません。RobotとOperator PCで同じ
+`ROS_DOMAIN_ID`を設定してください。実機なしのgraph/TF確認には次を使用できます。
+
+```bash
+ros2 launch sanehal_bringup sanehal.launch.py use_mock_hardware:=true \
+  start_lidar:=false start_pointcloud_to_laserscan:=false start_slam:=false
+```

sanehal_bringup/CMakeLists.txt

@@ -15,6 +15,7 @@ if(BUILD_TESTING)
   set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
   add_launch_test(test/test_pointcloud_to_laserscan.py TIMEOUT 30)
+  add_launch_test(test/test_robot_bringup.py TIMEOUT 45)
 endif()
 
 ament_auto_package(

sanehal_bringup/README.md

@@ -17,15 +17,16 @@ ls -l /dev/jt16_rs485 /dev/jt16_rs232 /dev/dxhub
 Log out and back in after adding the user to `dialout`.
 
 `jt16.launch.py` starts the official Hesai ROS 2 driver with
-`config/jt16_serial.yaml`. `sanehal.launch.py` starts both the drive/TF stack
-and the JT16 driver; pass `start_lidar:=false` when the driver is already
-running. The driver publishes `sensor_msgs/msg/PointCloud2` on
+`config/jt16_serial.yaml`. `sanehal.launch.py` is the Robot-side entry point and
+starts drive/TF, JT16, PointCloud2-to-LaserScan, and `slam_toolbox`; pass
+`start_lidar:=false` when the driver is already running. The driver publishes
+`sensor_msgs/msg/PointCloud2` on
 `/lidar_points` with `frame_id: hesai_lidar`.
 
 ```bash
 . install/setup.bash
 ros2 launch sanehal_bringup jt16.launch.py
-# Or start the robot and JT16 together on the Raspberry Pi:
+# Start the complete Robot-side stack on the Raspberry Pi (RViz stays off):
 ros2 launch sanehal_bringup sanehal.launch.py
 ```
 
@@ -46,17 +47,17 @@ depth of 10. Its cloud header uses the frame start time. The checked-in config
 uses the host receive timestamp (`use_timestamp_type: 1`) until the JT16 clock
 has been synchronized and its device timestamp has been validated. The RS232
 port supplies commands and angle calibration; if it is unavailable, configure
-a valid `correction_file_path` instead of leaving it empty.
+a valid `correction_file_path` instead of leaving it empty, then launch with
+`require_jt16_rs232:=false`. The RS485 data port remains mandatory.
 
 `pointcloud_to_laserscan.launch.py` converts `/lidar_points` to `/scan` using
 `config/pointcloud_to_laserscan_jt16.yaml`. It preserves the cloud timestamp and
 `hesai_lidar` frame. The converter subscribes to the cloud only while `/scan`
 has a subscriber, so use `ros2 topic echo`, RViz, or `slam_toolbox` when testing
 it by itself.
 
-`slam.launch.py` starts the SANEHAL-2 drive/TF stack, JT16 driver,
-PointCloud2-to-LaserScan converter, Jazzy `slam_toolbox` in online asynchronous
-mapping mode, and RViz. Disable components that are already running to avoid
+`slam.launch.py` is retained as a compatibility wrapper for the integrated
+`sanehal.launch.py`. Disable components that are already running to avoid
 duplicate publishers:
 
 ```bash
@@ -70,10 +71,56 @@ ros2 launch sanehal_bringup slam.launch.py \
   start_pointcloud_to_laserscan:=false
 ```
 
-The Robot-side bringup interface passed to Issue #27 is `start_lidar`,
-`start_pointcloud_to_laserscan`, `jt16_config_file`, `converter_params_file`,
-`pointcloud_topic`, `scan_topic`, and `use_sim_time`. The topic arguments are
-implemented as remaps; their defaults are `/lidar_points` and `/scan`.
+The Robot-side component switches are `start_description`, `start_control`,
+`start_lidar`, `start_pointcloud_to_laserscan`, `start_slam`, and `start_rviz`.
+RViz defaults to false on the Robot. Config paths and `/lidar_points`/`/scan`
+topic names are launch arguments. `use_mock_hardware:=true` selects the
+ros2_control GenericSystem only for hardware-free tests; production always uses
+ROBOTIS `dynamixel_hardware_interface`.
+
+For drive/TF diagnostics without serial devices:
+
+```bash
+ros2 launch sanehal_bringup sanehal.launch.py use_mock_hardware:=true \
+  start_lidar:=false start_pointcloud_to_laserscan:=false start_slam:=false
+```
+
+For description and static TF only, also pass `start_control:=false`. Device
+checks wait up to `device_wait_timeout` seconds and can be bypassed for an
+intentional external-data workflow with `wait_for_devices:=false`.
+
+The main launch arguments and defaults are:
+
+| Argument | Default | Purpose |
+| --- | --- | --- |
+| `use_mock_hardware` | `false` | Select GenericSystem for hardware-free tests |
+| `start_description` / `start_control` | `true` | Robot model/TF and drive stack |
+| `start_lidar` / `start_pointcloud_to_laserscan` | `true` | JT16 cloud and 2D scan |
+| `start_slam` | `true` | Online asynchronous `slam_toolbox` |
+| `start_rviz` | `false` | Local RViz; normally false on the Raspberry Pi |
+| `wait_for_devices` | `true` | Check serial device access before node startup |
+| `device_wait_timeout` | `10.0` | Device wait timeout in seconds |
+| `require_jt16_rs232` | `true` | Require the JT16 command port; disable only with a correction file |
+| `dynamixel_port` / `dynamixel_baud_rate` | `/dev/dxhub` / `1000000` | ROBOTIS hardware connection |
+| `pointcloud_topic` / `scan_topic` | `/lidar_points` / `/scan` | Sensor contracts |
+
+`jt16_rs485_device` and `jt16_rs232_device` are preflight paths and must match
+the paths in the selected `jt16_config_file`. The standalone `jt16.launch.py`
+uses the equivalent `require_rs232` argument.
+
+## Robot/Operator interface contract
+
+Set the same non-conflicting `ROS_DOMAIN_ID` on the Raspberry Pi and Operator
+PC. The Robot publishes `/map`, `/scan`, `/sanehal_base_controller/odom`,
+`/joint_states`, `/tf`, `/tf_static`, and `/robot_description`. `/lidar_points`
+is a high-bandwidth debug topic and need not be displayed during normal
+operation. `/lidar_imu` remains disabled until its hardware data contract is
+validated.
+
+Teleoperation sends `geometry_msgs/msg/TwistStamped` to
+`/sanehal_base_controller/cmd_vel`. The Robot-side controller timeout is 0.5 s;
+the Operator-side implementation in Issue #41 must also require a deadman
+button. A future teleop/Nav2 mux belongs upstream of this controller input.
 
 Inspect the raw JT16 contract before starting the converter:
 
@@ -96,16 +143,17 @@ If the SANEHAL-2 base is already running, avoid duplicate controller and TF
 publishers:
 
 ```bash
-ros2 launch sanehal_bringup slam.launch.py start_robot_bringup:=false
+ros2 launch sanehal_bringup sanehal.launch.py \
+  start_control:=false start_description:=false
 ```
 
 The live TF ownership is:
 
 - `slam_toolbox`: `map -> odom`
-- `sanehal_base_controller` (`diff_drive_controller`): `odom -> base_link`
-- `robot_state_publisher`: `base_link -> sanehal_base_link -> hesai_lidar`
+- `sanehal_base_controller` (`diff_drive_controller`): `odom -> base_footprint`
+- `robot_state_publisher`: `base_footprint -> base_link -> sanehal_base_link -> hesai_lidar`
 
-Do not add a static `map -> odom` or `odom -> base_link` publisher. Before
+Do not add a static `map -> odom` or `odom -> base_footprint` publisher. Before
 driving, confirm `/scan`, `/sanehal_base_controller/odom`, and the complete TF
 chain:
 
@@ -114,7 +162,7 @@ ros2 topic info /scan --verbose
 ros2 topic hz /scan
 ros2 topic hz /sanehal_base_controller/odom
 ros2 run tf2_ros tf2_echo base_link hesai_lidar
-ros2 run tf2_ros tf2_echo odom base_link
+ros2 run tf2_ros tf2_echo odom base_footprint
 ros2 run tf2_ros tf2_echo map odom
 ros2 topic echo /map --once
 ```
@@ -132,10 +180,10 @@ the cloud timestamp; and verify that the scan's `hesai_lidar` frame has a TF at
 that timestamp. Empty or sparse height slices and delayed timestamps commonly
 look like SLAM or TF failures.
 
-The configured `scan_time: 0.1` is metadata for the nominal 10 Hz cloud rate;
-it does not throttle conversion. Change it to `0.2` if hardware measurement
-shows 5 Hz. Missing angular bins are published as `+inf`, `time_increment` is
-zero, and the converter does not populate LaserScan intensities. Keep
+The configured `scan_time: 0.2` is metadata for the 5 Hz cloud rate measured
+on the SANEHAL-2 JT16 serial connection; it does not throttle conversion.
+Missing angular bins are published as `+inf`, `time_increment` is zero, and the
+converter does not populate LaserScan intensities. Keep
 `queue_size: 1` initially to avoid accumulating stale point clouds. On the
 Raspberry Pi and over Wi-Fi, display the raw PointCloud2 only while debugging;
 its serialization and transport can materially increase CPU and bandwidth.

sanehal_bringup/config/pointcloud_to_laserscan_jt16.yaml

@@ -1,7 +1,7 @@
 pointcloud_to_laserscan:
   ros__parameters:
     # Preserve the JT16 cloud frame and timestamp. slam_toolbox transforms the
-    # resulting scan from hesai_lidar to base_link at the scan timestamp.
+    # resulting scan from hesai_lidar to base_footprint at the scan timestamp.
     target_frame: ""
     transform_tolerance: 0.01
     # Prefer dropping an old cloud to accumulating latency on the Raspberry Pi.
@@ -18,9 +18,8 @@ pointcloud_to_laserscan:
     angle_max: 3.14159264
     angle_increment: 0.010471976
 
-    # The checked-in JT16 driver configuration defaults to 10 Hz. Set this to
-    # the reciprocal of the measured cloud rate if the unit runs at 5 Hz.
-    scan_time: 0.1
+    # The SANEHAL-2 JT16 serial stream was measured at 5 Hz on hardware.
+    scan_time: 0.2
     range_min: 0.3
     range_max: 30.0
     use_inf: true

sanehal_bringup/config/slam_toolbox_jt16.yaml

@@ -10,10 +10,11 @@ slam_toolbox:
 
     # TF ownership:
     #   slam_toolbox: map -> odom
-    #   sanehal_base_controller: odom -> base_link
+    #   sanehal_base_controller: odom -> base_footprint
+    #   robot_state_publisher: base_footprint -> base_link -> sensor frames
     map_frame: map
     odom_frame: odom
-    base_frame: base_link
+    base_frame: base_footprint
     scan_topic: /scan
     mode: mapping
     use_map_saver: true

sanehal_bringup/launch/jt16.launch.py

@@ -1,12 +1,41 @@
+import os
+import time
+
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
     config_file = LaunchConfiguration('config_file')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+
+    def wait_for_devices(context):
+        if LaunchConfiguration('wait_for_devices').perform(context).lower() not in (
+            '1', 'true', 'yes', 'on'
+        ):
+            return []
+        devices = [LaunchConfiguration('rs485_device').perform(context)]
+        if LaunchConfiguration('require_rs232').perform(context).lower() in (
+            '1', 'true', 'yes', 'on'
+        ):
+            devices.append(LaunchConfiguration('rs232_device').perform(context))
+        timeout = float(LaunchConfiguration('device_wait_timeout').perform(context))
+        deadline = time.monotonic() + timeout
+        while time.monotonic() <= deadline:
+            missing = [
+                path for path in devices
+                if not (os.path.exists(path) and os.access(path, os.R_OK | os.W_OK))
+            ]
+            if not missing:
+                return [LogInfo(msg=f'JT16 serial devices are ready: {devices}')]
+            time.sleep(0.1)
+        raise RuntimeError(
+            f'JT16 devices not readable and writable after {timeout:.1f}s: {missing}. '
+            'Check the config file, udev rules, and dialout membership.'
+        )
 
     config_file_argument = DeclareLaunchArgument(
         'config_file',
@@ -15,13 +44,30 @@ def generate_launch_description():
         ),
         description='Full path to the Hesai JT16 driver configuration file.',
     )
+    arguments = [
+        config_file_argument,
+        DeclareLaunchArgument('use_sim_time', default_value='false'),
+        DeclareLaunchArgument('wait_for_devices', default_value='true'),
+        DeclareLaunchArgument('device_wait_timeout', default_value='10.0'),
+        DeclareLaunchArgument('rs485_device', default_value='/dev/jt16_rs485'),
+        DeclareLaunchArgument('rs232_device', default_value='/dev/jt16_rs232'),
+        DeclareLaunchArgument(
+            'require_rs232', default_value='true',
+            description=(
+                'Require the JT16 command port during preflight. Set false only '
+                'when the driver config supplies a correction_file_path.'
+            ),
+        ),
+    ]
 
     jt16_driver = Node(
         package='hesai_ros_driver',
         executable='hesai_ros_driver_node',
         name='hesai_ros_driver_node',
         output='screen',
-        parameters=[{'config_path': config_file}],
+        parameters=[{'config_path': config_file, 'use_sim_time': use_sim_time}],
     )
 
-    return LaunchDescription([config_file_argument, jt16_driver])
+    return LaunchDescription(
+        arguments + [OpaqueFunction(function=wait_for_devices), jt16_driver]
+    )