ROS 第三讲 操控小乌龟

小乌龟模拟器是学习ROS基本操作的很好用的工卡具. 使用也非常方便:

roscore
rosrun turtlesim turtlesim_node

就会弹出类似如下的窗口(小乌龟样子是随机出现的):

使命行操控小乌龟

查看有节点信息(保持刚刚的terminal窗口, 然后再打开一个新的窗口):

rosnode list

会显示如下信息:

/rosout
/turtlesim

接下来可以看看/turtlesim 这个节点上面有哪些发布, 订阅,甚至服务等,使用:

rosnode info /turtlesim

显示如下信息:

--------------------------------------------------------------------------------
Node [/turtlesim]
Publications: 
 * /rosout [rosgraph_msgs/Log]
 * /turtle1/color_sensor [turtlesim/Color]
 * /turtle1/pose [turtlesim/Pose]

Subscriptions: 
 * /turtle1/cmd_vel [unknown type]

Services: 
 * /clear
 * /kill
 * /reset
 * /spawn
 * /turtle1/set_pen
 * /turtle1/teleport_absolute
 * /turtle1/teleport_relative
 * /turtlesim/get_loggers
 * /turtlesim/set_logger_level


contacting node http://prince_pc:45393/ ...
Pid: 19374
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound (34539 - 127.0.0.1:52134) [26]
    * transport: TCPROS

信息显示/turtlesim节点发布两个topic(暂时忽略日志相关), 一个控制颜色/turtle1/color_sensor(信息类型为turtlesim/Color),一个控制乌龟位置 /turtle1/pose(信息类型为turtlesim/Pose); /turtlesim 节点订阅/turtle/cmd_vel(信息类型未知,因为发布此topic的节点尚未执行); /turtlesim节点还有很多相关的服务,比如关掉节点的/kill等.

使用rqt_graph 查看后台的节点与topic情况

使用如下命名,查看一下turtlesim/Color的字段及其对应的类型:

rosmsg show tutrlesim/Color

显示:

uint8 r
uint8 g
uint8 b

表明三个字段均为8字节的整型. 然后使用rostopic echo /turtle1/color_sensor 可以查看此topic当前发布的信息情况:

---
r: 69
g: 86
b: 255

参数服务器维护着参数字典. 可用rosparam list 展示当前的参数,其信息类似如下:

/rosdistro
/roslaunch/uris/host_prince_pc__37877
/rosversion
/run_id
turtlesimturtlesim/background_b
/turtlesim/background_g
/turtlesim/background_r

使用rosparam get /turtlesim 获取当前各参数数据:

{background_b: 255, background_g: 86, background_r: 69}

使用rosparam set 修改参数数值,比如将背景色改成红色:

rosparam set /turtlesim/background_b 0
rosparam set /turtlesim/background_g 0
rosparam set /turtlesim/background_r 255
rosservice call /clear

类似的查询位置信息:

rostopic echo /turtle1/pose

显示:

x: 1.0
y: 1.0
theta: 0.0
linear_velocity: 0.0
angular_velocity: 0.0
---

使用服务修改

#绝对位置
rosservice call /turtle1/teleport_absolute 1 1 0
# 相对位置
rosservice call /turtle1/teleport_relative 1 0

向对应的topic ( /turtle1/cmd_vel)发布速度信息让其自己动起来:

rostopic pub -1 /turtle1/cmd_vel geometry_msgs/Twist -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, 1.8]'

其中:

-1: 表示此命令只发布一次 (持续三秒)
geometry_msgs/Twist: 表示此topic的信息类型是geometry_msgsTwist
 -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, 1.8]': 表示只在x轴方向有2的线速度以及在z方向上有1.8的角度速度

因为:

# rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

使用键盘移动小乌龟:

rosrun turtlesim turtle_teleop_key

编程操控小乌龟

上面使用命令行操控小乌龟, 接下来使用程序对小乌龟进行操控.

用程序让小乌龟动起来

首先再来查看一下小乌龟相关的topic:

rostopic list

显示:

/rosout
/rosout_agg
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose

要让小乌龟动起来,需向/turtle1/cmd_vel这个topic中发布速度数据. 下面命令的信息显示小乌龟没动是没有topic发布者:

rostopic info /turtle1/cmd_vel

显示:

Type: geometry_msgs/Twist

Publishers: None

