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 版本 ...
随机推荐
- 16_游戏编程模式ServiceLocator 服务定位
####简单说,就是某个系统作为一个服务,对全局系统可见. Service Locator (服务定位) ``` //简单粗暴的代码, 使用声音系统 // Use a static class? Au ...
- 浅谈FFT(快速傅里叶变换)
本文主要简单写写自己在算法竞赛中学习FFT的经历以及一些自己的理解和想法. FFT的介绍以及入门就不赘述了,网上有许多相关的资料,入门的话推荐这篇博客:FFT(最详细最通俗的入门手册),里面介绍得很详 ...
- [独孤九剑]Oracle知识点梳理(八)常见Exception
本系列链接导航: [独孤九剑]Oracle知识点梳理(一)表空间.用户 [独孤九剑]Oracle知识点梳理(二)数据库的连接 [独孤九剑]Oracle知识点梳理(三)导入.导出 [独孤九剑]Oracl ...
- ACM学习历程—HDU5666 Segment(数论)
http://acm.hdu.edu.cn/showproblem.php?pid=5666 这题的关键是q为质数,不妨设线段上点(x0, y0),则x0+y0=q. 那么直线方程则为y = y0/x ...
- swing之borderlayout
import java.awt.BorderLayout; import javax.swing.JButton; import javax.swing.JFrame; //1.继承 JFrame类 ...
- 微信小程序 request域名配置好之后,还是提示报错配置的域名不在request合法域名中
自己尝试着用nodejs搭个后台服务的时候,用了端口号,然后在小程序中使用的时候,报错说配置的域名不在request合法域名中 明明已经配置好了的啊,看着报错信息.仔细对比了一下两个url请求地址,发 ...
- Salesforce注册教程
打开网址 https://developer.salesforce.com/,点击右上角的Sign up(注册) 在页面输入基本信息 >Name:First(姓名) Last(姓) >E ...
- Springboot正常启动,但是访问404报错
原因: 查看是否配置文件中有以下配置: server.context-path=/hellopath 我这里是以/hellopath为例,如果有该配置的话,只能通过该路径访问到. 其他原因
- 解决windows下tomcat端口被占用[Address already in use: JVM_Bind]
有时候电脑比较卡,项目比较大的情况下,eclipse没有完全停止tomcat的情况下,下次启动会出现tomcat的端口被占用无法启动的情况,主要报如下错误 解决方案 window下打开命令窗口(快捷键 ...
- 在CentOS上安装Java开发环境:使用yum安装jdk
请参考百度经验:http://jingyan.baidu.com/article/4853e1e51d0c101909f72607.html 如果您阅读过此文章有所收获,请为我顶一个,如果文章中有错误 ...