ROS2机器人编程简述humble-第三章-BUMP AND GO BEHAVIOR IN PYTHON .4
2023-03-07 09:06:06 时间
前一篇,书中介绍了C++实现方式。在这一节,主要使用Python。
ROS2机器人编程简述humble-第三章-BUMP AND GO IN C++ .3
除了C++,Python是ROS2通过rcppy客户端库正式支持的语言之一。本节将再现在上一节中所做的,但使用Python。通过比较验证两种语言发展过程中的差异和相似性。此外,在前面的章节中解释了ROS2的原理,将认识到Python代码中ROS2的元素,因为原理是相同的。
cpp:
python:
main-cpp:
#include <memory>
#include "br2_fsm_bumpgo_cpp/BumpGoNode.hpp"
#include "rclcpp/rclcpp.hpp"
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto bumpgo_node = std::make_shared<br2_fsm_bumpgo_cpp::BumpGoNode>();
rclcpp::spin(bumpgo_node);
rclcpp::shutdown();
return 0;
}
main-python:
from geometry_msgs.msg import Twist
import rclpy
from rclpy.duration import Duration
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from rclpy.time import Time
from sensor_msgs.msg import LaserScan
class BumpGoNode(Node):
def __init__(self):
super().__init__('bump_go')
self.FORWARD = 0
self.BACK = 1
self.TURN = 2
self.STOP = 3
self.state = self.FORWARD
self.state_ts = self.get_clock().now()
self.TURNING_TIME = 2.0
self.BACKING_TIME = 2.0
self.SCAN_TIMEOUT = 1.0
self.SPEED_LINEAR = 0.3
self.SPEED_ANGULAR = 0.3
self.OBSTACLE_DISTANCE = 1.0
self.last_scan = None
self.scan_sub = self.create_subscription(
LaserScan,
'input_scan',
self.scan_callback,
qos_profile_sensor_data)
self.vel_pub = self.create_publisher(Twist, 'output_vel', 10)
self.timer = self.create_timer(0.05, self.control_cycle)
def scan_callback(self, msg):
self.last_scan = msg
def control_cycle(self):
if self.last_scan is None:
return
out_vel = Twist()
if self.state == self.FORWARD:
out_vel.linear.x = self.SPEED_LINEAR
if self.check_forward_2_stop():
self.go_state(self.STOP)
if self.check_forward_2_back():
self.go_state(self.BACK)
elif self.state == self.BACK:
out_vel.linear.x = -self.SPEED_LINEAR
if self.check_back_2_turn():
self.go_state(self.TURN)
elif self.state == self.TURN:
out_vel.angular.z = self.SPEED_ANGULAR
if self.check_turn_2_forward():
self.go_state(self.FORWARD)
elif self.state == self.STOP:
if self.check_stop_2_forward():
self.go_state(self.FORWARD)
self.vel_pub.publish(out_vel)
def go_state(self, new_state):
self.state = new_state
self.state_ts = self.get_clock().now()
def check_forward_2_back(self):
pos = round(len(self.last_scan.ranges) / 2)
return self.last_scan.ranges[pos] < self.OBSTACLE_DISTANCE
def check_forward_2_stop(self):
elapsed = self.get_clock().now() - Time.from_msg(self.last_scan.header.stamp)
return elapsed > Duration(seconds=self.SCAN_TIMEOUT)
def check_stop_2_forward(self):
elapsed = self.get_clock().now() - Time.from_msg(self.last_scan.header.stamp)
return elapsed < Duration(seconds=self.SCAN_TIMEOUT)
def check_back_2_turn(self):
elapsed = self.get_clock().now() - self.state_ts
return elapsed > Duration(seconds=self.BACKING_TIME)
def check_turn_2_forward(self):
elapsed = self.get_clock().now() - self.state_ts
return elapsed > Duration(seconds=self.TURNING_TIME)
def main(args=None):
rclpy.init(args=args)
bump_go_node = BumpGoNode()
rclpy.spin(bump_go_node)
bump_go_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
可以看到Python,基本一个全包了……
之前C++,分别为头文件,功能实现和主程序等。
launch:
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
kobuki_cmd = Node(package='br2_fsm_bumpgo_py',
executable='bump_go_main',
output='screen',
parameters=[{
'use_sim_time': True
}],
remappings=[
('input_scan', '/scan_raw'),
('output_vel', '/nav_vel')
])
ld = LaunchDescription()
ld.add_action(kobuki_cmd)
return ld
没有了CMakelist但需要setup:
from glob import glob
import os
from setuptools import setup
package_name = 'br2_fsm_bumpgo_py'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py'))
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='fmrico',
maintainer_email='fmrico@gmail.com',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'bump_go_main = br2_fsm_bumpgo_py.bump_go_main:main'
],
},
)
具体内容参考书中介绍。
剩下的几乎一致了。
为了运行程序,首先通过在终端中键入以下命令启动模拟程序:
$ ros2 launch br2 tiago sim.launch.py
打开另一个终端,然后运行程序:
$ ros2 run br2 fsm bumpgo py bump go main --ros-args -r output vel:=/nav vel -r
input scan:=/scan raw -p use sim time:=true
还可以使用类似于C++版本的启动器,只需键入:
$ ros2 launch br2 fsm bumpgo py bump and go.launch.py
建议的练习:
1.修改Bump and Go项目,使机器人感知到前方左右对角线上的障碍物。它不是总是转向同一侧,而是转向没有障碍物的一侧。
2.修改“凹凸”(避障)项目,使机器人准确地转向无障碍物或更远的障碍物的角度。尝试两种方法:
•开环:转弯前计算转弯时间和速度。
•闭环:旋转,直到检测到前方的没有障碍物(开阔区域)。
相关文章
- Cloudera最新研究:平衡ESG与利润,引领企业可持续发展
- Kali Linux 发布今年最后一个版本
- 推荐!从零开发一套基于React的加载动画库
- Bash 初学者系列 7:bash 中的条件语句(if else)
- 竟然将线程安全讲的如此清新脱俗,谈谈你对线程安全性的理解!
- 谷歌 Chrome 108 浏览器桌面端新增省内存 / 省电模式,提高新能 / 延长续航
- Podman 部署私有镜像仓库,你学会了吗?
- 写出个灵活的系统竟然可以如此简单!小白也能写出高级的Java业务!
- 绕过使用大数据的保护系统是否困难?
- 理解浏览器重绘和回流
- 静态分析器 Clang Static Analyzer (2) CodeChecker
- 如何更改 Ubuntu 的登录屏幕背景
- Bash 初学者系列 6:bash 中的字符串操作
- 成功进行数据迁移的策略
- LiteOS-A内核中的procfs文件系统分析
- Vue3,用组合编写更好的代码:动态返回(3/4)
- 从实现原理来讲,Nacos 为什么这么强?
- Gnoppix Linux 22.12 发布
- 开发大数据应用程序企业的四个成功要素
- 掌握强大的 Git 变基命令