Subscribers: 
 * /turtlesim (http://prince_pc:44987/)

查看下此topic需要的数据类型

rostopic type /turtle1/cmd_vel

显示:

geometry_msgs/Twist

再查看下其下面对应的字段与对应类型:

rosmsg show geometry_msgs/Twist

显示:

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

可知,这个msg 有线性速度与角速度两部分组成, 而每部分有三个字段. 不过这里插一句, 可以看到小乌龟模拟器是一个二维平面,因此管上面两个部分各有三个字段,不过只有 linear 的x与angular的z有作用.

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

import sys
import rospy as ros
from geometry_msgs.msg import Twist


def move_turtle(lin_vel, ang_vel):
    ros.init_node('move_turtle1', anonymous=False)
    pub = ros.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    rate = ros.Rate(10)
    vel = Twist()
    while not ros.is_shutdown():
        vel.linear.x = lin_vel
        vel.linear.y = 0
        vel.linear.z = 0
        vel.angular.x = 0
        vel.angular.y = 0
        vel.angular.z = ang_vel
        ros.loginfo("Linear Vel = %f: Angular Vel = % f", lin_vel, ang_vel)
        pub.publish(vel)
        rate.sleep()


if __name__ == '__main__':
    try:
        move_turtle(float(sys.argv[1]), float(sys.argv[2]))
    except ros.ROSInterruptException:
        pass

rosrun practice1 move_turtle1.py 0.2 0.04

会出现类似下面(黑色踪迹部分)的情况:

然后,　使用rqt_graph　可以看到刚刚创建的节点/move_turtle1发布,而/turtle1/cmd_vel在订阅:

接下来,　我们不但发布速度,　还想要知道小乌龟当前位置:

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


import sys
import rospy as ros
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose


def pose_callback(pose):
    ros.loginfo("Robot X = %f: Y=%f: Z = %f
", pose.x, pose.y, pose.theta)


def move_turtle(lin_vel, ang_vel):
    ros.init_node('move_turtle2', anonymous=False)
    pub = ros.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    ros.Subscriber('/turtle1/pose', Pose, pose_callback)
    rate = ros.Rate(10)
    vel = Twist()
    while not ros.is_shutdown():
        vel.linear.x = lin_vel
        vel.linear.y = 0
        vel.linear.z = 0
        vel.angular.x = 0
        vel.angular.y = 0
        vel.angular.z = ang_vel
        ros.loginfo("Linear Vel = %f: Angular Vel = % f", lin_vel, ang_vel)
        pub.publish(vel)
        rate.sleep()


if __name__ == '__main__':
    try:
        move_turtle(float(sys.argv[1]), float(sys.argv[2]))
    except ros.ROSInterruptException:
        pass

然后在小乌龟转圈的同时,terminal上面会显示类似下面的位置信息:

[INFO] [1627223458.078473]: Robot X = 4.921455: Y=8.336517: Z = 0.539711

[INFO] [1627223458.095107]: Robot X = 4.924197: Y=8.338168: Z = 0.541951

[INFO] [1627223458.110455]: Robot X = 4.926934: Y=8.339825: Z = 0.544191

[INFO] [1627223458.127032]: Robot X = 4.929668: Y=8.341488: Z = 0.546431

[INFO] [1627223458.142566]: Robot X = 4.932399: Y=8.343157: Z = 0.548671

然后通过后端的结构图,明显地展示/turtlesim 与/move_turtle２ 互相有指向的箭头,即二者相互订阅.

既然获得了小乌龟的具体位置与其速度,　我们是可以控制小乌龟只走特定距离的,而不是一直走下去.

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

import sys
import rospy as ros
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose

ROBOT_X = 0


def pose_callback(pose):
    global ROBOT_X
    ros.loginfo("Robot X = %f: Y=%f: Z = %f
", pose.x, pose.y, pose.theta)
    ROBOT_X = pose.x


def move_turtle(lin_vel, ang_vel, distance):
    global ROBOT_X
    ros.init_node('move_turtle3', anonymous=False)
    pub = ros.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    ros.Subscriber('/turtle1/pose', Pose, pose_callback)
    rate = ros.Rate(10)
    vel = Twist()
    while not ros.is_shutdown():
        vel.linear.x = lin_vel
        vel.linear.y = 0
        vel.linear.z = 0
        vel.angular.x = 0
        vel.angular.y = 0
        vel.angular.z = ang_vel
        ros.loginfo("Linear Vel = %f: Angular Vel = % f", lin_vel, ang_vel)
        if ROBOT_X >= distance:
            ros.loginfo('Robot exercises finished.')
            ros.logwarn('stopping robot')
            break
        pub.publish(vel)
        rate.sleep()


if __name__ == '__main__':
    try:
        move_turtle(float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]))
    except ros.ROSInterruptException:
        pass

rosrun practice1 move_turtle3.py 0.1 0. 8

小乌龟在模拟器中跑了一段距离后,停了下来, 同时terminal的日志类似下面的样子:

[INFO] [1627225494.849535]: Robot X = 7.997244: Y=5.544445: Z = 0.000000

[INFO] [1627225494.864902]: Robot X = 7.998845: Y=5.544445: Z = 0.000000

[INFO] [1627225494.881546]: Robot X = 8.000444: Y=5.544445: Z = 0.000000

[INFO] [1627225494.897005]: Robot X = 8.002045: Y=5.544445: Z = 0.000000

[INFO] [1627225494.913677]: Robot X = 8.003645: Y=5.544445: Z = 0.000000

[INFO] [1627225494.917665]: Linear Vel = 0.100000: Angular Vel =  0.000000
[INFO] [1627225494.923973]: Robot exercises finished.
[WARN] [1627225494.928156]: stopping robot
[INFO] [1627225494.929083]: Robot X = 8.005244: Y=5.544445: Z = 0.000000

现在我们可以编程让小乌龟跑起来了, 我们也可以编程来改变背景色, 这个也相对简单:

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


import rospy as ros
import random
from std_srvs.srv import Empty


def change_color():
    ros.init_node('change_color', anonymous=True)
    # Setting random values from 0-255 in the color parameters
    ros.set_param('/turtlesim/background_b', random.randint(0, 255))
    ros.set_param('/turtlesim/background_g', random.randint(0, 255))
    ros.set_param('/turtlesim/background_r', random.randint(0, 255))

    # Waiting for service /reset
    ros.wait_for_service('/reset')
    # Calling /reset service
    try:
        serv = ros.ServiceProxy('/reset', Empty)
        resp = serv()
        ros.loginfo("Executed service")
    except ros.ServiceException, e:
        ros.loginfo("Service call failed: %s" % e)
    ros.spin()


if __name__ == '__main__':
    try:
        change_color()
    except ros.ROSInterruptException:
        pass

# rosrun practice1 change_bg_color.py 
[INFO] [1627226337.913255]: Executed service

小乌龟模拟器就会随机变换颜色: