开发: C++知识库 Java知识库 JavaScript Python PHP知识库人工智能区块链大数据移动开发嵌入式开发工具数据结构与算法开发测试游戏开发网络协议系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑笔记本显卡显示器固态硬盘硬盘耳机手机 iphone vivo oppo 小米华为单反装机图拉丁

-> 人工智能 -> Ros学习笔记（六）——各部分代码实现 -> 正文阅读

[人工智能]Ros学习笔记（六）——各部分代码实现

Ros学习笔记（六）:——各部分代码实现

? ???基于之前几篇博客的基础，我们这次来讲解如何实现各类型的代码，这里用C++和python演示。

文章目录

Ros学习笔记（六）:——各部分代码实现

1.publisher的编程实现

（C++部分）

? ???首先，我们先展视这次所需要用到的代码:

cd ~/catkin_ws
catkin_make
source devel/setup.bash
roscore
rosrun turtlesim turtlesim_node
rosrun learning velocity_publisher

???我们先创建对应的功能包（learning）：

catkin create pkg learning roscpp rospy std_msgs geometry_msgs turtlesim

???创建如下功能包：

在这里插入图片描述

随后在learning文件中的src下建立如下c++代码文件

代码：

/**
 * 发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist，实现海龟的绕圈运动
 */
 
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

int main(int argc, char **argv)
{
	// ROS节点初始化
	ros::init(argc, argv, "velocity_publisher");

	// 创建节点句柄
    //管理节点的api等  用于管理节点功能
	ros::NodeHandle n;

	// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10  此时发送和订阅的话题名一定要一样否则程序无法正常启用
    //队列长度不对应容易丢参数
	ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

	// 设置循环的频率  不能太快否则数据传不出去
	ros::Rate loop_rate(10);

	int count = 0;
	while (ros::ok())
	{
	    // 初始化geometry_msgs::Twist类型的消息
        //设置海龟速度和角速度
		geometry_msgs::Twist vel_msg;
		vel_msg.linear.x = 0.5;
		vel_msg.angular.z = 0.2;

	    // 发布消息
		turtle_vel_pub.publish(vel_msg);
        //相当于print
		ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
				vel_msg.linear.x, vel_msg.angular.z);

	    // 按照循环频率延时
	    loop_rate.sleep();
	}
	return 0;
}

在这里插入图片描述

? ???代码完成后，就是编译了，但是在这之前我们还要去修改learning文件中的cmake文件（编译规则）。

在图示位置加入如下代码（把ros的可执行文件和相关库做链接）：

? ???add_executable(velocity_publishersrc/velocity_publisher.cpp)

? ???target_link_libraries(velocity_publisher ${catkin_LIBRARIES})

$[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Z0Dl8PRA-1637847094815)(C:\Users\Smile\AppData\Roaming\Typora\typora-user-images\image-20211124195714140.png)]$

（注意：这里如果报错 _topic.msg找不到，则删除下图中的topic）

在这里插入图片描述

先输入：

? ???????????????????????? cd ~/catkin_ws

进入相应的文件路径下。

? ????????????????????????catkin_make
在这里插入图片描述

随后输入：

? ??????????????????source devel/setup.bash

设置环境变量。

如果觉得每次都需要配置环境变量复杂，则可以在Cmake文件里加配置。

在这里插入图片描述

（隐藏文件用快捷键Ctrl + H调出）

在这里插入图片描述

随后加入如上代码。

? ?????????????????source / home/ydp/catkin_ws /devel/setup.bash

如果再打开命令行出现错误，则文件路径出错。

随后，先运行 ????????????????? roscore

在这里插入图片描述

随后，

? ?????????????????输入: rosrun turtlesim turtlesim_node

在这里插入图片描述

调出小乌龟。

随后：

? ?????????????????rosrun learning velocity_publisher

在这里插入图片描述

（编译我们的代码文件，效果如上）

（python部分）：

? ???src下继续创建.py文件（注意：python语言不需要编译，不用去改Cmake文件）

代码如下：

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

