使用RViz 模拟turtlebot3
roslaunch turtlebot3_fake turtlebot3_fake.launch
就会弹出rviz的窗口:
查看下topic:
rostopic list
显示:
/clicked_point
/cmd_vel
/initialpose
/joint_states
/move_base_simple/goal
/odom
/rosout
/rosout_agg
/tf
/tf_static
其中有/cmd_vel 这个topic, 再查看下后台结构图:
rqt_graph
显示topic /cmd_vel由/turtlebot3_fake_node订阅, 而其暂没有发布者,因此小车在模拟器中是静止的. 让小车动起来可用键盘:
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
也可以用程序控制:
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
@file: turtlebot1.py
@description: move turtlebot3 in rviz
"""
import sys
import rospy as ros
from geometry_msgs.msg import Twist
def move_turtle(lin_vel, ang_vel):
ros.init_node('move_turtlebot1', anonymous=False)
pub = ros.Publisher('/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_turtlebot1.py 0.1 0. 8
小车即可在rviz中动起来,类似:
刚刚, roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch 命令, 可以让我们通过键盘来控制小车的运动. 其实我们也可以不调用这个为我们准备好的roslauch命令, 我们完全可以使用程序实现一样的功能.
实时抓取用户键入信息
首先需要解决的是如何通过键盘的输入将其转化为对应topic所需的msg, 可以使用termios包来实现
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
@file: keyboard_publisher.py
@description:
"""
import sys, select, tty, termios
import rospy as ros
from std_msgs.msg import String
def keyboard_capture():
ros.init_node("keyboard_driver")
key_pub = ros.Publisher('keyin', String, queue_size=1)
rate = ros.Rate(1000)
# 控制台是回车"Enter"后接收用户的输入的, 下面两句是将此模式替换成,
# 用户输入立即接入用户输入信息,而不用等待回车输入
prev_attrib = termios.tcgetattr(sys.stdin)
tty.setcbreak(sys.stdin.fileno())
print "starting capture key in,immediately, press Ctrl-z to exit..."
while not ros.is_shutdown():
if select.select([sys.stdin], [], [], 0)[0] == [sys.stdin]:
key_pub.publish(sys.stdin.read(1))
rate.sleep()
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, prev_attrib)
if __name__ == '__main__':
keyboard_capture()
通过上面的程序即可捕获键盘实时键入的信息. 可先在一个terminal窗口中使用:
rostopic echo /keyin
然后再启动:
rosrun practice2 keyboard_publisher.py
那么上一个terminal中会显示类似如下信息,则说明我们成功了:
data: "w"
---
data: "a"
---
data: "d"
---
data: "s"
---
接下来,我们只需将用户键入的信息映射成速度信息并发布即可:
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
@file: key2vel_map.py
@description:
"""
import rospy as ros
from std_msgs.msg import String
from geometry_msgs.msg import Twist
class Key2Vel_Map(object):
def __init__(self, mode='uniform'):
"""
:param mode: 速度模式还是匀速(default)模式
"""
ros.init_node('key2vel')
self.vel_pub = ros.Publisher('/cmd_vel', Twist, queue_size=1)
if mode == 'uniform':
self.map_func = self.uniform_map
elif mode == 'accelerate':
self.map_func = self.accelerate_map
else:
ValueError('bad mode: `%s` value, please check!' % mode)
self.mode = mode
self.vel = [0, 0]
self.twist = Twist()
self.kmap = {'a': [0, 0.1], 's': [0, 0], 'd': [0, -0.1], 'w': [0.1, 0], 'x': [-0.1, 0]}
def key2map(self, key):
if key in self.kmap:
return self.kmap[key]
else:
return None
def uniform_map(self, key, multiple=10):
"""
:param key:
:param multiple: 0.1匀速度的倍数
:return:
"""
res = self.key2map(key)
if res:
return [x * multiple for x in res]
def accelerate_map(self, key):
res = self.key2map(key)
if res:
if key == 's':
self.vel = [0, 0]
else:
self.vel = [x + y for x, y in zip(res, self.vel)]
return self.vel
def key_callback(self, msg):
if len(msg.data) == 0 or not self.kmap.get(msg.data[0]):
return # unknown key
vels = self.map_func(msg.data[0])
self.twist.linear.x = vels[0]
self.twist.angular.z = vels[1]
def capture(self):
ros.Subscriber('keyin', String, self.key_callback)
while not ros.is_shutdown():
self.vel_pub.publish(self.twist)
ros.spin()
if __name__ == '__main__':
k2v = Key2Vel_Map(mode='accelerate')
k2v.capture()
此时我们就可以分别在不同的terminal中开启两个程序了,也可以写一个.launch文件,将两个程序同时启动. 首先,我们在 practice2包中创建launch目录, 然后在其中创建名为key2map.launch文件, 并在其中加入:
<launch>
<node name ="key2vel" pkg="practice2" type="key2vel_map.py" output="screen"/>
<node name ="keyboard_driver" pkg="practice2" type="keyboard_publisher.py" output="screen"/>
</launch>
启动:
roslaunch practice2 key2map.launch
然后我们就可以在当前的terminal中键入WASDX来操作小车了!
让小车在gazebo中跑起来
首先启动带有turtlebot的模拟环境:
roslaunch turtlebot3_gazebo turtlebot3_world.launch
会弹出如下类似的窗口:
可以使用
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
然后使用键盘来操作. terminal上面会出现类似信息,可知使用WADXS五键来控制:
... logging to /home/[username]/.ros/log/8498294a-f078-11eb-9bc2-2cfda1be536f/roslaunch-prince_pc-26012.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://[user_name]_pc:38397/
SUMMARY
========
PARAMETERS
* /model: burger
* /rosdistro: melodic
* /rosversion: 1.14.11
NODES
/
turtlebot3_teleop_keyboard (turtlebot3_teleop/turtlebot3_teleop_key)
ROS_MASTER_URI=http://localhost:11311
process[turtlebot3_teleop_keyboard-1]: started with pid [26061]
Control Your TurtleBot3!
---------------------------
Moving around:
w
a s d
x
w/x : increase/decrease linear velocity (Burger : ~ 0.22, Waffle and Waffle Pi : ~ 0.26)
a/d : increase/decrease angular velocity (Burger : ~ 2.84, Waffle and Waffle Pi : ~ 1.82)
space key, s : force stop
CTRL-C to quit
也可以使用程序来控制小车, 同样的,先看下当前的topic 有哪些:
# rostopic list
/clock
/cmd_vel
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/imu
/joint_states
/odom
/rosout
/rosout_agg
/scan
/tf
现在我们已经熟悉了/cmd_vel 这个topic的msg可以控制小车,比如写一个控制小车,走走停停的程序:
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
@file: run_or_wait.py
@description:
"""
import rospy as ros
from geometry_msgs.msg import Twist
def red_green_light():
cmd_vel_pub = ros.Publisher('/cmd_vel', Twist, queue_size=1)
ros.init_node('red_light_green_light', anonymous=False)
red_light_twist = Twist()
green_light_twist = Twist()
green_light_twist.linear.x = 0.5
driving_forward = True
light_change_time = ros.Time.now()
rate = ros.Rate(10)
while not ros.is_shutdown():
if driving_forward:
cmd_vel_pub.publish(green_light_twist)
print('go forward...', light_change_time)
else:
cmd_vel_pub.publish(red_light_twist)
print('stop...', light_change_time)
if light_change_time < ros.Time.now():
driving_forward = not driving_forward
light_change_time = ros.Time.now() + ros.Duration(3)
rate.sleep()
if __name__ == '__main__':
red_green_light()
运行后,小车就会走一会停一会. 我们可以控制gazebo中的小车了!
获取小车位置信息
上面的程序会使用小车很容易就卡在某个地方. 因为他漫无目的的走,根本不在意前方的障碍物. 我们慢慢来, 我们先让小车走固定距离, 这个咱们之前控制小乌龟时尝试过,这次咱们按类似的方法控制小车. 小车的位置信息topic是/odom, 其type与msg:
# rostopic type /odom
nav_msgs/Odometry
# rosmsg show nav_msgs/Odometry
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[36] covariance
geometry_msgs/TwistWithCovariance twist
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
float64[36] covariance
从上面略显复杂的数据结构中, 稍加寻找,发现其位置信息在:Odometry.pose.pose.position中, 其它的与小乌龟移动固定距离类似:
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
@file: run_or_wait.py
@description:
"""
import sys
import rospy as ros
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
ORIGIN_POSE = 0
CURRENT_POSE = 0
def pose_callback(msg):
global CURRENT_POSE, ORIGIN_POSE
CURRENT_POSE = msg.pose.pose.position.x
ros.loginfo('Robot X=%f\n', CURRENT_POSE - ORIGIN_POSE)
def move_turtlebot(lin_vel, ang_vel, distance):
global CURRENT_POSE, ORIGIN_POSE
ros.init_node('move_turtlebot', anonymous=False)
pub = ros.Publisher('/cmd_vel', Twist, queue_size=1)
ros.Subscriber('/odom', Odometry, pose_callback)
rate = ros.Rate(1)
rate.sleep()
vel = Twist()
ORIGIN_POSE = CURRENT_POSE
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
# rospy.loginfo("Linear Vel = %f: Angular Vel = %f",lin_vel, ang_vel)
if ((CURRENT_POSE - ORIGIN_POSE) >= distance):
ros.loginfo("Robot Reached destination")
ros.logwarn("Stopping robot")
vel.linear.x = 0.
vel.angular.z = 0.
pub.publish(vel)
rate.sleep()
break
pub.publish(vel)
rate.sleep()
if __name__ == '__main__':
try:
move_turtlebot(float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]))
except ros.ROSInterruptException:
pass
这样小车跑一会就停下来了.
使用sensor laser
我们限定了小车运动的方式以及运动范围避免他撞到周围的物体. 其实通过传感器,小车可以及时避障,然后, 修改路线.
之前的包practice1 没有传感器相关的依赖包, 这里我们重新创建一个practice2的包,加入sensor 相关的依赖包:
cd ~/catkin_ws/src
catkin_create_pkg practice2 rospy roscpp standard_msgs geometry_msgs sensor_msgs
cd ~/catkin_ws
catkin_make --pkg practice2
cd ~/catkin_ws/src/practice2
mkdir -p script
使用rostopic list, 展示的列表中, /scan 是关于激光扫描的topic:
# rostopic type /scan
sensor_msgs/LaserScan
# rosmsg show sensor_msgs/LaserScan
std_msgs/Header header
uint32 seq
time stamp
string frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities
如果此时打印相关msg的信息,大约会不断打印类似下面的数据(Ctrl-c 可以停下):
# rostopic echo /scan
header:
seq: 91
stamp:
secs: 62
nsecs: 152000000
frame_id: "base_scan"
angle_min: 0.0
angle_max: 6.28318977356
angle_increment: 0.0175019223243
time_increment: 0.0
scan_time: 0.0
range_min: 0.119999997318
range_max: 3.5
ranges: [inf, inf, inf, inf, inf, inf, inf, 3.073126792907715, 3.0457253456115723, 3.0183537006378174, 2.03729248046875, 1.973899245262146, 1.955241322517395, 1.9441207647323608, 1.9433691501617432, ...,inf]
intensities: [0.0, 0.0, ..., 0.0]
---
其中的range 表示在传感器可扫描角度中,各个方向上面的物体与小车距离. 要获得小车正前方障碍物与小车的距离可以这样计算:
range_ahead = msg.ranges[len(msg.ranges)/2]
其中msg表示msg时实例名.
取得以上信息后,就可以让小车自动避障了, 比如当前的障碍距离小车20cm时, 就让小车暂停, 并旋转一定角度, 后继承前进.
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
@file: run_or_wait.py
@description:
"""
import rospy as ros
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
class WanderBot(object):
def __init__(self):
ros.init_node('wanderBot')
self.ahead_distance = 1e10
self.ahead_threshold = 0.3
self.scan_sub = ros.Subscriber('/scan', LaserScan, callback=self.scan_callback)
self.cmd_vel_pub = ros.Publisher('/cmd_vel', Twist, queue_size=1)
self.rate = ros.Rate(1)
def scan_callback(self, msg):
self.ahead_distance = msg.ranges[len(msg.ranges) / 2]
ros.loginfo('ahead_distance: %f\n' % self.ahead_distance)
def wander(self):
vel = Twist()
while not ros.is_shutdown():
if self.ahead_distance < 0.4:
ros.loginfo('stop and turn around...')
vel.linear.x = 0
vel.angular.z = 0.2
else:
ros.loginfo('go ahead...')
# 注意这个方向才是与laser sensor 一致的方向,
# 实验一下即可得知
vel.linear.x = -0.2
vel.angular.z = 0
self.cmd_vel_pub.publish(vel)
self.rate.sleep()
if __name__ == '__main__':
wb = WanderBot()
wb.wander()
先打开rivz, 再执行上面的程序,会对小车的运动更有帮助:
roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch
# another new terminal
rosrun practice2 wanderbot.py
...
[INFO] [1627714129.363296, 348.200000]: ahead_distance: 1.163768
[INFO] [1627714129.421784, 348.264000]: go ahead...
[INFO] [1627714129.559045, 348.401000]: ahead_distance: 1.122308
[INFO] [1627714129.760064, 348.601000]: ahead_distance: 1.084583
[INFO] [1627714129.960899, 348.802000]: ahead_distance: 1.060412
[INFO] [1627714130.162832, 349.002000]: ahead_distance: 1.005350
[INFO] [1627714130.365322, 349.202000]: ahead_distance: 0.954956
...
浙公网安备 33010602011771号