Add SLAM

diff --git a/avr/motor_ctrl/main.c b/avr/motor_ctrl/main.c
index 6867e7898c0c21822da1f821170e8d7619250439..f588c35cdd10d282ab2efe00ac33ee1d91557c72 100644 (file)
--- 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 a139dff1bf949ea0183765a56a7e019216442d6d..d903f614351d0091f2b41eeb12222a650c7a4005 100644 (file)
--- a/avr/motor_ctrl/main.hex
+++ b/avr/motor_ctrl/main.hex
@@ -87,8 +87,8 @@
 :100560009A0090939B00A0939C00B0939D0084CF31\r
 :1005700023B160919A0070919B0080919C009091B2\r
 :100580009D00982F872F762F6627622B60939A0005\r
-:1005900070939B0080939C0090939D002AE939E41E\r
-:1005A0004BE955E40E946D120E944111462F572FCE\r
+:1005900070939B0080939C0090939D002AE93DED11\r
+:1005A0004DE955E40E946D120E944111462F572FCC\r
 :1005B0005093D5004093D4005093D3004093D20081\r
 :1005C00081E08093DB00A0CF83B190E0A0E0B0E0B9\r
 :1005D000C6CF23B180919A0090919B00A0919C007E\r
@@ -233,9 +233,9 @@
 :100E80002AEC37E34BEF5EE30E946D1220E030E086\r
 :100E900040E05FE30E946D12A70196010E94591085\r
 :100EA0008B018D839983A7019601C701B6010E942A\r
-:100EB000591098014D8159810E9458102AE939E44E\r
-:100EC0004BE955E40E946D126B017C012AE939E47B\r
-:100ED0004BE955E4B8018D8199810E946D128B0117\r
+:100EB000591098014D8159810E9458102AE93DED41\r
+:100EC0004DE955E40E946D126B017C012AE93DED6C\r
+:100ED0004DE955E4B8018D8199810E946D128B0115\r
 :100EE0008D8399838091A400811509F45CC360917E\r
 :100EF000A4009101261B3109B901072E000C880BB3\r
 :100F0000990B0E9479119601A7010E946D1220E0B1\r
@@ -272,9 +272,9 @@
 :1010F000880B990B0E9479112AE037ED43E25CE3FB\r
 :101100000E94CF100E9441116D877E878F87988B38\r
 :10111000B8016C0D7D1D072E000C880B990B0E94E9\r
-:1011200079112AE939E44BE156E40E94CF104B01D2\r
+:1011200079112AE93DED4DE156E40E94CF104B01C3\r
 :101130005C01698D7A8D6E0D7F1D072E000C880B6A\r
-:10114000990B0E9479112AE939E44BE156E40E9497\r
+:10114000990B0E9479112AE93DED4DE156E40E9488\r
 :10115000CF102B013C019401A5010E9458102AECEC\r
 :1011600037E34BEF5EE30E94CF106D8F7E8F182F19\r
 :10117000092F2091AF003091B0004091B100509103\r

diff --git a/docker/Dockerfile b/docker/Dockerfile
index d20b2094d6e2dd0678c8d1efd5cab9260899faa5..b2e4b017eef402f4f6fc20f421a9e1023351b1da 100644 (file)
--- 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 0a55de408705506de3d5120fff89663d5d04f47b..251c1bc129732a22ff31fb7f4964da33e7bf4156 100644 (file)
--- 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 (file)
index 0000000..f2c4ee5
--- /dev/null
+++ b/maps/.gitignore
@@ -0,0 +1 @@
+*.*

diff --git a/params/robot_localization.yaml b/params/robot_localization.yaml
index 3dc4200a8e232b3925f4a72eee9df4a90db3882b..895641c741e711cb371d105fbd03d8a094f7a97f 100644 (file)
--- 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 434faf4a78bb7474b8dd049d26798669e084f3f7..f75ed47d172d47e7a38148f0cb8989bf126f5401 100755 (executable)
--- 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 081f2d8d03daeda3698938d234fea94724bfcdec..ef4b3f15d1e01bff88c1f64e59eb82222886512a 100644 (file)
--- 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 <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
@@ -97,6 +98,7 @@ class A4wd3 : public rclcpp::Node {
                        pub_odom = this->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
                        pub_bat = this->create_publisher<sensor_msgs::msg::BatteryState>("battery", 10);
                        sub_cmd_vel = this->create_subscription<geometry_msgs::msg::Twist>("cmd_vel", 10, std::bind(&A4wd3::cmdvel_callback, this, std::placeholders::_1));
+                       sub_imu = this->create_subscription<sensor_msgs::msg::Imu>("imu", 10, std::bind(&A4wd3::imu_callback, this, std::placeholders::_1));
                        tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(*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<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
                rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr pub_bat;
                rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_cmd_vel;
+               rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu;
                std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
                std::vector<double> 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<uint8_t> 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()) {