void SE_average<2>::append(const mrpt::poses::CPose2D& p, const double weight)
{
m_count += weight;
m_accum_x += weight * p.x();
m_accum_y += weight * p.y();
m_rot_part.append(p.phi(), weight);
}
void SE_average<2>::get_average(mrpt::poses::CPose2D &ret_mean) const
{
ASSERT_ABOVE_(m_count,0);
ret_mean.x( m_accum_x / m_count );
ret_mean.y( m_accum_y / m_count );
const_cast<SO_average<2>*>(&m_rot_part)->enable_exception_on_undeterminate = this->enable_exception_on_undeterminate;
ret_mean.phi ( m_rot_part.get_average() );
}
/*-------------------------------------------------------------
changeOdometry
-------------------------------------------------------------*/
void CActivMediaRobotBase::changeOdometry(const mrpt::poses::CPose2D &newOdometry)
{
#if MRPT_HAS_ARIA
ArPose pos_actual;
THE_ROBOT->lock();
pos_actual.setPose( newOdometry.x()*1000, newOdometry.y()*1000, RAD2DEG( newOdometry.phi() ) );
THE_ROBOT->setDeadReconPose( pos_actual );
THE_ROBOT->unlock();
#else
THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used.");
#endif
}
bool getCurrentPoseAndSpeeds( mrpt::poses::CPose2D &curPose, float &curV, float &curW)
{
robotSim.getRealPose( curPose );
double phi = M_PI/2 - curPose.phi();
if(phi<0)
{
cout << "changing" <<phi << endl;
phi+= 2*M_PI;
cout << "to" << phi << endl;
}
//curPose.phi(phi);
/*float temp = curPose.x();
curPose.x(curPose.y());
curPose.y(temp);*/
curV = robotSim.getV();
curW = robotSim.getW();
return true;
}