小小林熬夜学编程

这个屌丝很懒，什么也没留下！

热门标签

5、ROS编程入门

作者：小小林熬夜学编程 | 2024-06-03 06:11:19

踩

ros编程

5.1ROS开发IDE搭建

本节内容介绍ROS的IDE搭建

集成开发环境（IDE，Integrated Development Environment ）

是用于提供程序开发环境的应用程序，一般包括代码编辑器、编译器、调试器和图形用户界面等工具。使用IDE可以完成例如代码补全，定义跳转，语法检查等功能，极大的提升开发效率。

我们课程配套使用的是微软的Visual Studio Code及其提供的插件来搭建IDE环境

https://code.visualstudio.com/

https://marketplace.visualstudio.com/items?itemName=ms-iot.vscode-ros

首先安装Visual Studio Code，在软件中心下载即可

安装完成后终端运行code即可打开软件

点击file->Open Folder打开工作空间

在工作空间下会有C++和python文件，再打开这类文件时如果提示需要安装C/C++或python的插件，可以点击红框处图标，搜索插件进行安装

手动搜索并安装一个ROS的插件

在IDE当中也可以调用终端，终端的打开后默认在工作空间的主目录下。可以对工作空间进行编译等操作。

以上就是IDE的搭建和简单的使用

5.2新建一个ROS功能包

本节内容将会带领大家创建一个属于自己的功能包

创建功能包：

cd catkin_ws/src

catkin_create_pkg bingda_practices

用catkin_create_pkg命令创建功能包，后面跟的第一个参数是功能包的名字

功能包创建成功后包下有两个文件，我们用Visual Studio Code打开

可以看到这里面的内容包含有功能包名，版本号，功能包功能描述，作者联系邮箱，开源协议等等。

在Visual Studio Code中调用终端运行catkin_make对工作空间进行编译

编译通过，没有报错，功能包的创建就此完成。

此外也可以在IDE中，在src目录下创建功能包，在弹出的框中输入包名完成功能包创建、

5.3编写一个发布/订阅节点（c++）

编写发布者节点：

打开Visual Studio Code找到我们刚建立的功能包，右键选择New Folder在功能包下新建一个文件夹取名src用来存放代码，src下新建一个talker.cpp文件

代码如下

#include "ros/ros.h"

#include "std_msgs/String.h"

int main(int argc, char **argv)

{

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

ros::NodeHandle n;

ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);

ros::Rate loop_rate(10);

int count = 0;

while (ros::ok())

{

std_msgs::String msg;

std::stringstream ss;

ss<<" hello world "<<count;

msg.data = ss.str();

ROS_INFO("%s",msg.data.c_str());

chatter_pub.publish(msg);

ros::spinOnce();

loop_rate.sleep();

count++;

}

return 0;

}

代码编写完成后编辑CMakeLists.txt文件

在find_package(catkin REQUIRED)括号内加入

roscpp std_msgs

将include_directories内的两行注释取消

在文件末尾加上以下两句，设置编译规则

add_executable(talker src/talker.cpp)

target_link_libraries(talker ${catkin_LIBRARIES})

修改完成后，调用终端编译工作空间。编译完成后在工作空间的devel->lib路径下对应的功能包里就会存放有编译生成的可执行文件。

打开几个终端分别运行以下几条命令可以验证话题输出的内容、频率跟cpp文件是一样的

roscore

rosrun bingda_practices talker

rostopic echo /chatter

rostopic hz /chatter

编写订阅者节点：

在功能包下的src文件下新建一个listener.cpp文件

#include "ros/ros.h"

#include "std_msgs/String.h"

void chatterCallback(const std_msgs::String::ConstPtr& msg)

{

ROS_INFO("I heard [%s]",msg->data.c_str());

}

int main(int argc, char **argv)

{

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

ros::NodeHandle n;

ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

ros::spin();

return 0;

}

在CMakeLists.txt文件末尾添加

add_executable(listener src/listener.cpp)

target_link_libraries(listener ${catkin_LIBRARIES})

调用终端编译工作空间，订阅者节点编写完成。

5.4编写一个发布/订阅节点（py）