import rospy
from geometry_msgs.msg import Twist

def velocity_publisher():
	# ROS节点初始化
    rospy.init_node('velocity_publisher', anonymous=True)

	# 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
    turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)

	#设置循环的频率
    rate = rospy.Rate(10) 

    while not rospy.is_shutdown():
		# 初始化geometry_msgs::Twist类型的消息
        vel_msg = Twist()
        vel_msg.linear.x = 0.5
        vel_msg.angular.z = 0.2

		# 发布消息
        turtle_vel_pub.publish(vel_msg)
    	rospy.loginfo("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
				vel_msg.linear.x, vel_msg.angular.z)

		# 按照循环频率延时
        rate.sleep()

if __name__ == '__main__':
    try:
        velocity_publisher()
    except rospy.ROSInterruptException:
        pass

随后还是启动：

? ??????????????????????????????roscore

? ????????????????????????rosrun turtlesim turtlesim_node

调用小乌龟，随后：

? ???????????????????输入：rosrun learning velocity_publisher

（注意：这里不是按回车，是按两下Tab键）
在这里插入图片描述

再输入： ?????????rosrun learning velocity_publisher .py

在这里插入图片描述

以上就是Publisher全部部分。

2.Subscriber实现

（c++部分）：

代码如下：

#include <ros/ros.h>

#include "turtlesim/Pose.h"

// 接收到订阅的消息后，会进入消息回调函数

void poseCallback(const turtlesim::Pose::ConstPtr& msg)

{

    // 将接收到的消息打印出来

    ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);

}



int main(int argc, char **argv)

{

    // 初始化ROS节点

    ros::init(argc, argv, "pose_subscriber");



    // 创建节点句柄

    ros::NodeHandle n;



    // 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback

    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);



    // 循环等待回调函数

    ros::spin();



    return 0;

}

? ????因为Subscriber不知道什么时候会有Publisher的消息传入进来，所以需要给一个回调函数 poseCallback（），让消息一进来就可以反应。

? ????同Publisher一样，我们要到Cmake文件中配置对应的C++编译规则。

输入：

? ????add_executable(pose_subscriber src/pose_subscriber.cpp)

? ????target_link_libraries(pose_subscriber ${catkin_LIBRARIES})

如图：
在这里插入图片描述

重新编译： ????????????catkin_make

在这里插入图片描述

调用小乌龟：

? ????????????????????????rosrun turtlesim turtle (加两个tap键)

? ????????????????????????rosrun turtlesim turtlesim_node

在这里插入图片描述

调用函数：

? ????????????????????????rosrun learning pos_subscriber

在这里插入图片描述

此时输入：

? ?????????????????rosrun turtlesim turtle_teleop_key 就可以移动小海龟了

此时我们Subcriber函数调出的命令行信息也会不断更新。

（Python实现）：

代码

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose

import rospy
from turtlesim.msg import Pose

def poseCallback(msg):
    rospy.loginfo("Turtle pose: x:%0.6f, y:%0.6f", msg.x, msg.y)

def pose_subscriber():
	# ROS节点初始化
    rospy.init_node('pose_subscriber', anonymous=True)

	# 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    rospy.Subscriber("/turtle1/pose", Pose, poseCallback)

	# 循环等待回调函数
    rospy.spin()

if __name__ == '__main__':
    pose_subscriber()

还是跟上述一样，将小海龟调出后，输入

? ????????????????????????rosrun learning pose_subscriber .py

效果如下：

在这里插入图片描述

3.话题消息的定义与使用

首先输入： ?????????????????torch person.msg

? ????????????????????????创建person.msg文件。

文件中写入如下内容：

string nane
uint8 sex
uint8 age

uint8 unknown = 0
uint8 male = 1
uint8 fenale = 2

可在命令行输入torch person.msg创建（在learning中创建一个msg文件，再进入文件夹中输入此命令）
在这里插入图片描述

在package包内添加如下(动态、运行)依赖：

? ?????????????????<build_depend>message_generation</build_depend>

? ?????????????????<exec_depend>message_runtime</exec_depend>

