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
小乌龟模拟器就会随机变换颜色:
标签：操控,ROS,turtle1,pose,乌龟,turtlesim,ros,vel 来源： https://www.cnblogs.com/vpegasus/p/ros_turtle.html