25 #include <core/exception.h> 27 #include <fvmodels/velocity/globfromrel.h> 28 #include <utils/time/time.h> 49 VelocityGlobalFromRelative::VelocityGlobalFromRelative(
VelocityModel *rel_velo_model,
52 this->relative_velocity = rel_velo_model;
53 this->relative_position = rel_pos_model;
61 throw Exception(
"Given velocity model does not return robot-centric cartesian coordinates. WILL NOT WORK!");
64 robot_ori = robot_poseage = 0.f;
66 velocity_x = velocity_y = 0.f;
92 VelocityGlobalFromRelative::~VelocityGlobalFromRelative()
98 VelocityGlobalFromRelative::setPanTilt(
float pan,
float tilt)
104 VelocityGlobalFromRelative::setRobotPosition(
float x,
float y,
float ori, timeval t)
107 gettimeofday(&now, 0);
114 VelocityGlobalFromRelative::setRobotVelocity(
float rel_vel_x,
float rel_vel_y, timeval t)
119 VelocityGlobalFromRelative::setTime(timeval t)
125 VelocityGlobalFromRelative::setTimeNow()
131 VelocityGlobalFromRelative::getTime(
long int *sec,
long int *usec)
139 VelocityGlobalFromRelative::getVelocity(
float *vel_x,
float *vel_y)
151 VelocityGlobalFromRelative::getVelocityX()
158 VelocityGlobalFromRelative::getVelocityY()
166 VelocityGlobalFromRelative::calc()
169 relative_velocity->getVelocity( &rel_vel_x, &rel_vel_y );
170 sin_ori = sin( robot_ori );
171 cos_ori = cos( robot_ori );
172 rel_dist = relative_position->get_distance();
174 velocity_x = rel_vel_x * cos_ori - rel_vel_y * sin_ori;
175 velocity_y = rel_vel_x * sin_ori + rel_vel_y * cos_ori;
183 VelocityGlobalFromRelative::reset()
196 VelocityGlobalFromRelative::getName()
const 198 return "VelocityModel::VelocityGlobalFromRelative";
203 VelocityGlobalFromRelative::getCoordinateSystem()
205 return COORDSYS_WORLD_CART;
virtual coordsys_type_t getCoordinateSystem()=0
Returns the used coordinate system, must be either COORDSYS_ROBOT_CART or COORDSYS_ROBOT_WORLD.
Fawkes library namespace.
Velocity model interface.
Relative Position Model Interface.
Base class for exceptions in Fawkes.
double time_diff_sec(const timeval &a, const timeval &b)
Calculate time difference of two time structs.