Esempio n. 1
0
/*-------------------------------------------------------------
					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
}
Esempio n. 2
0
/*-------------------------------------------------------------
					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
}