ros2-7.5 做一个自动巡检机器人

news/2025/2/14 15:26:00/

7.5.1 需求及设计

又到了小鱼老师带着做最佳实践项目了。需求:做一个在各个房间不断巡逻并记录图像的机器人

到达目标点后首先通过语音播放到达目标点信息,

再通过摄像头拍摄一张图片保存到本地。

7.5.2 编写巡检控制节点

在chapt7_ws/src下新建功能包,添加rclpy和nav2_simple_commander依赖。

目录src/autopartol_robot/autopartol_robot/新建文件patrol_node.py

from geometry_msgs.msg import PoseStamped,Pose
from nav2_simple_commander.robot_navigator import BasicNavigator,TaskResult
import rclpy
from rclpy.node import Node
import rclpy.time
from tf2_ros import TransformListener, Buffer #坐标监听器
from tf_transformations import quaternion_from_euler,euler_from_quaternion# 欧拉角转换class PatrolNode(BasicNavigator):def __init__(self,node_name='patrol_node'):super().__init__(node_name)self.buffer_ = Buffer()self.listner_ = TransformListener(self.buffer_,self)#声明相关参数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').valueself.initial_points_= self.get_parameter('target_points').valuedef get_pose_by_xyyaw(self, x, y, yaw):# 通过x,y,yaw return PoseStamped对象     pose = PoseStamped()pose.header.frame_id = 'map'pose.pose.position.x = xpose.pose.position.y = yquat = quaternion_from_euler(0,0,yaw)pose.pose.orientation.x =quat[0]pose.pose.orientation.y =quat[1]pose.pose.orientation.z =quat[2]pose.pose.orientation.w =quat[3]return posedef init_robot_pose(self):#初始化机器人位姿self.initial_point_ = self.get_parameter('initial_point').valueinit_pose = self.get_pose_by_xyyaw(self.initial_point_[0],self.initial_point_[1],self.initial_point_[2]) self.setInitialPose(init_pose)self.waitUntilNav2Active()#等待导航可用def get_target_points(self):#通过参数值获取目标点集合points = []self.target_points_ = self.get_parameter('target_points').valuefor index in range(int(len(self.target_points_)/3)):x = self.target_points_[index*3]y = self.target_points_[index*3+1]yaw = self.target_points_[index*3+2]points.append([x,y,yaw])self.get_logger().info(f'获取到目标点:{index}->{x},{y},{yaw}')return points    def nav_to_pose(self,target_pose):#导航到指定位姿self.waitUntilNav2Active()self.goToPose(target_pose)while not self.isTaskComplete():feedback = self.getFeedback()self.get_logger().info(f'剩余距离:{feedback.distance_remaining }')reslut = self.getResult()self.get_logger().info(f'导航结果:{reslut}')    def get_cuurent_pose(self):#通过TF获取当前位姿while rclpy.ok():try:result = self.buffer_.lookup_transform('map','base_footprint',rclpy.time.Time(seconds=0),rclpy.time.Duration(seconds=1))transform = result.transformself.get_logger().info(f'平移:{transform.translation}')# self.get_logger().info(f'旋转:{transform.rotation}')# ratation_euler = euler_from_quaternion([#     transform.rotation.x,#     transform.rotation.y,#     transform.rotation.z,#     transform.rotation.w]# )# self.get_logger().info(f'平移rpy:{ratation_euler}')return transformexcept Exception as e:self.get_logger().warn(f'获取坐标转换异常{str(e)}')def main():rclpy.init()patrol = PatrolNode() #节点patrol.initial_pose()while rclpy.ok():points = patrol.get_target_points()for point in 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()        

基本上吧之前的单接口调用综合起来。

为方便参数配置,在src/autopartol_robot/新建目录config,新增配置文件

patrol_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,]

注意目标点的选取是机器人可达的位置,不能选到障碍物。

启动gazebo模拟器,启动nav2.

再启动patrol_node

 ros2 run autopartol_robot patrol_node --ros-args --params-file /home/bohu/chapt7/chapt7_ws/install/autopartol_robot/share/autopartol_robot/config/patrol_config.yaml

会发现等一会开始初始化地图后,开始沿着设定目标点运动。效果如下

也有异常情况,机器人靠墙太近,gazebo看出距离墙还有段距离,但是在rviz里面局部代价地图有一点变形,导致机器以为有障碍物卡住的现象。

7.5.3 添加语音播报功能

在chatpt7_ws/src下新建功能包autopatrol_interfaces,功能包下新建目录srv,srv新新建接口文件

speachText.srv

string text    # 合成文字
---
bool result    # 合成结果

在CMakeLists.txt注册,并在package.xml添加标签

<member_of_group>rosidl_interface_packages</member_of_group>

重新构建

