ROS2机器人编程简述humble-第三章-BUMP AND GO BEHAVIOR IN PYTHON .4

zhangrelay

发布于 2023-02-10 09:34:14

5450

发布于 2023-02-10 09:34:14

前一篇，书中介绍了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