Mobile Robots – lab2

Introduction

The goal of this assignment is to observe properties of odometry in a robot localization task. Before the class, one should prepare: find in the documentation the dimensions of the robot (necessary for odometry) and the formulas needed to calculate odometry with the available data.

Task 1 – implementation

Verify the dimensions of the robot with measurements, estimate the accuracy of the measurements and its influence on odometry.
Implement odometric robot localization. The program should calculate the position and orientation of the robot (x, y, theta) based on the data from the encoders in two versions: using measured positions or speeds.

Use files containing preprocessed datasets with predefined movements to develop the algorithm. A snippet to read a CSV file is in the sample code section.
Write a subscriber to read encoder data from the robot (/pioneerX/joint_states topic. Test your odometry calculation on a robot.
Remark 1: Check the type of the messages in the topic and the units used; check other issues with data format.

Remark 2: The suggested order of testing and tuning the algorithm with data files is: forward/backward motion, right/left rotation in place, and movement along the square.
Plan experiments to determine odometry error.
- Consider which factors you want to observe.
- Plan the trajectory.
- Make an experiment and collect the results

Task 2 – verification

In the verification part, use both the data collected from the robot during the experiments and the rosbags provided.

!!!Caution!!! Do not replay the provided rosbags in the laboratory with the original topic names. Use the mapping option as in: ros2 bag play odom_backward -m /pioneer5/odom:=/bagodom /pioneer5/joint_states:=/bagjoint_states

Compare the pose calculated with your script from task 1 with the data received in the /pioneerX/odom topic.
Assuming that the value read from the /pioneerX/odom topic is the exact location of the robot – estimate the odometry error

Report

The report should be sent by email in pdf format and should contain

Formulas for determining robot pose based on positions and velocities, together with the list of parameters necessary for calculations.
Estimated theoretical measurement and odometry errors.
Observations and comments from task 2.
Summary of verification: experiments used for tests and the evaluation of odometry error.

Sample code

Time measurement

Standard approach, using system time – It is discouraged to use it in processing ROS messages

import datetime
a=datetime.datetime.now()
b=datetime.datetime.now()
c=b-a
print(c,c.seconds,c.microseconds)
print(c.total_seconds())

Reading CSV file

import csv

with open('forward.csv') as csvfile:
    reader = csv.DictReader(csvfile, delimiter=';', quotechar='|')
    for row in reader:        
        print row['pos_l'], row['pos_r']

Obtaining pose from odometry

import numpy as np
from nav_msgs.msg import Odometry

  ...

def yaw_from_quaternion(quaternion):
    """
    Converts quaternion (w in last place) to yaw angle
    quaternion = [x, y, z, w]

    modified from
    https://gist.github.com/salmagro/2e698ad4fbf9dae40244769c5ab74434
    """
    x = quaternion.x
    y = quaternion.y
    z = quaternion.z
    w = quaternion.w

    siny_cosp = 2 * (w * z + x * y)
    cosy_cosp = 1 - 2 * (y * y + z * z)
    yaw = np.arctan2(siny_cosp, cosy_cosp)

    return yaw

def callback_odom(msg):
    pos=msg.pose.pose.position
    print(f"({pos.x},{pos.y},{yaw_from_quaternion(msg.pose.pose.orientation)})")
  
 ...

  
def main(args=None):

     ...
     node.create_subscription(Odometry, f"/pioneer{nr}/odom", callback_odom, 10)