编写发布者节点：

打开Visual Studio Code在功能包下新建一个文件夹取名script用来存放python代码，script下新建一个talker.py文件

#!/usr/bin/env python

import rospy

from std_msgs.msg import String

def talker():

pub = rospy.Publisher("chatter",String,queue_size=10)

rospy.init_node("talker",anonymous=False)

rate =rospy.Rate(10)

while not rospy.is_shutdown():

hello_str = " hello world %s" % rospy.get_time()

rospy.loginfo(hello_str)

pub.publish(hello_str)

rate.sleep()

if __name__ == '__main__':

try:

talker()

except rospy.ROSInternalException:

pass

因为是python文件编写完成后直接运行即可

rosrun bingda_practices talker.py

如遇以下报错提示执行权限不够，参照前面章节介绍的给talker.py增加执行权限即可

接下来编写订阅者节点

在script下新建一个listener.py文件

#!/usr/bin/env python

import rospy

from std_msgs.msg import String

def callback(data):

rospy.loginfo(rospy.get_caller_id() + " I heard %s",data.data)

def listenr():

rospy.init_node("listener",anonymous=True)

rospy.Subscriber("chatter",String,callback)

rospy.spin()

if __name__ == '__main__':

listenr()

给listener.py文件增加可执行权限，执行talker.py后再执行listener.py即可订阅talker发布的消息。

在前面的章节中有提到过，ros中不同的节点可以使用不同的编程语言，当我们运行c++语言编写的talker节点时，同样可以通过python语言编写的listener节点来订阅

5.5编写launch文件

本节内容介绍怎样编写launch文件，通过launch的方式启动节点。

打开Visual Studio Code在功能包下新建一个文件夹取名launch用来存放launch文件，launch下新建一个talker.launch文件

</launch>

pkg指定的是bingda_practices功能包

type指定的是c++语言编写的可执行文件talker，也可以指定为python编写的talker.py文件

name是launch里对节点重命名为talker，也可以是其他名称

output输出方式为在屏幕输出

编写保存后即可通过launch的方法启动

launch下新建一个listener.launch文件

</launch>

编写保存后即可通过launch的方法启动listener.launch文件来订阅talker.launch发布的话题了

以上就是launch启动节点的方法，此外luanch还可以同时启动多个节点

新建一个test.launch文件，同时启动talker和listener节点

此外luanch还可以递归调用

</launch>

这个launch调用了bingda_practices/launch/下的talker.launch和listener.launch文件

5.6a自定义消息类型并编写节点发布/订阅（py）

自定义消息类型：

打开Visual Studio Code在功能包下新建一个文件夹取名msg用来存放消息文件，msg下新建一个student类型的消息，Student.msg文件。在Student.msg文件中填充以下几个变量

string first_name

string last_name

uint8 age

uint32 score

编辑CMakeLists.txt文件在find_package中加入message_generation

将以下内容取消注释并修改

将以下内容取消注释

在catkin_package中加上CATKIN_DEPENDS message_runtime

编辑package.xml文件

加入构建依赖项

<build_depend>message_runtime</build_depend>

<build_export_depend>message_runtime</build_export_depend>

保存修改，编译工作空间。

使用rosmsg show查看Student类型即可看到消息内容，与我们定义的内容完全一致

消息创建完成，接下来我们新建一个节点使用自定义的消息类型发布话题

在功能包的script下新建一个msg_pub.py文件

#!/usr/bin/env python

# license removed for brevity

import rospy

from bingda_practices.msg import Student

def msg_pub():

pub = rospy.Publisher('Student_info',Student,queue_size=10)

rospy.init_node('msg_pub',anonymous=False)

rate = rospy.Rate(10)

while not rospy.is_shutdown():

student = Student()

student.first_name = "BingDa"

student.last_name = "Robot"

student.age = 2

student.score = 100

rospy.loginfo('pub a student info')

pub.publish(student)

rate.sleep()

if __name__ == '__main__':

try:

msg_pub()

except rospy.ROSInterruptException:

pass

roscore

rosrun bingda_practices msg_pub.py

输出话题内容

接下来我们创建一个节点来订阅这个消息类型的话题

