From 1fe58030638bde220650fa52f022297791e9d75e Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Fri, 27 Mar 2026 19:33:49 +0100 Subject: [PATCH] Add SLAM --- avr/motor_ctrl/main.c | 2 +- avr/motor_ctrl/main.hex | 14 ++++---- docker/Dockerfile | 3 +- launch/a4wd3_launch.py | 65 ++++++++++++++++++++++++++++++++-- maps/.gitignore | 1 + params/robot_localization.yaml | 8 ++--- scripts/engine_man_test.py | 16 ++++----- src/hw_node.cpp | 13 ++++++- 8 files changed, 97 insertions(+), 25 deletions(-) create mode 100644 maps/.gitignore diff --git a/avr/motor_ctrl/main.c b/avr/motor_ctrl/main.c index 6867e78..f588c35 100644 --- a/avr/motor_ctrl/main.c +++ b/avr/motor_ctrl/main.c @@ -120,7 +120,7 @@ // wheel diameter=15.8cm, encoder=12*4cpr, gear ratio=1:51 // STEP_PER_M = 48*51/(d*pi) // Left real diameter: 0.12808, Right real diameter: 0.121 -#define STEP_PER_M 4969.2 +#define STEP_PER_M 5051.7 #define STEP_PER_M_LEFT (STEP_PER_M) #define STEP_PER_M_RIGHT (STEP_PER_M) #define WHEEL_DIST 0.49066 // Measured: 0.36 diff --git a/avr/motor_ctrl/main.hex b/avr/motor_ctrl/main.hex index a139dff..d903f61 100644 --- a/avr/motor_ctrl/main.hex +++ b/avr/motor_ctrl/main.hex @@ -87,8 +87,8 @@ :100560009A0090939B00A0939C00B0939D0084CF31 :1005700023B160919A0070919B0080919C009091B2 :100580009D00982F872F762F6627622B60939A0005 -:1005900070939B0080939C0090939D002AE939E41E -:1005A0004BE955E40E946D120E944111462F572FCE +:1005900070939B0080939C0090939D002AE93DED11 +:1005A0004DE955E40E946D120E944111462F572FCC :1005B0005093D5004093D4005093D3004093D20081 :1005C00081E08093DB00A0CF83B190E0A0E0B0E0B9 :1005D000C6CF23B180919A0090919B00A0919C007E @@ -233,9 +233,9 @@ :100E80002AEC37E34BEF5EE30E946D1220E030E086 :100E900040E05FE30E946D12A70196010E94591085 :100EA0008B018D839983A7019601C701B6010E942A -:100EB000591098014D8159810E9458102AE939E44E -:100EC0004BE955E40E946D126B017C012AE939E47B -:100ED0004BE955E4B8018D8199810E946D128B0117 +:100EB000591098014D8159810E9458102AE93DED41 +:100EC0004DE955E40E946D126B017C012AE93DED6C +:100ED0004DE955E4B8018D8199810E946D128B0115 :100EE0008D8399838091A400811509F45CC360917E :100EF000A4009101261B3109B901072E000C880BB3 :100F0000990B0E9479119601A7010E946D1220E0B1 @@ -272,9 +272,9 @@ :1010F000880B990B0E9479112AE037ED43E25CE3FB :101100000E94CF100E9441116D877E878F87988B38 :10111000B8016C0D7D1D072E000C880B990B0E94E9 -:1011200079112AE939E44BE156E40E94CF104B01D2 +:1011200079112AE93DED4DE156E40E94CF104B01C3 :101130005C01698D7A8D6E0D7F1D072E000C880B6A -:10114000990B0E9479112AE939E44BE156E40E9497 +:10114000990B0E9479112AE93DED4DE156E40E9488 :10115000CF102B013C019401A5010E9458102AECEC :1011600037E34BEF5EE30E94CF106D8F7E8F182F19 :10117000092F2091AF003091B0004091B100509103 diff --git a/docker/Dockerfile b/docker/Dockerfile index d20b209..b2e4b01 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -3,4 +3,5 @@ FROM ros:jazzy-ros-base RUN apt-get update && \ apt-get install -y ros-jazzy-rplidar-ros less libi2c-dev ros-jazzy-robot-localization \ ros-jazzy-slam-toolbox ros-jazzy-navigation2 ros-jazzy-nav2-bringup \ - ros-jazzy-demo-nodes-cpp ros-jazzy-teleop-twist-keyboard ros-jazzy-gpsd-client libgdal-dev ros-jazzy-teleop-twist-joy ros-jazzy-twist-mux ros-jazzy-gpsd-client + ros-jazzy-demo-nodes-cpp ros-jazzy-teleop-twist-keyboard ros-jazzy-gpsd-client libgdal-dev ros-jazzy-teleop-twist-joy ros-jazzy-twist-mux ros-jazzy-gpsd-client \ + ros-jazzy-slam-toolbox ros-jazzy-navigation2 ros-jazzy-nav2-bringup ros-jazzy-rplidar-ros diff --git a/launch/a4wd3_launch.py b/launch/a4wd3_launch.py index 0a55de4..251c1bc 100644 --- a/launch/a4wd3_launch.py +++ b/launch/a4wd3_launch.py @@ -5,15 +5,41 @@ import os from ament_index_python.packages import get_package_share_directory from launch.launch_description_sources import PythonLaunchDescriptionSource from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument -from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, EmitEvent, RegisterEventHandler, LogInfo +from launch.events import matches_action +from launch_ros.actions import Node, LifecycleNode +from launch_ros.events.lifecycle import ChangeState +from launch_ros.event_handlers import OnStateTransition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.conditions import IfCondition +from lifecycle_msgs.msg import Transition +# save map: ros2 run nav2_map_server map_saver_cli -f /mapfolder/mapfilename def generate_launch_description(): with_gps = LaunchConfiguration('with_gps', default=os.path.exists("/dev/serial/by-id/usb-u-blox_AG_-_www.u-blox.com_u-blox_GNSS_receiver-if00")) + with_nav = LaunchConfiguration('with_nav', default=os.path.exists("/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_ee207775dd54894783cd016372304813-if00-port0")) + map_file_name = LaunchConfiguration('map_file_name', default='') + slam_mode = LaunchConfiguration('slam_mode', default='mapping') + + start_async_slam_toolbox_node = LifecycleNode( + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + output="screen", + parameters=[os.path.join(get_package_share_directory('slam_toolbox'), 'config', 'mapper_params_online_async.yaml'), { + 'map_file_name': PathJoinSubstitution([os.path.join(get_package_share_directory("a4wd3"), "maps"), map_file_name]) , + 'mode': slam_mode, + 'map_start_at_dock': True, + 'max_laser_range': 16.0, + 'base_frame': 'base_link' + }], + namespace='', + ) + return LaunchDescription([ DeclareLaunchArgument('with_gps', default_value=with_gps, description='Enable GPS'), + DeclareLaunchArgument('map_file_name', default_value=map_file_name, description='Slam Toolbox map filename'), + DeclareLaunchArgument('slam_mode', default_value=slam_mode, description='Slam Toolbox mode mapping/localization'), Node( package='a4wd3', executable='hw_node', @@ -49,6 +75,13 @@ def generate_launch_description(): arguments = ['--x', '-0.04', '--y', '-0.035', '--frame-id', 'base_link', '--child-frame-id', 'gps'], output="screen" ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + name='tf_base_scan', + arguments = ['--x', '0.08125', '--y', '0.07425', '--yaw', '3.141592653589793', '--frame-id', 'base_link', '--child-frame-id', 'laser'], + output="screen" + ), Node( package='robot_localization', executable='ekf_node', @@ -71,4 +104,30 @@ def generate_launch_description(): ("gps/fix", "fix"), ], ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join(get_package_share_directory('rplidar_ros'), 'launch'),'/rplidar_a2m12_launch.py']), + launch_arguments={'serial_port': '/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_ee207775dd54894783cd016372304813-if00-port0'}.items(), + condition=IfCondition(with_nav) + ), + start_async_slam_toolbox_node, + EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(start_async_slam_toolbox_node), + transition_id=Transition.TRANSITION_CONFIGURE + ), + ), + RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=start_async_slam_toolbox_node, + start_state="configuring", + goal_state="inactive", + entities=[ + LogInfo(msg="[LifecycleLaunch] Slamtoolbox node is activating."), + EmitEvent(event=ChangeState( + lifecycle_node_matcher=matches_action(start_async_slam_toolbox_node), + transition_id=Transition.TRANSITION_ACTIVATE + )) + ] + ), + ), ]) diff --git a/maps/.gitignore b/maps/.gitignore new file mode 100644 index 0000000..f2c4ee5 --- /dev/null +++ b/maps/.gitignore @@ -0,0 +1 @@ +*.* diff --git a/params/robot_localization.yaml b/params/robot_localization.yaml index 3dc4200..895641c 100644 --- a/params/robot_localization.yaml +++ b/params/robot_localization.yaml @@ -9,7 +9,7 @@ ekf_filter_node: # The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict # cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the # filter will generate new output. Defaults to 1 / frequency if not specified. - sensor_timeout: 0.1 + #sensor_timeout: 0.05 # ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar @@ -20,7 +20,7 @@ ekf_filter_node: # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if # unspecified. - transform_time_offset: 0.0 + transform_time_offset: 0.1 # Use this parameter to provide specify how long the tf listener should wait for a transform to become available. # Defaults to 0.0 if unspecified. @@ -87,7 +87,7 @@ ekf_filter_node: # if unspecified, effectively making this parameter required for each sensor. odom0_config: [false, false, false, false, false, false, - true, true, true, + true, true, false, false, false, false, false, false, false] @@ -137,7 +137,7 @@ ekf_filter_node: true, true, true, false, false, false, true, true, true, - false, false, false] + true, true, true] imu0_nodelay: false imu0_differential: false imu0_relative: false diff --git a/scripts/engine_man_test.py b/scripts/engine_man_test.py index 434faf4..f75ed47 100755 --- a/scripts/engine_man_test.py +++ b/scripts/engine_man_test.py @@ -9,15 +9,15 @@ from pyshared.i2c import i2c_write_reg, i2c_read_reg def set_pwm(left, right): #left = right = -32768 - #i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", left, left, right, right)) - i2c_write_reg(0x50, 0x20, struct.pack(">hhhh", left, left, right, right)) + i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", left, left, right, right)) + #i2c_write_reg(0x50, 0x20, struct.pack(">hhhh", left, left, right, right)) if __name__ == "__main__": - set_pwm(int(sys.argv[1]), int(sys.argv[2])) + set_pwm(int(sys.argv[1]), int(sys.argv[2])) - while True: - motor1,motor2,motor3,motor4 = struct.unpack(">hhhh", i2c_read_reg(0x50, 0x1, 8)) - speed1, speed2, speed3, speed4 = struct.unpack(">hhhh", i2c_read_reg(0x50, 0x30, 8)) - error, = struct.unpack(">B", i2c_read_reg(0x50, 0xA1, 1)) + while True: + motor1,motor2,motor3,motor4 = struct.unpack(">hhhh", i2c_read_reg(0x50, 0x1, 8)) + speed1, speed2, speed3, speed4 = struct.unpack(">hhhh", i2c_read_reg(0x50, 0x30, 8)) + error, = struct.unpack(">B", i2c_read_reg(0x50, 0xA1, 1)) print "PWM: %3d %3d %3d %3d, Speed: %5d %5d %5d %5d, Error: %d" % (motor1, motor2, motor3, motor4, speed1, speed2, speed3, speed4, error) - sleep(0.1) + sleep(0.1) diff --git a/src/hw_node.cpp b/src/hw_node.cpp index 081f2d8..ef4b3f1 100644 --- a/src/hw_node.cpp +++ b/src/hw_node.cpp @@ -8,6 +8,7 @@ #include "geometry_msgs/msg/twist.hpp" #include "nav_msgs/msg/odometry.hpp" #include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/imu.hpp" #include #include #include @@ -97,6 +98,7 @@ class A4wd3 : public rclcpp::Node { pub_odom = this->create_publisher("odom", 10); pub_bat = this->create_publisher("battery", 10); sub_cmd_vel = this->create_subscription("cmd_vel", 10, std::bind(&A4wd3::cmdvel_callback, this, std::placeholders::_1)); + sub_imu = this->create_subscription("imu", 10, std::bind(&A4wd3::imu_callback, this, std::placeholders::_1)); tf_broadcaster = std::make_unique(*this); this->declare_parameter("enable_odom_tf", true); this->declare_parameter("odom_covar_xy", 0.01); @@ -116,10 +118,12 @@ class A4wd3 : public rclcpp::Node { rclcpp::Publisher::SharedPtr pub_odom; rclcpp::Publisher::SharedPtr pub_bat; rclcpp::Subscription::SharedPtr sub_cmd_vel; + rclcpp::Subscription::SharedPtr sub_imu; std::unique_ptr tf_broadcaster; std::vector cmd_vel; double pos_x=0, pos_y=0, angle=0; bool enable_odom_tf; + bool imu_received = false; void set_speed(float trans, float rot) { std::vector buf; @@ -144,6 +148,10 @@ class A4wd3 : public rclcpp::Node { cmd_vel[1] = msg->angular.z; } + void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { + imu_received=true; + } + void update_odom() { union ufloat values[5]; const int ret = i2c_read_reg(0x50, 0x38, 20, (uint8_t*)values); @@ -275,7 +283,10 @@ class A4wd3 : public rclcpp::Node { } void timer_callback() { - update_odom(); + if (imu_received) { + // Wait until IMU node is ready before we publish odometry so robot_localization will get correct orientation first + update_odom(); + } update_pwr(); if (!cmd_vel.empty()) { -- 2.39.5