ROSでLidarセンサーを使う時、ロボットの車体など一定角度の範囲に常に障害物があり、これを無視したい時があると思いますが、その方法を紹介します。

以下は、得られた/scan_rawトピックのデータの一定角度の範囲を無視して新しい/scanトピックを発行する例を紹介します。

※Ubuntu 18.04 ROS melodicにて動作確認済みです。

 

#!/usr/bin/python
# -*- coding: utf-8 -*-

import rospy
import numpy as np
from sensor_msgs.msg import LaserScan

scan_publisher = None

def convert_scan(data):
    global scan_publisher

    ignore_angle_min = -0.523599
    ignore_angle_max = 0.523599

    # 新しいLaserScanメッセージを作成します。
    filtered_scan = LaserScan()
    filtered_scan.header = data.header
    filtered_scan.angle_min = data.angle_min
    filtered_scan.angle_max = data.angle_max
    filtered_scan.angle_increment = data.angle_increment
    filtered_scan.time_increment = data.time_increment
    filtered_scan.scan_time = data.scan_time
    filtered_scan.range_min = data.range_min
    filtered_scan.range_max = data.range_max

    # 無視する範囲のデータを除外します。
    for i, angle in enumerate(np.arange(data.angle_min, data.angle_max, data.angle_increment)):
        if angle > ignore_angle_min and angle < ignore_angle_max:
            filtered_scan.ranges.append(float('nan'))
        else:
            filtered_scan.ranges.append(data.ranges[i])
            
    # フィルタリング後のデータをパブリッシュします。
    scan_publisher.publish(filtered_scan)


if __name__ == '__main__':
    rospy.init_node('ignore_back_scan', log_level=rospy.DEBUG)

    rospy.Subscriber('/scan_raw', LaserScan, convert_scan)
    scan_publisher = rospy.Publisher('/scan', LaserScan, queue_size=10)

    r = rospy.Rate(10)
    while not rospy.is_shutdown():
        r.sleep()

上記の例では、-30度から30度の範囲を除外するようにフィルタリングしています。無視する角度の範囲は、必要に応じて変更することができます。また、Subscribe/Publishするトピック名ももちろん変更することが可能です。

Ros

Related Posts

ArduinoのpulseInで割り込み処理ができるようにする
ArduinoのpulseInで割り込み処理ができるようにする
概要 超音波センサーHC-SR04等を用いてArduinoで距離計測を行いたいときに、Trigピンで超音波を発生させた後にEchoピンで、超音波が反射されてくるまでの時間を計測することで距離を計測できます。その際、pulseInという...
Read More
Arduino IDEのインストール方法と使い方
Arduino IDEのインストール方法と使い方
概要 Arduinoは入出力ポートを備えたマイコンボードで、ソースコードを書くことで、さまざまなハードウェアを制御することができます。 ソースコードを開発・ボードに書き込むためには開発環境であるArduino IDEのインストールが必...
Read More
WindowsでArduino Uno互換ボードに書き込む時にシリアルポートが選択できない時の対処法
WindowsでArduino Uno互換ボードに書き込む時にシリアルポートが選択できない時の対処法
概要 Arduino Uno R3の互換ボードを用いる時、WindowsでArduino IDEを開き、シリアルポートを繋げようとした際にグレーアウトされてしまって選択することができない場合があるので、解決方法について解説をします。 ...
Read More