ROS Learning-030 (提高篇-008 A Mobile Base-06) 控制移动平台 --- (Python)odom导航的例子:移动一个方块路径
ROS 提高篇 之 A Mobile Base-06 — 控制移动平台 — (Python)再次使用odom导航的一个例子:移动一个方块路径
我使用的虚拟机软件:VMware Workstation 11
使用的Ubuntu系统:Ubuntu 14.04.4 LTS
ROS 版本:ROS Indigo
注意:
1 . ROS 提高篇这个专栏的教学有门槛。
2 . 如果你没有学习前面的教程,请想学习前面的 beginner_Tutorials 和 learning_tf 的ROS 相关教程。
一 . 前言:
上一节,我们使用odom
重写了前进和返回的程序。这次,我们尝试以一个方块形路径移动移动机器人。(方块的4个点设置航点)
二 . 运行程序,看看效果:
在查看代码之前,我们先来启动这个节点,看看运行效果:
新开一个终端,执行下面的命令,启动一个虚拟的 TurtleBot 机器人:
$ roslaunch rbx1_bringup fake_turtlebot.launch
再开一个终端,启动 RViz :
$ rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz
最后开一个终端,运行 odom_out_and_back.py
节点:
$ rosrun rbx1_nav nav_square.py
当你将这句命令执行完,在 RVIz模拟器 中,你就可以看到下面图片里的运行效果:程序在模拟器中航行一个方块路径,机器人在这个路径上移动。
三 . 程序代码: nav_square.py
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from rbx1_nav.transform_utils import quat_to_angle, normalize_angle
from math import radians, copysign, sqrt, pow, pi
class NavSquare():
def __init__(self):
# 给出节点的名字
rospy.init_node('nav_square', anonymous=False)
# 设置rospy在程序退出时执行的关机函数
rospy.on_shutdown(self.shutdown)
# 我们将用多快的速度更新控制机器人运动的命令?
rate = 20
# 设定相同的值给rospy.Rate()
r = rospy.Rate(rate)
# 为目标正方形设置参数
goal_distance = rospy.get_param("~goal_distance", 1.0) # 米
goal_angle = rospy.get_param("~goal_angle", radians(90)) # 角度转换弧度
linear_speed = rospy.get_param("~linear_speed", 0.2) # m/s
angular_speed = rospy.get_param("~angular_speed", 0.7) # rad/s
angular_tolerance = rospy.get_param("~angular_tolerance", radians(2)) # 角度到弧度的公差
# 发布者控制机器人的速度
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# 配置base坐标系: 如果是TurtleBot机器人为: base_footprint, 如果是Pi Robot机器人为: base_link
self.base_frame = rospy.get_param('~base_frame', '/base_link')
# odom坐标系通常就叫:/odom
self.odom_frame = rospy.get_param('~odom_frame', '/odom')
# 初始化tf 监听器
self.tf_listener = tf.TransformListener()
# 给tf一些时间填充它的缓冲区
rospy.sleep(2)
# 配置odom坐标系
self.odom_frame = '/odom'
# 询问机器人使用的是/base_link坐标系还是/base_footprint坐标系
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# 初始化了一个Point类型的变量
position = Point()
# 周期循环:通过正方形的四边
for i in range(4):
# 初始化运动命令
move_cmd = Twist()
# 设定前进速度
move_cmd.linear.x = linear_speed
# 得到开始的姿态信息(位置和转角)
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
# 随时掌控机器人行驶的距离
distance = 0
# 进入循环,沿着一边移动
while distance < goal_distance and not rospy.is_shutdown():
# 发布一次Twist消息 和 sleep 1秒
self.cmd_vel.publish(move_cmd)
r.sleep()
# 给出正确的姿态信息(位置和转角)
(position, rotation) = self.get_odom()
# 计算相对于开始位置的欧几里得距离(即位移)
distance = sqrt(pow((position.x - x_start), 2) +
pow((position.y - y_start), 2))
# 在转动机器人前,停止它
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
# 给旋转配置运动命令
move_cmd.angular.z = angular_speed
# 跟踪记录最后的角度
last_angle = rotation
# 跟踪我们已经转动了多少角度
turn_angle = 0
# Begin the rotation
while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
# 发布一次Twist消息 和 sleep 1秒
self.cmd_vel.publish(move_cmd)
r.sleep()
# 给出正确的姿态信息(位置和转角)
(position, rotation) = self.get_odom()
# 计算自每次的旋转量
delta_angle = normalize_angle(rotation - last_angle)
turn_angle += delta_angle
last_angle = rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
# 为了机器人好,停止它
self.cmd_vel.publish(Twist())
def get_odom(self):
# Get the current transform between the odom and base frames
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return (Point(*trans), quat_to_angle(Quaternion(*rot)))
def shutdown(self):
# 当关闭这个节点时,总是让机器人停止不动。
rospy.loginfo("Stopping the robot...")
self.cmd_vel.publish(Twist())
rospy.sleep(1)
if __name__ == '__main__':
try:
NavSquare()
except rospy.ROSInterruptException:
rospy.loginfo("Navigation terminated.")
四 . 讲解程序
略,就不讲了,因为这个程序,相比前两节(编写一个out_and_back 程序 和 使用 odometry 消息类型 重写 out_and_back 程序),没有什么新的知识点。
五 . 航位推算的问题:
这个问题的根源:随时间积累的测量出现的小误差。如何解决这个问题?幸运的是,机器人学家好久以前就开始研究各种方法来在导航中 合并地标或者使用其他外部参考,我们将会在SLAM章节学习到。
搞定
总结:
下一节,我们讲如何人机交互,并在Rviz 中看到机器人的运动。
ROS Learning-030 (提高篇-008 A Mobile Base-06) 控制移动平台 --- (Python)odom导航的例子:移动一个方块路径的更多相关文章
- ROS Learning-025 (提高篇-003 A Mobile Base-01) 控制移动平台
ROS 提高篇 A Mobile Base-01 - 控制移动平台 - 基本知识 我使用的虚拟机软件:VMware Workstation 11 使用的Ubuntu系统:Ubuntu 14.04.4 ...
- ROS Learning-031 (提高篇-009 A Mobile Base-07) 控制移动平台 --- (操作)人机交互
ROS 提高篇 之 A Mobile Base-07 - 控制移动平台 - (操作)人机交互 我使用的虚拟机软件:VMware Workstation 11 使用的Ubuntu系统:Ubuntu 14 ...
- ROS Learning-029 (提高篇-007 A Mobile Base-05) 控制移动平台 --- (Python编程)控制虚拟机器人的移动(精确的制定目标位置)
ROS 提高篇 之 A Mobile Base-05 - 控制移动平台 - (Python编程)控制虚拟机器人的移动(精确的制定目标位置) 使用 odometry 消息类型 重写 out_and_ba ...
- ROS Learning-028 (提高篇-006 A Mobile Base-04) 控制移动平台 --- (Python编程)控制虚拟机器人的移动(不精确的制定目标位置)
ROS 提高篇 之 A Mobile Base-04 - 控制移动平台 - (Python编程)控制虚拟机器人的移动(不精确的制定目标位置) 我使用的虚拟机软件:VMware Workstation ...
- ROS Learning-027 (提高篇-005 A Mobile Base-03) 控制移动平台 --- Twist 消息
ROS 提高篇 之 A Mobile Base-03 - 控制移动平台 - Twist 消息 我使用的虚拟机软件:VMware Workstation 11 使用的Ubuntu系统:Ubuntu 14 ...
- ROS Learning-026 (提高篇-004 A Mobile Base-02) 控制移动平台 --- “分封制”
ROS 提高篇 之 A Mobile Base-02 - 控制移动平台 - "分封制" 我使用的虚拟机软件:VMware Workstation 11 使用的Ubuntu系统:Ub ...
- ROS Learning-032 (提高篇-010 Launch)Launch 深入研究 --- (启动文件编程)ROS 的 XML语法简介
ROS 提高篇 之 Launch 深入研究 - 01 - 启动文件的编程 - ROS 的 XML语法简介 我使用的虚拟机软件:VMware Workstation 11 使用的Ubuntu系统:Ubu ...
- ROS Learning-024 (提高篇-002) rviz的安装和使用
ROS 提高篇-002 - rviz 的安装和使用 我使用的虚拟机软件:VMware Workstation 11 使用的Ubuntu系统:Ubuntu 14.04.4 LTS ROS 版本:ROS ...
- ROS Learning-023 (提高篇-001) 准备工作 --- 安装一些必要的软件包
ROS 提高篇-001 - 准备工作 - 安装一些必要的软件 我使用的虚拟机软件:VMware Workstation 11 使用的Ubuntu系统:Ubuntu 14.04.4 LTS ROS 版本 ...
随机推荐
- URL OpenDocument
以前用在DASHBOARD 使用URL传参 到webi 报表.还是很多不理解,现在明白多了.于是做个较为详细的记录.尽管dashboard 很快就被淘汰了.也许没什么用.看看也好. 之前的报表,传参都 ...
- 【Oracle】异常信息的加工处理
引言 很多时候,我们调用oracle存储过程都会发生各种各样的异常信息,例如ORA-12899值过大,ORA-01400不能插入空值等.虽然说这类异常是前端没控制到位的缘故,但是现实很难100 ...
- filter原理
index.jsp: <a href="product-input.action">input</a> <form action="prod ...
- 201621123014《Java程序设计》第十三周学习总结
1. 本周学习总结 以你喜欢的方式(思维导图.OneNote或其他)归纳总结多网络相关内容. 2. 为你的系统增加网络功能(购物车.图书馆管理.斗地主等)-分组完成 为了让你的系统可以被多个用户通过网 ...
- 做什么职业,也别做程序员,尤其是Java程序员
千万别做程序员,尤其别做Java这种门槛低,入门快的程序员(别跟我说Java搞精通了也很牛之类的,原因不解释,做5年以上就知道了),程序员本来就是我见过最坑爹的职业了...Java程序员更是,现在满地 ...
- BZOJ - 2142 礼物 (扩展Lucas定理)
扩展Lucas定理模板题(貌似这玩意也只能出模板题了吧~~本菜鸡见识鄙薄,有待指正) 原理: https://blog.csdn.net/hqddm1253679098/article/details ...
- Linux性能监控工具sysstat系列简介
简介 sysstat提供了Linux性能监控的工具集,包括sar.sadf.mpstat.iostat.pidstat等,这些工具可以监控系统性能和使用情况.各工具的作用如下: iostat - 提供 ...
- 蓝桥杯 算法训练 ALGO-141 P1102
算法训练 P1102 时间限制:1.0s 内存限制:256.0MB 定义一个学生结构体类型student,包括4个字段,姓名.性别.年龄和成绩.然后在主函数中定义一个结构体数组(长度不超过10 ...
- Servlet3.0之八:基于Servlet3.0的文件上传@MultipartConfig
在Servlet2.5中,我们要实现文件上传功能时,一般都需要借助第三方开源组件,例如Apache的commons-fileupload组件,在Servlet3.0中提供了对文件上传的原生支持,我们不 ...
- mariaDB 安装/卸载+启动/关闭 服务
1.设置环境变量 无论是用户环境变量还是系统环境变量 2.启动服务 进入根目录 名字根据 --install 后的 参数来决定 叫MariaDB,MySQL 都可以 mysqld.exe --inst ...