diff --git a/README.md b/README.md index 0ca63d6..418d054 100644 --- a/README.md +++ b/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 +``` diff --git a/sanehal_bringup/CMakeLists.txt b/sanehal_bringup/CMakeLists.txt index 7bb0993..644cf21 100644 --- a/sanehal_bringup/CMakeLists.txt +++ b/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( diff --git a/sanehal_bringup/README.md b/sanehal_bringup/README.md index 797b636..87d666d 100644 --- a/sanehal_bringup/README.md +++ b/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,7 +47,8 @@ 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 @@ -54,9 +56,8 @@ a valid `correction_file_path` instead of leaving it empty. 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. diff --git a/sanehal_bringup/config/pointcloud_to_laserscan_jt16.yaml b/sanehal_bringup/config/pointcloud_to_laserscan_jt16.yaml index 3bef36f..1ccdb53 100644 --- a/sanehal_bringup/config/pointcloud_to_laserscan_jt16.yaml +++ b/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 diff --git a/sanehal_bringup/config/slam_toolbox_jt16.yaml b/sanehal_bringup/config/slam_toolbox_jt16.yaml index 53f29b7..7b495a1 100644 --- a/sanehal_bringup/config/slam_toolbox_jt16.yaml +++ b/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 diff --git a/sanehal_bringup/launch/jt16.launch.py b/sanehal_bringup/launch/jt16.launch.py index 9b5d7fe..5306abe 100644 --- a/sanehal_bringup/launch/jt16.launch.py +++ b/sanehal_bringup/launch/jt16.launch.py @@ -1,5 +1,8 @@ +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 @@ -7,6 +10,32 @@ 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] + ) diff --git a/sanehal_bringup/launch/sanehal.launch.py b/sanehal_bringup/launch/sanehal.launch.py index f14aa59..604a093 100644 --- a/sanehal_bringup/launch/sanehal.launch.py +++ b/sanehal_bringup/launch/sanehal.launch.py @@ -1,51 +1,164 @@ from launch import LaunchDescription -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.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node, SetRemap from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - sanehal_bringup = FindPackageShare('sanehal_bringup') - use_sim_time = LaunchConfiguration('use_sim_time') - start_lidar = LaunchConfiguration('start_lidar') - jt16_config_file = LaunchConfiguration('jt16_config_file') + bringup_share = FindPackageShare('sanehal_bringup') + description_share = FindPackageShare('sanehal_vehicle_description') + slam_toolbox_share = FindPackageShare('slam_toolbox') + + names = [ + 'use_sim_time', 'use_mock_hardware', 'start_description', 'start_control', + 'start_lidar', 'start_pointcloud_to_laserscan', 'start_slam', 'start_rviz', + 'wait_for_devices', 'device_wait_timeout', 'dynamixel_port', + 'dynamixel_baud_rate', 'controllers_file', 'jt16_config_file', + 'converter_params_file', 'slam_params_file', 'rviz_config_file', + 'pointcloud_topic', 'scan_topic', 'jt16_rs485_device', 'jt16_rs232_device', + 'require_jt16_rs232', + ] + cfg = {name: LaunchConfiguration(name) for name in names} arguments = [ + DeclareLaunchArgument('use_sim_time', default_value='false'), + DeclareLaunchArgument('use_mock_hardware', default_value='false'), + DeclareLaunchArgument('start_description', default_value='true'), + DeclareLaunchArgument('start_control', default_value='true'), + DeclareLaunchArgument('start_lidar', default_value='true'), + DeclareLaunchArgument('start_pointcloud_to_laserscan', default_value='true'), + DeclareLaunchArgument('start_slam', default_value='true'), DeclareLaunchArgument( - 'use_sim_time', - default_value='false', - description='Use a simulation clock for the robot stack.', + 'start_rviz', default_value='false', + description='Start RViz locally. Keep false on the Raspberry Pi.', ), + DeclareLaunchArgument('wait_for_devices', default_value='true'), + DeclareLaunchArgument('device_wait_timeout', default_value='10.0'), + DeclareLaunchArgument('dynamixel_port', default_value='/dev/dxhub'), + DeclareLaunchArgument('dynamixel_baud_rate', default_value='1000000'), DeclareLaunchArgument( - 'start_lidar', - default_value='true', - description='Start the Hesai JT16 driver.', + 'controllers_file', + default_value=PathJoinSubstitution([ + description_share, 'controllers', 'sanehal_controllers.yaml', + ]), ), DeclareLaunchArgument( 'jt16_config_file', - default_value=PathJoinSubstitution( - [sanehal_bringup, 'config', 'jt16_serial.yaml'] + default_value=PathJoinSubstitution([ + bringup_share, 'config', 'jt16_serial.yaml', + ]), + ), + DeclareLaunchArgument( + 'converter_params_file', + default_value=PathJoinSubstitution([ + bringup_share, 'config', 'pointcloud_to_laserscan_jt16.yaml', + ]), + ), + DeclareLaunchArgument( + 'slam_params_file', + default_value=PathJoinSubstitution([ + bringup_share, 'config', 'slam_toolbox_jt16.yaml', + ]), + ), + DeclareLaunchArgument( + 'rviz_config_file', + default_value=PathJoinSubstitution([ + bringup_share, 'config', 'slam_jt16.rviz', + ]), + ), + DeclareLaunchArgument('pointcloud_topic', default_value='/lidar_points'), + DeclareLaunchArgument('scan_topic', default_value='/scan'), + DeclareLaunchArgument('jt16_rs485_device', default_value='/dev/jt16_rs485'), + DeclareLaunchArgument('jt16_rs232_device', default_value='/dev/jt16_rs232'), + DeclareLaunchArgument( + 'require_jt16_rs232', default_value='true', + description=( + 'Require the JT16 RS232 port during preflight. Set false only ' + 'when jt16_config_file supplies a correction_file_path.' ), - description='Full path to the Hesai JT16 driver configuration file.', ), ] vehicle = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [sanehal_bringup, 'launch', 'sanehal_on_pi.launch.py'] - ) - ), - launch_arguments={'use_sim_time': use_sim_time}.items(), + PythonLaunchDescriptionSource(PathJoinSubstitution([ + bringup_share, 'launch', 'sanehal_on_pi.launch.py', + ])), + launch_arguments={ + 'use_sim_time': cfg['use_sim_time'], + 'use_mock_hardware': cfg['use_mock_hardware'], + 'start_description': cfg['start_description'], + 'start_control': cfg['start_control'], + 'wait_for_devices': cfg['wait_for_devices'], + 'device_wait_timeout': cfg['device_wait_timeout'], + 'dynamixel_port': cfg['dynamixel_port'], + 'dynamixel_baud_rate': cfg['dynamixel_baud_rate'], + 'controllers_file': cfg['controllers_file'], + }.items(), ) - lidar = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([sanehal_bringup, 'launch', 'jt16.launch.py']) - ), - launch_arguments={'config_file': jt16_config_file}.items(), - condition=IfCondition(start_lidar), + + lidar = GroupAction( + actions=[ + SetRemap(src='/lidar_points', dst=cfg['pointcloud_topic']), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([ + bringup_share, 'launch', 'jt16.launch.py', + ])), + launch_arguments={ + 'config_file': cfg['jt16_config_file'], + 'use_sim_time': cfg['use_sim_time'], + 'wait_for_devices': cfg['wait_for_devices'], + 'device_wait_timeout': cfg['device_wait_timeout'], + 'rs485_device': cfg['jt16_rs485_device'], + 'rs232_device': cfg['jt16_rs232_device'], + 'require_rs232': cfg['require_jt16_rs232'], + }.items(), + ), + ], + condition=IfCondition(cfg['start_lidar']), + ) + + converter = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([ + bringup_share, 'launch', 'pointcloud_to_laserscan.launch.py', + ])), + launch_arguments={ + 'use_sim_time': cfg['use_sim_time'], + 'pointcloud_topic': cfg['pointcloud_topic'], + 'scan_topic': cfg['scan_topic'], + 'converter_params_file': cfg['converter_params_file'], + }.items(), + condition=IfCondition(cfg['start_pointcloud_to_laserscan']), + ) + + slam = GroupAction( + actions=[ + SetRemap(src='/scan', dst=cfg['scan_topic']), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([ + slam_toolbox_share, 'launch', 'online_async_launch.py', + ])), + launch_arguments={ + 'slam_params_file': cfg['slam_params_file'], + 'use_sim_time': cfg['use_sim_time'], + 'autostart': 'true', + }.items(), + ), + ], + condition=IfCondition(cfg['start_slam']), + ) + + rviz = Node( + package='rviz2', executable='rviz2', name='rviz2', output='screen', + arguments=['-d', cfg['rviz_config_file']], + parameters=[{'use_sim_time': cfg['use_sim_time']}], + remappings=[ + ('/lidar_points', cfg['pointcloud_topic']), + ('/scan', cfg['scan_topic']), + ], + condition=IfCondition(cfg['start_rviz']), ) - return LaunchDescription(arguments + [vehicle, lidar]) + return LaunchDescription(arguments + [vehicle, lidar, converter, slam, rviz]) diff --git a/sanehal_bringup/launch/sanehal_on_pi.launch.py b/sanehal_bringup/launch/sanehal_on_pi.launch.py index 7ef48a5..4ad275b 100644 --- a/sanehal_bringup/launch/sanehal_on_pi.launch.py +++ b/sanehal_bringup/launch/sanehal_on_pi.launch.py @@ -1,115 +1,127 @@ -# Copyright 2020 ros2_control Development Team -# -# 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. +import os +import time from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.substitutions import ( - Command, - FindExecutable, - LaunchConfiguration, - PathJoinSubstitution, -) +from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction +from launch.conditions import IfCondition +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue from launch_ros.substitutions import FindPackageShare +def _as_bool(value): + return value.lower() in ('1', 'true', 'yes', 'on') + + +def _wait_for_dynamixel(context): + if not _as_bool(LaunchConfiguration('start_control').perform(context)): + return [] + if _as_bool(LaunchConfiguration('use_mock_hardware').perform(context)): + return [LogInfo(msg='Mock ros2_control hardware selected; skipping device check.')] + if not _as_bool(LaunchConfiguration('wait_for_devices').perform(context)): + return [] + + device = LaunchConfiguration('dynamixel_port').perform(context) + timeout = float(LaunchConfiguration('device_wait_timeout').perform(context)) + deadline = time.monotonic() + timeout + while time.monotonic() <= deadline: + if os.path.exists(device) and os.access(device, os.R_OK | os.W_OK): + return [LogInfo(msg=f'Dynamixel device is ready: {device}')] + time.sleep(0.1) + raise RuntimeError( + f'Dynamixel device {device} did not become readable and writable ' + f'within {timeout:.1f} seconds. Check udev rules and dialout membership.' + ) + + def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') - use_sim_time_argument = DeclareLaunchArgument( - 'use_sim_time', - default_value='false', - description='Use a simulation clock instead of the hardware clock.', - ) + use_mock_hardware = LaunchConfiguration('use_mock_hardware') + start_description = LaunchConfiguration('start_description') + start_control = LaunchConfiguration('start_control') + dynamixel_port = LaunchConfiguration('dynamixel_port') + dynamixel_baud_rate = LaunchConfiguration('dynamixel_baud_rate') + controllers_file = LaunchConfiguration('controllers_file') - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name='xacro')]), - ' ', - PathJoinSubstitution( - [ - FindPackageShare('sanehal_vehicle_description'), - 'urdf', - 'sanehal.urdf.xacro' - ] - ), - ] - ) - robot_description = { - 'robot_description': ParameterValue( - robot_description_content, - value_type=str, - ) - } + arguments = [ + DeclareLaunchArgument('use_sim_time', default_value='false'), + DeclareLaunchArgument( + 'use_mock_hardware', default_value='false', + description='Use ros2_control GenericSystem instead of real Dynamixel hardware.', + ), + DeclareLaunchArgument( + 'start_description', default_value='true', + description='Start robot_state_publisher.', + ), + DeclareLaunchArgument( + 'start_control', default_value='true', + description='Start controller_manager and wheel controllers.', + ), + DeclareLaunchArgument( + 'wait_for_devices', default_value='true', + description='Wait for required hardware devices before starting.', + ), + DeclareLaunchArgument( + 'device_wait_timeout', default_value='10.0', + description='Maximum seconds to wait for a hardware device.', + ), + DeclareLaunchArgument( + 'dynamixel_port', default_value='/dev/dxhub', + description='Dynamixel serial device.', + ), + DeclareLaunchArgument( + 'dynamixel_baud_rate', default_value='1000000', + description='Dynamixel bus baud rate.', + ), + DeclareLaunchArgument( + 'controllers_file', + default_value=PathJoinSubstitution([ + FindPackageShare('sanehal_vehicle_description'), + 'controllers', 'sanehal_controllers.yaml', + ]), + description='ros2_control controller parameter file.', + ), + ] - robot_controllers = PathJoinSubstitution( - [ + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([ FindPackageShare('sanehal_vehicle_description'), - 'controllers', - 'sanehal_controllers.yaml', - ] - ) + 'urdf', 'sanehal.urdf.xacro', + ]), + ' use_mock_hardware:=', use_mock_hardware, + ' dynamixel_port:=', dynamixel_port, + ' dynamixel_baud_rate:=', dynamixel_baud_rate, + ]) + robot_description = { + 'robot_description': ParameterValue(robot_description_content, value_type=str) + } control_node = Node( - package='controller_manager', - executable='ros2_control_node', - parameters=[ - robot_description, - robot_controllers, - {'use_sim_time': use_sim_time}, - ], - output='both', - # arguments=['--ros-args', '--log-level', logger] + package='controller_manager', executable='ros2_control_node', + parameters=[robot_description, controllers_file, {'use_sim_time': use_sim_time}], + output='both', condition=IfCondition(start_control), ) - robot_state_pub_node = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='both', - parameters=[robot_description, {'use_sim_time': use_sim_time}], + robot_state_publisher = Node( + package='robot_state_publisher', executable='robot_state_publisher', + output='both', parameters=[robot_description, {'use_sim_time': use_sim_time}], + condition=IfCondition(start_description), ) - - joint_state_broadcaster_spawner = Node( - package='controller_manager', - executable='spawner', + controller_spawner = Node( + package='controller_manager', executable='spawner', output='screen', arguments=[ - 'joint_state_broadcaster', - '--controller-manager', - '/controller_manager' + 'joint_state_broadcaster', 'sanehal_base_controller', + '--controller-manager', '/controller_manager', + '--controller-manager-timeout', '30', + '--service-call-timeout', '10', '--switch-timeout', '10', + '--activate-as-group', ], + condition=IfCondition(start_control), ) - robot_controller_spawner = Node( - package='controller_manager', - executable='spawner', - arguments=['sanehal_base_controller', '-c', '/controller_manager'], - ) - - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], - ) - ) - - nodes = [ - use_sim_time_argument, - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_robot_controller_spawner, - ] - - return LaunchDescription(nodes) + return LaunchDescription(arguments + [ + OpaqueFunction(function=_wait_for_dynamixel), + control_node, robot_state_publisher, controller_spawner, + ]) diff --git a/sanehal_bringup/launch/slam.launch.py b/sanehal_bringup/launch/slam.launch.py index f13a5db..90b4cff 100644 --- a/sanehal_bringup/launch/slam.launch.py +++ b/sanehal_bringup/launch/slam.launch.py @@ -1,173 +1,64 @@ +"""Compatibility entry point for the integrated Robot-side bringup.""" + from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - GroupAction, - IncludeLaunchDescription, -) -from launch.conditions import IfCondition +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.actions import SetRemap from launch_ros.substitutions import FindPackageShare def generate_launch_description(): bringup_share = FindPackageShare('sanehal_bringup') - slam_toolbox_share = FindPackageShare('slam_toolbox') - - use_sim_time = LaunchConfiguration('use_sim_time') - start_robot_bringup = LaunchConfiguration('start_robot_bringup') - start_lidar = LaunchConfiguration('start_lidar') - start_pointcloud_to_laserscan = LaunchConfiguration( - 'start_pointcloud_to_laserscan' - ) - start_rviz = LaunchConfiguration('start_rviz') - jt16_config_file = LaunchConfiguration('jt16_config_file') - converter_params_file = LaunchConfiguration('converter_params_file') - pointcloud_topic = LaunchConfiguration('pointcloud_topic') - scan_topic = LaunchConfiguration('scan_topic') - slam_params_file = LaunchConfiguration('slam_params_file') - rviz_config_file = LaunchConfiguration('rviz_config_file') - + names = [ + 'use_sim_time', 'start_robot_bringup', 'start_lidar', + 'start_pointcloud_to_laserscan', 'start_rviz', 'jt16_config_file', + 'converter_params_file', 'pointcloud_topic', 'scan_topic', + 'slam_params_file', 'rviz_config_file', + ] + cfg = {name: LaunchConfiguration(name) for name in names} + defaults = { + 'use_sim_time': 'false', + 'start_robot_bringup': 'true', + 'start_lidar': 'true', + 'start_pointcloud_to_laserscan': 'true', + 'start_rviz': 'true', + 'jt16_config_file': PathJoinSubstitution([ + bringup_share, 'config', 'jt16_serial.yaml', + ]), + 'converter_params_file': PathJoinSubstitution([ + bringup_share, 'config', 'pointcloud_to_laserscan_jt16.yaml', + ]), + 'pointcloud_topic': '/lidar_points', + 'scan_topic': '/scan', + 'slam_params_file': PathJoinSubstitution([ + bringup_share, 'config', 'slam_toolbox_jt16.yaml', + ]), + 'rviz_config_file': PathJoinSubstitution([ + bringup_share, 'config', 'slam_jt16.rviz', + ]), + } arguments = [ - DeclareLaunchArgument( - 'use_sim_time', - default_value='false', - description='Use a simulation clock instead of the hardware clock.', - ), - DeclareLaunchArgument( - 'start_robot_bringup', - default_value='true', - description='Start robot_state_publisher, ros2_control, and wheel odometry.', - ), - DeclareLaunchArgument( - 'start_lidar', - default_value='true', - description='Start the Hesai JT16 driver.', - ), - DeclareLaunchArgument( - 'start_pointcloud_to_laserscan', - default_value='true', - description='Convert the JT16 PointCloud2 topic to LaserScan.', - ), - DeclareLaunchArgument( - 'start_rviz', - default_value='true', - description='Start RViz with the JT16 SLAM display configuration.', - ), - DeclareLaunchArgument( - 'jt16_config_file', - default_value=PathJoinSubstitution( - [bringup_share, 'config', 'jt16_serial.yaml'] - ), - description='Full path to the Hesai JT16 driver configuration file.', - ), - DeclareLaunchArgument( - 'converter_params_file', - default_value=PathJoinSubstitution( - [bringup_share, 'config', 'pointcloud_to_laserscan_jt16.yaml'] - ), - description='PointCloud2-to-LaserScan parameter file.', - ), - DeclareLaunchArgument( - 'pointcloud_topic', - default_value='/lidar_points', - description='JT16 PointCloud2 input topic.', - ), - DeclareLaunchArgument( - 'scan_topic', - default_value='/scan', - description='LaserScan topic consumed by slam_toolbox.', - ), - DeclareLaunchArgument( - 'slam_params_file', - default_value=PathJoinSubstitution( - [bringup_share, 'config', 'slam_toolbox_jt16.yaml'] - ), - description='Full path to the slam_toolbox parameter file.', - ), - DeclareLaunchArgument( - 'rviz_config_file', - default_value=PathJoinSubstitution( - [bringup_share, 'config', 'slam_jt16.rviz'] - ), - description='Full path to the RViz configuration.', - ), + DeclareLaunchArgument(name, default_value=defaults[name]) for name in names ] - robot_bringup = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [bringup_share, 'launch', 'sanehal_on_pi.launch.py'] - ) - ), - launch_arguments={'use_sim_time': use_sim_time}.items(), - condition=IfCondition(start_robot_bringup), - ) - - lidar = GroupAction( - actions=[ - SetRemap(src='/lidar_points', dst=pointcloud_topic), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [bringup_share, 'launch', 'jt16.launch.py'] - ) - ), - launch_arguments={'config_file': jt16_config_file}.items(), - ), - ], - condition=IfCondition(start_lidar), - ) - - converter = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [bringup_share, 'launch', 'pointcloud_to_laserscan.launch.py'] - ) - ), + integrated_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([ + bringup_share, 'launch', 'sanehal.launch.py', + ])), launch_arguments={ - 'use_sim_time': use_sim_time, - 'pointcloud_topic': pointcloud_topic, - 'scan_topic': scan_topic, - 'converter_params_file': converter_params_file, + 'use_sim_time': cfg['use_sim_time'], + 'start_description': cfg['start_robot_bringup'], + 'start_control': cfg['start_robot_bringup'], + 'start_lidar': cfg['start_lidar'], + 'start_pointcloud_to_laserscan': cfg['start_pointcloud_to_laserscan'], + 'start_slam': 'true', + 'start_rviz': cfg['start_rviz'], + 'jt16_config_file': cfg['jt16_config_file'], + 'converter_params_file': cfg['converter_params_file'], + 'pointcloud_topic': cfg['pointcloud_topic'], + 'scan_topic': cfg['scan_topic'], + 'slam_params_file': cfg['slam_params_file'], + 'rviz_config_file': cfg['rviz_config_file'], }.items(), - condition=IfCondition(start_pointcloud_to_laserscan), - ) - - async_slam = GroupAction( - actions=[ - SetRemap(src='/scan', dst=scan_topic), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [slam_toolbox_share, 'launch', 'online_async_launch.py'] - ) - ), - launch_arguments={ - 'slam_params_file': slam_params_file, - 'use_sim_time': use_sim_time, - 'autostart': 'true', - }.items(), - ), - ] - ) - - rviz = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - output='screen', - arguments=['-d', rviz_config_file], - parameters=[{'use_sim_time': use_sim_time}], - remappings=[ - ('/lidar_points', pointcloud_topic), - ('/scan', scan_topic), - ], - condition=IfCondition(start_rviz), - ) - - return LaunchDescription( - arguments + [robot_bringup, lidar, converter, async_slam, rviz] ) + return LaunchDescription(arguments + [integrated_bringup]) diff --git a/sanehal_bringup/package.xml b/sanehal_bringup/package.xml index 3dca778..9b449b3 100644 --- a/sanehal_bringup/package.xml +++ b/sanehal_bringup/package.xml @@ -23,11 +23,13 @@ launch_ros ros2_control ros2_controllers + hardware_interface dynamixel_hardware_interface hesai_ros_driver pointcloud_to_laserscan slam_toolbox nav2_map_server + sanehal_vehicle_description ament_lint_auto ament_lint_common @@ -35,6 +37,8 @@ launch_testing_ament_cmake rclpy sensor_msgs_py + nav_msgs + tf2_ros std_msgs diff --git a/sanehal_bringup/test/test_pointcloud_to_laserscan.py b/sanehal_bringup/test/test_pointcloud_to_laserscan.py index fbb862c..cb20801 100644 --- a/sanehal_bringup/test/test_pointcloud_to_laserscan.py +++ b/sanehal_bringup/test/test_pointcloud_to_laserscan.py @@ -103,7 +103,7 @@ def test_projection_contract(self): self.assertAlmostEqual(scan.angle_min, -3.14159265, places=6) self.assertAlmostEqual(scan.angle_max, 3.14159264, places=6) self.assertAlmostEqual(scan.angle_increment, 0.010471976, places=7) - self.assertAlmostEqual(scan.scan_time, 0.1, places=6) + self.assertAlmostEqual(scan.scan_time, 0.2, places=6) self.assertAlmostEqual(scan.range_min, 0.3, places=6) self.assertAlmostEqual(scan.range_max, 30.0, places=6) self.assertEqual(len(scan.ranges), 600) diff --git a/sanehal_bringup/test/test_robot_bringup.py b/sanehal_bringup/test/test_robot_bringup.py new file mode 100644 index 0000000..3996116 --- /dev/null +++ b/sanehal_bringup/test/test_robot_bringup.py @@ -0,0 +1,93 @@ +import time +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +import launch_testing.actions +from nav_msgs.msg import Odometry +import pytest +import rclpy +from sensor_msgs.msg import JointState +from tf2_ros import Buffer, TransformListener + + +@pytest.mark.launch_test +def generate_test_description(): + launch_file = ( + get_package_share_directory('sanehal_bringup') + + '/launch/sanehal.launch.py' + ) + bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource(launch_file), + launch_arguments={ + 'use_mock_hardware': 'true', + 'wait_for_devices': 'false', + 'start_lidar': 'false', + 'start_pointcloud_to_laserscan': 'false', + 'start_slam': 'false', + 'start_rviz': 'false', + }.items(), + ) + return LaunchDescription([bringup, launch_testing.actions.ReadyToTest()]) + + +class TestRobotBringup(unittest.TestCase): + + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node('test_robot_bringup') + self.joint_state = None + self.odom = None + self.node.create_subscription( + JointState, '/joint_states', self._on_joint_state, 10 + ) + self.node.create_subscription( + Odometry, '/sanehal_base_controller/odom', self._on_odom, 10 + ) + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self.node) + + def tearDown(self): + self.node.destroy_node() + + def _on_joint_state(self, message): + self.joint_state = message + + def _on_odom(self, message): + self.odom = message + + def test_controller_topics_and_tf_contract(self): + deadline = time.monotonic() + 20.0 + transforms_ready = False + while time.monotonic() < deadline: + rclpy.spin_once(self.node, timeout_sec=0.1) + transforms_ready = ( + self.tf_buffer.can_transform('odom', 'base_footprint', rclpy.time.Time()) + and self.tf_buffer.can_transform( + 'base_footprint', 'base_link', rclpy.time.Time() + ) + and self.tf_buffer.can_transform( + 'base_footprint', 'hesai_lidar', rclpy.time.Time() + ) + ) + if self.joint_state is not None and self.odom is not None and transforms_ready: + break + + self.assertIsNotNone(self.joint_state) + self.assertEqual( + set(self.joint_state.name), + {'left_wheel_joint', 'right_wheel_joint'}, + ) + self.assertIsNotNone(self.odom) + self.assertEqual(self.odom.header.frame_id, 'odom') + self.assertEqual(self.odom.child_frame_id, 'base_footprint') + self.assertTrue(transforms_ready) diff --git a/sanehal_vehicle_description/README.md b/sanehal_vehicle_description/README.md index 67c624f..ae67eb4 100644 --- a/sanehal_vehicle_description/README.md +++ b/sanehal_vehicle_description/README.md @@ -7,6 +7,11 @@ JT16のframeは `sanehal_base_link` からfixed joint `jt16_joint` で接続す 同じ値にしてください。TFは `robot_state_publisher` が配信するため、driverの launchから同じstatic TFを重複配信しないでください。 +走行中のTF所有者は、`sanehal_base_controller`が +`odom -> base_footprint`、`robot_state_publisher`が +`base_footprint -> base_link -> sanehal_base_link -> hesai_lidar`です。 +`slam_toolbox`だけが`map -> odom`を配信します。 + SANEHAL-2で実測した取付TFは `xyz="-0.080 0 0.250"` [m]、`rpy="0 0 0"` [rad] です。`hesai_lidar` の物理基準はJT16底面中心で、コネクタ面は機体後方を向きます。 diff --git a/sanehal_vehicle_description/controllers/sanehal_controllers.yaml b/sanehal_vehicle_description/controllers/sanehal_controllers.yaml index acad496..249a15e 100644 --- a/sanehal_vehicle_description/controllers/sanehal_controllers.yaml +++ b/sanehal_vehicle_description/controllers/sanehal_controllers.yaml @@ -27,7 +27,9 @@ sanehal_base_controller: publish_rate: 50.0 odom_frame_id: odom - base_frame_id: base_link + # diff_drive_controller is the sole owner of odom -> base_footprint. + # robot_state_publisher owns base_footprint -> base_link and all child TFs. + base_frame_id: base_footprint pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] diff --git a/sanehal_vehicle_description/package.xml b/sanehal_vehicle_description/package.xml index 7c73fe6..aa474a0 100644 --- a/sanehal_vehicle_description/package.xml +++ b/sanehal_vehicle_description/package.xml @@ -19,7 +19,9 @@ launch_ros ros2_control ros2_controllers + hardware_interface dynamixel_hardware_interface + mock_components ament_lint_auto ament_lint_common diff --git a/sanehal_vehicle_description/ros2_control/sanehal.ros2_control.xacro b/sanehal_vehicle_description/ros2_control/sanehal.ros2_control.xacro index a6769b0..a68188a 100644 --- a/sanehal_vehicle_description/ros2_control/sanehal.ros2_control.xacro +++ b/sanehal_vehicle_description/ros2_control/sanehal.ros2_control.xacro @@ -1,22 +1,29 @@ - + - dynamixel_hardware_interface/DynamixelHardware - /dev/dxhub - 1000000 - 500 - - /param/dxl_model - 2 - 2 - true - - 1, 0, 0, -1 - 1, 0, 0, -1 + + mock_components/GenericSystem + false + + + dynamixel_hardware_interface/DynamixelHardware + ${dynamixel_port} + ${dynamixel_baud_rate} + 500 + + /param/dxl_model + 2 + 2 + true + + 1, 0, 0, -1 + 1, 0, 0, -1 + @@ -29,7 +36,8 @@ - + + dxl 2 @@ -38,8 +46,8 @@ 1 0 - - + + dxl 1 @@ -48,7 +56,8 @@ 1 0 - + + diff --git a/sanehal_vehicle_description/urdf/sanehal.urdf.xacro b/sanehal_vehicle_description/urdf/sanehal.urdf.xacro index 5e71b46..935386f 100644 --- a/sanehal_vehicle_description/urdf/sanehal.urdf.xacro +++ b/sanehal_vehicle_description/urdf/sanehal.urdf.xacro @@ -2,6 +2,9 @@ + + + @@ -16,7 +19,13 @@ + + + + + + + name="Sanehal" + prefix="$(arg prefix)" + use_mock_hardware="$(arg use_mock_hardware)" + dynamixel_port="$(arg dynamixel_port)" + dynamixel_baud_rate="$(arg dynamixel_baud_rate)" /> diff --git a/sanehal_vehicle_description/urdf/sanehal_display.urdf.xacro b/sanehal_vehicle_description/urdf/sanehal_display.urdf.xacro index 94d3968..059875f 100644 --- a/sanehal_vehicle_description/urdf/sanehal_display.urdf.xacro +++ b/sanehal_vehicle_description/urdf/sanehal_display.urdf.xacro @@ -12,7 +12,13 @@ + + + + + +