//SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com //SPDX-License-Identifier: LGPL-3.0-or-later //Some lines are derived from https://github.com/ros-planning/navigation/tree/noetic-devel/amcl. #include "emcl/Pose.h" #include <sstream> #include <cmath> namespace emcl2 { Pose::Pose(double x, double y, double t) { set(x, y, t); } void Pose::set(double x, double y, double t) { x_ = x; y_ = y; t_ = t; } void Pose::set(const Pose &p) { x_ = p.x_; y_ = p.y_; t_ = p.t_; } std::string Pose::to_s(void) { std::stringstream s; s << "x:" << x_ << "\ty:" << y_ << "\tt:" << t_; return s.str(); } void Pose::normalizeAngle(void) { while(t_ > M_PI) t_ -= 2*M_PI; while(t_ < -M_PI) t_ += 2*M_PI; } Pose Pose::operator -(const Pose &p) const { Pose ans(x_ - p.x_, y_ - p.y_, t_ - p.t_); ans.normalizeAngle(); return ans; } Pose Pose::operator =(const Pose &p) { x_ = p.x_; y_ = p.y_; t_ = p.t_; return *this; } void Pose::move(double length, double direction, double rotation, double fw_noise, double rot_noise) { x_ += (length + fw_noise)*cos(direction + rot_noise + t_); y_ += (length + fw_noise)*sin(direction + rot_noise + t_); t_ += rotation + rot_noise; normalizeAngle(); } bool Pose::nearlyZero(void) { return fabs(x_) < 0.001 and fabs(y_) < 0.001 and fabs(t_) < 0.001; } uint16_t Pose::get16bitRepresentation(void) { int tmp = t_/M_PI*(1<<15); while(tmp < 0) tmp += (1<<16); while(tmp >= (1<<16)) tmp -= (1<<16); return (uint16_t)tmp; } uint16_t Pose::get16bitRepresentation(double t) { int tmp = t/M_PI*(1<<15); while(tmp < 0) tmp += (1<<16); while(tmp >= (1<<16)) tmp -= (1<<16); return (uint16_t)tmp; } }