Remark: It is obligatory to follow laboratory safety rules, especially while operating robots
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
scan
topic.
ros2_lidar_subscriber.py
to read data from Hokuyo laser scanner
#!/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()
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)