Foxy 用の auto.py (参考)

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
import sys
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from rclpy.qos import qos_profile_sensor_data
from rclpy.clock import Clock, ClockType
from rclpy.time import Duration
import numpy as np

class PublisherSubscriber(Node):
    def __init__(self):
        super().__init__('publisher_subscriber')
        self.publisher = self.create_publisher(Twist, 'cmd_vel', 10)

        self.subscription = self.create_subscription(
            LaserScan,
            'scan',
            self.subscription_callback,
            qos_profile=qos_profile_sensor_data)
        self.subscription  # prevent unused variable warning

        self.move_cmd = Twist()
        self.rotate_cmd = Twist()
        self.stop_cmd = Twist()

        self.move_cmd.linear.x = 0.2
        self.move_cmd.linear.y = 0.0
        self.move_cmd.linear.z = 0.0
        self.move_cmd.angular.x = 0.0
        self.move_cmd.angular.y = 0.0
        self.move_cmd.angular.z = 0.0

        self.rotate_cmd.linear.x = 0.0
        self.rotate_cmd.linear.y = 0.0
        self.rotate_cmd.linear.z = 0.0
        self.rotate_cmd.angular.x = 0.0
        self.rotate_cmd.angular.y = 0.0
        self.rotate_cmd.angular.z = 0.2

        self.distance = 100

    def __del__(self):
        self.publisher.publish(self.stop_cmd)

    def subscription_callback(self, data):
        data_float = np.array(data.ranges)
        data_float = np.where(data_float<data.range_min, data.range_max, data_float)
        self.distance = min(min(data_float[0:30]), min(data_float[330:360]))

        if self.distance < 0.5:
            self.publisher.publish(self.rotate_cmd)
        else:
            self.publisher.publish(self.move_cmd)

def main():
    rclpy.init(args=sys.argv)
    pubsub = PublisherSubscriber()
    rclpy.spin(pubsub)

    pubsub.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()