Lab 3 - Struktury danych w planowaniu ruchu (cz. 2)

Logotyp PP Logo IRIM

Metody i algorytmy planowania ruchu - laboratorium

Lab 3 - Struktury danych w planowaniu ruchu - dostęp do mapy wysokościowej i OctoMap z poziomu kodu

humble

1. Wprowadzenie

Celem zajęć jest poznanie mechanizmów subskrybowania danych z topików, które przechowują informację o modelu otoczenia robota. Testowana będzie mapa wysokościowa i Octomap.

⚠️ Pamiętaj, aby w każdym nowym terminalu zanim rozpoczniesz pracę skonfigurować środowisko ROS komendą
source /opt/ros/humble/setup.bash lub source install/setup.bash

2. Mapy wysokościowe - elevation map (ROS 1 Noetic)

Na początek przejdziemy przez te same kroki co na poprzednich zajęciach w celu uruchomienia symulacji z mapą wysokościową.

Przykładowe tworzenie i wyświetlanie mapy wysokościowej pokazane zostanie na przykładzie paczki: https://github.com/ANYbotics/elevation_mapping

Paczka ta nie została jeszcze w pełni zaimplementowana w ROS 2, dlatego tę część instrukcji (pkt. 2.) wykonać należy w ROS 1 (w wersji Noetic). W tym celu proszę skorzystać z przygotowanego obrazu w dockerze (obraz ma już zainstalowane wymagane pakiety ROSa).

Jeśli korzystasz z komputera w sali lab. 321, użyj komendy docker images, aby sprawdzić, czy na liście jest obraz o nazwie ros1_miapr. Jeśli nie ma takiego obrazu, pobierz plik tar z obrazem i go wczytaj:

mkdir -p ~/miapr && cd ~/miapr

pip install --upgrade --no-cache-dir gdown

test -f ros1_img.tar || python3 -m gdown "https://drive.google.com/uc?id=1xMOBmKESodcqaL6PYAT1zSdC5IGLToSP&confirm=t" 

docker load -i ros1_img.tar

Później uruchom kontener poleceniem:

xhost + && docker run -it \
    --env="DISPLAY" \
    --env="QT_X11_NO_MITSHM=1" \
    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
    --net=host \
    --privileged \
    --gpus=all \
    --name=ros1_miapr_lab2 \
    ros1_miapr:latest

Aby otworzyć kolejny terminal w dockerze, należy korzystać z polecenia:

docker exec -it ros1_miapr_lab2 bash

Poniższe komendy wykonuj w kontenerze. Pobierz biblioteki z repozytorium github:

cd /home/catkin_ws/src
git clone https://github.com/ANYbotics/elevation_mapping
git clone https://github.com/ANYbotics/kindr.git
git clone https://github.com/ANYbotics/kindr_ros.git
git clone https://github.com/ANYbotics/message_logger.git 
git clone https://github.com/ANYbotics/point_cloud_io.git

Następnie skompiluj pobraną paczkę i odśwież przestrzeń roboczą:

cd /home/catkin_ws
source /opt/ros/noetic/setup.bash
catkin_make
source devel/setup.bash

Uruchom symulację poleceniem i czekaj (uruchomienie symulacji po raz pierwszy może zająć kilka minut):

roslaunch elevation_mapping_demos turtlesim3_waffle_demo.launch

Powinien pojawić się robot turtlebot3 waffle w środowisku Gazebo. Aby sterować robotem z klawiatury, należy użyć modułu turtlebot3_teleop (w osobnym terminalu):

export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

Sterowanie robotem odbywa się za pomocą klawiszy ‘a’, ‘s’, ‘d’, ‘w’, ‘x’. Podczas poruszania się robota powinna budować się mapa wysokościowa terenu. Aktualizacja mapy może zająć około 60 s, dlatego należy poruszać się z niewielką prędkością.

2.1 Dostęp do komórek mapy wysokościowej z poziomu kodu

Uruchom symulację robota Turtlebot3 z mapą wysokościową:

roslaunch elevation_mapping_demos turtlesim3_waffle_demo.launch

Po uruchomieniu modułu do budowy mapy wysokościowej sprawdź format danych zapisanych w topiku /elevation_mapping/elevation_map_raw poleceniem:

rostopic info /elevation_mapping/elevation_map_raw

Dokumentacja typu grid_map_msgs/GridMap znajduje się pod adresem: http://docs.ros.org/kinetic/api/grid_map_msgs/html/msg/GridMap.html

