feat: Nav2 SLAM integration (Issue #422) #428
@ -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
|
||||||
@ -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
|
||||||
@ -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
|
||||||
@ -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
219
jetson/ros2_ws/src/saltybot_nav2_slam/config/nav2_params.yaml
Normal 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
|
||||||
@ -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
|
||||||
@ -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',
|
||||||
|
),
|
||||||
|
])
|
||||||
@ -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(),
|
||||||
|
),
|
||||||
|
])
|
||||||
@ -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(),
|
||||||
|
),
|
||||||
|
])
|
||||||
@ -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
39
jetson/ros2_ws/src/saltybot_nav2_slam/package.xml
Normal 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,0 +1 @@
|
|||||||
|
# Nav2 SLAM integration for SaltyBot
|
||||||
@ -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
4
jetson/ros2_ws/src/saltybot_nav2_slam/setup.cfg
Normal 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
39
jetson/ros2_ws/src/saltybot_nav2_slam/setup.py
Normal 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",
|
||||||
|
],
|
||||||
|
},
|
||||||
|
)
|
||||||
9
jetson/ros2_ws/src/saltybot_remote_monitor/.gitignore
vendored
Normal file
9
jetson/ros2_ws/src/saltybot_remote_monitor/.gitignore
vendored
Normal file
@ -0,0 +1,9 @@
|
|||||||
|
build/
|
||||||
|
install/
|
||||||
|
log/
|
||||||
|
*.pyc
|
||||||
|
__pycache__/
|
||||||
|
.pytest_cache/
|
||||||
|
*.egg-info/
|
||||||
|
dist/
|
||||||
|
*.egg
|
||||||
@ -0,0 +1,6 @@
|
|||||||
|
remote_monitor:
|
||||||
|
ros__parameters:
|
||||||
|
ws_port: 9091
|
||||||
|
auth_token: 'default_token'
|
||||||
|
update_rate_hz: 2
|
||||||
|
enable_msgpack: true
|
||||||
@ -0,0 +1,23 @@
|
|||||||
|
from launch import LaunchDescription
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
import os
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
pkg_dir = get_package_share_directory('saltybot_remote_monitor')
|
||||||
|
config_file = os.path.join(pkg_dir, 'config', 'remote_monitor.yaml')
|
||||||
|
|
||||||
|
remote_monitor_node = Node(
|
||||||
|
package='saltybot_remote_monitor',
|
||||||
|
executable='remote_monitor',
|
||||||
|
name='remote_monitor',
|
||||||
|
parameters=[config_file],
|
||||||
|
output='screen',
|
||||||
|
respawn=True,
|
||||||
|
respawn_delay=5,
|
||||||
|
)
|
||||||
|
|
||||||
|
return LaunchDescription([
|
||||||
|
remote_monitor_node,
|
||||||
|
])
|
||||||
32
jetson/ros2_ws/src/saltybot_remote_monitor/package.xml
Normal file
32
jetson/ros2_ws/src/saltybot_remote_monitor/package.xml
Normal file
@ -0,0 +1,32 @@
|
|||||||
|
<?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_remote_monitor</name>
|
||||||
|
<version>0.1.0</version>
|
||||||
|
<description>
|
||||||
|
Remote monitoring WebSocket relay with mobile-friendly UI.
|
||||||
|
2Hz JSON telemetry aggregation with msgpack compression, token auth,
|
||||||
|
5min history buffer, and critical alerts (fall, low battery, node crash).
|
||||||
|
</description>
|
||||||
|
<maintainer email="seb@vayrette.com">seb</maintainer>
|
||||||
|
<license>MIT</license>
|
||||||
|
|
||||||
|
<depend>rclpy</depend>
|
||||||
|
<depend>sensor_msgs</depend>
|
||||||
|
<depend>geometry_msgs</depend>
|
||||||
|
<depend>diagnostic_msgs</depend>
|
||||||
|
<depend>std_msgs</depend>
|
||||||
|
|
||||||
|
<exec_depend>python3-aiohttp</exec_depend>
|
||||||
|
<exec_depend>python3-msgpack</exec_depend>
|
||||||
|
<exec_depend>python3-launch-ros</exec_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,0 +1,287 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
|
||||||
|
import asyncio
|
||||||
|
import json
|
||||||
|
import math
|
||||||
|
import threading
|
||||||
|
import time
|
||||||
|
from collections import deque
|
||||||
|
from datetime import datetime
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Dict, Optional
|
||||||
|
|
||||||
|
import msgpack
|
||||||
|
import rclpy
|
||||||
|
from aiohttp import web
|
||||||
|
from diagnostic_msgs.msg import DiagnosticArray
|
||||||
|
from geometry_msgs.msg import Twist
|
||||||
|
from rclpy.node import Node
|
||||||
|
from sensor_msgs.msg import Imu, NavSatFix
|
||||||
|
from std_msgs.msg import Float32, Bool, String
|
||||||
|
|
||||||
|
|
||||||
|
class TelemetryBuffer:
|
||||||
|
"""Circular buffer for 5-minute history at 2Hz."""
|
||||||
|
|
||||||
|
def __init__(self, duration_seconds=300):
|
||||||
|
self.max_samples = int(duration_seconds * 2)
|
||||||
|
self.buffer = deque(maxlen=self.max_samples)
|
||||||
|
|
||||||
|
def add(self, data: Dict):
|
||||||
|
self.buffer.append({**data, 'ts': time.time()})
|
||||||
|
|
||||||
|
def get_history(self):
|
||||||
|
return list(self.buffer)
|
||||||
|
|
||||||
|
|
||||||
|
class RemoteMonitorNode(Node):
|
||||||
|
"""WebSocket relay for remote monitoring with telemetry aggregation."""
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('saltybot_remote_monitor')
|
||||||
|
|
||||||
|
self.declare_parameter('ws_port', 9091)
|
||||||
|
self.declare_parameter('auth_token', 'default_token')
|
||||||
|
self.declare_parameter('update_rate_hz', 2)
|
||||||
|
self.declare_parameter('enable_msgpack', True)
|
||||||
|
|
||||||
|
self.ws_port = self.get_parameter('ws_port').value
|
||||||
|
self.auth_token = self.get_parameter('auth_token').value
|
||||||
|
self.update_rate_hz = self.get_parameter('update_rate_hz').value
|
||||||
|
self.enable_msgpack = self.get_parameter('enable_msgpack').value
|
||||||
|
|
||||||
|
self.battery = {'voltage': 0.0, 'current': 0.0, 'soc': 0.0}
|
||||||
|
self.motors = {'left': 0.0, 'right': 0.0}
|
||||||
|
self.imu = {'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}
|
||||||
|
self.gps = {'lat': 0.0, 'lon': 0.0, 'alt': 0.0}
|
||||||
|
self.health = {'cpu_temp': 0.0, 'gpu_temp': 0.0, 'ram_pct': 0.0, 'disk_pct': 0.0}
|
||||||
|
self.social = {'is_speaking': False, 'face_id': None}
|
||||||
|
self.alerts = []
|
||||||
|
self.history_buffer = TelemetryBuffer()
|
||||||
|
self.ws_clients = set()
|
||||||
|
self.loop = None
|
||||||
|
|
||||||
|
self.create_subscription(DiagnosticArray, '/diagnostics', self.diag_callback, 10)
|
||||||
|
self.create_subscription(Imu, '/saltybot/imu', self.imu_callback, 10)
|
||||||
|
self.create_subscription(NavSatFix, '/gps/fix', self.gps_callback, 10)
|
||||||
|
self.create_subscription(String, '/saltybot/balance_state', self.balance_callback, 10)
|
||||||
|
self.create_subscription(Bool, '/social/speech/is_speaking', self.social_speech_callback, 10)
|
||||||
|
self.create_subscription(String, '/social/face/active', self.social_face_callback, 10)
|
||||||
|
|
||||||
|
period = 1.0 / self.update_rate_hz
|
||||||
|
self.update_timer = self.create_timer(period, self.telemetry_update_callback)
|
||||||
|
|
||||||
|
self.ws_thread = threading.Thread(target=self.start_ws_server, daemon=True)
|
||||||
|
self.ws_thread.start()
|
||||||
|
|
||||||
|
self.get_logger().info(
|
||||||
|
f'Remote monitor initialized on port {self.ws_port}, '
|
||||||
|
f'update rate {self.update_rate_hz}Hz, msgpack={self.enable_msgpack}'
|
||||||
|
)
|
||||||
|
|
||||||
|
def diag_callback(self, msg: DiagnosticArray):
|
||||||
|
"""Parse diagnostics for battery and system health."""
|
||||||
|
for status in msg.status:
|
||||||
|
kv = {pair.key: pair.value for pair in status.values}
|
||||||
|
|
||||||
|
if kv.get('battery_voltage_v'):
|
||||||
|
self.battery = {
|
||||||
|
'voltage': float(kv.get('battery_voltage_v', 0)),
|
||||||
|
'current': float(kv.get('battery_current_a', 0)),
|
||||||
|
'soc': float(kv.get('battery_soc_pct', 0)),
|
||||||
|
}
|
||||||
|
if self.battery['soc'] < 15:
|
||||||
|
self.add_alert('CRITICAL', 'Low battery', self.battery['soc'])
|
||||||
|
|
||||||
|
if kv.get('cpu_temp_c'):
|
||||||
|
self.health = {
|
||||||
|
'cpu_temp': float(kv.get('cpu_temp_c', 0)),
|
||||||
|
'gpu_temp': float(kv.get('gpu_temp_c', 0)),
|
||||||
|
'ram_pct': float(kv.get('ram_pct', 0)),
|
||||||
|
'disk_pct': float(kv.get('disk_pct', 0)),
|
||||||
|
}
|
||||||
|
|
||||||
|
def imu_callback(self, msg: Imu):
|
||||||
|
"""Extract roll/pitch/yaw from quaternion."""
|
||||||
|
q = msg.orientation
|
||||||
|
self.imu = self.quat_to_euler(q.x, q.y, q.z, q.w)
|
||||||
|
|
||||||
|
def gps_callback(self, msg: NavSatFix):
|
||||||
|
"""Store GPS data."""
|
||||||
|
self.gps = {
|
||||||
|
'lat': msg.latitude,
|
||||||
|
'lon': msg.longitude,
|
||||||
|
'alt': msg.altitude,
|
||||||
|
}
|
||||||
|
|
||||||
|
def balance_callback(self, msg: String):
|
||||||
|
"""Parse balance state for motor commands."""
|
||||||
|
try:
|
||||||
|
state = json.loads(msg.data)
|
||||||
|
cmd = state.get('motor_cmd', 0)
|
||||||
|
norm = max(-1, min(1, cmd / 1000))
|
||||||
|
self.motors = {'left': norm, 'right': norm}
|
||||||
|
|
||||||
|
if abs(norm) > 0.9:
|
||||||
|
self.add_alert('WARNING', 'High motor load', norm)
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
def social_speech_callback(self, msg: Bool):
|
||||||
|
"""Update social speaking state."""
|
||||||
|
self.social['is_speaking'] = msg.data
|
||||||
|
|
||||||
|
def social_face_callback(self, msg: String):
|
||||||
|
"""Update recognized face ID."""
|
||||||
|
self.social['face_id'] = msg.data
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def quat_to_euler(qx, qy, qz, qw):
|
||||||
|
"""Convert quaternion to Euler angles."""
|
||||||
|
sinr_cosp = 2 * (qw * qx + qy * qz)
|
||||||
|
cosr_cosp = 1 - 2 * (qx * qx + qy * qy)
|
||||||
|
roll = math.atan2(sinr_cosp, cosr_cosp)
|
||||||
|
|
||||||
|
sinp = 2 * (qw * qy - qz * qx)
|
||||||
|
pitch = math.asin(max(-1, min(1, sinp)))
|
||||||
|
|
||||||
|
siny_cosp = 2 * (qw * qz + qx * qy)
|
||||||
|
cosy_cosp = 1 - 2 * (qy * qy + qz * qz)
|
||||||
|
yaw = math.atan2(siny_cosp, cosy_cosp)
|
||||||
|
|
||||||
|
return {
|
||||||
|
'roll': (roll * 180) / math.pi,
|
||||||
|
'pitch': (pitch * 180) / math.pi,
|
||||||
|
'yaw': (yaw * 180) / math.pi,
|
||||||
|
}
|
||||||
|
|
||||||
|
def add_alert(self, level: str, message: str, value: Optional[float] = None):
|
||||||
|
"""Add alert with timestamp."""
|
||||||
|
alert = {
|
||||||
|
'ts': time.time(),
|
||||||
|
'level': level,
|
||||||
|
'message': message,
|
||||||
|
'value': value,
|
||||||
|
}
|
||||||
|
self.alerts.append(alert)
|
||||||
|
if len(self.alerts) > 50:
|
||||||
|
self.alerts.pop(0)
|
||||||
|
|
||||||
|
def telemetry_update_callback(self):
|
||||||
|
"""Periodic telemetry aggregation and broadcast."""
|
||||||
|
telemetry = {
|
||||||
|
'timestamp': datetime.now().isoformat(),
|
||||||
|
'battery': self.battery,
|
||||||
|
'motors': self.motors,
|
||||||
|
'imu': self.imu,
|
||||||
|
'gps': self.gps,
|
||||||
|
'health': self.health,
|
||||||
|
'social': self.social,
|
||||||
|
'alerts': self.alerts[-10:],
|
||||||
|
}
|
||||||
|
|
||||||
|
self.history_buffer.add(telemetry)
|
||||||
|
|
||||||
|
if self.ws_clients and self.loop:
|
||||||
|
try:
|
||||||
|
asyncio.run_coroutine_threadsafe(
|
||||||
|
self.broadcast_telemetry(telemetry),
|
||||||
|
self.loop
|
||||||
|
)
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().warn(f'Broadcast error: {e}')
|
||||||
|
|
||||||
|
async def broadcast_telemetry(self, telemetry: Dict):
|
||||||
|
"""Send telemetry to all connected WebSocket clients."""
|
||||||
|
if self.enable_msgpack:
|
||||||
|
data = msgpack.packb(telemetry)
|
||||||
|
else:
|
||||||
|
data = json.dumps(telemetry).encode('utf-8')
|
||||||
|
|
||||||
|
disconnected = set()
|
||||||
|
for ws in self.ws_clients:
|
||||||
|
try:
|
||||||
|
await ws.send_bytes(data)
|
||||||
|
except Exception:
|
||||||
|
disconnected.add(ws)
|
||||||
|
|
||||||
|
self.ws_clients -= disconnected
|
||||||
|
|
||||||
|
async def websocket_handler(self, request):
|
||||||
|
"""Handle WebSocket connections."""
|
||||||
|
token = request.rel_url.query.get('token')
|
||||||
|
|
||||||
|
if token != self.auth_token:
|
||||||
|
return web.Response(status=401, text='Unauthorized')
|
||||||
|
|
||||||
|
ws = web.WebSocketResponse()
|
||||||
|
await ws.prepare(request)
|
||||||
|
self.ws_clients.add(ws)
|
||||||
|
|
||||||
|
try:
|
||||||
|
history = self.history_buffer.get_history()
|
||||||
|
if self.enable_msgpack:
|
||||||
|
await ws.send_bytes(msgpack.packb({'type': 'history', 'data': history}))
|
||||||
|
else:
|
||||||
|
await ws.send_str(json.dumps({'type': 'history', 'data': history}))
|
||||||
|
|
||||||
|
async for msg in ws:
|
||||||
|
if msg.type == web.WSMsgType.TEXT:
|
||||||
|
try:
|
||||||
|
req = json.loads(msg.data)
|
||||||
|
if req.get('cmd') == 'get_history':
|
||||||
|
history = self.history_buffer.get_history()
|
||||||
|
if self.enable_msgpack:
|
||||||
|
await ws.send_bytes(msgpack.packb({'type': 'history', 'data': history}))
|
||||||
|
else:
|
||||||
|
await ws.send_str(json.dumps({'type': 'history', 'data': history}))
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().error(f'WebSocket error: {e}')
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().warn(f'WebSocket connection lost: {e}')
|
||||||
|
finally:
|
||||||
|
self.ws_clients.discard(ws)
|
||||||
|
|
||||||
|
return ws
|
||||||
|
|
||||||
|
async def index_handler(self, request):
|
||||||
|
"""Serve mobile-friendly HTML UI."""
|
||||||
|
html = Path(__file__).parent.parent / 'static' / 'index.html'
|
||||||
|
if html.exists():
|
||||||
|
return web.FileResponse(html)
|
||||||
|
return web.Response(text='UI not found', status=404)
|
||||||
|
|
||||||
|
def start_ws_server(self):
|
||||||
|
"""Start aiohttp WebSocket server."""
|
||||||
|
self.loop = asyncio.new_event_loop()
|
||||||
|
asyncio.set_event_loop(self.loop)
|
||||||
|
|
||||||
|
app = web.Application()
|
||||||
|
app.router.add_get('/', self.index_handler)
|
||||||
|
app.router.add_get('/ws', self.websocket_handler)
|
||||||
|
|
||||||
|
runner = web.AppRunner(app)
|
||||||
|
self.loop.run_until_complete(runner.setup())
|
||||||
|
site = web.TCPSite(runner, '0.0.0.0', self.ws_port)
|
||||||
|
self.loop.run_until_complete(site.start())
|
||||||
|
|
||||||
|
self.get_logger().info(f'WebSocket server running on port {self.ws_port}')
|
||||||
|
self.loop.run_forever()
|
||||||
|
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = RemoteMonitorNode()
|
||||||
|
|
||||||
|
try:
|
||||||
|
rclpy.spin(node)
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
5
jetson/ros2_ws/src/saltybot_remote_monitor/setup.cfg
Normal file
5
jetson/ros2_ws/src/saltybot_remote_monitor/setup.cfg
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
[develop]
|
||||||
|
script_dir=$base/lib/saltybot_remote_monitor
|
||||||
|
|
||||||
|
[install]
|
||||||
|
script_dir=$base/lib/saltybot_remote_monitor
|
||||||
34
jetson/ros2_ws/src/saltybot_remote_monitor/setup.py
Normal file
34
jetson/ros2_ws/src/saltybot_remote_monitor/setup.py
Normal file
@ -0,0 +1,34 @@
|
|||||||
|
from setuptools import setup
|
||||||
|
import os
|
||||||
|
from glob import glob
|
||||||
|
|
||||||
|
package_name = 'saltybot_remote_monitor'
|
||||||
|
|
||||||
|
setup(
|
||||||
|
name=package_name,
|
||||||
|
version='0.1.0',
|
||||||
|
packages=[package_name],
|
||||||
|
data_files=[
|
||||||
|
('share/ament_index/resource_index/packages',
|
||||||
|
['resource/' + package_name]),
|
||||||
|
('share/' + package_name, ['package.xml']),
|
||||||
|
(os.path.join('share', package_name, 'launch'),
|
||||||
|
glob('launch/*.py')),
|
||||||
|
(os.path.join('share', package_name, 'config'),
|
||||||
|
glob('config/*.yaml')),
|
||||||
|
(os.path.join('share', package_name, 'static'),
|
||||||
|
glob('static/*')),
|
||||||
|
],
|
||||||
|
install_requires=['setuptools'],
|
||||||
|
zip_safe=True,
|
||||||
|
maintainer='seb',
|
||||||
|
maintainer_email='seb@vayrette.com',
|
||||||
|
description='Remote monitoring WebSocket relay with mobile UI',
|
||||||
|
license='MIT',
|
||||||
|
tests_require=['pytest'],
|
||||||
|
entry_points={
|
||||||
|
'console_scripts': [
|
||||||
|
'remote_monitor = saltybot_remote_monitor.remote_monitor_node:main',
|
||||||
|
],
|
||||||
|
},
|
||||||
|
)
|
||||||
180
jetson/ros2_ws/src/saltybot_remote_monitor/static/index.html
Normal file
180
jetson/ros2_ws/src/saltybot_remote_monitor/static/index.html
Normal file
@ -0,0 +1,180 @@
|
|||||||
|
<!DOCTYPE html>
|
||||||
|
<html lang="en">
|
||||||
|
<head>
|
||||||
|
<meta charset="UTF-8">
|
||||||
|
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||||
|
<title>Saltybot Remote Monitor</title>
|
||||||
|
<style>
|
||||||
|
* { margin: 0; padding: 0; box-sizing: border-box; }
|
||||||
|
body { font-family: 'Courier New', monospace; background: #0a0e27; color: #e0e0ff; padding: 8px; }
|
||||||
|
.header { text-align: center; margin-bottom: 12px; padding: 8px; background: #1a1f3a; border: 1px solid #06b6d4; border-radius: 4px; }
|
||||||
|
.header h1 { font-size: 18px; color: #f59e0b; margin-bottom: 4px; }
|
||||||
|
.status { font-size: 11px; color: #888; }
|
||||||
|
.status.connected { color: #22c55e; }
|
||||||
|
.status.disconnected { color: #ef4444; }
|
||||||
|
.grid { display: grid; grid-template-columns: repeat(auto-fit, minmax(140px, 1fr)); gap: 8px; margin-bottom: 12px; }
|
||||||
|
.card { background: #1a1f3a; border: 1px solid #06b6d4; border-radius: 4px; padding: 8px; font-size: 12px; }
|
||||||
|
.card.critical { border-color: #ef4444; background: #2a1a1a; }
|
||||||
|
.card.warning { border-color: #f59e0b; background: #2a1f0a; }
|
||||||
|
.card-title { color: #06b6d4; font-weight: bold; margin-bottom: 4px; text-transform: uppercase; font-size: 10px; }
|
||||||
|
.card-value { font-size: 16px; font-weight: bold; color: #22c55e; }
|
||||||
|
.card-value.critical { color: #ef4444; }
|
||||||
|
.card-value.warning { color: #f59e0b; }
|
||||||
|
.card-unit { font-size: 10px; color: #888; margin-left: 4px; }
|
||||||
|
.alert-list { background: #1a1f3a; border: 1px solid #ef4444; border-radius: 4px; padding: 8px; margin-bottom: 12px; max-height: 120px; overflow-y: auto; }
|
||||||
|
.alert-item { padding: 4px; margin: 2px 0; border-left: 3px solid #f59e0b; font-size: 11px; color: #f59e0b; }
|
||||||
|
.alert-item.critical { border-left-color: #ef4444; color: #ef4444; }
|
||||||
|
.progress-bar { width: 100%; height: 8px; background: #0a0e27; border-radius: 2px; margin-top: 4px; overflow: hidden; }
|
||||||
|
.progress-fill { height: 100%; background: #22c55e; transition: width 0.3s ease; }
|
||||||
|
.progress-fill.warning { background: #f59e0b; }
|
||||||
|
.progress-fill.critical { background: #ef4444; }
|
||||||
|
.timestamp { text-align: center; font-size: 10px; color: #666; margin-top: 8px; }
|
||||||
|
@media (max-width: 480px) { body { padding: 4px; } .header h1 { font-size: 16px; } .grid { grid-template-columns: repeat(2, 1fr); gap: 6px; } .card { padding: 6px; font-size: 11px; } .card-value { font-size: 14px; } }
|
||||||
|
</style>
|
||||||
|
</head>
|
||||||
|
<body>
|
||||||
|
<div class="header">
|
||||||
|
<h1>⚡ REMOTE MONITOR</h1>
|
||||||
|
<div class="status disconnected" id="status">Connecting...</div>
|
||||||
|
</div>
|
||||||
|
<div class="alert-list" id="alertList" style="display: none;"></div>
|
||||||
|
<div class="grid">
|
||||||
|
<div class="card" id="batteryCard">
|
||||||
|
<div class="card-title">Battery</div>
|
||||||
|
<div class="card-value" id="soc">0<span class="card-unit">%</span></div>
|
||||||
|
<div style="font-size: 10px; color: #888;">
|
||||||
|
<div id="voltage">0V</div>
|
||||||
|
<div id="current">0A</div>
|
||||||
|
</div>
|
||||||
|
<div class="progress-bar"><div class="progress-fill" id="socBar" style="width: 100%;"></div></div>
|
||||||
|
</div>
|
||||||
|
<div class="card">
|
||||||
|
<div class="card-title">Motors</div>
|
||||||
|
<div style="font-size: 10px;">
|
||||||
|
<div>L: <span class="card-value" id="motorL">0</span><span class="card-unit">%</span></div>
|
||||||
|
<div style="margin-top: 4px;">R: <span class="card-value" id="motorR">0</span><span class="card-unit">%</span></div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
<div class="card">
|
||||||
|
<div class="card-title">Attitude</div>
|
||||||
|
<div style="font-size: 11px;">
|
||||||
|
<div>R: <span id="roll">0</span>°</div>
|
||||||
|
<div>P: <span id="pitch">0</span>°</div>
|
||||||
|
<div>Y: <span id="yaw">0</span>°</div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
<div class="card">
|
||||||
|
<div class="card-title">GPS</div>
|
||||||
|
<div style="font-size: 10px;">
|
||||||
|
<div id="lat">Waiting...</div>
|
||||||
|
<div id="lon">--</div>
|
||||||
|
<div id="alt">-- m</div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
<div class="card" id="cpuCard">
|
||||||
|
<div class="card-title">CPU Temp</div>
|
||||||
|
<div class="card-value" id="cpuTemp">0<span class="card-unit">°C</span></div>
|
||||||
|
</div>
|
||||||
|
<div class="card" id="gpuCard">
|
||||||
|
<div class="card-title">GPU Temp</div>
|
||||||
|
<div class="card-value" id="gpuTemp">0<span class="card-unit">°C</span></div>
|
||||||
|
</div>
|
||||||
|
<div class="card">
|
||||||
|
<div class="card-title">RAM</div>
|
||||||
|
<div class="card-value" id="ramPct">0<span class="card-unit">%</span></div>
|
||||||
|
<div class="progress-bar"><div class="progress-fill" id="ramBar" style="width: 0%;"></div></div>
|
||||||
|
</div>
|
||||||
|
<div class="card">
|
||||||
|
<div class="card-title">Disk</div>
|
||||||
|
<div class="card-value" id="diskPct">0<span class="card-unit">%</span></div>
|
||||||
|
<div class="progress-bar"><div class="progress-fill" id="diskBar" style="width: 0%;"></div></div>
|
||||||
|
</div>
|
||||||
|
<div class="card">
|
||||||
|
<div class="card-title">Social</div>
|
||||||
|
<div style="font-size: 11px;">
|
||||||
|
<div>Speaking: <span id="speaking">No</span></div>
|
||||||
|
<div>Face: <span id="faceId">none</span></div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
<div class="timestamp" id="timestamp">--</div>
|
||||||
|
<script>
|
||||||
|
const TOKEN = 'default_token';
|
||||||
|
const WS_URL = `ws://${window.location.host}/ws?token=${TOKEN}`;
|
||||||
|
let ws = null;
|
||||||
|
function getStatusClass(value, thresholds) {
|
||||||
|
if (thresholds.critical && value >= thresholds.critical) return 'critical';
|
||||||
|
if (thresholds.warning && value >= thresholds.warning) return 'warning';
|
||||||
|
return '';
|
||||||
|
}
|
||||||
|
function updateUI(telemetry) {
|
||||||
|
if (!telemetry) return;
|
||||||
|
const soc = telemetry.battery?.soc || 0;
|
||||||
|
document.getElementById('soc').textContent = soc.toFixed(0);
|
||||||
|
document.getElementById('voltage').textContent = (telemetry.battery?.voltage || 0).toFixed(1) + 'V';
|
||||||
|
document.getElementById('current').textContent = (telemetry.battery?.current || 0).toFixed(1) + 'A';
|
||||||
|
document.getElementById('socBar').style.width = soc + '%';
|
||||||
|
const batteryClass = getStatusClass(soc, { critical: 10, warning: 20 });
|
||||||
|
document.getElementById('batteryCard').className = 'card ' + batteryClass;
|
||||||
|
document.getElementById('socBar').className = 'progress-fill ' + batteryClass;
|
||||||
|
document.getElementById('soc').className = 'card-value ' + batteryClass;
|
||||||
|
document.getElementById('motorL').textContent = ((telemetry.motors?.left || 0) * 100).toFixed(0);
|
||||||
|
document.getElementById('motorR').textContent = ((telemetry.motors?.right || 0) * 100).toFixed(0);
|
||||||
|
document.getElementById('roll').textContent = (telemetry.imu?.roll || 0).toFixed(0);
|
||||||
|
document.getElementById('pitch').textContent = (telemetry.imu?.pitch || 0).toFixed(0);
|
||||||
|
document.getElementById('yaw').textContent = (telemetry.imu?.yaw || 0).toFixed(0);
|
||||||
|
document.getElementById('lat').textContent = (telemetry.gps?.lat || 0).toFixed(6);
|
||||||
|
document.getElementById('lon').textContent = (telemetry.gps?.lon || 0).toFixed(6);
|
||||||
|
document.getElementById('alt').textContent = (telemetry.gps?.alt || 0).toFixed(1) + 'm';
|
||||||
|
const cpuTemp = telemetry.health?.cpu_temp || 0;
|
||||||
|
const gpuTemp = telemetry.health?.gpu_temp || 0;
|
||||||
|
document.getElementById('cpuTemp').textContent = cpuTemp.toFixed(0);
|
||||||
|
document.getElementById('gpuTemp').textContent = gpuTemp.toFixed(0);
|
||||||
|
document.getElementById('cpuCard').className = 'card ' + getStatusClass(cpuTemp, { critical: 85, warning: 70 });
|
||||||
|
document.getElementById('gpuCard').className = 'card ' + getStatusClass(gpuTemp, { critical: 85, warning: 70 });
|
||||||
|
const ramPct = telemetry.health?.ram_pct || 0;
|
||||||
|
const diskPct = telemetry.health?.disk_pct || 0;
|
||||||
|
document.getElementById('ramPct').textContent = ramPct.toFixed(0);
|
||||||
|
document.getElementById('diskPct').textContent = diskPct.toFixed(0);
|
||||||
|
document.getElementById('ramBar').style.width = ramPct + '%';
|
||||||
|
document.getElementById('diskBar').style.width = diskPct + '%';
|
||||||
|
document.getElementById('speaking').textContent = telemetry.social?.is_speaking ? 'Yes' : 'No';
|
||||||
|
document.getElementById('faceId').textContent = telemetry.social?.face_id || 'none';
|
||||||
|
document.getElementById('timestamp').textContent = new Date(telemetry.timestamp).toLocaleTimeString();
|
||||||
|
updateAlerts(telemetry.alerts || []);
|
||||||
|
}
|
||||||
|
function updateAlerts(alerts) {
|
||||||
|
const list = document.getElementById('alertList');
|
||||||
|
if (alerts.length === 0) { list.style.display = 'none'; return; }
|
||||||
|
list.style.display = 'block';
|
||||||
|
list.innerHTML = alerts.map(a => `<div class="alert-item ${a.level.toLowerCase()}">[${a.level}] ${a.message} ${a.value ? '(' + a.value.toFixed(2) + ')' : ''}</div>`).join('');
|
||||||
|
}
|
||||||
|
function connect() {
|
||||||
|
ws = new WebSocket(WS_URL);
|
||||||
|
ws.onopen = () => {
|
||||||
|
document.getElementById('status').textContent = 'Connected';
|
||||||
|
document.getElementById('status').className = 'status connected';
|
||||||
|
ws.send(JSON.stringify({ cmd: 'get_history' }));
|
||||||
|
};
|
||||||
|
ws.onmessage = (e) => {
|
||||||
|
try {
|
||||||
|
const msg = JSON.parse(e.data);
|
||||||
|
if (msg.type === 'history') {
|
||||||
|
msg.data.forEach(t => updateUI(t));
|
||||||
|
} else {
|
||||||
|
updateUI(msg);
|
||||||
|
}
|
||||||
|
} catch (err) {
|
||||||
|
console.error('Decode error:', err);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
ws.onclose = () => {
|
||||||
|
document.getElementById('status').textContent = 'Disconnected';
|
||||||
|
document.getElementById('status').className = 'status disconnected';
|
||||||
|
setTimeout(connect, 3000);
|
||||||
|
};
|
||||||
|
}
|
||||||
|
connect();
|
||||||
|
</script>
|
||||||
|
</body>
|
||||||
|
</html>
|
||||||
@ -0,0 +1,30 @@
|
|||||||
|
import unittest
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
|
||||||
|
class TestRemoteMonitor(unittest.TestCase):
|
||||||
|
"""Basic tests for remote monitor functionality."""
|
||||||
|
|
||||||
|
def test_imports(self):
|
||||||
|
"""Test that the module can be imported."""
|
||||||
|
from saltybot_remote_monitor import remote_monitor_node
|
||||||
|
self.assertIsNotNone(remote_monitor_node)
|
||||||
|
|
||||||
|
def test_config_file_exists(self):
|
||||||
|
"""Test that config file exists."""
|
||||||
|
config_file = Path(__file__).parent.parent / 'config' / 'remote_monitor.yaml'
|
||||||
|
self.assertTrue(config_file.exists())
|
||||||
|
|
||||||
|
def test_launch_file_exists(self):
|
||||||
|
"""Test that launch file exists."""
|
||||||
|
launch_file = Path(__file__).parent.parent / 'launch' / 'remote_monitor.launch.py'
|
||||||
|
self.assertTrue(launch_file.exists())
|
||||||
|
|
||||||
|
def test_html_ui_exists(self):
|
||||||
|
"""Test that HTML UI exists."""
|
||||||
|
html_file = Path(__file__).parent.parent / 'static' / 'index.html'
|
||||||
|
self.assertTrue(html_file.exists())
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
unittest.main()
|
||||||
Loading…
x
Reference in New Issue
Block a user