seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

Merge pull request 'feat: Nav2 SLAM integration (Issue #422)' (#428) from sl-controls/issue-422-nav2-slam into main

Browse Source
This commit is contained in:
sl-jetson 2026-03-04 23:59:32 -05:00
commit c1b3a4368d
16 changed files with 898 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
jetson/ros2_ws/src/saltybot_nav2_slam/config/costmap_common_params.yaml Normal file
View File

@ -0,0 +1,47 @@
costmap_common_params:
update_frequency: 10.0
publish_frequency: 5.0
global_frame: map
robot_base_frame: base_link
use_sim_time: false
rolling_window: false
robot_radius: 0.35
plugins:
- name: static_layer
type: nav2_costmap_2d::StaticLayer
map_subscribe_transient_local: true
enabled: true
- name: obstacle_layer
type: nav2_costmap_2d::ObstacleLayer
enabled: true
observation_sources: scan camera_depth
scan:
topic: /scan
max_obstacle_height: 2.5
min_obstacle_height: 0.0
clearing: true
marking: true
data_type: LaserScan
camera_depth:
topic: /camera/depth/color/points
max_obstacle_height: 2.5
min_obstacle_height: 0.0
clearing: false
marking: true
data_type: PointCloud2
sensor_frame: camera_link
expected_update_rate: 15.0
observation_persistence: 0.0
inf_is_valid: false
filter: passthrough
filter_value: 5.0
- name: inflation_layer
type: nav2_costmap_2d::InflationLayer
enabled: true
inflation_radius: 0.55
cost_scaling_factor: 10.0
inflate_unknown: false
inflate_around_unknown: true
lethal_cost_threshold: 100
unknown_cost_value: -1
resolution: 0.05

59
jetson/ros2_ws/src/saltybot_nav2_slam/config/dwb_local_planner_params.yaml Normal file
View File

@ -0,0 +1,59 @@
dwb_local_planner:
ros__parameters:
max_vel_x: 5.5
min_vel_x: 0.0
max_vel_y: 0.0
min_vel_y: 0.0
max_vel_theta: 3.0
min_vel_theta: -3.0
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 2.5
decel_lim_x: -2.5
decel_lim_y: 0.0
decel_lim_theta: -2.5
vx_samples: 20
vy_samples: 1
vtheta_samples: 40
sim_time: 1.7
sim_granularity: 0.05
angular_sim_granularity: 0.1
path_distance_bias: 32.0
goal_distance_bias: 24.0
occdist_scale: 0.02
forward_point_distance: 0.325
use_dwa: true
dwa_padding_inaccuracies: 0.1
max_allowed_time_female: 0.0
oscillation_max_iterations: 1800
oscillation_max_distance: 0.02
use_velocity_scaled_lookahead_dist: false
min_approach_linear_velocity: 0.1
approach_velocity_scaling_dist: 1.0
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
rotate_to_goal:
slowing_factor: 5.0
lookahead_time: -1.0
base_obstacle:
scale: 0.02
max_scaling_factor: 0.02
forward_point_distance: 0.325
goal_align:
scale: 24.0
forward_point_distance: 0.325
path_align:
scale: 32.0
forward_point_distance: 0.325
max_allowed_time_female: 0.0
path_dist:
scale: 32.0
max_allowed_time_female: 0.0
goal_dist:
scale: 24.0
max_allowed_time_female: 0.0
prune_plan: true
prune_distance: 1.5
debug_trajectory_details: false
publish_evaluation: true
publish_scored_sampling_policies: false
publish_trajectories: false

36
jetson/ros2_ws/src/saltybot_nav2_slam/config/global_costmap_params.yaml Normal file
View File

@ -0,0 +1,36 @@
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: map
robot_base_frame: base_link
use_sim_time: false
rolling_window: false
robot_radius: 0.35
plugins:
- name: static_layer
type: nav2_costmap_2d::StaticLayer
map_subscribe_transient_local: true
enabled: true
- name: obstacle_layer
type: nav2_costmap_2d::ObstacleLayer
enabled: true
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.5
min_obstacle_height: 0.0
clearing: true
marking: true
data_type: LaserScan
- name: inflation_layer
type: nav2_costmap_2d::InflationLayer
enabled: true
inflation_radius: 0.55
cost_scaling_factor: 10.0
inflate_unknown: false
inflate_around_unknown: true
resolution: 0.05
track_unknown_space: true
unknown_cost_value: -1

51
jetson/ros2_ws/src/saltybot_nav2_slam/config/local_costmap_params.yaml Normal file
View File

@ -0,0 +1,51 @@
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 10.0
publish_frequency: 5.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: false
rolling_window: true
width: 3
height: 3
resolution: 0.05
robot_radius: 0.35
plugins:
- name: obstacle_layer
type: nav2_costmap_2d::ObstacleLayer
enabled: true
observation_sources: scan camera_depth
scan:
topic: /scan
max_obstacle_height: 2.5
min_obstacle_height: 0.0
clearing: true
marking: true
data_type: LaserScan
raytrace_max_range: 10.0
raytrace_min_range: 0.0
expected_update_rate: 5.5
observation_persistence: 0.0
camera_depth:
topic: /camera/depth/color/points
max_obstacle_height: 2.5
min_obstacle_height: 0.0
clearing: false
marking: true
data_type: PointCloud2
sensor_frame: camera_link
expected_update_rate: 15.0
observation_persistence: 0.5
inf_is_valid: false
filter: passthrough
filter_value: 5.0
- name: inflation_layer
type: nav2_costmap_2d::InflationLayer
enabled: true
inflation_radius: 0.55
cost_scaling_factor: 10.0
inflate_unknown: true
inflate_around_unknown: false
track_unknown_space: true
unknown_cost_value: 0

219
jetson/ros2_ws/src/saltybot_nav2_slam/config/nav2_params.yaml Normal file
View File

@ -0,0 +1,219 @@
amcl:
ros__parameters:
use_sim_time: false
alpha1: 0.2
alpha2: 0.2
alpha3: 0.2
alpha4: 0.2
alpha5: 0.2
base_frame_id: "base_link"
beam_search_increment: 0.1
do_beamskip: false
global_frame_id: "map"
lambda_short: 0.1
laser_likelihood_max_dist: 2.0
laser_model_type: "likelihood_field"
max_beams: 60
max_particles: 2000
min_particles: 500
odom_frame_id: "odom"
pf_err: 0.05
pf_z: 0.99
recovery_alpha_fast: 0.0
recovery_alpha_slow: 0.0
resample_interval: 1
robot_model_type: "differential"
save_pose_rate: 0.5
sigma_hit: 0.2
sigma_short: 0.05
tf_broadcast: true
transform_tolerance: 1.0
update_min_a: 0.2
update_min_d: 0.25
z_hit: 0.5
z_max: 0.05
z_rand: 0.5
z_short: 0.05
amcl_map_client:
ros__parameters:
use_sim_time: false
amcl_rclcpp_node:
ros__parameters:
use_sim_time: false
bt_navigator:
ros__parameters:
use_sim_time: false
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
enable_groot_monitoring: true
groot_zmq_publisher_port: 1666
groot_zmq_server_port: 1667
default_nav_through_poses_bt_xml: $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
default_nav_to_pose_bt_xml: $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_assisted_teleop_action_bt_node
- nav2_back_up_action_bt_node
- nav2_drive_on_heading_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_truncate_path_local_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: false
controller_server:
ros__parameters:
use_sim_time: false
controller_frequency: 20.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
failure_tolerance: 0.3
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_core::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
general_goal_checker:
stateful: True
plugin: "nav2_core::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: true
min_vel_x: 0.0
max_vel_x: 5.5
max_vel_theta: 3.0
min_speed_xy: 0.0
max_speed_xy: 5.5
acc_lim_x: 2.5
acc_lim_theta: 2.5
decel_lim_x: -2.5
decel_lim_theta: -2.5
vx_samples: 20
vtheta_samples: 40
sim_time: 1.7
linear_granularity: 0.05
angular_granularity: 0.1
transform_tolerance: 0.2
trans_stopped_velocity: 0.25
short_circuit_trajectory_evaluation: true
stateful: True
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
BaseObstacle.scale: 0.02
PathAlign.scale: 32.0
GoalAlign.scale: 24.0
PathDist.scale: 32.0
GoalDist.scale: 24.0
RotateToGoal.scale: 32.0
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: false
planner_server:
ros__parameters:
expected_planner_frequency: 20.0
use_sim_time: false
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 0.5
use_astar: true
allow_unknown: true
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: false
behavior_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
spin:
plugin: "nav2_behaviors/Spin"
backup:
plugin: "nav2_behaviors/BackUp"
drive_on_heading:
plugin: "nav2_behaviors/DriveOnHeading"
assisted_teleop:
plugin: "nav2_behaviors/AssistedTeleop"
wait:
plugin: "nav2_behaviors/Wait"
global_frame: odom
robot_base_frame: base_link
transform_timeout: 0.1
use_sim_time: false
simulate_ahead_time: 2.0
max_rotations: 1.0
max_backup_dist: 0.15
backup_speed: 0.5
behavior_server_rclcpp_node:
ros__parameters:
use_sim_time: false
robot_state_publisher:
ros__parameters:
use_sim_time: false
lifecycle_manager_navigation:
ros__parameters:
use_sim_time: false
autostart: true
node_names: ['controller_server', 'planner_server', 'behavior_server', 'bt_navigator']
lifecycle_manager_localization:
ros__parameters:
use_sim_time: false
autostart: true
node_names: ['amcl']
map_server:
ros__parameters:
use_sim_time: false
yaml_filename: "map.yaml"
velocity_smoother:
ros__parameters:
use_sim_time: false
smoothing_frequency: 20.0
scale_velocities: false
feedback: "odometry"
max_velocity: [5.5, 0.0, 3.0]
min_velocity: [-5.5, 0.0, -3.0]
max_accel: [2.5, 0.0, 2.5]
max_decel: [-2.5, 0.0, -2.5]
odom_topic: "odom"
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0

58
jetson/ros2_ws/src/saltybot_nav2_slam/config/slam_toolbox_params.yaml Normal file
View File

@ -0,0 +1,58 @@
slam_toolbox:
ros__parameters:
mode: mapping
async_mode: true
use_scan_matching: true
use_scan_bamatching: true
minimum_time_interval: 0.5
minimum_travel_distance: 0.2
minimum_rotation_deg: 5.0
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.05
correlation_search_space_smear_deviation: 0.1
correlation_search_space_std_dev_ratio: 0.5
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
scan_buffer_size: 10
scan_buffer_max_scan_distance: 12.0
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_search_step_size: 0.0349
fine_angle_search_step_size: 0.00349
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_queue_size: 10
loop_match_minimum_response_fine: 0.1
loop_match_minimum_response_coarse: 0.3
minimum_score_to_accept_loop_closure: 0.07
do_final_optimization: true
final_optimization_particle_filter_size: 800
final_optimization_min_particles: 400
final_optimization_max_particles: 2000
final_optimization_threshold_metric_improvement: 0.0002
add_loops_as_edges: false
num_for_scan_matching: 0
icp_iterations: 10
interactive_mode: false
print_timing_information: false
map_frame: map
odom_frame: odom
base_frame: base_link
scan_topic: /scan
publish_tf: true
tf_buffer_duration: 30.0
transform_timeout: 0.2
queue_size: 10
throttle_scans: 1
use_map_saver: true
map_start_at_dock: true
map_start_pose: [0.0, 0.0, 0.0]
rollout_velocity_smoothing_factor: 0.07
force_turtlebot_rep: false

28
jetson/ros2_ws/src/saltybot_nav2_slam/launch/depth_to_costmap.launch.py Normal file
View File

@ -0,0 +1,28 @@
"""RealSense Depth to Costmap Integration"""
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time')
return LaunchDescription([
DeclareLaunchArgument('use_sim_time', default_value='false'),
Node(
package='depth_image_proc',
executable='convert_node',
name='depth_image_converter',
namespace='camera',
remappings=[
('image_rect', '/camera/aligned_depth_to_color/image_raw'),
('camera_info', '/camera/color/camera_info'),
('points', '/camera/depth/color/points'),
],
parameters=[{'use_sim_time': use_sim_time}],
output='screen',
),
])

66
jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_slam_bringup.launch.py Normal file
View File

@ -0,0 +1,66 @@
"""Nav2 SLAM Bringup for SaltyBot (Issue #422)"""
import os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
nav2_slam_dir = get_package_share_directory('saltybot_nav2_slam')
bringup_dir = get_package_share_directory('saltybot_bringup')
nav2_params = os.path.join(nav2_slam_dir, 'config', 'nav2_params.yaml')
slam_params = os.path.join(nav2_slam_dir, 'config', 'slam_toolbox_params.yaml')
return LaunchDescription([
DeclareLaunchArgument('use_sim_time', default_value='false'),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(bringup_dir, 'launch', 'sensors.launch.py')
),
launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(),
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(nav2_slam_dir, 'launch', 'odometry_bridge.launch.py')
),
launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(),
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(nav2_slam_dir, 'launch', 'nav2_slam_integrated.launch.py')
),
launch_arguments={
'slam_params_file': slam_params,
'use_sim_time': LaunchConfiguration('use_sim_time'),
}.items(),
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(nav2_slam_dir, 'launch', 'depth_to_costmap.launch.py')
),
launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(),
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(
get_package_share_directory('nav2_bringup'),
'launch', 'navigation_launch.py'
)
),
launch_arguments={
'use_sim_time': LaunchConfiguration('use_sim_time'),
'params_file': nav2_params,
'autostart': 'true',
'map_subscribe_transient_local': 'true',
}.items(),
),
])

