0
$\begingroup$

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
        data_queue.put(item)


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):
            pub_servo.publish(i)
            time.sleep(scan_time)
            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

            pub_laser.publish(laser)
    if i == 180:
        pub_servo.publish(0)
    r.sleep()

Expected result

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

$\endgroup$

0

Your Answer

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