在这里插入图片描述

打开Cmake文件：

加入如下：

? ?????????????????message_generation
在这里插入图片描述

随后加：

? ?????????????????add_message_files(FILES person.msg)

? ?????????????????generate_messages(DEPENDENCIES std_msgs)

再加：

? ?????????????????message_runtime

在这里插入图片描述

随后重新 catkin_make编译一次。

（如果如上操作有报错，先检查括号和字母大小写有无错误）

查看生成的头文件：
在这里插入图片描述

创建代码文件：

（注意：本来应该是name被我打成nane了，本人比较懒就没改，大家注意对应的位置改改）

person_subscriber.cpp:

/***********************************************************************

Copyright 2020 GuYueHome (www.guyuehome.com).

***********************************************************************/



/**

 * 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person

 */

 

#include <ros/ros.h>

#include "learning/person.h"



// 接收到订阅的消息后，会进入消息回调函数

void personInfoCallback(const learning::person::ConstPtr& msg)

{

    // 将接收到的消息打印出来

    ROS_INFO("Subcribe person Info: name:%s  age:%d  sex:%d", 

			 msg->nane.c_str(), msg->age, msg->sex);

}



int main(int argc, char **argv)

{

    // 初始化ROS节点

    ros::init(argc, argv, "person_subscriber");



    // 创建节点句柄

    ros::NodeHandle n;



    // 创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personInfoCallback

    ros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback);



    // 循环等待回调函数

    ros::spin();



    return 0;

}

person_publisher.cpp:

/***********************************************************************

Copyright 2020 GuYueHome (www.guyuehome.com).

***********************************************************************/



/**

 * 该例程将发布/person_info话题，自定义消息类型learning_topic::Person

 */

 

#include <ros/ros.h>

#include "learning/person.h"



int main(int argc, char **argv)

{

    // ROS节点初始化

    ros::init(argc, argv, "person_publisher");



    // 创建节点句柄

    ros::NodeHandle n;



    // 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10

    ros::Publisher person_info_pub = n.advertise<learning::person>("/person_info", 10);



    // 设置循环的频率

    ros::Rate loop_rate(1);



    int count = 0;

    while (ros::ok())

    {

        // 初始化learning_topic::Person类型的消息

    	learning::person person_msg;

		person_msg.nane = "Tom";

		person_msg.age  = 18;

		person_msg.sex  = learning::person::male;



        // 发布消息

		person_info_pub.publish(person_msg);



       	ROS_INFO("Publish Person Info: name:%s  age:%d  sex:%d", 

				  person_msg.nane.c_str(), person_msg.age, person_msg.sex);



        // 按照循环频率延时

        loop_rate.sleep();

    }



    return 0;

}

Cmake文件加入如下代码：

add_executable(person_publisher src/person_publisher.cpp)

target_link_libraries(person_publisher ${catkin_LIBRARIES})

add_dependencies(person_publisher${PROJECT_NAME}_generate_messages_cpp)

add_executable(person_subscriber src/person_subscriber.cpp)

target_link_libraries(person_subscriber ${catkin_LIBRARIES})

add_dependencies(person_subscriber${PROJECT_NAME}_generate_messages_cpp)

在这里插入图片描述

因为我们的数据是动态生成的所以多出那就add语句让动态软件与文件关联。

首先老套入输入： ????????????????? roscore

随后输入： ????????????????? rosrun learning person_subcriber

? ??????????????????????????? rosrun learning person_publisher
在这里插入图片描述

就可建立两者的连接。

（注意：此时就算关掉roscore二者还是会继续传输，因为节点管理器只负责配对，不负责连接）

（python实现）：

person_publisher:

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将发布/person_info话题，自定义消息类型learning_topic::Person

import rospy
from learning.msg import person