src/autopartol_robot/autopartol_robot/目录下新建文件speaker.py

import rclpy
from rclpy.node import Node
from autopatrol_interfaces.srv import SpeachText
import espeakngclass Speaker(Node):def __init__(self):super().__init__('speaker')self.speech_service_ = self.create_service(SpeachText,'speech_text',self.speech_text_callback)self.speaker_ = espeakng.Speaker()self.speaker_.voice ='zh'def speech_text_callback(self,request,response):self.get_logger().info(f'正在朗读 {request.text}')self.speaker_.say(request.text)self.speaker_.wait()response.result = Truereturn responsedef main():rclpy.init()node = Speaker()rclpy.spin(node)rclpy.shutdown()

修改patrol_node.py,增加语音合成调用

from geometry_msgs.msg import PoseStamped,Pose
from nav2_simple_commander.robot_navigator import BasicNavigator,TaskResult
import rclpy
from rclpy.node import Node
import rclpy.time
from tf2_ros import TransformListener, Buffer #坐标监听器
from tf_transformations import quaternion_from_euler,euler_from_quaternion# 欧拉角转换
from autopatrol_interfaces.srv import SpeachTextclass PatrolNode(BasicNavigator):def __init__(self,node_name='patrol_node'):super().__init__(node_name)self.buffer_ = Buffer()self.listner_ = TransformListener(self.buffer_,self)#声明相关参数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').valueself.initial_points_= self.get_parameter('target_points').valueself.speech_client_ = self.create_client(SpeachText,'speech_text')def get_pose_by_xyyaw(self, x, y, yaw):# 通过x,y,yaw return PoseStamped对象     pose = PoseStamped()pose.header.frame_id = 'map'pose.pose.position.x = xpose.pose.position.y = yquat = quaternion_from_euler(0,0,yaw)pose.pose.orientation.x =quat[0]pose.pose.orientation.y =quat[1]pose.pose.orientation.z =quat[2]pose.pose.orientation.w =quat[3]return posedef init_robot_pose(self):#初始化机器人位姿self.initial_point_ = self.get_parameter('initial_point').valueinit_pose = self.get_pose_by_xyyaw(self.initial_point_[0],self.initial_point_[1],self.initial_point_[2]) self.setInitialPose(init_pose)self.waitUntilNav2Active()#等待导航可用def get_target_points(self):#通过参数值获取目标点集合points = []self.target_points_ = self.get_parameter('target_points').valuefor index in range(int(len(self.target_points_)/3)):x = self.target_points_[index*3]y = self.target_points_[index*3+1]yaw = self.target_points_[index*3+2]points.append([x,y,yaw])self.get_logger().info(f'获取到目标点:{index}->{x},{y},{yaw}')return points    def nav_to_pose(self,target_pose):#导航到指定位姿self.waitUntilNav2Active()self.goToPose(target_pose)while not self.isTaskComplete():feedback = self.getFeedback()self.get_logger().info(f'剩余距离:{feedback.distance_remaining }')reslut = self.getResult()self.get_logger().info(f'导航结果:{reslut}')    def get_cuurent_pose(self):#通过TF获取当前位姿while rclpy.ok():try:result = self.buffer_.lookup_transform('map','base_footprint',rclpy.time.Time(seconds=0),rclpy.time.Duration(seconds=1))transform = result.transformself.get_logger().info(f'平移:{transform.translation}')# self.get_logger().info(f'旋转:{transform.rotation}')# ratation_euler = euler_from_quaternion([#     transform.rotation.x,#     transform.rotation.y,#     transform.rotation.z,#     transform.rotation.w]# )# self.get_logger().info(f'平移rpy:{ratation_euler}')return transformexcept Exception as e:self.get_logger().warn(f'获取坐标转换异常{str(e)}')def speech_text(self,text):#调用服务合成语音while not self.speech_client_.wait_for_service(timeout_sec=1):self.get_logger().info("wait for speech service")request = SpeachText.Request()request.text = textfuture = self.speech_client_.call_async(request)rclpy.spin_until_future_complete(self,future)if future.result() is not None:result = future.result().resultif result:self.get_logger().info(f'语音合成成功:{text}')else:self.get_logger().warn(f'语音合成失败:{text}')else:self.get_logger().warn('语音合成服务请求失败')def main():rclpy.init()patrol = PatrolNode() #节点patrol.speech_text('正在准备初始化位置')patrol.init_robot_pose()patrol.speech_text('初始化位置完成')while rclpy.ok():points = patrol.get_target_points()for point in points:x,y,yaw = point[0],point[1],point[2]target_pose = patrol.get_pose_by_xyyaw(x,y,yaw)patrol.speech_text(text=f'准备前往目标点{x},{y}')patrol.nav_to_pose(target_pose)rclpy.shutdown()        

效果跟上面类似,日志输出多了语音的

[speaker-2] [INFO] [1737017187.818829250] [speaker]: 正在朗读 准备前往目标点1.0,2.0
[patrol_node-1] [INFO] [1737017191.194245817] [patrol_node]: 语音合成成功:准备前往目标点1.0,2.0
[patrol_node-1] [INFO] [1737017195.311579068] [patrol_node]: Nav2 is ready for use!
[patrol_node-1] [INFO] [1737017195.314096555] [patrol_node]: Navigating to goal: 1.0 2.0...
[patrol_node-1] [INFO] [1737017195.509438991] [patrol_node]: 剩余距离:0.2445448786020279
[patrol_node-1] [INFO] [1737017195.612344544] [patrol_node]: 剩余距离:2.230710744857788
[patrol_node-1] [INFO] [1737017195.716231929] [patrol_node]: 剩余距离:2.230710744857788
[patrol_node-1] [INFO] [1737017195.819218225] [patrol_node]: 剩余距离:2.230710744857788
[patrol_node-1] [INFO] [1737017195.923079415] [patrol_node]: 剩余距离:2.230710744857788 

7.5.4订阅图像并记录

from geometry_msgs.msg import PoseStamped,Pose
from nav2_simple_commander.robot_navigator import BasicNavigator,TaskResult
import rclpy
from rclpy.node import Node
import rclpy.time
from tf2_ros import TransformListener, Buffer #坐标监听器
from tf_transformations import quaternion_from_euler,euler_from_quaternion# 欧拉角转换
from autopatrol_interfaces.srv import SpeachText
from sensor_msgs.msg import Image #消息接口
from cv_bridge import CvBridge #图像转换
import cv2 #保存图像class PatrolNode(BasicNavigator):def __init__(self,node_name='patrol_node'):super().__init__(node_name)self.buffer_ = Buffer()self.listner_ = TransformListener(self.buffer_,self)#声明相关参数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').valueself.initial_points_= self.get_parameter('target_points').valueself.speech_client_ = self.create_client(SpeachText,'speech_text')# 订阅与保存图像相关定义self.declare_parameter('image_save_path', '')self.image_save_path = self.get_parameter('image_save_path').valueself.bridge = CvBridge()self.latest_img_ = Noneself.image_sub_ = self.create_subscription(Image,'/camera_sensor/image_raw',self.img_callback,1)def img_callback(self,msg):self.latest_img_ = msgdef record_img(self):if self.latest_img_ is not Node:pose = self.get_cuurent_pose()cv_image = self.bridge.imgmsg_to_cv2(self.latest_img_)cv2.imwrite(f'{self.image_save_path}img_{pose.translation.x:3.2f}_{pose.translation.y:3.2f}.png',cv_image)   def get_pose_by_xyyaw(self, x, y, yaw):# 通过x,y,yaw return PoseStamped对象     pose = PoseStamped()pose.header.frame_id = 'map'pose.pose.position.x = xpose.pose.position.y = yquat = quaternion_from_euler(0,0,yaw)pose.pose.orientation.x =quat[0]pose.pose.orientation.y =quat[1]pose.pose.orientation.z =quat[2]pose.pose.orientation.w =quat[3]return posedef init_robot_pose(self):#初始化机器人位姿self.initial_point_ = self.get_parameter('initial_point').valueinit_pose = self.get_pose_by_xyyaw(self.initial_point_[0],self.initial_point_[1],self.initial_point_[2]) self.setInitialPose(init_pose)self.waitUntilNav2Active()#等待导航可用def get_target_points(self):#通过参数值获取目标点集合points = []self.target_points_ = self.get_parameter('target_points').valuefor index in range(int(len(self.target_points_)/3)):x = self.target_points_[index*3]y = self.target_points_[index*3+1]yaw = self.target_points_[index*3+2]points.append([x,y,yaw])self.get_logger().info(f'获取到目标点:{index}->{x},{y},{yaw}')return points    def nav_to_pose(self,target_pose):#导航到指定位姿self.waitUntilNav2Active()self.goToPose(target_pose)while not self.isTaskComplete():feedback = self.getFeedback()self.get_logger().info(f'剩余距离:{feedback.distance_remaining }')reslut = self.getResult()self.get_logger().info(f'导航结果:{reslut}')    def get_cuurent_pose(self):#通过TF获取当前位姿while rclpy.ok():try:result = self.buffer_.lookup_transform('map','base_footprint',rclpy.time.Time(seconds=0),rclpy.time.Duration(seconds=1))transform = result.transformself.get_logger().info(f'平移:{transform.translation}')# self.get_logger().info(f'旋转:{transform.rotation}')# ratation_euler = euler_from_quaternion([#     transform.rotation.x,#     transform.rotation.y,#     transform.rotation.z,#     transform.rotation.w]# )# self.get_logger().info(f'平移rpy:{ratation_euler}')return transformexcept Exception as e:self.get_logger().warn(f'获取坐标转换异常{str(e)}')def speech_text(self,text):#调用服务合成语音while not self.speech_client_.wait_for_service(timeout_sec=1):self.get_logger().info("wait for speech service")request = SpeachText.Request()request.text = textfuture = self.speech_client_.call_async(request)rclpy.spin_until_future_complete(self,future)if future.result() is not None:result = future.result().resultif result:self.get_logger().info(f'语音合成成功:{text}')else:self.get_logger().warn(f'语音合成失败:{text}')else:self.get_logger().warn('语音合成服务请求失败')def main():rclpy.init()patrol = PatrolNode() #节点patrol.speech_text('正在准备初始化位置')patrol.init_robot_pose()patrol.speech_text('初始化位置完成')while rclpy.ok():points = patrol.get_target_points()for point in points:x,y,yaw = point[0],point[1],point[2]target_pose = patrol.get_pose_by_xyyaw(x,y,yaw)patrol.speech_text(text=f'准备前往目标点{x},{y}')patrol.nav_to_pose(target_pose)patrol.speech_text(text=f'已到达目标点{x},{y},准备记录图像')patrol.record_img()patrol.speech_text(text=f'图像记录完成')rclpy.shutdown()        

重新构建后运行,拍照效果如下


http://www.ppmy.cn/news/1564920.html

相关文章

【解锁新技能!Flux.1如何实现远程AI生成图像随时随地创作】

&#x1f49d;&#x1f49d;&#x1f49d;欢迎来到我的博客&#xff0c;很高兴能够在这里和您见面&#xff01;希望您在这里可以感受到一份轻松愉快的氛围&#xff0c;不仅可以获得有趣的内容和知识&#xff0c;也可以畅所欲言、分享您的想法和见解。 推荐:kwan 的首页,持续学…

20250120面试鸭特训营第28天

更多特训营笔记详见个人主页【面试鸭特训营】专栏 250120 1. 说说 Java 中 HashMap 的原理&#xff1f; HashMap 的底层结构 HashMap 底层由 node 数组、单链表、红黑树构成。根据哈希函数计算得到哈希值&#xff0c;哈希值确定了元素保存在 node 数组中的具体下标。HashMap…

Ghost硬盘对拷教程分享

Ghost32是一款老古董级别的备份和恢复软件&#xff0c;通常用于创建系统镜像以及恢复系统。它最初由Symantec公司开发&#xff0c;用于在计算机系统上进行备份、克隆和恢复操作。 由于这个软件早已停止更新很多年了&#xff0c;并且也是全英文的用户界面&#xff0c;对国内用户…

山西省乡镇界面图层shp格式arcgis数据乡镇名称和编码2020年wgs84坐标无偏移测评

这篇文档将深入解析标题和描述中提及的IT知识点&#xff0c;主要关注地理信息系统&#xff08;GIS&#xff09;和ArcGIS软件的应用&#xff0c;以及shp文件格式的相关知识。 我们要理解"山西省乡镇界面图层shp格式arcgis数据乡镇名称和编码2020年wgs84坐标无偏移.zip&quo…

[HDCTF2019]Maze

[HDCTF2019]Maze 一、查壳 有壳upx&#xff0c;32位&#xff0c;解壳 二、IDA分析 刚一点进去就发现花指令&#xff0c;不出意外无法F5. 花指令&#xff0c;其特征就是在jnz后面call里面出现非法指令。&#xff08;会爆红&#xff09; 所以我们现在要解决花指令 三、解决…

c# PDF文件合并工具

界面 主要用于发票PDF文件的合并。经常出差要报销的很有用。 代码 using System; using System.Collections.Generic; using System.ComponentModel; using System.Data; using System.Diagnostics; using System.Drawing; using System.IO; using System.Linq; using System…

Python毕业设计选题:基于django+vue的智能租房系统的设计与实现

开发语言&#xff1a;Python框架&#xff1a;djangoPython版本&#xff1a;python3.7.7数据库&#xff1a;mysql 5.7数据库工具&#xff1a;Navicat11开发软件&#xff1a;PyCharm 系统展示 租客注册 添加租客界面 租客管理 房屋类型管理 房屋信息管理 系统管理 摘要 本文首…

springboot基于安卓的智启教育服务平台app

基于Spring Boot的智启教育服务平台App是一个结合了Spring Boot后端框架与安卓前端技术的综合性教育服务平台。 一、技术背景与架构 1.开发语言&#xff1a;后端采用Java语言开发&#xff0c;充分利用Java的跨平台性、面向对象特性和强大的后端处理能力。前端则使用安卓开发技…