如何将激光雷达中的激光扫描数据拆分为多个部分并在 rviz 上查看

Question

我试图将激光扫描范围数据拆分为子类别，并希望将每个类别发布到不同的激光主题中。

要指定更多，脚本应该获取一个主题作为输入 - /scan 并且脚本应该发布三个主题，如下所示 = scan1、scan2、scan3

有没有办法拆分激光扫描并发布回来并在 rviz 上查看它们

我尝试了以下

def callback(laser):
    current_time = rospy.Time.now()
    regions["l_f_fork"] = laser.ranges[0:288]
    regions["l_f_s"] = laser.ranges[289:576]
    regions["stand"] = laser.ranges[576:864]
    l.header.stamp = current_time
    l.header.frame_id = 'laser'
    l.angle_min = 0
    l.angle_max = 1.57
    l.angle_increment =0
    l.time_increment = 0
    l.range_min = 0.0
    l.range_max = 100.0
    l.ranges = regions["l_f_fork"]
    l.intensities = [0]

    left_fork.publish(l)

    # l.ranges = regions["l_f_s"]
    # left_side.publish(l)

    # l.ranges = regions["stand"]
    # left_side.publish(l)

rospy.loginfo("publishing new info")

我在rviz上可以看到不同的主题，但它们是在同一条线上，

Answer 1

教程

• 以下代码将 LaserScan 数据分成三个相等的部分：

  #! /usr/bin/env python3
  """
  Program to split LaserScan into three parts.
  """
  
  import rospy
  from sensor_msgs.msg import LaserScan
  
  
  class LaserScanSplit():
      """
      Class for splitting LaserScan into three parts.
      """
  
      def __init__(self):
  
          self.update_rate = 50
          self.freq = 1./self.update_rate
  
          # Initialize variables
          self.scan_data = []
  
          # Subscribers
          rospy.Subscriber("/scan", LaserScan, self.lidar_callback)
  
          # Publishers
          self.pub1 = rospy.Publisher('/scan1', LaserScan, queue_size=10)
          self.pub2 = rospy.Publisher('/scan2', LaserScan, queue_size=10)
          self.pub3 = rospy.Publisher('/scan3', LaserScan, queue_size=10)
  
          # Timers
          rospy.Timer(rospy.Duration(self.freq), self.laserscan_split_update)
  
      def lidar_callback(self, msg):
          """
          Callback function for the Scan topic
          """
          self.scan_data = msg
  
      def laserscan_split_update(self, event):
          """
          Function to update the split scan topics
          """
  
          scan1 = LaserScan()
          scan2 = LaserScan()
          scan3 = LaserScan()
  
          scan1.header = self.scan_data.header
          scan2.header = self.scan_data.header
          scan3.header = self.scan_data.header
  
          scan1.angle_min = self.scan_data.angle_min
          scan2.angle_min = self.scan_data.angle_min
          scan3.angle_min = self.scan_data.angle_min
  
          scan1.angle_max = self.scan_data.angle_max
          scan2.angle_max = self.scan_data.angle_max
          scan3.angle_max = self.scan_data.angle_max
  
          scan1.angle_increment = self.scan_data.angle_increment
          scan2.angle_increment = self.scan_data.angle_increment
          scan3.angle_increment = self.scan_data.angle_increment
  
          scan1.time_increment = self.scan_data.time_increment
          scan2.time_increment = self.scan_data.time_increment
          scan3.time_increment = self.scan_data.time_increment
  
          scan1.scan_time = self.scan_data.scan_time
          scan2.scan_time = self.scan_data.scan_time
          scan3.scan_time = self.scan_data.scan_time
  
          scan1.range_min = self.scan_data.range_min
          scan2.range_min = self.scan_data.range_min
          scan3.range_min = self.scan_data.range_min
  
          scan1.range_max = self.scan_data.range_max
          scan2.range_max = self.scan_data.range_max
          scan3.range_max = self.scan_data.range_max
  
          # LiDAR Range
          n = len(self.scan_data.ranges)
  
          scan1.ranges = [float('inf')] * n
          scan2.ranges = [float('inf')] * n
          scan3.ranges = [float('inf')] * n
  
          # Splitting Block [three equal parts]
          scan1.ranges[0 : n//3] = self.scan_data.ranges[0 : n//3]
          scan2.ranges[n//3 : 2*n//3] = self.scan_data.ranges[n//3 : 2*n//3]
          scan3.ranges[2*n//3 : n] = self.scan_data.ranges[2*n//3 : n]
  
          # Publish the LaserScan
          self.pub1.publish(scan1)
          self.pub2.publish(scan2)
          self.pub3.publish(scan3)
  
      def kill_node(self):
          """
          Function to kill the ROS node
          """
          rospy.signal_shutdown("Done")
  
  if __name__ == '__main__':
  
      rospy.init_node('laserscan_split_node')
      LaserScanSplit()
      rospy.spin()
  

• 以下是 Gazebo 和 RViz 环境中机器人和障碍物的截图：

参考资料：

• ROS1 Python Boilerplate

• atreus