bool LPoint::Equal(const LPoint a_point, B_INT Marge)
{
B_INT delta_x, delta_y;
delta_x = babs((_x - a_point._x));
delta_y = babs((_y - a_point._y));
if ((delta_x <= Marge) && (delta_y <= Marge))
return true;
else
return false;
}
int main(int argc, char** argv)
{
ROS_INFO("Running babslam_main...");
srand (static_cast <unsigned> (time(0)));
ros::init(argc, argv, "babs_slam");
ros::NodeHandle nh_;
babs_slam babs(&nh_);
//sensor_model_test(babs);
//measurement_model_test(babs);
//motion_model_test(babs);
ros::Rate naptime(1/babs.dt);
bool warmedup = false;
while(!warmedup){
ROS_INFO("waiting for data");
warmedup=babs.warmCallbacks();
ros::spinOnce();
naptime.sleep();
}
while(ros::ok()){
babs.update();
ros::spinOnce();
naptime.sleep();
}
ros::spin();
return 0;
}
//
// This function finds out which sortfunction to use with sorting
// the crossings.
//
void kbLine::SortLineCrossings()
{
TDLI<kbNode> I( linecrosslist );
B_INT dx, dy;
dx = babs( m_link->GetEndNode()->GetX() - m_link->GetBeginNode()->GetX() );
dy = babs( m_link->GetEndNode()->GetY() - m_link->GetBeginNode()->GetY() );
if ( dx > dy )
{ // thislink is more horizontal then vertical
if ( m_link->GetEndNode()->GetX() > m_link->GetBeginNode()->GetX() )
I.mergesort( NODE_X_ASCENDING_L );
else
I.mergesort( NODE_X_DESCENDING_L );
}
else
{ // this link is more vertical then horizontal
if ( m_link->GetEndNode()->GetY() > m_link->GetBeginNode()->GetY() )
I.mergesort( NODE_Y_ASCENDING_L );
else
I.mergesort( NODE_Y_DESCENDING_L );
}
}
bool LPoint::Equal(const B_INT X, const B_INT Y, B_INT Marge)
{
return (bool)((babs(_x - X) <= Marge) && (babs(_y - Y) <= Marge));
}
