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); } } }