Mobile Robots – laboratory 1 – Introduction

Remark: It is obligatory to follow laboratory safety rules, especially while operating robots

Introduction

The goal of this assignment is to get familiar with ROS environment in the laboratory, robots and reading robot sensors with scripts in python.

The assignment can be solved using standard ROS2 subscriber/publisher approach.

Pioneer robots connected to it create their own namespaces named
/pioneerX, where X is the robot number. You may list available topics and observe data send in any topic using
ros2 topic list ros2 topic echo topicname

The format of data packets sent in a topic may be checked with
ros2 topic info topicname ros2 interface show msgtype

If you want a general information about the data transmitted in a topic use
ros2 topic hz topicname
ros2 topic bw topicname

Task 1: Getting familiar with topics related to Pioneer robots

List the topics available in the running ROS2 system.
Find all topics related with a single robot
Check the message format of selected topics, observe the data transmitted in the topic and try to determine the meaning of the data

Task 2: Accessing data from laser scanner

Check the format of the messages in scan topic.
Modify and run sample code ros2_lidar_subscriber.py to read data from Hokuyo laser scanner

Task 3: Data visualization

Use the code from visualization example to plot data using matplotlib (add calculations marked in "TODO" comments to run the example)
Convert data from sensors to a Cartesian coordinates and show them in a plot

Report

The report should be sent by email in pdf format before the next classes and it should contain

Description of a single topic published by a robot which were analyzed in T1 (data format, message components, frequency and bandwidth)
Final code used in T3 with exemplary plots.

Sample code

ROS2 subscriber example (ros2_lidar_subscriber.py)

#!/usr/bin/env python

# run: python3 subscriber_scan.py 5  (where 5 is robot number)
import sys
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan

def callback_scan(msg):
    ## extract ranges from message
    scan=list(msg.ranges)
    print("  Scan min: %f  front: %f" % ( min(scan),scan[362]))
    print ()


def main(args=None):
    if len(sys.argv) < 2:
        sys.stderr.write('Usage: sys.argv[0] \n')
        sys.exit(1)
    rclpy.init()
    nr=sys.argv[1]

    node = Node('listener')

    # Subscribe topics and bind with callback functions
    node.create_subscription(LaserScan, f"/pioneer{nr}/scan", callback_scan, 10)

    # spin(node) simply keeps python from exiting until this node is stopped
    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Visualization example

import math
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as mpatches

from time import sleep

loop_count=25  # drawing loop iterations
beam_half_angle=7.5 # half of angular beam width

# 
# A function to calculate Cartesian coordinates to polar
#  result: a tuple (rho,phi)
#  rho - radius, phi - angle in degrees
def cart2pol(x, y):
    #TODO: calculations
    return(rho, phi)

# A function to transform polar  coordinates to Cartesian
# input angle in degrees
# returns a tuple (x,y)
def pol2cart(rho, phi):
    #TODO: calculations
    return(x, y)

# plotting data
def plotwedges(ax,data):
    pol=[cart2pol(x[0],x[1]) for x in data ]
    for item in pol:
        #print item[0],item[1]
        wedge = mpatches.Wedge([0,0], item[0], item[1]-beam_half_angle, item[1]+beam_half_angle, alpha=0.4, ec="black",fc="CornflowerBlue")
        ax.add_patch(wedge)

def plotarrows(ax,arrlist):
    y=[[0,0]+x for x in arrlist ]
    soa =np.array(y) 
    X,Y,U,V = zip(*soa)
    ax.quiver(X,Y,U,V,angles='xy',scale_units='xy',scale=1)


plt.figure()
ax = plt.gca()
ax.set_aspect('equal')
ax.set_xlim([-6,6])
ax.set_ylim([-6,6])
plt.ion()
plt.show()

for i in range(loop_count):
    skan=[[1,0],[1,1],[1.5,0.8]]
    ax.cla()
    plotwedges(ax,skan)  
    plotarrows(ax,skan)
    ax.set_xlim([-6,6])
    ax.set_ylim([-6,6])
    plt.draw()
    plt.pause(0.0001)
    sleep(0.2)

References

ROS2 CLI tools

Writing a simple publisher and subscriber (Python)