任务执行
This commit is contained in:
ywb
@@ -0,0 +1,350 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
alpha1: 0.2
|
||||
alpha2: 0.2
|
||||
alpha3: 0.2
|
||||
alpha4: 0.2
|
||||
alpha5: 0.2
|
||||
base_frame_id: "base_footprint"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
lambda_short: 0.1
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: 100.0
|
||||
laser_min_range: -1.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: "nav2_amcl::DifferentialMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
tf_broadcast: true
|
||||
transform_tolerance: 5.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
|
||||
scan_topic: scan_corrected
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
wait_for_service_timeout: 1000
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_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_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_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_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_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
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_assisted_teleop_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
- nav2_is_battery_charging_condition_bt_node
|
||||
|
||||
bt_navigator_navigate_through_poses_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
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"] # "precise_goal_checker"
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
# Goal checker parameters
|
||||
#precise_goal_checker:
|
||||
# plugin: "nav2_controller::SimpleGoalChecker"
|
||||
# xy_goal_tolerance: 0.05
|
||||
# yaw_goal_tolerance: 0.05
|
||||
# stateful: True
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.05
|
||||
yaw_goal_tolerance: 0.05
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
min_vel_x: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 0.26
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 1.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.26
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 3.2
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -3.2
|
||||
vx_samples: 20
|
||||
vy_samples: 5
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 5.0
|
||||
xy_goal_tolerance: 0.05
|
||||
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
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.22
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan_corrected
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
robot_radius: 0.22
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan_corrected
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
always_send_full_costmap: True
|
||||
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
# Overridden in launch by the "map" launch configuration or provided default value.
|
||||
# To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below.
|
||||
yaml_filename: ""
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
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"
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
assisted_teleop:
|
||||
plugin: "nav2_behaviors/AssistedTeleop"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 5.0
|
||||
use_sim_time: true
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: False
|
||||
feedback: "OPEN_LOOP"
|
||||
max_velocity: [0.26, 0.0, 1.0]
|
||||
min_velocity: [-0.26, 0.0, -1.0]
|
||||
max_accel: [2.5, 0.0, 3.2]
|
||||
max_decel: [-2.5, 0.0, -3.2]
|
||||
odom_topic: "odom"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
@@ -0,0 +1,362 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
alpha1: 0.4
|
||||
alpha2: 0.3
|
||||
alpha3: 0.1
|
||||
alpha4: 0.1
|
||||
alpha5: 0.04
|
||||
base_frame_id: "base_footprint"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
lambda_short: 0.1
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: 100.0
|
||||
laser_min_range: -1.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: 2
|
||||
robot_model_type: "nav2_amcl::OmniMotionModel"
|
||||
scan_topic: scan_corrected
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.02
|
||||
tf_broadcast: true
|
||||
transform_tolerance: 5.0
|
||||
update_min_a: 0.06
|
||||
update_min_d: 0.025
|
||||
z_hit: 0.7
|
||||
z_max: 0.001
|
||||
z_rand: 0.059
|
||||
z_short: 0.24
|
||||
|
||||
# Initial Pose
|
||||
set_initial_pose: True
|
||||
initial_pose.x: 0.0
|
||||
initial_pose.y: 0.0
|
||||
initial_pose.z: 0.0
|
||||
initial_pose.yaw: 0.0
|
||||
|
||||
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_footprint
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
enable_groot_monitoring: True
|
||||
groot_zmq_publisher_port: 1666
|
||||
groot_zmq_server_port: 1667
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
||||
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_back_up_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_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_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_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: 10.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 3.0
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.15
|
||||
movement_time_allowance: 8.0
|
||||
# Goal checker parameters
|
||||
#precise_goal_checker:
|
||||
# plugin: "nav2_controller::SimpleGoalChecker"
|
||||
# xy_goal_tolerance: 0.05
|
||||
# yaw_goal_tolerance: 0.05
|
||||
# stateful: True
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.05
|
||||
yaw_goal_tolerance: 0.05
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
min_vel_x: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 0.26
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 0.5
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.26
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 0.5
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -0.5
|
||||
vx_samples: 20
|
||||
vy_samples: 5
|
||||
vtheta_samples: 40
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 5.0
|
||||
xy_goal_tolerance: 0.05
|
||||
trans_stopped_velocity: 0.1
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 23.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 18.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
controller_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_footprint
|
||||
use_sim_time: False
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
footprint: "[[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]]"
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 4.0
|
||||
inflation_radius: 0.30
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan_corrected_corrected
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
local_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
local_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
use_sim_time: False
|
||||
footprint: "[[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]]"
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan_corrected_corrected
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 4.0
|
||||
inflation_radius: 0.30
|
||||
always_send_full_costmap: True
|
||||
global_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
# Overridden in launch by the "map" launch configuration or provided default value.
|
||||
# To use in yaml, remove the default "map" value in the navigation2_active.launch.py file & provide full path to map below.
|
||||
yaml_filename: ""
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 1.0
|
||||
use_sim_time: False
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 2.0
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
planner_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
recoveries_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
recovery_plugins: ["spin", "backup", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_recoveries/Spin"
|
||||
backup:
|
||||
plugin: "nav2_recoveries/BackUp"
|
||||
wait:
|
||||
plugin: "nav2_recoveries/Wait"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_footprint
|
||||
transform_timeout: 0.1
|
||||
use_sim_time: False
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: False
|
||||
feedback: "OPEN_LOOP"
|
||||
max_velocity: [0.26, 0.0, 0.5]
|
||||
min_velocity: [-0.26, 0.0, -0.5]
|
||||
max_accel: [2.5, 0.0, 0.5]
|
||||
max_decel: [-2.5, 0.0, -0.5]
|
||||
odom_topic: "odom"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
@@ -0,0 +1,624 @@
|
||||
#include "agv_pro_base/agv_pro_driver.h"
|
||||
|
||||
uint16_t AGV_PRO::crc16_ibm(const uint8_t* data, size_t length) {
|
||||
uint16_t crc = 0xFFFF;
|
||||
for (size_t i = 0; i < length; ++i) {
|
||||
crc ^= static_cast<uint16_t>(data[i]);
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
if (crc & 0x0001)
|
||||
crc = (crc >> 1) ^ 0xA001;
|
||||
else
|
||||
crc = crc >> 1;
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> AGV_PRO::build_serial_frame(uint8_t cmd_id, const std::vector<uint8_t>& payload)
|
||||
{
|
||||
std::vector<uint8_t> frame(SEND_DATA_SIZE, 0x00);
|
||||
frame[0] = 0xFE;
|
||||
frame[1] = 0xFE;
|
||||
frame[2] = 0x0B;
|
||||
frame[3] = cmd_id;
|
||||
|
||||
for (size_t i = 0; i < payload.size() && i < 8; ++i) {
|
||||
frame[4 + i] = payload[i];
|
||||
}
|
||||
|
||||
uint16_t crc = crc16_ibm(frame.data(), 12);
|
||||
frame[12] = (crc >> 8) & 0xff;
|
||||
frame[13] = crc & 0xff;
|
||||
|
||||
return frame;
|
||||
}
|
||||
|
||||
void AGV_PRO::print_hex(const std::string& label, const std::vector<uint8_t>& data, std::optional<size_t> override_size) {
|
||||
std::stringstream ss;
|
||||
for (auto b : data) {
|
||||
ss << std::hex << std::uppercase << std::setfill('0') << std::setw(2)
|
||||
<< static_cast<int>(b) << " ";
|
||||
}
|
||||
size_t len = override_size.value_or(data.size());
|
||||
RCLCPP_INFO(this->get_logger(), "%s (%zu bytes): [%s]", label.c_str(), len, ss.str().c_str());
|
||||
}
|
||||
|
||||
void AGV_PRO::send_serial_frame(const std::vector<uint8_t>& frame, bool debug)
|
||||
{
|
||||
try {
|
||||
size_t bytes_transmit_size = boost::asio::write(*serial_port_, boost::asio::buffer(frame));
|
||||
if (debug) {
|
||||
print_hex("Sent", frame, bytes_transmit_size);
|
||||
}
|
||||
} catch (const std::exception &ex) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port: %s", ex.what());
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<uint8_t> AGV_PRO::read_serial_response(
|
||||
const std::vector<uint8_t>& expected_header,
|
||||
size_t payload_size,
|
||||
double timeout_sec)
|
||||
{
|
||||
std::vector<uint8_t> sliding_buf;
|
||||
uint8_t byte = 0;
|
||||
|
||||
rclcpp::Time start_time = this->now();
|
||||
rclcpp::Duration timeout = rclcpp::Duration::from_seconds(timeout_sec);
|
||||
|
||||
while ((this->now() - start_time) < timeout) {
|
||||
boost::asio::mutable_buffers_1 buf(&byte, 1);
|
||||
boost::system::error_code ec;
|
||||
size_t n = serial_port_->read_some(buf, ec);
|
||||
if (ec) {
|
||||
RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str());
|
||||
return {};
|
||||
}
|
||||
if (n == 1) {
|
||||
sliding_buf.push_back(byte);
|
||||
if (sliding_buf.size() > expected_header.size()) {
|
||||
sliding_buf.erase(sliding_buf.begin());
|
||||
}
|
||||
if (sliding_buf == expected_header) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (sliding_buf != expected_header) {
|
||||
RCLCPP_WARN(this->get_logger(), "Timeout waiting for header");
|
||||
return {};
|
||||
}
|
||||
|
||||
size_t remain_len = payload_size + 2;
|
||||
std::vector<uint8_t> remain_buf(remain_len);
|
||||
size_t total_read = 0;
|
||||
|
||||
while (total_read < remain_len && (this->now() - start_time) < timeout) {
|
||||
boost::asio::mutable_buffers_1 buf(&remain_buf[total_read], remain_len - total_read);
|
||||
boost::system::error_code ec;
|
||||
size_t n = serial_port_->read_some(buf, ec);
|
||||
if (ec) {
|
||||
RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str());
|
||||
return {};
|
||||
}
|
||||
total_read += n;
|
||||
}
|
||||
|
||||
if (total_read != remain_len) {
|
||||
RCLCPP_WARN(this->get_logger(), "Timeout or incomplete data payload");
|
||||
return {};
|
||||
}
|
||||
|
||||
std::vector<uint8_t> full_buf = expected_header;
|
||||
full_buf.insert(full_buf.end(), remain_buf.begin(), remain_buf.end());
|
||||
|
||||
return full_buf;
|
||||
}
|
||||
|
||||
bool AGV_PRO::is_power_on(){
|
||||
auto power_query_frame = build_serial_frame(GET_POWER_STATE, {});
|
||||
send_serial_frame(power_query_frame,true);
|
||||
|
||||
const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, 0x12};
|
||||
auto power_query_response = read_serial_response(expected_header, 8, 12.0);
|
||||
|
||||
print_hex("recv_buf", power_query_response);
|
||||
|
||||
if (power_query_response.size() != 14) return false;
|
||||
|
||||
uint16_t received_crc = (power_query_response[12] << 8) | power_query_response[13];
|
||||
uint16_t computed_crc = crc16_ibm(power_query_response.data(), 12);
|
||||
if (received_crc != computed_crc) {
|
||||
RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc);
|
||||
return false;
|
||||
}
|
||||
|
||||
int is_poweron_status = static_cast<int8_t>(power_query_response[4]);
|
||||
RCLCPP_INFO(this->get_logger(), "is_poweron_status: %d", is_poweron_status);
|
||||
|
||||
if (is_poweron_status == 0){
|
||||
auto status_query_frame = build_serial_frame(POWER_ON, {});
|
||||
send_serial_frame(status_query_frame,true);
|
||||
|
||||
rclcpp::sleep_for(std::chrono::milliseconds(1000));// Sleep for 1000 milliseconds to allow the device enough time to process the previous command
|
||||
|
||||
const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, 0x10};
|
||||
auto status_query_response = read_serial_response(expected_header, 8, 5.0);// Read the serial response with the specified expected header, payload size, and timeout of 5 seconds
|
||||
print_hex("recv_buf", status_query_response);
|
||||
|
||||
if (status_query_response.size() != 14) return false;
|
||||
|
||||
uint16_t received_crc = (status_query_response[12] << 8) | status_query_response[13];
|
||||
uint16_t computed_crc = crc16_ibm(status_query_response.data(), 12);
|
||||
if (received_crc != computed_crc) {
|
||||
RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc);
|
||||
return false;
|
||||
}
|
||||
|
||||
int poweron_status = static_cast<int8_t>(status_query_response[4]);
|
||||
std::string status_msg;
|
||||
|
||||
switch (poweron_status) {
|
||||
case 1:
|
||||
status_msg = "Motor is operating normally.";
|
||||
RCLCPP_INFO(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
|
||||
return true;
|
||||
case 2:
|
||||
status_msg = "Emergency stop button is not released.";
|
||||
RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
|
||||
return false;
|
||||
case 3:
|
||||
status_msg = "Battery voltage is below 19.5V.";
|
||||
RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
|
||||
return false;
|
||||
case 4:
|
||||
status_msg = "CAN initialization error.";
|
||||
RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
|
||||
return false;
|
||||
case 5:
|
||||
status_msg = "Motor initialization error.";
|
||||
RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
|
||||
return false;
|
||||
default:
|
||||
RCLCPP_WARN(this->get_logger(), "power_status: %d, Unknown power status code", poweron_status);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else{
|
||||
RCLCPP_INFO(this->get_logger(), "Motor is operating normally.");
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
void AGV_PRO::set_auto_report(bool enable){
|
||||
auto frame = build_serial_frame(0x23, {static_cast<uint8_t>(enable)});
|
||||
send_serial_frame(frame,true);
|
||||
}
|
||||
|
||||
void AGV_PRO::clearSerialBuffer(int fd) {
|
||||
if (tcflush(fd, TCIOFLUSH) < 0) {
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to flush serial buffer: %s", std::strerror(errno));
|
||||
} else {
|
||||
RCLCPP_INFO(this->get_logger(), "Serial buffer flushed.");
|
||||
}
|
||||
}
|
||||
|
||||
void AGV_PRO::disableDTR_RTS(int fd) {
|
||||
int status;
|
||||
if (::ioctl(fd, TIOCMGET, &status) == 0) {
|
||||
status &= ~(TIOCM_DTR | TIOCM_RTS);
|
||||
if (::ioctl(fd, TIOCMSET, &status) != 0) {
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to clear DTR and RTS: %s", std::strerror(errno));
|
||||
} else {
|
||||
RCLCPP_INFO(this->get_logger(), "DTR and RTS lines disabled successfully.");
|
||||
}
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to read modem status: %s", std::strerror(errno));
|
||||
}
|
||||
}
|
||||
|
||||
void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
{
|
||||
linearX = std::clamp(msg->linear.x, -1.5, 1.5);
|
||||
linearY = std::clamp(msg->linear.y, -1.0, 1.0);
|
||||
angularZ = std::clamp(msg->angular.z, -1.0, 1.0);
|
||||
|
||||
int16_t x_send = static_cast<int16_t>(linearX * 100);
|
||||
int16_t y_send = static_cast<int16_t>(linearY * 100);
|
||||
int16_t rot_send = static_cast<int16_t>(angularZ * 100);
|
||||
|
||||
uint8_t buf[14] = { 0xfe,0xfe,0x0b,0x21 };
|
||||
|
||||
buf[4] = (x_send >> 8) & 0xff;
|
||||
buf[5] = x_send & 0xff;
|
||||
buf[6] = (y_send >> 8) & 0xff;
|
||||
buf[7] = y_send & 0xff;
|
||||
buf[8] = (rot_send >> 8) & 0xff;
|
||||
buf[9] = rot_send & 0xff;
|
||||
buf[10] = 0x00;
|
||||
buf[11] = 0x00;
|
||||
|
||||
uint16_t crc = crc16_ibm(buf, 12);
|
||||
buf[12] = (crc >> 8) & 0xff;
|
||||
buf[13] = crc & 0xff;
|
||||
|
||||
std::vector<uint8_t> data_vec(buf, buf + sizeof(buf));
|
||||
|
||||
try
|
||||
{
|
||||
boost::asio::write(*serial_port_,boost::asio::buffer(data_vec));
|
||||
// print_hex("Sent", data_vec);//debug
|
||||
}
|
||||
catch(const std::exception &ex)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
|
||||
}
|
||||
}
|
||||
|
||||
void AGV_PRO::handleSetDigitalOutput(
|
||||
const std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Request> request,
|
||||
std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Response> response)
|
||||
{
|
||||
uint8_t output_number = request->pin;
|
||||
uint8_t output_state = request->state;
|
||||
|
||||
if (output_number < 1 || output_number > 6){
|
||||
RCLCPP_ERROR(this->get_logger(), "Invalid output pin number: %u", output_number);
|
||||
response->success = false;
|
||||
response->message = "Invalid output pin number";
|
||||
return;
|
||||
}
|
||||
|
||||
auto frame = build_serial_frame(SET_OUTPUT_IO, {output_number, output_state});
|
||||
send_serial_frame(frame, true);
|
||||
|
||||
const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, SET_OUTPUT_IO};
|
||||
auto response_frame = read_serial_response(expected_header, 8, 5.0);
|
||||
|
||||
// print_hex("recv_buf", response_frame); //debug
|
||||
|
||||
uint8_t status = response_frame[4];
|
||||
if (status == 0x01) {
|
||||
RCLCPP_INFO(this->get_logger(), "SetDigitalOutput succeeded");
|
||||
response->success = true;
|
||||
response->message = "Success";
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "SetDigitalOutput failed with status: 0x%02X", status);
|
||||
response->success = false;
|
||||
response->message = "Failed with status code";
|
||||
}
|
||||
}
|
||||
|
||||
void AGV_PRO::handleGetDigitalInput(
|
||||
const std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Request> request,
|
||||
std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> response)
|
||||
{
|
||||
uint8_t input_number = request->pin;
|
||||
|
||||
if (input_number < 1 || input_number > 6){
|
||||
RCLCPP_ERROR(this->get_logger(), "Invalid input pin number: %u", input_number);
|
||||
response->success = false;
|
||||
response->message = "Invalid input pin number";
|
||||
return;
|
||||
}
|
||||
|
||||
auto frame = build_serial_frame(GET_INPUT_IO, {input_number});
|
||||
send_serial_frame(frame, true);
|
||||
|
||||
const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, GET_INPUT_IO};
|
||||
auto response_frame = read_serial_response(expected_header, 8, 5.0);
|
||||
// print_hex("recv_buf", response_frame); //debug
|
||||
|
||||
uint8_t status = response_frame[5];
|
||||
if (status == 0xff) {
|
||||
RCLCPP_ERROR(this->get_logger(), "GetDigitalInput failed with status: 0x%02X", status);
|
||||
response->success = false;
|
||||
} else {
|
||||
RCLCPP_INFO(this->get_logger(), "GetDigitalInput succeeded, state: %u", status);
|
||||
response->state = static_cast<int32_t>(status);
|
||||
response->success = true;
|
||||
response->message = "Success";
|
||||
}
|
||||
}
|
||||
|
||||
bool AGV_PRO::readData()
|
||||
{
|
||||
std::vector<uint8_t> buf_length(1);
|
||||
std::vector<uint8_t> data_buf(RECEIVE_PAYLOAD_SIZE);
|
||||
|
||||
uint8_t byte = 0;
|
||||
boost::system::error_code ec;
|
||||
|
||||
while (true)
|
||||
{
|
||||
size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec);
|
||||
if (ec) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
|
||||
return false;
|
||||
}
|
||||
if (ret != 1 || byte != 0xfe) {
|
||||
continue;
|
||||
}
|
||||
|
||||
ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec);
|
||||
if (ec) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
|
||||
return false;
|
||||
}
|
||||
if (ret == 1 && byte == 0xfe) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(buf_length), ec);
|
||||
if (ec) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
if (buf_length[0] != RECEIVE_PAYLOAD_SIZE) {
|
||||
//RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]);
|
||||
return false;
|
||||
}
|
||||
|
||||
ret = boost::asio::read(*serial_port_, boost::asio::buffer(data_buf), ec);
|
||||
if (ec || ret != data_buf.size())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to receive full payload");
|
||||
return false;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> recv_buf;
|
||||
recv_buf.push_back(0xFE);
|
||||
recv_buf.push_back(0xFE);
|
||||
recv_buf.push_back(0x1C);
|
||||
recv_buf.insert(recv_buf.end(), data_buf.begin(), data_buf.end());
|
||||
|
||||
// print_hex("recv_buf", recv_buf); //debug
|
||||
|
||||
if (recv_buf[3] != 0x25) {
|
||||
//RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]); //debug
|
||||
return false;
|
||||
}
|
||||
|
||||
uint16_t received_crc = recv_buf[RECEIVE_FRAME_SIZE-1] | (recv_buf[RECEIVE_FRAME_SIZE-2] << 8);
|
||||
uint16_t computed_crc = crc16_ibm(recv_buf.data(), RECEIVE_FRAME_SIZE-2);
|
||||
|
||||
if (received_crc != computed_crc) {
|
||||
RCLCPP_WARN(this->get_logger(), "CRC error: received 0x%04X, calculated 0x%04X", received_crc, computed_crc);
|
||||
return false;
|
||||
}
|
||||
|
||||
vx = static_cast<double>(static_cast<int8_t>(recv_buf[4])) * 0.01;
|
||||
vy = static_cast<double>(static_cast<int8_t>(recv_buf[5])) * 0.01;
|
||||
vtheta = static_cast<double>(static_cast<int8_t>(recv_buf[6])) * 0.01;
|
||||
|
||||
motor_status = recv_buf[7];
|
||||
motor_error = recv_buf[8];
|
||||
battery_voltage = static_cast<float>(recv_buf[9]) / 10.0f;
|
||||
enable_status = recv_buf[10];
|
||||
|
||||
imu_data.linear_acceleration.x = static_cast<double>(static_cast<int16_t>((recv_buf[11] << 8) | recv_buf[12])) * 0.01;
|
||||
imu_data.linear_acceleration.y = static_cast<double>(static_cast<int16_t>((recv_buf[13] << 8) | recv_buf[14])) * 0.01;
|
||||
imu_data.linear_acceleration.z = static_cast<double>(static_cast<int16_t>((recv_buf[15] << 8) | recv_buf[16])) * 0.01;
|
||||
|
||||
imu_data.angular_velocity.x = static_cast<double>(static_cast<int16_t>((recv_buf[17] << 8) | recv_buf[18])) * 0.01;
|
||||
imu_data.angular_velocity.y = static_cast<double>(static_cast<int16_t>((recv_buf[19] << 8) | recv_buf[20])) * 0.01;
|
||||
imu_data.angular_velocity.z = static_cast<double>(static_cast<int16_t>((recv_buf[21] << 8) | recv_buf[22])) * 0.01;
|
||||
|
||||
roll = static_cast<double>(static_cast<int16_t>((recv_buf[23] << 8) | recv_buf[24])) * 0.01;
|
||||
pitch = static_cast<double>(static_cast<int16_t>((recv_buf[25] << 8) | recv_buf[26])) * 0.01;
|
||||
yaw = static_cast<double>(static_cast<int16_t>((recv_buf[27] << 8) | recv_buf[28])) * 0.01;
|
||||
|
||||
// RCLCPP_INFO(this->get_logger(),
|
||||
// "IMU Data - Accel[x: %.2f, y: %.2f, z: %.2f], "
|
||||
// "Gyro[x: %.2f, y: %.2f, z: %.2f], "
|
||||
// "RPY[roll: %.2f, pitch: %.2f, yaw: %.2f]",
|
||||
// imu_data.linear_acceleration.x,
|
||||
// imu_data.linear_acceleration.y,
|
||||
// imu_data.linear_acceleration.z,
|
||||
// imu_data.angular_velocity.x,
|
||||
// imu_data.angular_velocity.y,
|
||||
// imu_data.angular_velocity.z,
|
||||
// roll, pitch, yaw);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void AGV_PRO::publisherVoltage()
|
||||
{
|
||||
std_msgs::msg::Float32 voltage_msg,voltage_backup_msg;
|
||||
voltage_msg.data = battery_voltage;
|
||||
pub_voltage->publish(voltage_msg);
|
||||
}
|
||||
|
||||
void AGV_PRO::publisherImuSensor()
|
||||
{
|
||||
sensor_msgs::msg::Imu ImuSensor;
|
||||
|
||||
ImuSensor.header.stamp = this->get_clock()->now();
|
||||
ImuSensor.header.frame_id = "imu_link";
|
||||
|
||||
tf2::Quaternion qua;
|
||||
qua.setRPY(0, 0, yaw * M_PI / 180.0);
|
||||
|
||||
ImuSensor.orientation.x = qua[0];
|
||||
ImuSensor.orientation.y = qua[1];
|
||||
ImuSensor.orientation.z = qua[2];
|
||||
ImuSensor.orientation.w = qua[3];
|
||||
|
||||
ImuSensor.angular_velocity.x = imu_data.angular_velocity.x;
|
||||
ImuSensor.angular_velocity.y = imu_data.angular_velocity.y;
|
||||
ImuSensor.angular_velocity.z = imu_data.angular_velocity.z;
|
||||
|
||||
ImuSensor.linear_acceleration.x = imu_data.linear_acceleration.x;
|
||||
ImuSensor.linear_acceleration.y = imu_data.linear_acceleration.y;
|
||||
ImuSensor.linear_acceleration.z = imu_data.linear_acceleration.z;
|
||||
|
||||
ImuSensor.orientation_covariance[0] = 1e6;
|
||||
ImuSensor.orientation_covariance[4] = 1e6;
|
||||
ImuSensor.orientation_covariance[8] = 1e-6;
|
||||
|
||||
ImuSensor.angular_velocity_covariance[0] = 1e6;
|
||||
ImuSensor.angular_velocity_covariance[4] = 1e6;
|
||||
ImuSensor.angular_velocity_covariance[8] = 1e-6;
|
||||
|
||||
pub_imu->publish(ImuSensor);
|
||||
}
|
||||
|
||||
void AGV_PRO::publisherOdom(double dt)
|
||||
{
|
||||
currentTime = this->get_clock()->now();
|
||||
|
||||
double delta_x = (vx * cos(theta) - vy * sin(theta)) * dt;
|
||||
double delta_y = (vx * sin(theta) + vy * cos(theta)) * dt;
|
||||
double delta_th = vtheta * dt;
|
||||
|
||||
x += delta_x;
|
||||
y += delta_y;
|
||||
theta += delta_th;
|
||||
|
||||
geometry_msgs::msg::TransformStamped odom_trans;
|
||||
odom_trans.header.stamp = currentTime;
|
||||
odom_trans.header.frame_id = frame_id_of_odometry_;
|
||||
odom_trans.child_frame_id = child_frame_id_of_odometry_;
|
||||
|
||||
tf2::Quaternion quat;
|
||||
quat.setRPY(0.0, 0.0, theta);
|
||||
geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(quat);
|
||||
|
||||
odom_trans.transform.translation.x = x;
|
||||
odom_trans.transform.translation.y = y;
|
||||
odom_trans.transform.translation.z = 0.0;
|
||||
odom_trans.transform.rotation = odom_quat;
|
||||
|
||||
odomBroadcaster->sendTransform(odom_trans);
|
||||
|
||||
nav_msgs::msg::Odometry odom;
|
||||
odom.header.stamp = currentTime;
|
||||
odom.header.frame_id = frame_id_of_odometry_;
|
||||
odom.child_frame_id = child_frame_id_of_odometry_;
|
||||
|
||||
odom.pose.pose.position.x = x;
|
||||
odom.pose.pose.position.y = y;
|
||||
odom.pose.pose.position.z = 0.0;
|
||||
odom.pose.pose.orientation = odom_quat;
|
||||
odom.pose.covariance = this->odom_pose_covariance;
|
||||
|
||||
odom.twist.twist.linear.x = vx;
|
||||
odom.twist.twist.linear.y = vy;
|
||||
odom.twist.twist.angular.z = vtheta;
|
||||
odom.twist.covariance = this->odom_twist_covariance;
|
||||
|
||||
pub_odom->publish(odom);
|
||||
}
|
||||
|
||||
void AGV_PRO::Control()
|
||||
{
|
||||
if (true == readData())
|
||||
{
|
||||
currentTime = this->get_clock()->now();
|
||||
double dt = 0.0;
|
||||
if (lastTime.nanoseconds() != 0) {
|
||||
dt = (currentTime - lastTime).seconds();
|
||||
}
|
||||
|
||||
lastTime = currentTime;
|
||||
publisherOdom(dt);
|
||||
// RCLCPP_INFO(this->get_logger(), "dt:%f", dt);
|
||||
publisherVoltage();
|
||||
publisherImuSensor();
|
||||
}
|
||||
}
|
||||
|
||||
AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name)
|
||||
{
|
||||
this->declare_parameter<std::string>("port_name","/dev/agvpro_controller");
|
||||
this->declare_parameter<std::string>("odometry.frame_id", "odom");
|
||||
this->declare_parameter<std::string>("odometry.child_frame_id", "base_footprint");
|
||||
this->declare_parameter<std::string>("imu.frame_id", "imu_link");
|
||||
this->declare_parameter<std::string>("namespace", "");
|
||||
|
||||
this->get_parameter_or<std::string>("port_name",device_name_,std::string("/dev/agvpro_controller"));
|
||||
this->get_parameter_or<std::string>("odometry.frame_id",frame_id_of_odometry_,std::string("odom"));
|
||||
this->get_parameter_or<std::string>("odometry.child_frame_id",child_frame_id_of_odometry_,std::string("base_footprint"));
|
||||
this->get_parameter_or<std::string>("imu.frame_id",frame_id_of_imu_,std::string("imu_link"));
|
||||
this->get_parameter_or<std::string>("namespace",name_space_,std::string(""));
|
||||
|
||||
if (name_space_ != "") {
|
||||
frame_id_of_odometry_ = name_space_ + "/" + frame_id_of_odometry_;
|
||||
child_frame_id_of_odometry_ = name_space_ + "/" + child_frame_id_of_odometry_;
|
||||
frame_id_of_imu_ = name_space_ + "/" + frame_id_of_imu_;
|
||||
}
|
||||
|
||||
odomBroadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(this);
|
||||
pub_imu = this->create_publisher<sensor_msgs::msg::Imu>("imu", 20);
|
||||
pub_odom = this->create_publisher<nav_msgs::msg::Odometry>("odom", rclcpp::SensorDataQoS());
|
||||
pub_voltage = create_publisher<std_msgs::msg::Float32>("voltage", 10);
|
||||
cmd_sub = this->create_subscription<geometry_msgs::msg::Twist>(
|
||||
"/cmd_vel", 10, std::bind(&AGV_PRO::cmdCallback, this, std::placeholders::_1));
|
||||
|
||||
set_output_service = this->create_service<agv_pro_msgs::srv::SetDigitalOutput>(
|
||||
"set_digital_output",
|
||||
std::bind(&AGV_PRO::handleSetDigitalOutput, this, std::placeholders::_1, std::placeholders::_2)
|
||||
);
|
||||
|
||||
get_input_service = this->create_service<agv_pro_msgs::srv::GetDigitalInput>(
|
||||
"get_digital_input",
|
||||
std::bind(&AGV_PRO::handleGetDigitalInput, this, std::placeholders::_1, std::placeholders::_2)
|
||||
);
|
||||
|
||||
lastTime = this->get_clock()->now();
|
||||
|
||||
try{
|
||||
serial_port_ = std::make_unique<boost::asio::serial_port>(io_);
|
||||
|
||||
serial_port_->open(device_name_);
|
||||
serial_port_->set_option(boost::asio::serial_port_base::baud_rate(1000000));
|
||||
serial_port_->set_option(boost::asio::serial_port_base::character_size(8));
|
||||
serial_port_->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
|
||||
serial_port_->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));
|
||||
serial_port_->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));
|
||||
|
||||
int fd = serial_port_->native_handle();
|
||||
this->clearSerialBuffer(fd);
|
||||
this->disableDTR_RTS(fd);
|
||||
|
||||
rclcpp::sleep_for(std::chrono::milliseconds(3000));//esp32 Restart time
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
|
||||
RCLCPP_INFO(this->get_logger(), "Using device: %s", device_name_.c_str());
|
||||
|
||||
boost::asio::serial_port_base::baud_rate baud_option;
|
||||
serial_port_->get_option(baud_option);
|
||||
unsigned int current_baud = baud_option.value();
|
||||
RCLCPP_INFO(this->get_logger(), "Baud_rate: %u", current_baud);
|
||||
}
|
||||
catch (const std::exception &ex){
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
|
||||
return;
|
||||
}
|
||||
|
||||
if (this->is_power_on()) {
|
||||
this->set_auto_report(1);
|
||||
|
||||
control_timer_ = this->create_wall_timer(
|
||||
std::chrono::milliseconds(20),
|
||||
std::bind(&AGV_PRO::Control, this)
|
||||
);
|
||||
RCLCPP_INFO(this->get_logger(), "Control timer started");
|
||||
}
|
||||
else {
|
||||
RCLCPP_WARN(this->get_logger(), "Control timer not started.");
|
||||
}
|
||||
}
|
||||
|
||||
AGV_PRO::~AGV_PRO()
|
||||
{
|
||||
if (serial_port_ && serial_port_->is_open()) {
|
||||
this->set_auto_report(0);
|
||||
serial_port_->cancel();
|
||||
serial_port_->close();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user