Dane dotyczące mapy przechowywane są w strukturze std_msgs/Float32MultiArray : http://docs.ros.org/kinetic/api/std_msgs/html/msg/Float32MultiArray.html

Struktura ta umożliwia przechowywanie wielowarstwowych tablic wielowymiarowych. W przypadku mapy wysokościowej mamy dwie warstwy - elevation i variance. Indeksowanie danych jest nietypowe i pokazane zostało na przykładzie zadania poniżej.

🔨 Zadanie 2.1.1

W katalogu catkin_ws/src utwórz nową paczkę o nazwie elevation_map_io:

catkin_create_pkg elevation_map_io rospy grid_map_msgs

Następnie utwórz w niej katalog scripts, a w nim stwórz plik invert_map.py i dodaj mu prawa do wykonywania. Skompiluj workspace.

Napisz węzeł (w utworzonym skrypcie), który zmieni znak wysokości każdej z komórek oryginalnej mapy i wynik opublikuj w nowej mapie. Ponieważ mapa wysokościowa może działać stosunkowo wolno, poniżej podane jest rozwiązanie. Uruchom przykład poleceniem rosrun i wyświetl uzyskaną mapę (topik /map_copy):

#!/usr/bin/env python3
import rospy
from grid_map_msgs import msg # import occupancy grid data

map_cpy = msg.GridMap()

def callback(elev_map):
    #print available layers
    for n in elev_map.basic_layers:
        print(n)
    # access the first layer (index 0) - elevation
    stride0 = elev_map.data[0].layout.dim[0].stride
    stride1 = elev_map.data[0].layout.dim[1].stride
    # dimension
    cols = elev_map.data[0].layout.dim[0].size
    rows = elev_map.data[0].layout.dim[1].size
    # offset
    offset = elev_map.data[0].layout.data_offset
    map_cpy.data = elev_map.data
    # create list to edit tuple
    data_tmp = list(map_cpy.data[0].data)
    for i in range(cols):
        for j in range(rows):
            data_tmp[offset + i + stride1 * j + 0] = -elev_map.data[0].data[offset + i + stride1 * j + 0]
    map_cpy.info = elev_map.info
    map_cpy.layers = elev_map.layers
    map_cpy.basic_layers = elev_map.basic_layers
    map_cpy.outer_start_index = elev_map.outer_start_index
    map_cpy.inner_start_index = elev_map.inner_start_index
    # copy to tuple
    map_cpy.data[0].data = tuple(data_tmp)

def mapListener():
    #create node
    rospy.init_node('map_listener', anonymous=True)
    #subscribe /map topic
    rospy.Subscriber("/elevation_mapping/elevation_map_raw", msg.GridMap, callback)
    # spin() simply keeps python from exiting until this node is stopped
    pub = rospy.Publisher('map_copy', msg.GridMap, queue_size=10)

    rate = rospy.Rate(1)  # 1hz
    while not rospy.is_shutdown():
        #publish copy of the map
        pub.publish(map_cpy)
        rate.sleep()

if __name__ == '__main__':
    mapListener()

3. Dostęp do komórek mapy OctoMap (ROS 2 Humble)

Zakończ działanie wszystkich węzłów z poprzedniego zadania. W dalszej części instrukcji będziemy pracować na ROS 2 (bez dockera). Aby uruchomić bibliotekę Octomap, należy ją wcześniej zainstalować z repozytorium:

sudo apt-get install ros-humble-octomap ros-humble-octomap-server python3-vcstool  ros-humble-turtlebot3*  ros-humble-sensor-msgs-py

Następnie pobierz repozytorium zawierające skrypty uruchomieniowe oraz przykładowe mapy:

cd ~/ros2_ws/src
git clone --branch humble https://github.com/dominikbelter/example_maps
git clone https://github.com/iKrishneel/octomap_server2.git
cd octomap_server2 && vcs import . < deps.repos

Skompiluj wszystkie paczki znajdujące się w przestrzeni roboczej:

cd ~/ros2_ws/
colcon build --symlink-install

⚠️ W przypadku błędu w kompilacji:

fatal error: message_filters/message_event.hpp: No such file or directory
   44 | #include <message_filters/message_event.hpp>

należy przejść do katalogu /home/student/ros2_ws/src/perception_pcl i cofnąć się do jednego z wcześniejszych commitów:

git checkout -b new_branch 44d02743e451296d6bc871b101cea33c59adc1d6

Jeżeli kompilacja zakończyła się sukcesem, należy zmodyfikować model robota Turtlebot3, tak aby korzystał z kamery głębi. W tym celu otworzyć plik:

sudo gedit /opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf

I dokonać zmian w liniach:

line 369:    <sensor name="camera" type="depth">
line 400:    <frame_name>camera_rgb_optical_frame</frame_name>

Teraz można uruchomić symulację robota Turtlebot3 wraz z systemem do budowy mapy:

source install/setup.bash
export TURTLEBOT3_MODEL=waffle
ros2 launch example_maps turtlesim3_waffle_octomap.launch.py

Do sterowania robotem wykorzystamy moduł turtlebot3_teleop (w osobnym terminalu):

cd ~/ros2_ws
source install/setup.bash
export TURTLEBOT3_MODEL=waffle
ros2 run turtlebot3_teleop teleop_keyboard

Aby dostać się do danych przechowywanych w mapie OctoMap, wykorzystamy publikowany przez ten moduł topik o nazwie /octomap_point_cloud_centers. W tym topiku przechowywane są środki wokseli, które są zajęte. Sprawdź typ danych poleceniem:

ros2 topic info /octomap_point_cloud_centers

Typ sensor_msgs/PointCloud2 opisany jest pod adresem: https://docs.ros2.org/galactic/api/sensor_msgs/msg/PointCloud2.html

🔨 Zadanie 3.1

Utwórz paczkę w katalogu ~/ros2_ws/src o nazwie octomap_manipulation (analogicznie jak na poprzednich zajęciach, do dependencies dodaj sensor_msgs). Wykorzystując poniższy szablon projektu, napisz węzeł odczytujący i wyświetlający w terminalu współrzędne zajętych voxeli:

import rclpy
from rclpy.node import Node
import numpy as np
from sensor_msgs.msg import PointCloud2
from sensor_msgs_py import point_cloud2

class OctomapSubscriber(Node):
    def __init__(self):
        super().__init__('octomap_subscriber')
        self.subscription = self.create_subscription(
            PointCloud2,
            'octomap_point_cloud_centers',
            self.listener_callback,
            10)
        self.subscription  # prevent unused variable warning

    def listener_callback(self, cloud_msg):
        self.get_logger().info('I heard a pointcloud')
        gen = point_cloud2.read_points(cloud_msg)
        print(type(gen))
        # TODO:  print x y z

def main(args=None):
    rclpy.init(args=args)
    octomap_subscriber = OctomapSubscriber()
    rclpy.spin(octomap_subscriber)
    octomap_subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

4. Wyświetlanie zadanej ścieżki w RViz

Podczas planowania ruchu przydatne jest wyświetlanie zaplanowanej ścieżki. Wykorzystamy do tego typ nav_msgs/Path: https://docs.ros2.org/foxy/api/nav_msgs/msg/Path.html

Uruchom przykład poniżej i wyświetl ścieżkę z topiku /my_path w RViz (wpisz odpowiedni frame_id w Global Options -> Fixed Frame).

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
import numpy as np

class PathPublisher(Node):
    def __init__(self):
        super().__init__('path_publisher')
        self.publisher_ = self.create_publisher(Path, 'my_path', 10)
        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)

    def timer_callback(self):
        path = Path()
        path.header.frame_id = 'odom'

        # create path
        pose1 = PoseStamped()
        pose1.pose.position.x = 0.0
        pose1.pose.position.y = 0.0
        pose1.pose.position.z = 0.0
        #orientation defined as a quaternion
        pose1.pose.orientation.x = 0.0
        pose1.pose.orientation.y = 0.0
        pose1.pose.orientation.z = 0.0
        pose1.pose.orientation.w = 1.0
        pose1.header.frame_id = 'odom'
        pose1.header.stamp = self.get_clock().now().to_msg()
        path.poses.append(pose1)

        pose2 = PoseStamped()
        pose2.pose.position.x = -1.0
        pose2.pose.position.y = -1.0
        pose2.pose.position.z = 1.0
        # orientation defined as a quaternion
        pose2.pose.orientation.x = 0.0
        pose2.pose.orientation.y = 0.0
        pose2.pose.orientation.z = 0.0
        pose2.pose.orientation.w = 1.0
        pose2.header.frame_id = 'odom'
        pose2.header.stamp = self.get_clock().now().to_msg()
        path.poses.append(pose2)

        #publish path
        self.publisher_.publish(path)
        self.get_logger().info('Publishing path')

def main(args=None):
    rclpy.init(args=args)
    path_publisher = PathPublisher()
    rclpy.spin(path_publisher)
    path_publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

🔨 Zadanie 4.1

Zmodyfikuj przykład tak, aby w RViz wyświetlał się okrąg.

path