albertomosconi / robotics_project1

First project for Robotics course at polimi

Robotics Project 1

The first project for the Robotics course at Politecnico di Milano, academic year 2021/2022.

Team

Student ID	Name	Surname
10754004	Mahdieh	Jalali
10574957	Silvia	Lombardo
10653349	Alberto	Mosconi

Project structure

• CMakeLists.txt: the configuration for the cmake compiler, defines how to build the code.
• package.xml: the package manifest, defines the package's properties.
• config/
  • display.rviz: the configuration for visualizing the robot movement and GT pose in Rviz.
  • initial_pose.yml: the coordinates of the robot (x, y and theta).
  • robot_parameters.yml: the parameters of the robot (L, W, N, T and R).
  • parameters.cfg: the dynamic reconfigure configuration file, where the parameter to select the integration method is defined.
• launch/
  • main.launch: the main launch file for the package, starts all the nodes.
  • bag.launch: a launch file which on top of starting the node also plays a bag, starts rviz and rqt_reconfigure.
• msg/
  • WheelSpeed.msg: the definition of the custom message WheelSpeed, used when publishing the wheels rpms.
• src/
  • main.cpp: the main ros file, contains all the variables and methods for the project's node.
  • reset_to_pose.cpp: the code for the node of the service responsible for resetting the robot's pose to the given coordinates.
  • broadcast_gt_pose.cpp: the node that subscribes to the gt pose topic and broadcasts its tf frame.
• srv/
  • reset_to_pose.srv: the specification of the request and response of the service responsible for resetting the robot's pose to the given coordinates.
• tuning/
  • genetic.py: starts the genetic evolution to find the best parameters.
  • ParameterSet.py: the class that holds a possible set of parameters, contains the method to compare the computed solution to the ground truth pose.
  • pose.txt: a log obtained with rostopic echo of the gt pose data.
  • stamps.txt: a log with the timestamps of the messages of wheel_states.
  • wheel_states.txt: a log of the wheel_states position data.

ROS Parameters

• INITIAL_X : previous_odometry [1], initial value of x in pose (x , y , theta)
• INITIAL_Y : previous_odometry [2], initial value of y in pose (x , y , theta)
• INITIAL_THETA : previous_odometry [0], initial value of theta in pose (x , y , theta)
• RADIUS : RADIUS, wheel radius which initaly defined as 0.07 [m]
• L : LENGTH, robot length which initaily defined as 0.200 [m]
• W : WIDTH, robot width which initialy defined as 0.169 [m]
• N : ENCODER_RESOLUTION, which initaly defined as 42 CPR
• T : GEAR_RATIO, which initialy defined as 5
• ODOMETRY_METHOD : Can get value of either "0" as EULER or "1" as RUNGE_KUTTA integration method

TF Tree

Custom messages

WheelSpeed

Header header   # message header
float64 rpm_fl  # speed of front-left wheel
float64 rpm_fr  # speed of front-right wheel
float64 rpm_rr  # speed of rear-right wheel
float64 rpm_rl  # speed of rear-left wheel

Usage

To run the node use the main.launch file with the following command:

roslaunch project1 main.launch

Alternatively, a bag can be played by running the bag.launch file and by passing a command line argument with the bag number. This will also start rviz and rqt_reconfigure.

roslaunch project1 bag.launch bag:=3

To call the reset_to_pose service use the following command, making sure to set the parameter values as arguments.

rosservice call /reset_to_pose <x> <y> <theta>

Additional info

Variables

• sec, previous_sec: Time [second]
• nsec, previous_nsec: Time [nano second]
• dt: Time intervals [second]
• positions, previous_positions: Wheel tick data
• dp: Wheel movement between two messages in a row [rad]
• computed_speeds[4], speeds[4]: Wheel speeds using ticks (u1,u2,u3,u4) [rad/s]
• robot_speed[3], previous_robot_speed[3]: Robot Linear and Angular Speeds using ticks
• wheel_speeds[4]: Given Wheel speeds (u1,u2,u4,u3) [rad/min]
• odometry[3], previous_odometry[3]: Robot pose (theta,x,y)
• computed_wheel_rpms[4]: Wheel speeds using given linear and angular velocities in RPM (variable: robot_speed)

Calibration

In order to match robot pose to the ground truth pose, it is needed to fine-tune 4 parameters (wheel radius, robot width and length and encoder_resolution).

An optimal configuration was acheived with a combination of manual tweaking and a genetic algorithm. This algorithm (source), starts with a population of 100 random sets of parameters, and over the course of several "generations", selects those which result in a trajectory closer to the GT pose.

• Wheel radius (R): 0.072
• Encoder resolution (N): 40
• Robot width (W): 0.1435
• Robot length (L): 0.19789

ros_graph output