/*-------------------------------------------------------------
getOdometryFull
-------------------------------------------------------------*/
void CActivMediaRobotBase::getOdometryFull(
poses::CPose2D &out_odom,
double &out_lin_vel,
double &out_ang_vel,
int64_t &out_left_encoder_ticks,
int64_t &out_right_encoder_ticks
)
{
#if MRPT_HAS_ARIA
ASSERTMSG_(THE_ROBOT!=NULL, "Robot is not connected")
THE_ROBOT->lock();
// Odometry:
ArPose pose = THE_ROBOT->getEncoderPose();
out_odom.x( pose.getX() * 0.001 );
out_odom.y( pose.getY() * 0.001 );
out_odom.phi( DEG2RAD( pose.getTh() ) );
// Velocities:
out_lin_vel = THE_ROBOT->getVel() * 0.001;
out_ang_vel = DEG2RAD( THE_ROBOT->getRotVel() );
// Encoders:
out_left_encoder_ticks = THE_ROBOT->getLeftEncoder();
out_right_encoder_ticks = THE_ROBOT->getRightEncoder();
THE_ROBOT->unlock();
#else
MRPT_UNUSED_PARAM(out_odom); MRPT_UNUSED_PARAM(out_lin_vel); MRPT_UNUSED_PARAM(out_ang_vel);
MRPT_UNUSED_PARAM(out_left_encoder_ticks); MRPT_UNUSED_PARAM(out_right_encoder_ticks);
THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used.");
#endif
}
/*-------------------------------------------------------------
getOdometry
-------------------------------------------------------------*/
void CActivMediaRobotBase::getOdometry(poses::CPose2D &out_odom)
{
#if MRPT_HAS_ARIA
ASSERTMSG_(THE_ROBOT!=NULL, "Robot is not connected")
THE_ROBOT->lock();
ArPose pose = THE_ROBOT->getEncoderPose();
out_odom.x( pose.getX() * 0.001 );
out_odom.y( pose.getY() * 0.001 );
out_odom.phi( DEG2RAD( pose.getTh() ) );
THE_ROBOT->unlock();
#else
THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used.");
#endif
}