话题订阅--输出乌龟位位姿

原创已于 2022-03-10 18:10:33 修改 · 750 阅读

3 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶 #人工智能 #机器学习

于 2022-03-10 18:09:52 首次发布

本文介绍了如何在ROS环境中使用launch文件启动乌龟GUI，并通过键盘控制节点。核心内容包括创建订阅节点，以C++和Python实现，监听/turtle1/pose消息以获取乌龟的实时位置信息。

<!--启动乌龟GUI与键盘控制节点-->
<launch>
    <!--乌龟GUI-->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <!--键盘控制-->
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />

</launch>

运行launch文件

在这里插入图片描述
获取信息

在这里插入图片描述
获取乌龟实时位置（乌龟运动时数字变化）

rostopic echo /turtle1/pose

二、实现订阅节点

创建功能包需要依赖的功能包: roscpp rospy std_msgs turtlesim

1. C++:

#include  "ros/ros.h"
#include  "turtlesim/Pose.h"

/*
    需求：订阅乌龟的位姿信息并输出到控制台
        1.包含头文件
        2.初始化ROS节点
        3.创建节点句柄
        4.创建订阅对象
        5.处理订阅到的数据（回调函数）
        6.spin()
*/

//处理订阅到的数据
void doPose(const turtlesim::Pose::ConstPtr &pose){
    ROS_INFO("乌龟的位姿信息:坐标(%.2f,%.2f),朝向(%.2f,线速度(%.2f),角速度(%.2f)",
            pose->x,pose->y,pose->theta,pose->linear_velocity,pose->angular_velocity);
}

int main(int argc, char *argv[])
{
        setlocale(LC_ALL,"");
        // 2.初始化ROS节点
        ros::init(argc,argv,"sub_pose");
        // 3.创建节点句柄
        ros::NodeHandle nh;
        // 4.创建订阅对象
        ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);
        // 5.处理订阅到的数据（回调函数）
        // 6.spin()
        ros::spin();

    return 0;
}

运行launch文件（海龟已经被封装）

roslaunch plumbing_test start_turtle.launch

运行pose文件：

rosrun plumbing_test test02_sub_pose

在这里插入图片描述

2. Python:

#! /usr/bin/env python
import rospy
from turtlesim.msg import Pose

"""
    需求:订阅并输出乌龟位姿信息

    1.导包
    2.初始化ROS节点
    3.创建订阅对象
    4.使用回调函数处理订阅到的信息
    5.spin()
"""

def doPose(pose):
    rospy.loginfo("P->乌龟位姿信息:坐标(%.2f,%.2f),朝向:%.2f,线速度:%.2f,角速度:%.2f",
                    pose.x,pose.y,pose.theta,pose.linear_velocity,pose.angular_velocity)

if __name__ == "__main__":
    # 2.初始化ROS节点
    rospy.init_node("sub_pose_p")
    # 3.创建订阅对象
    sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=100)
    # 4.使用回调函数处理订阅到的信息
    # 5.spin()
    rospy.spin()
    pass

运行launch文件（海龟已经被封装）

roslaunch plumbing_test start_turtle.launch

运行pose文件：

rosrun plumbing_test test02_sub_pose_p.py

在这里插入图片描述