在功能包的script下新建一个msg_pub.py文件

#!/usr/bin/env python

import rospy

from bingda_practices.msg import Student

def callback(data):

rospy.loginfo("Student Name is %s %s;Age is %d ;Socer is %d.", data.first_name,data.last_name ,data.age,data.score)

def msg_sub():

rospy.init_node('msg_sub', anonymous=False)

rospy.Subscriber("student_info", Student, callback)

rospy.spin()

if __name__ == '__main__':

msg_sub()

5.6b自定义消息类型并编写节点发布/订阅（cpp）

使用c++编写一个节点使用自定义的消息类型发布话题

在功能包的src下新建一个msg_pub.cpp文件

#include "ros/ros.h"

#include "bingda_practices/Student.h"

#include <sstream>

int main(int argc, char **argv)

{

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

ros::NodeHandle n;

ros::Publisher chatter_pub = n.advertise<bingda_practices::Student>("student_info", 1000);

ros::Rate loop_rate(10);

uint8_t count = 0;

while (ros::ok())

{

bingda_practices::Student student;

student.first_name = "BingDa";

student.last_name = "Robot";

student.age = count;

student.score = 100;

chatter_pub.publish(student);

ros::spinOnce();

loop_rate.sleep();

++count;

}

return 0;

}

在这个msg_pub.cpp文件中包含了我们自定义消息的头文件bingda_practices/Student.h而Visual Studio Code中没有包含工作空间的路径，需要先包含工作空间下的头文件目录

"/home/bingda/catkin_ws/devel/include**"

编辑CMakeLists.txt文件，在文件末尾加上以下三句，设置编译规则

add_executable(msg_pub src/msg_pub.cpp)

target_link_libraries(msg_pub ${catkin_LIBRARIES})

add_dependencies(msg_pub bingda_practices_generate_messages_cpp)

完成工作空间编译后，运行节点并输出话题内容

在功能包的src下新建一个msg_sub.cpp文件

#include "ros/ros.h"

#include "bingda_practices/Student.h"

void chatterCallback(const bingda_practices::Student::ConstPtr& msg)

{

ROS_INFO("Student Name is %s %s;Age is %d ;Socer is %d.", msg->first_name.c_str(),msg->last_name.c_str() ,msg->age,msg->score);

}

int main(int argc, char **argv)

{

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

ros::NodeHandle n;

ros::Subscriber sub = n.subscribe("student_info", 1000, chatterCallback);

ros::spin();

return 0;

}

编辑CMakeLists.txt文件，在文件末尾加上以下三句，设置编译规则

add_executable(msg_sub src/msg_sub.cpp)

target_link_libraries(msg_sub ${catkin_LIBRARIES})

add_dependencies(msg_sub bingda_practices_generate_messages_cpp)

完成工作空间编译后，运行节点订阅msg_pub节点发布的话题

5.7a自定义服务类型并编写服务端/客户端（py）

自定义服务类型：

打开Visual Studio Code在功能包下新建一个文件夹取名srv用来存放服务文件，srv下新建一个AddTwoInts.srv文件。假如要实现的功能是客户端发送两个数字，服务端响应这两个数字的和。AddTwoInts.srv文件中填充以下几个变量

int64 a

int64 b

---

int64 sum

分隔符以上的部分是客户端的请求，分别是a，b两个数。分隔符以下的部分是服务端应答的ab之和sum

编辑CMakeLists.txt文件，将生成服务文件部分取消注释，并添加刚创建的服务文件

保存修改，编译工作空间后查看验证服务文件

编写一个python的节点使用这个服务，在功能包的script下新建add_two_ints_server.py文件

#!/usr/bin/env python

import rospy

from bingda_practices.srv import AddTwoInts,AddTwoIntsResponse

def handle_add_two_ints(req):

print("Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b)))

return AddTwoIntsResponse(req.a + req.b)

def add_two_ints_server():

rospy.init_node('add_two_ints_server')

s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints)

print("Ready to add two ints.")

rospy.spin()

if __name__ == "__main__":

add_two_ints_server()

保存运行，可以看到已经成功运行了服务器处在等待请求状态

