25 #include <core/exception.h> 26 #include <fvmodels/velocity/globfromrel.h> 27 #include <utils/time/time.h> 37 namespace firevision {
47 VelocityGlobalFromRelative::VelocityGlobalFromRelative(
VelocityModel * rel_velo_model,
50 this->relative_velocity = rel_velo_model;
51 this->relative_position = rel_pos_model;
60 "Given velocity model does not return robot-centric cartesian coordinates. WILL NOT WORK!");
63 robot_ori = robot_poseage = 0.f;
65 velocity_x = velocity_y = 0.f;
90 VelocityGlobalFromRelative::~VelocityGlobalFromRelative()
95 VelocityGlobalFromRelative::setPanTilt(
float pan,
float tilt)
100 VelocityGlobalFromRelative::setRobotPosition(
float x,
float y,
float ori, timeval t)
103 gettimeofday(&now, 0);
109 VelocityGlobalFromRelative::setRobotVelocity(
float rel_vel_x,
float rel_vel_y, timeval t)
114 VelocityGlobalFromRelative::setTime(timeval t)
119 VelocityGlobalFromRelative::setTimeNow()
124 VelocityGlobalFromRelative::getTime(
long int *sec,
long int *usec)
131 VelocityGlobalFromRelative::getVelocity(
float *vel_x,
float *vel_y)
142 VelocityGlobalFromRelative::getVelocityX()
148 VelocityGlobalFromRelative::getVelocityY()
154 VelocityGlobalFromRelative::calc()
156 relative_velocity->getVelocity(&rel_vel_x, &rel_vel_y);
157 sin_ori = sin(robot_ori);
158 cos_ori = cos(robot_ori);
159 rel_dist = relative_position->get_distance();
161 velocity_x = rel_vel_x * cos_ori - rel_vel_y * sin_ori;
162 velocity_y = rel_vel_x * sin_ori + rel_vel_y * cos_ori;
168 VelocityGlobalFromRelative::reset()
180 VelocityGlobalFromRelative::getName()
const 182 return "VelocityModel::VelocityGlobalFromRelative";
186 VelocityGlobalFromRelative::getCoordinateSystem()
188 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.