ROS2机器人编程简述humble-第三章-BUMP AND GO BEHAVIOR IN PYTHON .4_ros2机器狗趴下代码

前一篇，书中介绍了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.修改“凹凸”（避障）项目，使机器人准确地转向无障碍物或更远的障碍物的角度。尝试两种方法：

•开环：转弯前计算转弯时间和速度。

•闭环：旋转，直到检测到前方的没有障碍物（开阔区域）。