我们可以尝试使用rosservice call的方法调用一下这个服务

再编写一个python的客户端节点，在功能包的script下新建add_two_ints_client.py文件

#!/usr/bin/env python

import sys

import rospy

from bingda_practices.srv import *

def add_two_ints_client(x, y):

rospy.wait_for_service('add_two_ints')

try:

add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)

resp1 = add_two_ints(x, y)

return resp1.sum

except rospy.ServiceException as e:

print("Service call failed: %s"%e)

if __name__ == "__main__":

if len(sys.argv) == 3:

x = int(sys.argv[1])

y = int(sys.argv[2])

else:

sys.exit(1)

print("Requesting %s+%s"%(x, y))

print("%s + %s = %s"%(x, y, add_two_ints_client(x, y)))

5.7a自定义服务类型并编写服务端/客户端（cpp）

本节内容介绍怎样在c++中使用自定义的服务类型实现一个服务的客户端和服务端

在功能包的src下新建一个add_two_ints_server.cpp文件

#include "ros/ros.h"

#include "bingda_practices/AddTwoInts.h"

bool add(bingda_practices::AddTwoInts::Request &req,

bingda_practices::AddTwoInts::Response &res)

{

res.sum = req.a + req.b;

ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);

ROS_INFO("sending back response: [%ld]", (long int)res.sum);

return true;

}

int main(int argc, char **argv)

{

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

ros::NodeHandle n;

ros::ServiceServer service = n.advertiseService("add_two_ints", add);

ROS_INFO("Ready to add two ints.");

ros::spin();

return 0;

}

编辑CMakeLists.txt文件，在文件末尾加上以下三句，设置编译规则

add_executable(add_two_ints_server src/add_two_ints_server.cpp)

target_link_libraries(add_two_ints_server ${catkin_LIBRARIES})

add_dependencies(add_two_ints_server bingda_practices_generate_messages_cpp)

保存编译后即可运行服务端节点并使用rosservice call的方法调用它

在功能包的src下新建一个add_two_ints_client.cpp文件

#include "ros/ros.h"

#include "bingda_practices/AddTwoInts.h"

int main(int argc, char **argv)

{

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

if (argc != 3)

{

ROS_INFO("usage: add_two_ints_client X Y");

return 1;

}

ros::NodeHandle n;

ros::ServiceClient client = n.serviceClient<bingda_practices::AddTwoInts>("add_two_ints");

bingda_practices::AddTwoInts srv;

srv.request.a = atoll(argv[1]);

srv.request.b = atoll(argv[2]);

if (client.call(srv))

{

ROS_INFO("Sum: %ld", (long int)srv.response.sum);

}

else

{

ROS_ERROR("Failed to call service add_two_ints");

return 1;

}

return 0;

}

编辑CMakeLists.txt文件，在文件末尾加上以下三句，设置编译规则

add_executable(add_two_ints_client src/add_two_ints_client.cpp)

target_link_libraries(add_two_ints_client ${catkin_LIBRARIES})

add_dependencies(add_two_ints_client bingda_practices_generate_messages_cpp)

5.8TF坐标变换编程入门

在ROS下TF变换的概念非常重要，它可以进行静态坐标转换，例如机器人本体中心到雷达中心的转换。也可以实现动态坐标转换，例如机器人中心和里程计坐标的变换。本节内容将要介绍如何实现TF的静态和动态坐标转换。

静态坐标转换：

实现静态坐标转换只需在launch文件中启动一个静态坐标转换的节点

在功能包lauch文件夹下新建一个launch文件tf_transform.launch

<node pkg="tf" type="static_transform_publisher" name="static_transform"

args="0.1 0.0 0.2 0.0 0.0 0.0 /tf1 /tf2 20">

</node>

</launch>

调用tf功能包下的static_transform_publisher节点并传入参数，这几个参数分别是x，y，z三个方向的距离变换，航向，俯仰，横滚三个角度的变换。

0.1和0.2表示x轴方向偏移0.1米，z轴方向偏移0.2米，/tf1 /tf2表示这里的参数描述的是tf2相对于tf1的变换关系。20表示发布间隔20毫秒。

