代码收藏家技术教程 2024-08-13

ROS（Robot Operating System）Python实现总结

ROS总结（python）

推荐使用ubuntu20.04 安装运行ROS1 noetic 更加稳定

修改python代码可执行权限chmod +x your_script.py

鱼香ROS一键安装：wget ``http://fishros.com/install`` -O fishros && . fishros

初始化工作区

mkdir catkin_ws
cd ./catkin_ws
mkdir src
catkin_init_workspace

进行编译

//根目录下
catkin_make  //产生build,dev
catkin_make install//产生install

创建功能包

//src目录下
catkin_create_pkg <pkg name> [depend1][depend2][depend3]
//catkin_create_pkg test_pkg roscpp rospy ros std_msgs

src/pkg/src功能包的源代码 src/pkg/include头文件

//根目录
catkin_make//编译
source /catkin_ws/devel/setup.bash//设置功能包的环境变量

或者直接将source /catkin_ws/devel/setup.bash添加到./bashrc

topic

publiser实现

//创建功能包
catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim

/src/learning_topic/src创建cpp代码文件

/src/learning_topic/scripts 创建py代码

实现Publisher的方法：

初始化ROS节点

向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型

创建消息数据

按照一定的频率循环发布消息

以python代码为例

创建代码

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


# 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist

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

编译代码
1. 配置cmakelist.txt
  
  将这一段取消注释，scripts/后面加自己的文件名称
2. 编译
```
catkin_make
```

运行代码

rosrun learning_topic velocity_publisher.py

Subscriber

创建代码同上
#! /usr/bin/env /usr/local/bin/python3.10
# -*- coding: utf-8 -*-


# 该例程将订阅/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()

如何实现一个Subscriber：

初始化ROS节点

订阅需要的话题

循环等待话题消息，接收到消息后进入回调函数

在回调函数中完成消息处理

rqt_graph查看图示

massage的定义与使用

消息的定义

自定义一个消息类型person，并完成发布订阅整个过程

learning_topic新建msg目录，创建自定义消息文件
写入person消息信息,定义msg数据接口

string name
uint8 sex
uint8 age

uint8 unknown = 0
uint8 male = 1
uint8 female = 2

在package.xml中添加功能包依赖

添加动态生成程序的功能包依赖。打开package.xml文件，将下面代码拷到文件指定位置：

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

build_depend为编译依赖，这里依赖的是一个会动态产生message的功能包 exer_depend为执行依赖，这里依赖的是一个动态runtime运行的功能包

配置cmakelist.txt（与第三部相对应）
1. 在package.xml添加了功能包编译依赖，在CMakeList.txt里的find_package中也要加上对应的部分:
2. message_generation
3. 需要将定义的Person.msg作为消息接口，针对它做编译；需要指明编译这个消息接口需要哪些ROS已有的包；
4. 因为在package.xml添加了功能包执行依赖，在CMakeList.txt里的catkin_package中也要加上对应的部分 7. message_runtime
编译生成对应的python库
```
  catkin_make
```
devel/lib/python3/dist-packages/learning_topic/msg 下找到编译之后的Python的包：

使用自定义的消息进行发布与订阅

创建person_publisher.py和person_subscriber.py

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



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

import rospy
from learning_topic.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.name = "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.name, person_msg.age, person_msg.sex)

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

if __name__ == '__main__':
    try:
        velocity_publisher()
    except rospy.ROSInterruptException:
        pass
#! /usr/bin/env /usr/local/bin/python3.10
# -*- coding: utf-8 -*-


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

import rospy
from learning_topic.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()

编译运行(记得先添加为可执行文件)

catkin_make
roscore
rosrun learning_topic person_subscriber.py
rosrun learning_topic person_publisher.py
rqt_graph

Service

//创建功能包
~/catkin_ws/src$ catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim

client

如何实现一个客户端Client

初始化ROS

创建一个Client实例

发布服务请求数据

等待Server处理之后的应答结果

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

# 该例程将请求/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()))

Server

如何实现一个服务器端Server

初始化ROS

创建一个Server实例

循环等待服务请求，进入回调函数

在回调函数中完成服务功能的处理，并反馈应答数据

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


# 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger

import rospy
import _thread,time
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse

pubCommand = False
turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)

