ICode9
精准搜索请尝试： 精确搜索
首页
编程语言>
数据库
系统相关
互联网
其他分享
Python
Java
JavaScript
android
PHP
首页 > 其他分享> 文章详细

ROS 第三讲操控小乌龟

2021-07-25 23:32:27  阅读：340  来源： 互联网

标签：操控 ROS turtle1 pose 乌龟 turtlesim ros vel


小乌龟模拟器是学习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

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

关于我们 | 联系我们 | 留言反馈

专注分享技术，共同学习，共同进步。侵权联系[81616952@qq.com]

Copyright (C)ICode9.com, All Rights Reserved.

ICode9版权所有