35
jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_slam_integrated.launch.py Normal file
View File

@ -0,0 +1,35 @@
"""SLAM Toolbox integrated launch for SaltyBot"""
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
slam_share = get_package_share_directory('slam_toolbox')
nav2_slam_dir = get_package_share_directory('saltybot_nav2_slam')
slam_params = LaunchConfiguration('slam_params_file')
use_sim_time = LaunchConfiguration('use_sim_time')
return LaunchDescription([
DeclareLaunchArgument(
'slam_params_file',
default_value=os.path.join(nav2_slam_dir, 'config', 'slam_toolbox_params.yaml'),
),
DeclareLaunchArgument('use_sim_time', default_value='false'),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(slam_share, 'launch', 'online_async_launch.py')
),
launch_arguments={
'params_file': slam_params,
'use_sim_time': use_sim_time,
}.items(),
),
])

36
jetson/ros2_ws/src/saltybot_nav2_slam/launch/odometry_bridge.launch.py Normal file
View File

@ -0,0 +1,36 @@
"""VESC Telemetry to Odometry Bridge"""
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time')
return LaunchDescription([
DeclareLaunchArgument('use_sim_time', default_value='false'),
Node(
package='saltybot_nav2_slam',
executable='vesc_odometry_bridge',
name='vesc_odometry_bridge',
parameters=[{
'use_sim_time': use_sim_time,
'vesc_state_topic': '/vesc/state',
'odometry_topic': '/odom',
'odom_frame_id': 'odom',
'base_frame_id': 'base_link',
'update_frequency': 50,
'wheel_base': 0.35,
'wheel_radius': 0.10,
'max_rpm': 60000,
'publish_tf': True,
'tf_queue_size': 10,
}],
output='screen',
),
])

39
jetson/ros2_ws/src/saltybot_nav2_slam/package.xml Normal file
View File

@ -0,0 +1,39 @@
<?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>saltybot_nav2_slam</name>
<version>0.1.0</version>
<description>
Nav2 SLAM integration for SaltyBot autonomous navigation.
Combines SLAM Toolbox (RPLIDAR 2D SLAM), RealSense depth for obstacle avoidance,
VESC odometry, and DWB planner for autonomous navigation up to 20km/h.
</description>
<maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
<license>MIT</license>
<depend>rclpy</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>nav_msgs</depend>
<depend>geometry_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<exec_depend>nav2_bringup</exec_depend>
<exec_depend>slam_toolbox</exec_depend>
<exec_depend>rplidar_ros</exec_depend>
<exec_depend>realsense2_camera</exec_depend>
<exec_depend>depth_image_proc</exec_depend>
<exec_depend>pointcloud_to_laserscan</exec_depend>
<buildtool_depend>ament_python</buildtool_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>

0
jetson/ros2_ws/src/saltybot_nav2_slam/resource/saltybot_nav2_slam Normal file
View File

1
jetson/ros2_ws/src/saltybot_nav2_slam/saltybot_nav2_slam/__init__.py Normal file
View File

@ -0,0 +1 @@
# Nav2 SLAM integration for SaltyBot

180
jetson/ros2_ws/src/saltybot_nav2_slam/saltybot_nav2_slam/vesc_odometry_bridge.py Normal file
View File

@ -0,0 +1,180 @@
#!/usr/bin/env python3
"""VESC telemetry to Nav2 odometry bridge."""
import json
import math
import time
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped, Quaternion
from tf2_ros import TransformBroadcaster
class VESCOdometryBridge(Node):
"""ROS2 node bridging VESC telemetry to Nav2 odometry."""
def __init__(self):
super().__init__("vesc_odometry_bridge")
self.declare_parameter("vesc_state_topic", "/vesc/state")
self.declare_parameter("odometry_topic", "/odom")
self.declare_parameter("odom_frame_id", "odom")
self.declare_parameter("base_frame_id", "base_link")
self.declare_parameter("update_frequency", 50)
self.declare_parameter("wheel_base", 0.35)
self.declare_parameter("wheel_radius", 0.10)
self.declare_parameter("max_rpm", 60000)
self.declare_parameter("publish_tf", True)
self.vesc_state_topic = self.get_parameter("vesc_state_topic").value
self.odometry_topic = self.get_parameter("odometry_topic").value
self.odom_frame_id = self.get_parameter("odom_frame_id").value
self.base_frame_id = self.get_parameter("base_frame_id").value
frequency = self.get_parameter("update_frequency").value
self.wheel_base = self.get_parameter("wheel_base").value
self.wheel_radius = self.get_parameter("wheel_radius").value
self.publish_tf = self.get_parameter("publish_tf").value
self.last_rpm = 0
self.last_update_time = time.time()
self.x = 0.0
self.y = 0.0
self.theta = 0.0
self.vx = 0.0
self.vy = 0.0
self.vtheta = 0.0
self.pub_odom = self.create_publisher(Odometry, self.odometry_topic, 10)
if self.publish_tf:
self.tf_broadcaster = TransformBroadcaster(self)
self.create_subscription(String, self.vesc_state_topic, self._on_vesc_state, 10)
period = 1.0 / frequency
self.create_timer(period, self._publish_odometry)
self.get_logger().info(
f"VESC odometry bridge initialized: "
f"wheel_base={self.wheel_base}m, wheel_radius={self.wheel_radius}m"
)
def _on_vesc_state(self, msg: String) -> None:
"""Update VESC telemetry from JSON."""
try:
data = json.loads(msg.data)
current_rpm = data.get("rpm", 0)
motor_rad_s = (current_rpm / 60.0) * (2.0 * math.pi)
wheel_velocity = motor_rad_s * self.wheel_radius
self.vx = wheel_velocity
self.vy = 0.0
self.vtheta = 0.0
self.last_rpm = current_rpm
except json.JSONDecodeError:
pass
def _publish_odometry(self) -> None:
"""Publish odometry message and TF."""
current_time = time.time()
dt = current_time - self.last_update_time
self.last_update_time = current_time
if abs(self.vtheta) > 0.001:
self.theta += self.vtheta * dt
self.x += (self.vx / self.vtheta) * (math.sin(self.theta) - math.sin(self.theta - self.vtheta * dt))
self.y += (self.vx / self.vtheta) * (math.cos(self.theta - self.vtheta * dt) - math.cos(self.theta))
else:
self.x += self.vx * math.cos(self.theta) * dt
self.y += self.vx * math.sin(self.theta) * dt
odom_msg = Odometry()
odom_msg.header.stamp = self.get_clock().now().to_msg()
odom_msg.header.frame_id = self.odom_frame_id
odom_msg.child_frame_id = self.base_frame_id
odom_msg.pose.pose.position.x = self.x
odom_msg.pose.pose.position.y = self.y
odom_msg.pose.pose.position.z = 0.0
odom_msg.pose.pose.orientation = self._euler_to_quaternion(0, 0, self.theta)
odom_msg.pose.covariance = [
0.1, 0, 0, 0, 0, 0,
0, 0.1, 0, 0, 0, 0,
0, 0, 0.1, 0, 0, 0,
0, 0, 0, 0.1, 0, 0,
0, 0, 0, 0, 0.1, 0,
0, 0, 0, 0, 0, 0.1,
]
odom_msg.twist.twist.linear.x = self.vx
odom_msg.twist.twist.linear.y = self.vy
odom_msg.twist.twist.linear.z = 0.0
odom_msg.twist.twist.angular.z = self.vtheta
odom_msg.twist.covariance = [
0.05, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0,
0, 0, 0.05, 0, 0, 0,
0, 0, 0, 0.05, 0, 0,
0, 0, 0, 0, 0.05, 0,
0, 0, 0, 0, 0, 0.05,
]
self.pub_odom.publish(odom_msg)
if self.publish_tf:
self._publish_tf(odom_msg.header.stamp)
def _publish_tf(self, timestamp) -> None:
"""Publish odom → base_link TF."""
tf_msg = TransformStamped()
tf_msg.header.stamp = timestamp
tf_msg.header.frame_id = self.odom_frame_id
tf_msg.child_frame_id = self.base_frame_id
tf_msg.transform.translation.x = self.x
tf_msg.transform.translation.y = self.y
tf_msg.transform.translation.z = 0.0
tf_msg.transform.rotation = self._euler_to_quaternion(0, 0, self.theta)
self.tf_broadcaster.sendTransform(tf_msg)
@staticmethod
def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> Quaternion:
"""Convert Euler angles to quaternion."""
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
q = Quaternion()
q.w = cr * cp * cy + sr * sp * sy
q.x = sr * cp * cy - cr * sp * sy
q.y = cr * sp * cy + sr * cp * sy
q.z = cr * cp * sy - sr * sp * cy
return q
def main(args=None):
rclpy.init(args=args)
node = VESCOdometryBridge()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()

4
jetson/ros2_ws/src/saltybot_nav2_slam/setup.cfg Normal file
View File

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

39
jetson/ros2_ws/src/saltybot_nav2_slam/setup.py Normal file
View File

@ -0,0 +1,39 @@
from setuptools import setup
package_name = "saltybot_nav2_slam"
setup(
name=package_name,
version="0.1.0",
packages=[package_name],
data_files=[
("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
(f"share/{package_name}", ["package.xml"]),
(f"share/{package_name}/launch", [
"launch/nav2_slam_bringup.launch.py",
"launch/nav2_slam_integrated.launch.py",
"launch/depth_to_costmap.launch.py",
"launch/odometry_bridge.launch.py",
]),
(f"share/{package_name}/config", [
"config/nav2_params.yaml",
"config/slam_toolbox_params.yaml",
"config/costmap_common_params.yaml",
"config/global_costmap_params.yaml",
"config/local_costmap_params.yaml",
"config/dwb_local_planner_params.yaml",
]),
],
install_requires=["setuptools"],
zip_safe=True,
maintainer="sl-controls",
maintainer_email="sl-controls@saltylab.local",
description="Nav2 SLAM integration with RPLIDAR + RealSense for autonomous navigation",
license="MIT",
tests_require=["pytest"],
entry_points={
"console_scripts": [
"vesc_odometry_bridge = saltybot_nav2_slam.vesc_odometry_bridge:main",
],
},
)