def command_thread():        
        while True:
                if pubCommand:
                        vel_msg = Twist()
                        vel_msg.linear.x = 0.5
                        vel_msg.angular.z = 0.2
                        turtle_vel_pub.publish(vel_msg)
                        
                time.sleep(0.1)

def commandCallback(req):
        global pubCommand
        pubCommand = bool(1-pubCommand)

        # 显示请求数据
        rospy.loginfo("Publish turtle velocity command![%d]", pubCommand)

        # 反馈数据
        return TriggerResponse(1, "Change turtle command state!")

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

        # 创建一个名为/turtle_command的server，注册回调函数commandCallback
    s = rospy.Service('/turtle_command', Trigger, commandCallback)

        # 循环等待回调函数
    print ("Ready to receive turtle command.")

    _thread.start_new_thread(command_thread, ())
    rospy.spin()

if __name__ == "__main__":
    turtle_command_server()

配置cmake

编译运行

cd ~/catkin_ws
catkin_make
roscore
rosrun turtlesim turtlesim_node
rosrun learning_service turtle_spawn.py
roscore
rosrun turtlesim turtlesim_node
rosrun learning_service turtle_command_server
#让海龟动起来
rosservice call /turtle_command "{}"

服务数据SRV的定义与使用

服务数据的定义

创建srv

在package.xml中添加功能包依赖

添加编译依赖和执行依赖

build_depend为编译依赖，这里依赖的是一个会动态产生message的功能包 exer_depend为执行依赖，这里依赖的是一个动态runtime运行的功能包

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

配置cmake（和上一步对应）

编译

catkin_make

使用自定义数据进行发布

创建代码

person_client.py

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


# 该例程将请求/show_person服务，服务数据类型learning_service::Person

import sys
import rospy
from learning_service.srv import Person, PersonRequest

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

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

                # 请求服务调用，输入请求数据
        response = person_client("Tom", 20, PersonRequest.male)
        return response.result
    except rospy.ServiceException as e:
        print ("Service call failed: %s"%e)

if __name__ == "__main__":
        #服务调用并显示调用结果
    print ("Show person result : %s" %(person_client()))

person_server.py

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


# 该例程将执行/show_person服务，服务数据类型learning_service::Person

import rospy
from learning_service.srv import Person, PersonResponse

def personCallback(req):
        # 显示请求数据
    rospy.loginfo("Person: name:%s  age:%d  sex:%d", req.name, req.age, req.sex)

        # 反馈数据
    return PersonResponse("OK")

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

        # 创建一个名为/show_person的server，注册回调函数personCallback
    s = rospy.Service('/show_person', Person, personCallback)

        # 循环等待回调函数
    print ("Ready to show person informtion.")
    rospy.spin()

if __name__ == "__main__":
    person_server()

配置cmake

运行

roscore
rosrun learning_service person_server.py
rosrun learning_service person_client.py

Parameter

在ROS Master中，存在一个参数服务器（Parameter Server），它是一个全局字典，即一个全局变量的存储空间，用来保存各个节点的配置参数。各个节点都可以对参数进行全局访问。

参数命令行的使用

#列出当前多有参数
$ rosparam list
#显示某个参数值
$ rosparam get param_key
#设置某个参数值
$ rosparam set param_key param_value
#保存参数到某个文件
$ rosparam dump file_name
#从文件读取参数
$ rosparam load file_name
#删除参数
$ rosparam delete param_key

通过程序来使用参数

创建功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_parameter roscpp rospy std_srvs

py代码

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


# 该例程设置/读取海龟例程中的参数

import sys
import rospy
from std_srvs.srv import Empty

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

        # 读取背景颜色参数
    red   = rospy.get_param('/turtlesim/background_r')
    green = rospy.get_param('/turtlesim/background_g')
    blue  = rospy.get_param('/turtlesim/background_b')

    rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)

        # 设置背景颜色参数
    rospy.set_param("/turtlesim/background_r", 255);
    rospy.set_param("/turtlesim/background_g", 255);
    rospy.set_param("/turtlesim/background_b", 255);

    rospy.loginfo("Set Backgroud Color[255, 255, 255]");

        # 读取背景颜色参数
    red   = rospy.get_param('/turtlesim/background_r')
    green = rospy.get_param('/turtlesim/background_g')
    blue  = rospy.get_param('/turtlesim/background_b')

    rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)

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

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

