void
process()
{
if(!colorsegment_->Segment(imginfo_->yuv_image_))
return;
//get the colors of the scan_points
scanpts_->process();
//detect the white points
whites_->process();
if(whites_->img_white_.size()<=0)
{
// is_restart_=true;
ROS_WARN("don't detect the white points");
return ;
}
if(!is_power_off_)
is_restart_=true;
if(is_restart_)
{
is_restart_=false;
robot.isglobal_=true;
}
//localization
if(robot.isglobal_)
{
ROS_INFO("start global localization");
robot.isglobal_=glocation_->process(whites_->weights_,whites_->robot_white_,robot.visual_location_,robot.angle_);
}
else
{
DPoint delta_loc=odometry_->getWorldLocaton();
Angle delta_ang=odometry_->getDeltaAngle();
odometry_->clear(robot.angle_);
robot.realvtrans_ = odometry_->getRealVelocity();
robot.worldvtrans_ = odometry_->getWorldVelocity();
robot.angular_velocity_ = odometry_->getAngularVelocity();
location_->process(whites_->weights_,whites_->robot_white_,robot.visual_location_,robot.angle_,delta_loc,delta_ang);
}
PPoint correct_pt = PPoint(tranfer_->get_offset().angle(),tranfer_->get_offset().norm());
tranfer_->calculateWorldCoordinates(correct_pt,robot.visual_location_,robot.angle_,robot.final_location_);
obstacles_->process(colorsegment_->segment_result_,robot.final_location_,robot.angle_);
ball_info_.pos_known=ball_finder_->Process(colorsegment_->segment_result_,robot.final_location_,robot.angle_);
if(is_show_whites_ || is_show_obstacles_ || is_show_ball_ )
{
if(field_image_.empty())
ROS_INFO("Field.bmp is empty");
cv::Mat image = field_image_.clone();
cv::Mat orgnal = imginfo_->getBGRImage().clone();
static double length = 1920;
static double width = 1314;
if(WIDTH_RATIO<1)
{
length = 1920;
width = 882;
}
if(is_show_scan_points)
scanpts_->showScanPoints();
if(is_show_obstacles_)
{
obstacles_->showObstacles(orgnal);
if(!field_image_.empty())
obstacles_->showObstacles(image,robot.final_location_,robot.angle_,length,width);
}
if(is_show_whites_)
{
whites_->showWhitePoints(orgnal);
if(!field_image_.empty())
whites_->showWhitePoints(image,robot.final_location_,robot.angle_,length,width);
}
if(is_show_ball_)
{
ball_finder_->showBall(orgnal);
if(!field_image_.empty())
ball_finder_->showBall(image,robot.final_location_,robot.angle_,length,width);
}
}
}
