94 lines
2.2 KiB
C++
94 lines
2.2 KiB
C++
/**
|
|
*
|
|
* PoseChange.hpp - C++ header for PoseChange 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;
|
|
|
|
|
|
/**
|
|
* A class representing the forward and angular poseChange of a robot.
|
|
*/
|
|
class PoseChange
|
|
{
|
|
friend class Scan;
|
|
|
|
public:
|
|
|
|
/**
|
|
* Creates a new PoseChange object with specified poseChange.
|
|
*/
|
|
PoseChange(double dxy_mm, double dtheta_degrees, double dtSeconds)
|
|
{
|
|
this->dxy_mm = dxy_mm;
|
|
this->dtheta_degrees = dtheta_degrees;
|
|
this->dt_seconds = dtSeconds;
|
|
}
|
|
|
|
/**
|
|
* Creates a new PoseChange object with zero poseChange.
|
|
*/
|
|
PoseChange(void)
|
|
{
|
|
this->dxy_mm = 0;
|
|
this->dtheta_degrees = 0;
|
|
this->dt_seconds = 0;
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* Updates this PoseChange object.
|
|
* @param dxy_mm new forward distance traveled in millimeters
|
|
* @param dtheta_degrees new angular rotation in degrees
|
|
* @param dtSeconds time in seconds since last poseChange
|
|
*/
|
|
void update(double dxy_mm, double dtheta_degrees, double dtSeconds)
|
|
{
|
|
double velocity_factor = (dtSeconds > 0) ? (1 / dtSeconds) : 0;
|
|
|
|
this->dxy_mm = dxy_mm * velocity_factor;
|
|
|
|
this->dtheta_degrees = dtheta_degrees * velocity_factor;
|
|
}
|
|
|
|
friend ostream& operator<< (ostream & out, PoseChange & poseChange)
|
|
{
|
|
char str[100];
|
|
|
|
sprintf(str, "<dxy=%7.0f mm dtheta = %+3.3f degrees dt = %f s",
|
|
poseChange.dxy_mm,
|
|
poseChange.dtheta_degrees,
|
|
poseChange.dt_seconds);
|
|
|
|
out << str;
|
|
|
|
return out;
|
|
}
|
|
|
|
|
|
double dxy_mm;
|
|
double dtheta_degrees;
|
|
double dt_seconds;
|
|
};
|