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

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.mzph.cn/bicheng/68466.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

OpenHarmony API 设计规范

OpenHarmony API 设计规范 修订记录 版本作者时间更新内容v0.1&#xff0c;试运行版OpenHarmony API SIG2022年11月初版发布 目的 API是软件实现者提供给使用者在编程界面上的定义&#xff0c;API在很大程度上体现了软件实体的能力范围。 同时&#xff0c;API定义的好坏极…

【React】新建React项目

目录 create-react-app基础运用React核心依赖React 核心思想&#xff1a;数据驱动React 采用 MVC体系package.jsonindex.html好书推荐 官方提供了快速构建React 项目的脚手架&#xff1a; create-react-app &#xff0c;目前使用它安装默认是19版本&#xff0c;我们这里降为18…

Linux手写FrameBuffer任意引脚驱动spi屏幕

一、硬件设备 开发板&#xff1a;香橙派 5Plus&#xff0c;cpu&#xff1a;RK3588&#xff0c;带有 40pin 外接引脚。 屏幕&#xff1a;SPI 协议 0.96 寸 OLED。 二、需求 主要是想给板子增加一个可视化的监视器&#xff0c;并且主页面可调。 平时跑个模型或者服务&#xff0c;…

网络安全构成要素

一、防火墙 组织机构内部的网络与互联网相连时&#xff0c;为了避免域内受到非法访问的威胁&#xff0c;往往会设置防火墙。 使用NAT&#xff08;NAPT&#xff09;的情况下&#xff0c;由于限定了可以从外部访问的地址&#xff0c;因此也能起到防火墙的作用。 二、IDS入侵检…

React Native的现状与未来:从发展到展望

每周跟踪AI热点新闻动向和震撼发展 想要探索生成式人工智能的前沿进展吗&#xff1f;订阅我们的简报&#xff0c;深入解析最新的技术突破、实际应用案例和未来的趋势。与全球数同行一同&#xff0c;从行业内部的深度分析和实用指南中受益。不要错过这个机会&#xff0c;成为AI领…

数据结构——链表(概念,类型,java实现、增删、优缺点)

我是一个计算机专业研0的学生卡蒙Camel&#x1f42b;&#x1f42b;&#x1f42b;&#xff08;刚保研&#xff09; 记录每天学习过程&#xff08;主要学习Java、python、人工智能&#xff09;&#xff0c;总结知识点&#xff08;内容来自&#xff1a;自我总结网上借鉴&#xff0…

app版本控制java后端接口版本管理

java api version 版本控制 java接口版本管理 1 自定义 AppVersionHandleMapping 自定义AppVersionHandleMapping实现RequestMappingHandlerMapping里面的方法 public class AppVersionHandleMapping extends RequestMappingHandlerMapping {Overrideprotected RequestCondit…

LRU 算法详解与 Java 的两种实现方式

LRU 算法详解与 Java 的两种实现方式 LRU 算法详解与 Java 的两种实现方式一、LRU 算法简介二、LRU 算法原理三、LRU 算法应用场景四、Java 实现 LRU 算法(一)基于 LinkedHashMap 实现(二)基于双向链表和 HashMap 实现五、总结LRU 算法详解与 Java 的两种实现方式 一、LRU…

基于 Python 的财经数据接口库:AKShare

AKShare 是基于 Python 的财经数据接口库&#xff0c;目的是实现对股票、期货、期权、基金、外汇、债券、指数、加密货币等金融产品的基本面数据、实时和历史行情数据、衍生数据从数据采集、数据清洗到数据落地的一套工具&#xff0c;主要用于学术研究目的。 安装 安装手册见…

在 macOS 上,用命令行连接 MySQL(/usr/local/mysql/bin/mysql -u root -p)

根据你提供的文件内容&#xff0c;MySQL 的安装路径是 /usr/local/mysql。要直接使用 mysql 命令&#xff0c;你需要找到 mysql 可执行文件的路径。 在 macOS 上&#xff0c;mysql 客户端通常位于 MySQL 安装目录的 bin 子目录中。因此&#xff0c;完整的路径应该是&#xff1…

【QT】: 初识 QWidget 控件 | QWidget 核心属性(API) | qrc 文件

&#x1f525; 目录 1. 控件概述 控件体系的发展阶段 2. QWidget 核心属性 2.1 核心属性概览2.2 用件可用&#xff08;Enabled&#xff09; 2.3 坐标系&#xff08;Geometry&#xff09; **实例 1: 控制按钮的位置**实例 2: 表白 程序 2.4 窗口标题&#xff08;windowTiltle&a…

通过外部链接启动 Flutter App(详细介绍及示例)

通过外部链接启动 Flutter App&#xff08;firebase_dynamic_links 和 app_links&#xff09; 详细介绍 通过外部链接启动flutter App 的使用及示例 在我们的APP中&#xff0c;经常有点击链接启动并进入APP的需求&#xff08;如果未安装跳转到应用商店&#xff09;。Android通…

git操作(Windows中GitHub)

使用git控制GitHub中的仓库版本&#xff0c;并在Windows桌面中创建与修改代码&#xff0c;与GitHub仓库进行同步。 创建自己的GitHub仓库 创建一个gen_code实验性仓库用来学习和验证git在Windows下的使用方法&#xff1a; gen_code仓库 注意&#xff0c;创建仓库时不要设置…

MySQL DCL 数据控制

文章目录 1.新建用户2.删除用户3.用户授权4.撤销用户权限5.查看用户权限6.修改用户密码7.权限类型参考文献 1.新建用户 连接到 MySQL 服务器后&#xff0c;管理员或特权用户可以使用 CREATE USER 语句创建新用户。 CREATE USER usernamehost IDENTIFIED BY password;-- 示例 …

python爬虫爬取淘宝商品比价||淘宝商品详情API接口

最近在学习北京理工大学的爬虫课程&#xff0c;其中一个实例是讲如何爬取淘宝商品信息&#xff0c;现整理如下&#xff1a; 功能描述&#xff1a;获取淘宝搜索页面的信息&#xff0c;提取其中的商品名称和价格 探讨&#xff1a;淘宝的搜索接口 翻页的处理 技术路线:requests…

【Vim Masterclass 笔记13】第 7 章:Vim 核心操作之——文本对象与宏操作 + S07L28:Vim 文本对象

文章目录 Section 7&#xff1a;Text Objects and MacrosS07L28 Text Objects1 文本对象的含义2 操作文本对象的基本语法3 操作光标所在的整个单词4 删除光标所在的整个句子5 操作光标所在的整个段落6 删除光标所在的中括号内的文本7 删除光标所在的小括号内的文本8 操作尖括号…

深度学习 Pytorch 张量的索引、分片、合并以及维度调整

张量作为有序的序列&#xff0c;也是具备数值索引的功能&#xff0c;并且基本索引方法和python原生的列表、numpy中的数组基本一致。 不同的是&#xff0c;pytorch中还定义了一种采用函数来进行索引的方式。 作为pytorch中的基本数据类型&#xff0c;张量既具备了列表、数组的基…

强推未发表!3D图!Transformer-LSTM+NSGAII工艺参数优化、工程设计优化!

目录 效果一览基本介绍程序设计参考资料 效果一览 基本介绍 1.Transformer-LSTMNSGAII多目标优化算法&#xff0c;工艺参数优化、工程设计优化&#xff01;&#xff08;Matlab完整源码和数据&#xff09; Transformer-LSTM模型的架构&#xff1a;输入层&#xff1a;多个变量作…

SpringCloud系列教程:微服务的未来(十一)服务注册、服务发现、OpenFeign快速入门

本篇博客将通过实例演示如何在 Spring Cloud 中使用 Nacos 实现服务注册与发现&#xff0c;并使用 OpenFeign 进行服务间调用。你将学到如何搭建一个完整的微服务通信框架&#xff0c;帮助你快速开发可扩展、高效的分布式系统。 目录 前言 服务注册和发现 服务注册 ​编辑 …

跨境电商使用云手机用来做什么呢?

随着跨境电商的发展&#xff0c;越来越多的卖家开始尝试使用云手机来协助他们的业务&#xff0c;这是因为云手机具有许多优势。那么&#xff0c;具体来说&#xff0c;跨境电商使用云手机可以做哪些事情呢&#xff1f; &#xff08;一&#xff09;实现多账号登录和管理 跨境电商…