运行静态坐标转换roslaunch bingda_practices tf_transform.launch

运行rqt_tf_tree工具rosrun rqt_tf_tree rqt_tf_tree

在rqt_tf_tree工具就可以看到tf1到tf2的坐标变换

打开rviz界面，添加一个TF坐标显示的选项，并在Fixed Frame处基座标选择tf1

此时就可以明显看到tf2相对于tf1在x轴和z轴的偏移

动态坐标转换：

以上就是静态坐标转换的方法，在应用中我们经常还会用到动态的坐标转换。例如机器人从起点出发后，里程计坐标相对于本体就会产生一个偏移，这个偏移是随着机器人的运动不断改变的。

在功能包的script下新建tf_transform.py文件

#!/usr/bin/env python

import rospy

import tf

import math

def tf_transform():

rospy.init_node('tf_transform', anonymous=False)

tf_broadcaster = tf.TransformBroadcaster()

rate = rospy.Rate(10) # 10hz

angle = 0.0

rospy.loginfo('Start TF Transform')

while not rospy.is_shutdown():

x = math.cos(angle)*0.3

y = math.sin(angle)*0.3

z = 0.2

quat = tf.transformations.quaternion_from_euler(0,0,angle)

current_time = rospy.Time.now()

tf_broadcaster.sendTransform((x,y,z),quat,current_time,'tf3','tf1')

tf_broadcaster.sendTransform((x,y,z),(0,0,0,1),current_time,'tf4','tf1')

angle += 0.01

rate.sleep()

if __name__ == '__main__':

tf_transform()

运行rosrun bingda_practices tf_transform.py

打开rviz界面，添加TF坐标显示，基座标选择tf1

以上就是在python中进行tf动态变换的代码，这个节点会持续发布两个坐标tf3和tf4，tf3和tf4围绕着tf1以0.3为半径做圆周运动。tf4相对于tf1只做位置变换不做角度变换。tf3在位置变换的同时做角度变换。如果想要进行静态变换只需将位置变换和角度变换的值改为恒量即可。

运行rqt_tf_tree工具rosrun rqt_tf_tree rqt_tf_tree可以看到tf1到tf3和tf4的坐标变换关系

接下来我们再用c++的代码实现，在src下新建一个tf_transform.cpp文件

#include <ros/ros.h>

#include <tf/transform_broadcaster.h>

#include <tf/transform_datatypes.h>

int main(int argc, char** argv){

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

ros::NodeHandle n;

ros::Rate r(10);

tf::TransformBroadcaster broadcaster;

float angle = 0.0;

float x = 0.0;

float y = 0.0;

float z = 0.0;

ROS_INFO("Start TF Transform");

while(n.ok()){

x = cos(angle)*0.3;

y = sin(angle)*0.3;

z = 0.2;

broadcaster.sendTransform(

tf::StampedTransform(

tf::Transform(tf::createQuaternionFromRPY(0.0,0.0,angle), tf::Vector3(x, y, z)),

ros::Time::now(),"tf1", "tf3"));

broadcaster.sendTransform(

tf::StampedTransform(

tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(x, y, z*2)),

ros::Time::now(),"tf1", "tf4"));

angle += 0.01;

r.sleep();

}

编辑package.xml文件

加入构建依赖项

<build_depend>tf</build_depend>

<build_export_depend>tf</build_export_depend>

编辑CMakeLists.txt文件在find_package中加入tf

在CMakeLists.txt文件末尾加上以下两句，设置编译规则

add_executable(tf_transform src/tf_transform.cpp)

target_link_libraries(tf_transform ${catkin_LIBRARIES})

编译工作空间后运行c++代码生成的节点

rosrun bingda_practices tf_transform

在这里同样是实现了tf3和tf4围绕着tf1以0.3为半径做圆周运动。tf4相对于tf1只做位置变换不做角度变换。tf3在位置变换的同时做角度变换。

运行rqt_tf_tree工具rosrun rqt_tf_tree rqt_tf_tree可以看到tf1到tf3和tf4的坐标变换关系

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小小林熬夜学编程/article/detail/666034