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\n", 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\n", 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

小烏龜模擬器就會隨機變換顏色: