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').value
        self.initial_points_= self.get_parameter('target_points').value

    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 = x
        pose.pose.position.y = y
        quat = 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 pose
    def init_robot_pose(self):
        #初始化机器人位姿
        self.initial_point_ = self.get_parameter('initial_point').value
        init_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').value
        for 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.transform
                self.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,