if __name__ == "__main__":
    parameter_config()

配置camke

编译运行(可以看到小乌龟背景颜色变化)

cd ~/catkin_ws
catkin_make
roscore
rosrun turtlesim turtlesim_node
rosrun learning_parameter parameter_config.py

ROS中的坐标管理系统

TF功能包

一个机器人中，可以有很多坐标系，我们需要去描述任意两个坐标系之间的关系，涉及到大量的矩阵运算。我们可以用ROS中的TF(Transform)功能包来解决问题。

可以看看这一篇文章：https://blog.csdn.net/takedachia/article/details/122602199 or 参考古月居第17讲

查看当前TF tree，查看坐标之间的关系

rosrun tf view_frames

坐标的相对位置关系

rosrun tf tf_echo turtle1 turtle2

坐标关系可视化rviz

rosrun rviz rviz -d `rospack find turtle_tf` /rviz/turtle_rviz.rviz

Fixed Frame选 world。Add选添加TF

TF坐标系的广播与监听的编程实现

创建功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim

实现广播与监听

如何实现一个TF广播器：

定义TF广播器（TransformBroadcaster）

创建坐标变换值

发布坐标变换（sendTransform）

如何实现一个TF监听器：

定义TF监听器（TransformListener）

查找坐标变换（waitForTransform、lookupTransform）

turtle_tf_broadcaster.py

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

# 该例程将请求/show_person服务，服务数据类型learning_service::Person

import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
    br = tf.TransformBroadcaster()
    br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                     rospy.Time.now(),
                     turtlename,
                     "world")

if __name__ == '__main__':
    rospy.init_node('turtle_tf_broadcaster')
    turtlename = rospy.get_param('~turtle')
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()

turtle_tf_listener.py

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


# 该例程将请求/show_person服务，服务数据类型learning_service::Person

import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')

    listener = tf.TransformListener()

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(4, 2, 0, 'turtle2')

    turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        angular = 4 * math.atan2(trans[1], trans[0])
        linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        cmd = geometry_msgs.msg.Twist()
        cmd.linear.x = linear
        cmd.angular.z = angular
        turtle_vel.publish(cmd)

        rate.sleep()

配置cmake

编译运行

roscore
rosrun turtlesim turtlesim_node
#启动两只海龟广播节点
rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle1_tf_broadcaster _turtle:=turtle1
rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle2_tf_broadcaster _turtle:=turtle2
#启动监听
rosrun learning_tf turtle_tf_listener.py
#键盘控制
rosrun turtlesim turtle_teleop_key

launch文件的使用

#launch文件可以自动运行roscore
cd ~/catkin_ws/src
catkin_create_pkg learning_launch
#写好launch文件之后，launch
roslaunch <pkg_name> <launch file name>

learning_launch 功能包下直接创建launch文件夹，直接创建launch文件

启动节点

#simple.launch
<launch>
    <node pkg="learning_topic" type="person_subscriber.py" name="talker" output="screen" />
    <node pkg="learning_topic" type="person_publisher.py" name="listener" output="screen" /> 
</launch>

python文件的启动，type后应写上文件名后缀.py

launch文件控制参数

<launch>

        <param name="/turtle_number"   value="2"/>

    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
                <param name="turtle_name1"   value="Tom"/>
                <param name="turtle_name2"   value="Jerry"/>

                <rosparam file="$(find learning_launch)/config/param.yaml" command="load"/>
        </node>

    <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen"/>

</launch>

启动两个海龟，加载参数文件，最后启动键盘控制节点

查看参数可以看到参数被修改，参数文件成功被加载

启动广播与监听

<launch>

        <!-- Turtlesim Node-->
        <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
        <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

        <node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py">
          <param name="turtle" type="string" value="turtle1" />
        </node>
        <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py">
          <param name="turtle" type="string" value="turtle2" /> 
        </node>

    <node pkg="learning_tf" type="turtle_tf_listener.py" name="listener" />

</launch>

Remap 重命名

<launch>

        <include file="$(find learning_launch)/launch/simple.launch" />

    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
                <remap from="/turtle1/cmd_vel" to="/cmd_vel"/>
        </node>

</launch>