How can I make the data consistently located at 90 degrees to each other + one degree step in RVIZ?

My task is to organize ultrasonic SLAM and for this I took three sensors located at 90 degrees and installed them on a servo drive. In sonars_data array of data - left sonar data, center sonar data, right sonar data.

The code is given below.

data_queue = Queue(3)

def callback(sonars_data):
    for i in sonars_data.data:
        item = i / 10

scan_time = 0.1

def laser():
    time_begin = rospy.Time.now()
    r = rospy.Rate(10)  # 10hz

    while not rospy.is_shutdown():
        time_end = rospy.Time.now()
        # Инициализация массива для данных с трех датчиков
        sonars_data = np.zeros(180)
        laser = LaserScan()
        for i in range(0, 180, 3):
            if not data_queue.empty():
                sonar_data = data_queue.get()
                sonars_data[i] = sonar_data

            laser.header.stamp = time_end
            laser.header.frame_id = 'sonar'
            laser.angle_min = 0
            laser.angle_max = np.pi
            laser.angle_increment = np.pi/ 180
            laser.time_increment = 0.001
            laser.scan_time = scan_time
            laser.range_min = 0.1
            laser.range_max = 100
            laser.ranges = sonars_data

    if i == 180:

Expected result

If this is not possible, please advise how to do it differently.



Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.