128 lines
3.7 KiB
C++
128 lines
3.7 KiB
C++
/**
|
|
*
|
|
* BreezySLAM: Simple, efficient SLAM in C++
|
|
*
|
|
* WheeledRobot.hpp - C++ header for WheeledRobot class
|
|
*
|
|
* Copyright (C) 2014 Simon D. Levy
|
|
|
|
* This code is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU Lesser General Public License as
|
|
* published by the Free Software Foundation, either version 3 of the
|
|
* License, or (at your option) any later version.
|
|
*
|
|
* This code is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Lesser General Public License
|
|
* along with this code. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
|
|
#include <iostream>
|
|
#include <vector>
|
|
using namespace std;
|
|
|
|
class Velocities;
|
|
|
|
/**
|
|
* An abstract class for wheeled robots. Supports computation of forward and angular
|
|
* velocities based on odometry. Your subclass should should implement the
|
|
* extractOdometry method.
|
|
*/
|
|
class WheeledRobot
|
|
{
|
|
|
|
protected:
|
|
|
|
/**
|
|
* Builds a WheeledRobot object. Parameters should be based on the specifications for
|
|
* your robot.
|
|
* @param wheel_radius_mm radius of each odometry wheel, in meters
|
|
* @param half_axle_length_mm half the length of the axle between the odometry wheels, in meters
|
|
* @return a new WheeledRobot object
|
|
*/
|
|
WheeledRobot(double wheel_radius_mm, double half_axle_length_mm)
|
|
{
|
|
this->wheel_radius_mm = wheel_radius_mm;
|
|
this->half_axle_length_mm = half_axle_length_mm;
|
|
|
|
this->timestamp_seconds_prev = 0;
|
|
this->left_wheel_degrees_prev = 0;
|
|
this->right_wheel_degrees_prev = 0;
|
|
}
|
|
|
|
/**
|
|
* Computes forward and angular velocities based on odometry.
|
|
* @param timestamp time stamp, in whatever units your robot uses
|
|
* @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses
|
|
* @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses
|
|
* @return velocities object representing velocities for these odometry values
|
|
*/
|
|
|
|
Velocities
|
|
computeVelocities(
|
|
double timestamp,
|
|
double left_wheel_odometry,
|
|
double right_wheel_odometry);
|
|
|
|
/**
|
|
* Extracts usable odometry values from your robot's odometry.
|
|
* @param timestamp time stamp, in whatever units your robot uses
|
|
* @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses
|
|
* @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses
|
|
* @param timestamp_seconds gets time stamp in seconds
|
|
* @param left_wheel_degrees gets left wheel rotation in degrees
|
|
* @param right_wheel_degrees gets right wheel rotation in degrees
|
|
*/
|
|
virtual void extractOdometry(
|
|
double timestamp,
|
|
double left_wheel_odometry,
|
|
double right_wheel_odometry,
|
|
double & timestamp_seconds,
|
|
double & left_wheel_degrees,
|
|
double & right_wheel_degrees) = 0;
|
|
|
|
friend ostream& operator<< (ostream & out, WheeledRobot & robot)
|
|
{
|
|
|
|
char subclassStr[100];
|
|
robot.descriptorString(subclassStr);
|
|
|
|
char str[200];
|
|
sprintf(str, "<Wheel radius=%f m Half axle Length=%f m | %s>",
|
|
robot.wheel_radius_mm, robot.half_axle_length_mm, subclassStr);
|
|
|
|
out << str;
|
|
|
|
return out;
|
|
}
|
|
|
|
|
|
/**
|
|
* Gets a descriptor string for your robot.
|
|
* @param str gets the descriptor string
|
|
*/
|
|
virtual void descriptorString(char * str) = 0;
|
|
|
|
private:
|
|
|
|
double wheel_radius_mm;
|
|
double half_axle_length_mm;
|
|
|
|
double timestamp_seconds_prev;
|
|
double left_wheel_degrees_prev;
|
|
double right_wheel_degrees_prev;
|
|
|
|
static double radians(double degrees)
|
|
{
|
|
return degrees * M_PI / 180;
|
|
}
|
|
|
|
|
|
};
|