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.
/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.
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
/pioneerX/odom
topic.
/pioneerX/odom
topic
is the exact location of the robot – estimate the odometry error
import datetime
a=datetime.datetime.now()
b=datetime.datetime.now()
c=b-a
print(c,c.seconds,c.microseconds)
print(c.total_seconds())
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']
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)