ROS2自主导航

通过里程计可以获取机器人的位置信息激光雷达可以获取环境的位置和距离信息先启动上一章的仿真机器人。配置运行ros2 launch bot_description gazebo_sim.launch.py再打开一个终端打开rviz固定节点改为里程计点击“Add”选择“By topic”里的odeometry保留数据的长度改为1000再点击“Add”添加“Laserscan”点的大小size可自行调整将保留数据的时间改为1000再打开一个终端运行键盘控制节点控制机器人移动ros2 run teleop_twist_keyboard teleop_twist_keyboard奇了怪了我在rviz中就能正常显示运动是gazebo没加载好吗因为它停止维护了真的就在正常移动啊显示的是墙的点云从演示效果可以看出激光雷达能够初步获取周围环境的轮廓信息但在机器人运动过程中障碍物位置信息存在明显偏差主要受数据不同步与传感器噪声影响。同步定位与地图构建SLAM技术融合里程计与激光雷达数据能够有效抑制噪声、实现运动补偿最终完成实时地图构建与精准定位全局路径规划确认一条从起点到终点的路线局部路径规划依据实时传感器信息动态调整局部轨迹实现避障与平稳运动使用slam_toolbox建图安装slam_toolboxsudo apt install ros-humble-slam-toolbox创建一个新的工作空间在里面创建src文件夹将之前项目src下的bot_description复制到该src目录下mkdir map_ws cd map_ws/ mkdir src cp -r ../ros2/src/bot_description src/可以看到已经成功移动过来了构建配置运行ros2 launch bot_description gazebo_sim.launch.py再打开一个终端进行建图ros2 launch slam_toolbox online_async_launch.py use_sim_time:True再打开一个新的终端输入rviz2在rviz中查看建立的地图固定坐标改为map点击“Add”选择“By topic”里的Map由slam_toolbox发布等一下就加载成功了再打开一个终端运行键盘控制节点控制小车移动尽可能的慢ros2 run teleop_twist_keyboard teleop_twist_keyboard把整个房间逛一遍就能得到完整的地图了我这个很漂呀好像是要修改什么参数的但我不想细究了应该是太漂了吧有好几个房间似的好像是有参数可以校准的但我暂时不去研究了将地图保存为文件安装nav2-map-server可能之前已经安装好了sudo apt install ros-humble-nav2-map-server在src下新建一个功能包在其中新建一个maps文件夹之前的终端不要关闭啊键盘走完了可以关闭关闭了重新打开然后右键在终端中打开maps目录运行ros2 run nav2_map_server map_saver_cli -f room可以看到生成了2个文件pgm是图片yaml是描述文件机器人导航框架Navigation2安装Navigation2sudo apt install ros-humble-navigation2还可以安装启动示例功能包sudo apt install ros-humble-nav2-bringup配置navigation2参数在bot_navigation2下新建一个config文件夹将navigation2提供的默认参数配置复制到该文件夹中在终端中打开该文件夹运行cp /opt/ros/humble/share/nav2_bringup/params/nav2_params.yaml .编写launch启动导航有了参数我们就可以编写launch文件传递参数并启动导航了新建launch文件夹新建navigation2.launch.py文件import os import launch import launch_ros from ament_index_python.packages import get_package_share_directory from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): bot_navigation2_dir get_package_share_directory(bot_navigation2) nav2_bringup_dir get_package_share_directory(nav2_bringup) rviz_config_dir os.path.join(nav2_bringup_dir, rviz, nav2_default_view.rviz) # 创建 Launch 配置 use_sim_time launch.substitutions.LaunchConfiguration(use_sim_time, defaulttrue) map_yaml_path launch.substitutions.LaunchConfiguration(map, defaultos.path.join(bot_navigation2_dir, maps, room.yaml)) nav2_param_path launch.substitutions.LaunchConfiguration(params_file, defaultos.path.join(bot_navigation2_dir, config, nav2_params.yaml)) return launch.LaunchDescription([ # 声明新的 Launch 参数 launch.actions.DeclareLaunchArgument(use_sim_time, default_valueuse_sim_time, descriptionUse simulation (Gazebo) clock if true), launch.actions.DeclareLaunchArgument(map, default_valuemap_yaml_path, descriptionFull path to map file to load), launch.actions.DeclareLaunchArgument(params_file, default_valuenav2_param_path, descriptionFull path to param file to load), launch.actions.IncludeLaunchDescription( PythonLaunchDescriptionSource([nav2_bringup_dir, /launch, /bringup_launch.py]), # 使用 Launch 参数替换原有参数 launch_arguments{map: map_yaml_path,use_sim_time: use_sim_time,params_file: nav2_param_path}.items(), ), launch_ros.actions.Node( packagerviz2, executablerviz2, namerviz2, arguments[-d, rviz_config_dir], parameters[{use_sim_time: use_sim_time}], outputscreen), ])在CMakeLists.txt文件中添加拷贝到install目录的代码然后在工作空间中colcon build配置启动仿真ros2 launch bot_description gazebo_sim.launch.py再打开一个终端配置启动nav2.launchros2 launch bot_navigation2 navigation2.launch.py然后就看到这样一个地图界面真的就是我前面保存的地图就是很飘我也没去管点击“2D Pose Estimate”估计机器人大概位置然后调整机器人前方的朝向浅紫色是全局的代价地图超紫的是局部的代价地图我这个图真的很不准大家看看就得了可以选择“Global Costmap”是否开关单点与路点导航点击“Nav2 Goal”选择一个点然后给一个该点的朝向确实走了但很乱啊还可以设置路点导航多点导航点击左下角的“Waypoint / Nav Through...”然后点击“Nav2 Goal”就能设置多个点及朝向机器人行走过程中会进行动态避障使用话题初始化机器人位姿导航后第一个操作就是初始化机器人位姿使用amcl话题计算机器人位置打开一个终端配置运行ros2 launch bot_description gazebo_sim.launch.py再打开一个终端配置运行ros2 launch bot_navigation2 navigation2.launch.py再打开一个终端配置通过“ros2 node info /amcl”命令查看节点的相关信息可以看出这个节点订阅了雷达的scan话题、map、initialpose用于接收初始化位姿向该话题发布数据就可初始化位姿了在gazebo中可以看到初始位置一般都是000发布数据map后面不给数据默认是0ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped {header: {frame_id: map}} --once打开rviz可以看到位置已经初始化完成了navigation2中有个python包将常用的导航相关操作封装成了相关类我们可以直接来调用在src下新建一个功能包ros2 pkg create bot_application --build-type ament_python --license Apache-2.0新建一个init_robot_pose.py文件from geometry_msgs.msg import PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator import rclpy def main(): rclpy.init() navigator BasicNavigator() initial_pose PoseStamped() initial_pose.header.frame_id map initial_pose.header.stamp 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 navigator.setInitialPose(initial_pose) navigator.waitUntilNav2Active() rclpy.spin(navigator) rclpy.shutdown() if __name__ __main__: main()在setup.py中添加可执行文件然后构建、配置、运行之前的两个终端新的终端运行ros2 run bot_application init_robot_pose在rviz中可以看到也是初始化位姿了使用TF获取机器人实时位置使用tf监听就能获得机器人在地图中的实时位置新建一个get_robot_pose.py文件import rclpy from rclpy.node import Node from tf2_ros import TransformListener, Buffer from tf_transformations import euler_from_quaternion class TFListener(Node): def __init__(self): super().__init__(tf2_listener) self.buffer Buffer() self.listener TransformListener(self.buffer, self) self.timer self.create_timer(1, self.get_transform) def get_transform(self): try: tf self.buffer.lookup_transform( map, base_footprint, rclpy.time.Time(seconds0), rclpy.time.Duration(seconds1)) transform tf.transform rotation_euler euler_from_quaternion([ transform.rotation.x, transform.rotation.y, transform.rotation.z, transform.rotation.w ]) self.get_logger().info( f平移:{transform.translation},旋转四元数:{transform.rotation}:旋转欧拉角:{rotation_euler}) except Exception as e: self.get_logger().warn(f不能够获取坐标变换原因: {str(e)}) def main(): rclpy.init() node TFListener() rclpy.spin(node) rclpy.shutdown()在setup.py中添加可执行文件构建配置运行前两个终端ros2 run bot_application get_robot_pose开始获取不到坐标因为没用初始化位置点击“2D Pose Estimate”初始化位置然后就有数据了给机器人一个Goal位置也会实时变换调用接口进行单点导航navigation2提供了用于导航调用的动作服务启动前两个终端并初始化位姿再打开一个终端查看当前的动作ros2 action list -t其中的/navigate_to_pose就是用于导航到点处理的服务看一下当前消息接口的内容ros2 interface show nav2_msgs/action/NavigateToPose上面是goal的内容下面是结果、反馈接下来我们可以发送一个请求设置目标位置ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose {pose: {header: {frame_id: map}, pose: {position :{x: 2.0, y: 1.0}}}} --feedback机器人真的在移动了只是我这个雷达很漂可以看一下反馈最终位置在-0.05,-0.36,0.091导航总共27秒……当然我们也可以通过代码完成相同的功能新建一个nav_to_pose.py文件from geometry_msgs.msg import PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator from rclpy.duration import Duration import rclpy def main(): rclpy.init() navigator BasicNavigator() navigator.waitUntilNav2Active() goal_pose PoseStamped() goal_pose.header.frame_id map goal_pose.header.stamp navigator.get_clock().now().to_msg() goal_pose.pose.position.x 1.0 goal_pose.pose.position.y 1.0 goal_pose.pose.orientation.w 1.0 navigator.goToPose(goal_pose) while not navigator.isTaskComplete(): feedback navigator.getFeedback() navigator.get_logger().info( f预计: {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9} s 后到达) result navigator.getResult() navigator.get_logger().info(f导航结果:{result})在setup.py中添加可执行文件再构建配置运行前两个终端新建终端我们使用了waitUntilNav2Active()函数里的(0,0,0)初始化位姿如果要指定初始化位姿还需运行init_robot_pose.py文件ros2 run bot_application nav_to_pose小车确实移动了oo我前面的目标xyz设置的是000大家可以改一下位置使用接口完成路点导航使用follow_waypoints服务进行多点导航新建一个waypoint_follower.py文件from geometry_msgs.msg import PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator import rclpy def main(): rclpy.init() navigator BasicNavigator() navigator.waitUntilNav2Active() goal_poses [] goal_pose1 PoseStamped() goal_pose1.header.frame_id map goal_pose1.header.stamp navigator.get_clock().now().to_msg() goal_pose1.pose.position.x 2.0 goal_pose1.pose.position.y 1.0 goal_pose1.pose.orientation.w 1.0 goal_poses.append(goal_pose1) goal_pose2 PoseStamped() goal_pose2.header.frame_id map goal_pose2.header.stamp navigator.get_clock().now().to_msg() goal_pose2.pose.position.x 0.0 goal_pose2.pose.position.y 1.0 goal_pose2.pose.orientation.w 1.0 goal_poses.append(goal_pose2) goal_pose3 PoseStamped() goal_pose3.header.frame_id map goal_pose3.header.stamp navigator.get_clock().now().to_msg() goal_pose3.pose.position.x 0.0 goal_pose3.pose.position.y 0.0 goal_pose3.pose.orientation.w 1.0 goal_poses.append(goal_pose3) # 调用路点导航服务 navigator.followWaypoints(goal_poses) # 判断结束及获取反馈 while not navigator.isTaskComplete(): feedback navigator.getFeedback() navigator.get_logger().info(f当前目标编号{feedback.current_waypoint}) # 最终结果判断 result navigator.getResult() navigator.get_logger().info(f导航结果:{result})在setup.py中添加可执行文件构建配置运行前两个终端新建终端运行ros2 run bot_application waypoint_follower机器人系统架构设计——实战需求巡检机器人能够在不同的目标点之间进行循环移动到达每个目标点时播放对应的语音提示到达目标点时通过摄像头拍摄实时图像并保存到本地下图来自鱼香ROS机器人B站up主编写巡检控制节点在src目录下新建一个功能包ros2 pkg create autopartol_robot --buil d-type ament_python --dependencies rclpy nav2_simple_commander --license Apache-2.0新建一个partol_node.py文件import rclpy from geometry_msgs.msg import PoseStamped, Pose from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult from rclpy.node import Node import rclpy.time from tf2_ros import TransformListener, Buffer from tf_transformations import euler_from_quaternion, quaternion_from_euler import math class PartolNode(BasicNavigator): def __init__(self, node_namepartol_node): super().__init__(node_name) # 导航相关定义 self.declare_parameter(initial_point, [0.0, 0.0, 0.0]) self.declare_parameter(target_points, [0.0, 0.0, 0.0, 1.0, 1.0, 1.57]) self.initial_point_ self.get_parameter(initial_point).value self.target_points_ self.get_parameter(target_points).value def get_pose_by_xyyaw(self, x, y, yaw): 通过 x,y,yaw 合成 PoseStamped pose PoseStamped() pose.header.frame_id map pose.pose.position.x x pose.pose.position.y y rotation_quat quaternion_from_euler(0, 0, yaw) pose.pose.orientation.x rotation_quat[0] pose.pose.orientation.y rotation_quat[1] pose.pose.orientation.z rotation_quat[2] pose.pose.orientation.w rotation_quat[3] return pose def init_robot_pose(self): 初始化机器人位姿 # 从参数获取初始化点 self.initial_point_ self.get_parameter(initial_point).value # 合成位姿并进行初始化 self.setInitialPose(self.get_pose_by_xyyaw( self.initial_point_[0], self.initial_point_[1], self.initial_point_[2])) # 等待直到导航激活 self.waitUntilNav2Active() def get_target_points(self): 通过参数值获取目标点集合 points [] self.target_points_ self.get_parameter(target_points).value for index in range(int(len(self.target_points_)/3)): x self.target_points_[index*3] y self.target_points_[index*31] yaw self.target_points_[index*32] points.append([x, y, yaw]) self.get_logger().info(f获取到目标点: {index}-({x},{y},{yaw})) return points def nav_to_pose(self, target_pose): 导航到指定位姿 result self.goToPose(target_pose) while not self.isTaskComplete(): feedback self.getFeedback() self.get_logger().info(f剩余距离: {feedback.distance_remaining}) # 最终结果判断 result self.getResult() self.get_logger().info(f导航结果{result}) def get_current_pose(self): 通过TF获取当前位姿 while rclpy.ok(): try: tf self.buffer_.lookup_transform( map, base_footprint, rclpy.time.Time(seconds0), rclpy.time.Duration(seconds1)) transform tf.transform rotation_euler euler_from_quaternion([ transform.rotation.x, transform.rotation.y, transform.rotation.z, transform.rotation.w ]) self.get_logger().info( f平移:{transform.translation},旋转四元数:{transform.rotation}:旋转欧拉角:{rotation_euler}) return transform except Exception as e: self.get_logger().warn(f不能够获取坐标变换原因: {str(e)}) def main(): rclpy.init() patrol PartolNode() patrol.init_robot_pose() while rclpy.ok(): for point in patrol.get_target_points(): x, y, yaw point[0], point[1], point[2] # 导航到目标点 target_pose patrol.get_pose_by_xyyaw(x, y, yaw) patrol.nav_to_pose(target_pose) rclpy.shutdown()在setup.py文件里添加可执行文件新建一个config文件夹新建partol_config.yaml文件用来设置参数注意确认以下这些导航点的位置不要在柱子、墙体等不可进入区域patrol_node: ros__parameters: initial_point: [0.0, 0.0, 0.0] target_points: [ 0.0,0.0,0.0, 1.0,2.0,3.14, -4.5,1.5,1.57, -8.0,-5.0,1.57, 1.0,-5.0,3.14, ]在setup.py中配置install拷贝构建配置运行前两个终端新建一个终端运行ros2 run autopartol_robot partol_node --ros-args --params-file ./src/autopartol_robot/config/partol_config.yaml ode --ros-args --params-file ./src/autopartol_robot/config/partol是在运行了