def velocity_publisher():
	# ROS节点初始化
    rospy.init_node('person_publisher', anonymous=True)

	# 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10
    person_info_pub = rospy.Publisher('/person_info', person, queue_size=10)

	#设置循环的频率
    rate = rospy.Rate(10) 

    while not rospy.is_shutdown():
		# 初始化learning_topic::Person类型的消息
    	person_msg = person()
    	person_msg.nane = "Tom";
    	person_msg.age  = 18;
    	person_msg.sex  = person.male;

		# 发布消息
    	person_info_pub.publish(person_msg)
    	rospy.loginfo("Publsh person message[%s, %d, %d]", 
				person_msg.nane, person_msg.age, person_msg.sex)

		# 按照循环频率延时
    	rate.sleep()

if __name__ == '__main__':
    try:
    	velocity_publisher()
    except rospy.ROSInterruptException:
    	pass

person_subscriber:

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person

import rospy
from learning.msg import person

def personInfoCallback(msg):
    rospy.loginfo("Subcribe Person Info: name:%s  age:%d  sex:%d", 
			 msg.name, msg.age, msg.sex)

def person_subscriber():
	# ROS节点初始化
    rospy.init_node('person_subscriber', anonymous=True)

	# 创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personInfoCallback
    rospy.Subscriber("/person_info", person, personInfoCallback)

	# 循环等待回调函数
    rospy.spin()

if __name__ == '__main__':
    person_subscriber()

随后输入：

? ??????????????????????????????????roscore

? ?????????????????rosrun learning person_publisher.py

? ?????????????????rosrun learning person_subscreber.py

在这里插入图片描述

4.客户端Client的编程实现

(C++实现)；

首先，为了分别区分，我们来创建一个新的功能包：

输入：

? ?????????????????cd catkin_ws/src（进入路径）

? catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim（安装功能包）

在这里插入图片描述

随后创建文件：

turtle_spawn.cpp

/***********************************************************************
Copyright 2020 GuYueHome (www.guyuehome.com).
***********************************************************************/

/**
 * 该例程将请求/spawn服务，服务数据类型turtlesim::Spawn
 */

#include <ros/ros.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
    // 初始化ROS节点
	ros::init(argc, argv, "turtle_spawn");

    // 创建节点句柄
	ros::NodeHandle node;

    // 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
	ros::service::waitForService("/spawn");
	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");

    // 初始化turtlesim::Spawn的请求数据
	turtlesim::Spawn srv;
	srv.request.x = 2.0;
	srv.request.y = 2.0;
	srv.request.name = "turtle2";

    // 请求服务调用
	ROS_INFO("Call service to spwan turtle[x:%0.6f, y:%0.6f, name:%s]", 
			 srv.request.x, srv.request.y, srv.request.name.c_str());

	add_turtle.call(srv);

	// 显示服务调用结果
	ROS_INFO("Spwan turtle successfully [name:%s]", srv.response.name.c_str());

	return 0;
};

随后同样的配置Cmake

? ?????????????????add_executable(turtle_spawn src/turtle_spawn.cpp)

? ?????????????????target_link_libraries(turtle_spawn ${catkin_LIBRARIES})

在这里插入图片描述

随后：

? ?????????????????日常 carkin_make

? ????????????????? roscore

开启小乌龟： rosrun turtlesim turtlesim_node

? ????输入： rosrun learning_service turtle_spawn

在这里插入图片描述

其中，第一个INFO是发送请求service

? ?????第二个INFO是发送回复

（python实现）：

在这里插入图片描述

（别忘了。）

代码：

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将请求/spawn服务，服务数据类型turtlesim::Spawn

import sys
import rospy
from turtlesim.srv import Spawn

def turtle_spawn():
	# ROS节点初始化
    rospy.init_node('turtle_spawn')

	# 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
    rospy.wait_for_service('/spawn')
    try:
        add_turtle = rospy.ServiceProxy('/spawn', Spawn)

		# 请求服务调用，输入请求数据
        response = add_turtle(2.0, 2.0, 0.0, "turtle2")
        return response.name
    except rospy.ServiceException as e:
        print ("Service call failed: %s"%e)

if __name__ == "__main__":
	#服务调用并显示调用结果
    print ("Spwan turtle successfully [name:%s]" %(turtle_spawn()))