文章列表
第六步：SLAM导航
准备完结，但是这步更是折磨。
手动导航
- 启动命令：
ros2 launch auto_drive auto_drive_env.launch.py yaw:=1.5708 slam:=true nav2:=true rviz:=true - 手动导航方法见第二篇文章。
- 用这行指令可以启动SLAM导航。但是此时地图很小（不能直接导航到终点）。即使一点点导航，小车有可能走到终点，但是有很大概率在转弯过程中卡死。
这两个问题肯定有官方解决方法，但是鉴于我们是来应付作业的，这里用最凑合、最偷懒的方法解决：
- 地图小解决方法：等到巡线结束，地图就够大了。
转弯卡死解决方法：
- 对
/opt/ros/jazzy/shared/turtlebot4_navigation/config/nav2.yaml文件中的controller_server进行了修改。具体修改内容过多。总的来说，减小了加速度和速度的上下限，增加了目标相关的权重，禁用了部分评估器，简化不必要的约束，降低控制器复杂度以提高效率和稳定性。 - 怎么调的我也记不住了，总之就是参考Model Predictive Path Integral Controller — Nav2 1.0.0 documentation。参考代码如下：
- 对
bt_navigator:
ros__parameters:
use_sim_time: true
enable_stamped_cmd_vel: 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
action_server_result_timeout: 900.0
navigators: ["navigate_to_pose", "navigate_through_poses"]
navigate_to_pose:
plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
navigate_through_poses:
plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
error_code_names:
- compute_path_error_code
- follow_path_error_code
controller_server:
ros__parameters:
use_sim_time: true
enable_stamped_cmd_vel: 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_plugins: ["progress_checker"]
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
use_realtime_priority: false
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
general_goal_checker:
stateful: true
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
time_steps: 40
model_dt: 0.05
batch_size: 1000
ax_max: 1.0
ax_min: -1.0
ay_max: 0.0
az_max: 1.0
vx_std: 0.1
vy_std: 0.0
wz_std: 0.1
vx_max: 2.0
vx_min: -2.0
vy_max: 0.0
wz_max: 1.0
iteration_count: 1
prune_distance: 0.6
transform_tolerance: 0.1
temperature: 0.3
gamma: 0.015
motion_model: "DiffDrive"
visualize: true
regenerate_noises: true
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
AckermannConstraints:
min_turning_r: 0.01
critics: [
"ConstraintCritic", "CostCritic", "GoalCritic",
"GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
"PathAngleCritic", "PreferForwardCritic"]
ConstraintCritic:
enabled: true
cost_power: 1
cost_weight: 10.0
GoalCritic:
enabled: true
cost_power: 1
cost_weight: 10.0
threshold_to_consider: 0.4
GoalAngleCritic:
enabled: true
cost_power: 1
cost_weight: 10.0
threshold_to_consider: 0.4
PreferForwardCritic:
enabled: false
cost_power: 1
cost_weight: 5.0
threshold_to_consider: 0.5
CostCritic:
enabled: false
cost_power: 1
cost_weight: 3.81
critical_cost: 300.0
consider_footprint: true
collision_cost: 1000000.0
near_goal_distance: 1.0
trajectory_point_step: 2
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 10.0
max_path_occupancy_ratio: 0.07
trajectory_point_step: 1
threshold_to_consider: 0.4
offset_from_furthest: 2
use_path_orientations: false
PathFollowCritic:
enabled: true
cost_power: 1
cost_weight: 30.0
offset_from_furthest: 2
threshold_to_consider: 0.4
PathAngleCritic:
enabled: true
cost_power: 1
cost_weight: 10.0
offset_from_furthest: 2
threshold_to_consider: 0.4
max_angle_to_furthest: 0.3
mode: 0
local_costmap:
local_costmap:
ros__parameters:
use_sim_time: true
enable_stamped_cmd_vel: true
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.06
robot_radius: 0.175
plugins: ["static_layer", "voxel_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 4.0
inflation_radius: 0.45
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
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:
use_sim_time: true
enable_stamped_cmd_vel: true
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
use_sim_time: true
robot_radius: 0.175
resolution: 0.06
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
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.45
always_send_full_costmap: true
planner_server:
ros__parameters:
use_sim_time: true
enable_stamped_cmd_vel: true
#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
enable_stamped_cmd_vel: 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:
use_sim_time: true
enable_stamped_cmd_vel: true
local_costmap_topic: local_costmap/costmap_raw
global_costmap_topic: global_costmap/costmap_raw
local_footprint_topic: local_costmap/published_footprint
global_footprint_topic: global_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: map
local_frame: odom
robot_base_frame: base_link
transform_tolerance: 0.1
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
enable_stamped_cmd_vel: true
loop_rate: 20
stop_on_failure: false
action_server_result_timeout: 900.0
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
enable_stamped_cmd_vel: 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
collision_monitor:
ros__parameters:
use_sim_time: true
enable_stamped_cmd_vel: true
base_frame_id: "base_link"
odom_frame_id: "odom"
cmd_vel_in_topic: "cmd_vel_smoothed"
cmd_vel_out_topic: "cmd_vel"
state_topic: "collision_monitor_state"
transform_tolerance: 0.2
source_timeout: 1.0
base_shift_correction: True
stop_pub_timeout: 2.0
# Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
# and robot footprint for "approach" action type.
polygons: ["FootprintApproach"]
FootprintApproach:
type: "polygon"
action_type: "approach"
footprint_topic: "/local_costmap/published_footprint"
time_before_collision: 1.2
simulation_time_step: 0.1
min_points: 6
visualize: False
enabled: True
observation_sources: ["scan"]
scan:
type: "scan"
topic: "scan"
min_height: 0.15
max_height: 2.0
enabled: True
docking_server:
ros__parameters:
use_sim_time: true
enable_stamped_cmd_vel: true
controller_frequency: 50.0
initial_perception_timeout: 5.0
wait_charge_timeout: 5.0
dock_approach_timeout: 30.0
undock_linear_tolerance: 0.05
undock_angular_tolerance: 0.1
max_retries: 3
base_frame: "base_link"
fixed_frame: "odom"
dock_backwards: false
dock_prestaging_tolerance: 0.5
# Types of docks
dock_plugins: ['simple_charging_dock']
simple_charging_dock:
plugin: 'opennav_docking::SimpleChargingDock'
docking_threshold: 0.05
staging_x_offset: -0.7
use_external_detection_pose: true
use_battery_status: false # true
use_stall_detection: false # true
external_detection_timeout: 1.0
external_detection_translation_x: -0.18
external_detection_translation_y: 0.0
external_detection_rotation_roll: -1.57
external_detection_rotation_pitch: -1.57
external_detection_rotation_yaw: 0.0
filter_coef: 0.1
# Dock instances
# The following example illustrates configuring dock instances.
# docks: ['home_dock'] # Input your docks here
# home_dock:
# type: 'simple_charging_dock'
# frame: map
# pose: [0.0, 0.0, 0.0]
controller:
k_phi: 3.0
k_delta: 2.0
v_linear_min: 0.15
v_linear_max: 0.15自动导航
自动导航，就是在巡线结束之后，自动设置几个路径点，让小车导航通过这几个路径点（就相当于走出迷宫了）。
- 判断巡线结束的方法：摄像头里没拍到黄色的像素了（零阶矩为0）。
自动设置路径点的方法：
- 不同于教程，需要删掉以下代码才可以使程序正常运行（不知道原因）：
nav.waitUntilNav2Active() # if autostarted, else use lifecycleStartup() - 然后修改
auto_drive_node.py，参考代码如下：
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, PoseStamped
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np
import time
import os
from threading import Thread
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
class ADNode(Node):
def __init__(self):
super().__init__('auto_drive_node')
self.vel_pub = self.create_publisher(Twist, '/cmd_vel_unstamped', 1)
self.timer = self.create_timer(0.02, self.timer_callback)
self.bridge = CvBridge()
self.img_sub = self.create_subscription(Image,
'/oakd/rgb/preview/image_raw',
self.img_callback,
1)
self.linear_speed = 1.2
self.yaw_speed = 0.0
self.red_pattern = cv2.imread('/home/zwz/turtlebot4_ws/src/auto_drive/auto_drive/red_pattern.png')
self.green_pattern = cv2.imread('/home/zwz/turtlebot4_ws/src/auto_drive/auto_drive/green_pattern.png')
self.stop_pattern = cv2.imread('/home/zwz/turtlebot4_ws/src/auto_drive/auto_drive/stop_pattern.png')
self.cnt = 0
'''
DO NOT USE BACK_SLASH OR ENTER
create green light:
gz service --service /world/drive_track/create --reqtype gz.msgs.EntityFactory --reptype gz.msgs.Boolean --timeout 2000 --req 'name: "green_light"; sdf_filename: "/home/zwz/turtlebot4_ws/src/auto_drive/world/green_light.sdf"; pose: {position: {x: -2, y: 4, z: 0}, orientation: {x: 0, y: 0, z: 1, w: 0}}'
create red light:
gz service --service /world/drive_track/create --reqtype gz.msgs.EntityFactory --reptype gz.msgs.Boolean --timeout 2000 --req 'name: "red_light"; sdf_filename: "/home/zwz/turtlebot4_ws/src/auto_drive/world/red_light.sdf"; pose: {position: {x: -2, y: 4, z: 0}, orientation: {x: 0, y: 0, z: 1, w: 0}}'
remove red light:
gz service --service /world/drive_track/remove --reqtype gz.msgs.Entity --reptype gz.msgs.Boolean --timeout 2000 --req 'type: MODEL; name: "red_light"'
'''
self.create_green_light_cmd = """gz service --service /world/drive_track/create --reqtype gz.msgs.EntityFactory --reptype gz.msgs.Boolean --timeout 2000 --req 'name: "green_light"; sdf_filename: "/home/zwz/turtlebot4_ws/src/auto_drive/world/green_light.sdf"; pose: {position: {x: -2, y: 4, z: 0}, orientation: {x: 0, y: 0, z: 1, w: 0}}'"""
self.create_red_light_cmd = """gz service --service /world/drive_track/create --reqtype gz.msgs.EntityFactory --reptype gz.msgs.Boolean --timeout 2000 --req 'name: "red_light"; sdf_filename: "/home/zwz/turtlebot4_ws/src/auto_drive/world/red_light.sdf"; pose: {position: {x: -2, y: 4, z: 0}, orientation: {x: 0, y: 0, z: 1, w: 0}}'"""
self.remove_red_light_cmd = """gz service --service /world/drive_track/remove --reqtype gz.msgs.Entity --reptype gz.msgs.Boolean --timeout 2000 --req 'type: MODEL; name: "red_light"'"""
# for create and remove traffic light
os.system(self.create_red_light_cmd)
# os.system(self.create_green_light_cmd)
self.change_traffic_light_thread = Thread(target=self.change_traffic_light)
# navigation
self.navigator = BasicNavigator()
# Set initial pose
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = self.navigator.get_clock().now().to_msg()
initial_pose.pose.position.x = 0.0
initial_pose.pose.position.y = 0.0
initial_pose.pose.orientation.w = 1.0
initial_pose.pose.orientation.z = 0.0
self.navigator.setInitialPose(initial_pose)
def nav(self):
# set our goal poses
goal_poses = []
goal_pose1 = PoseStamped()
goal_pose1.header.frame_id = 'map'
goal_pose1.header.stamp = self.navigator.get_clock().now().to_msg()
goal_pose1.pose.position.x = -1.3
goal_pose1.pose.position.y = 3.0
goal_pose1.pose.orientation.w = 0.7071068
goal_pose1.pose.orientation.z = -0.7071068
goal_poses.append(goal_pose1)
goal_pose2 = PoseStamped()
goal_pose2.header.frame_id = 'map'
goal_pose2.header.stamp = self.navigator.get_clock().now().to_msg()
goal_pose2.pose.position.x = -0.3
goal_pose2.pose.position.y = 1.8
goal_pose2.pose.orientation.w = 0.7071068
goal_pose2.pose.orientation.z = -0.7071068
goal_poses.append(goal_pose2)
goal_pose3 = PoseStamped()
goal_pose3.header.frame_id = 'map'
goal_pose3.header.stamp = self.navigator.get_clock().now().to_msg()
goal_pose3.pose.position.x = -1.3
goal_pose3.pose.position.y = 0.7
goal_pose3.pose.orientation.w = 0.7071068
goal_pose3.pose.orientation.z = -0.7071068
goal_poses.append(goal_pose3)
goal_pose4 = PoseStamped()
goal_pose4.header.frame_id = 'map'
goal_pose4.header.stamp = self.navigator.get_clock().now().to_msg()
goal_pose4.pose.position.x = 0.4
goal_pose4.pose.position.y = 0.0
goal_pose4.pose.orientation.w = 1.0
goal_pose4.pose.orientation.z = 0.0
goal_poses.append(goal_pose4)
self.navigator.goThroughPoses(goal_poses)
while not self.navigator.isTaskComplete():
feedback = self.navigator.getFeedback()
print(feedback)
def change_traffic_light(self):
time.sleep(7)
os.system(self.create_green_light_cmd)
os.system(self.remove_red_light_cmd)
def img_callback(self, msg):
try:
cv_img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
except Exception as e:
self.get_logger().error('bridge.imgmsg_to_cv2 failed')
return
# turn bgr8 into hsv
hsv_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2HSV)
lower_yellow = np.array([15, 50, 50])
upper_yellow = np.array([45, 255, 255])
# filter out yellow line
mask = cv2.inRange(hsv_img, lower_yellow, upper_yellow)
# cv2.imwrite(f'/home/zwz/turtlebot4_ws/src/auto_drive/auto_drive/imgs/{self.cnt}.png', cv_img)
# self.cnt += 1
cv2.imshow('img', cv_img)
cv2.waitKey(1)
# print(mask.shape) (240, 320)
mask = mask[220:, :]
moments = cv2.moments(mask)
if moments['m00'] == 0:
self.timer.destroy()
self.nav()
return
# check traffic light
now_traffic_light_pattern = cv_img[5:133, 270:315, :]
now_stop_sign_pattern = cv_img[20:128, 2:113, :]
red_diff = now_traffic_light_pattern.astype("float") - self.red_pattern.astype("float")
squared_red_diff = np.square(red_diff)
red_mse = np.mean(squared_red_diff)
green_diff = now_traffic_light_pattern.astype("float") - self.green_pattern.astype("float")
squared_green_diff = np.square(green_diff)
green_mse = np.mean(squared_green_diff)
stop_diff = now_stop_sign_pattern.astype("float") - self.stop_pattern.astype("float")
squared_stop_diff = np.square(stop_diff)
stop_mse = np.mean(squared_stop_diff)
print(f'red_mse: {red_mse}; green_mse: {green_mse}; stop_mse: {stop_mse}')
if red_mse < 1000 and red_mse < green_mse:
self.linear_speed = 0.0
if not self.change_traffic_light_thread.is_alive():
self.change_traffic_light_thread.start()
if green_mse < 4000 and green_mse < red_mse:
self.linear_speed = 1.2
if stop_mse < 3000:
self.linear_speed = 0
time.sleep(7)
self.linear_speed = 1.2
time.sleep(2)
try:
x_center = moments['m10'] / moments['m00']
self.yaw_speed = (160 - x_center) * 0.08
except:
self.get_logger().error("ERROR:moments['m00'] == 0")
pass
def timer_callback(self):
msg = Twist()
msg.angular.z = self.yaw_speed
msg.linear.x = self.linear_speed
self.vel_pub.publish(msg)
def main(args=None):
rclpy.init(args=args)
ad_node = ADNode()
rclpy.spin(ad_node)
ad_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()- 为什么要设置路径点？因为只设置一个终点的话，会从迷宫外面绕过去。
- 路径点的位置怎么设置的？瞪眼法+多试。
- 增加/减少路径点会怎么样？有可能会在转弯时卡死，看运气。
启动：
ros2 launch auto_drive auto_drive_env.launch.py yaw:=1.5708 slam:=true nav2:=true rviz:=trueros2 run auto_drive auto_drive_node.py
ROS2+Gazebo简易自动驾驶全部完结！！！
**粗体** _斜体_ [链接](http://example.com) `代码` - 列表 > 引用。你还可以使用@来通知其他用户。