Exemplo n.º 1
0
    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);
            }
        }
    }
Exemplo n.º 2
0
/*-----------------------------------------------------------------------------
* Motion thread tick function (copy from runswift MotionAdapter.cpp)
This would be like processFrame() function as in our code
*---------------------------------------------------------------------------*/
void RSWalkModule2014::processFrame() {


	// If we are changing commands we need to reset generators
	if(walk_request_->motion_ != prev_command && !walk_request_->perform_kick_){
	    if(DEBUG_OUTPUT) cout << "RESETTING GENERATORS! " << WalkRequestBlock::getName(walk_request_->motion_) << " != " << WalkRequestBlock::getName(prev_command) << "\n";
		standing = true;
	}

	// Setting walk kick parameters in bodyModel
        bodyModel.walkKick = walk_request_->perform_kick_;
        bodyModel.walkKickLeftLeg = walk_request_->kick_with_left_;
        bodyModel.walkKickHeading = walk_request_->kick_heading_;

	// 1. Need odometry to send to WalkGenerator. makeJoints updates odometry for localization
	Odometry odo = Odometry(odometry_->displacement.translation.x, odometry_->displacement.translation.y, odometry_->displacement.rotation);
	Odometry prev = Odometry(odo); // Another copy for after makeJoints call

	// 2. Convert our request to runswift request
	// 2.1 Head
	ActionCommand::Head head; // Use default head. Not using HeadGenerator

	// 2.2 Body
	ActionCommand::Body body;

	if (walk_request_->motion_ == WalkRequestBlock::STAND)
	{
		if(DEBUG_OUTPUT) cout << "Requested: STANDUP\n";
		body.actionType = ActionCommand::Body::STAND; 
		body.forward = 0;
		body.left = 0;
		body.turn = 0;
		body.isFast = false; 
		body.bend = 0;
	}
	else if (walk_request_->motion_ == WalkRequestBlock::STAND_STRAIGHT)
	{
		if (DEBUG_OUTPUT) cout << "Requested: STANDSTRAIGHT\n";
		body.actionType = ActionCommand::Body::STAND_STRAIGHT;
		body.forward = 0;
		body.left = 0;
		body.turn = 0;
		body.isFast = false;
		body.bend = 0;
	} 
	else if (walk_request_->motion_ == WalkRequestBlock::WALK)
	{	
//		if (walk_request_->walk_to_target_){
//                        if(DEBUG_OUTPUT) cout << "Requested: Target\n";
//			processTargetModeWalk(body);
//		} else{
       			if(DEBUG_OUTPUT) cout << "Requested: WALK\n";
			body.actionType = ActionCommand::Body::WALK;
			body.forward = walk_request_->speed_.translation.x * walk_params_->bh_params_.rsSpeedMax.translation.x;
			body.left = walk_request_->speed_.translation.y * walk_params_->bh_params_.rsSpeedMax.translation.y;
			body.turn = walk_request_->speed_.rotation * walk_params_->bh_params_.rsSpeedMax.rotation;
			body.bend = 1;

			// This was a hack for odometry turning. 
			//if (walk_request_->speed_.translation.x > 0.5) //TODO threshold
				//body.turn += walk_params_->bh_params_.rs_turn_angle_offset;
			body.isFast = false;
//		}

	}
	else if (walk_request_->motion_ == WalkRequestBlock::NONE || walk_request_->motion_ == WalkRequestBlock::WAIT)
	{
		if(DEBUG_OUTPUT) cout << "Requested: " << WalkRequestBlock::getName(walk_request_->motion_) << "\n";
		body.actionType = ActionCommand::Body::ActionType::NONE;
		body.forward = 0;
		body.bend = 1;
		body.left = 0;
		body.turn = 0;
		body.isFast = false;
	}
	else{
		if(DEBUG_OUTPUT) cout << "Other Request: " << walk_request_->motion_ << endl; 
		body.actionType = ActionCommand::Body::ActionType::STAND;
		body.forward = 0;
		body.left = 0;
		body.turn = 0;
		body.isFast = false;			
	}

/*	if (walk_request_->walk_to_target_){//perform_kick_){// && body.actionType != ActionCommand::Body::ActionType::NONE){
		if(DEBUG_OUTPUT) cout << "RS Kick request" << endl;
		body.actionType = ActionCommand::Body::ActionType::KICK;
		body.isFast = true;
		body.forward = 0;
		body.left = 0;
		body.turn = 0;//walk_request_->kick_heading_;
		body.bend = 1;
		body.speed = 0.5;
		body.power = 0.5;
		if (walk_request_->kick_with_left_)
			body.foot = ActionCommand::Body::LEFT;
		else
			body.foot = ActionCommand::Body::RIGHT;
		
	}
*/

        if(!body_model_->feet_on_ground_ && body.actionType != ActionCommand::Body::ActionType::NONE)
        {
                // Need something here so robot stops when picked up
		body.actionType = ActionCommand::Body::ActionType::REF_PICKUP;
        }

	// 2.3 LEDs and Sonar
	ActionCommand::LED leds; // Not important, use defaults - Josiah
        float sonar = 0.0;

	// Now we have everything we need for a runswift request
	ActionCommand::All request(head,body,leds,sonar);

	// 3. Create runswift sensor object
	SensorValues sensors;

	// 3.1 Sense Joints 
	for (int ut_ind=0; ut_ind<NUM_JOINTS; ut_ind++){
		if (ut_ind != RHipYawPitch){ // RS does not have this joint because RHYP is same as LHYP on Nao
			int rs_ind = utJointToRSJoint[ut_ind];
			sensors.joints.angles[rs_ind] = joints_->values_[ut_ind] * robot_joint_signs[ut_ind]; // changed from raw_joints - Josiah
			sensors.joints.temperatures[rs_ind] = sensors_->joint_temperatures_[ut_ind];
		}
	}

	// Detect if legs are getting hot
	if(sensors_->joint_temperatures_[RKneePitch] > 100 || sensors_->joint_temperatures_[RHipPitch] > 100)
		bodyModel.isRightLegHot = true;
	if(sensors_->joint_temperatures_[LKneePitch] > 100 || sensors_->joint_temperatures_[LHipPitch] > 100)
		bodyModel.isLeftLegHot = true;

	// 3.2 Sensors
	for (int ut_ind=0; ut_ind<bumperRR + 1; ut_ind++){
		int rs_ind = utSensorToRSSensor[ut_ind];
		sensors.sensors[rs_ind] = sensors_->values_[ut_ind];
	}

	
	// 4. If robot is remaining still, calbrate x and y gyroscopes
	// 4.1 Calibrate gyroX:
	double cur_gyroX = sensors.sensors[RSSensors::InertialSensor_GyrX];
	double delta_gyroX = abs(cur_gyroX - last_gyroX);
	// maintain moving averages for gyro and delta_gyro
	avg_gyroX = avg_gyroX * (1.0 - window_size) + cur_gyroX * window_size;
	avg_delta_gyroX = avg_delta_gyroX * (1.0- window_size) + delta_gyroX * window_size;
 	if (avg_delta_gyroX < delta_threshold) {
		// robot remains still, do calibration
		offsetX = avg_gyroX;
		// reset avg_delta so it does not keep recalibrating
		avg_delta_gyroX = reset;
		calX_count += 1; 
		if (calX_count == 1) {
                  cout << "(First calibration, may not be accurate) A GyroX calibration was triggered, offsetX set to: " << offsetX << endl;
		}
		else {
		  cout << "A GyroX calibration was triggered, offsetX set to: " << offsetX << endl;
		}
		last_gyroX_time = frame_info_-> seconds_since_start;
	}
	else {
		if (DEBUG_OUTPUT) cout << "avg_delta_gyroX is: " << avg_delta_gyroX <<  " GyroX not stable, no calibration" << endl;
	}
      	last_gyroX = cur_gyroX;

	// 4.2 Calibrate gyroY:
	double cur_gyroY = sensors.sensors[RSSensors::InertialSensor_GyrY];
	double delta_gyroY = abs(cur_gyroY - last_gyroY);
	// maintain moving averages for gyro and delta_gyro
	avg_gyroY = avg_gyroY * (1.0 - window_size) + cur_gyroY * window_size;
	avg_delta_gyroY = avg_delta_gyroY * (1.0- window_size) + delta_gyroY * window_size;
 	if (avg_delta_gyroY < delta_threshold) {
		// robot remains still, do calibration
		offsetY = avg_gyroY;
		// reset avg_delta so it does not keep recalibrating
		avg_delta_gyroY = reset;
		calY_count += 1;
		if (calY_count == 1) {
                  cout << "(First calibration, may not be accurate) A GyroY calibration was triggered, offsetY set to: " << offsetY << endl;
		}
		else {     
	 	  cout << "A GyroY calibration was triggered, offsetY set to: " << offsetY << endl;
		}
		last_gyroY_time = frame_info_-> seconds_since_start;
	}
	else {
		if (DEBUG_OUTPUT) cout << "avg_delta_gyroY is: " << avg_delta_gyroY <<  " GyroY not stable, no calibration" << endl;
	}
	last_gyroY = cur_gyroY;

	// After each gyro has completed two cycles it is calibrated.
	if (calX_count >= 2)
	  bodyModel.isGyroXCalibrated = true;
	if (calY_count >= 2)
	  bodyModel.isGyroYCalibrated = true;

	if ((calX_count >= 2 && calY_count >= 2) || (getSystemTime() - calibration_write_time < 600))
	{
	  odometry_->walkDisabled = false;
	  calX_count = 2;
	  calY_count = 2;
	  
	  if (last_gyroY_time  > last_calibration_write + 30 && last_gyroX_time > last_calibration_write + 30)
	  {
	    calibration_write_time = getSystemTime();
	    GyroConfig config;
	    config.offsetX = offsetX;
	    config.offsetY = offsetY;
	    config.calibration_write_time = calibration_write_time;
	    config.saveToFile("gyro_calibration.txt");
	    last_calibration_write = frame_info_->seconds_since_start;
	  }
	} else if ((calX_count < 2 || calY_count < 2)) {
		odometry_->walkDisabled = true;
	}

	// Apply offset and convert from rad/sec to rad /frame
	sensors.sensors[RSSensors::InertialSensor_GyrX] = (sensors.sensors[RSSensors::InertialSensor_GyrX] - offsetX) * 0.01;
	sensors.sensors[RSSensors::InertialSensor_GyrY] = (sensors.sensors[RSSensors::InertialSensor_GyrY] - offsetY) * 0.01;


	// 5. Prepare BodyModel to pass to makeJoints

	// 5.1 Get Kinematics ready for bodyModel. We should figure out what parameter values should be.
	kinematics.setSensorValues(sensors); 
	// TODO apply camera calibrations to kinematics. For now we use default of 0
//	kinematics.parameters.cameraPitchTop=2.4;
//	kinematics.parameters.cameraYawTop=sensors_->values_[HeadYaw];
//	kinematics.parameters.cameraRollTop=0;
//	kinematics.parameters.cameraYawBottom=sensors_->values_[HeadPitch];
//	kinematics.parameters.cameraPitchBottom=sensors_->values_[HeadYaw];
//	kinematics.parameters.cameraRollBottom=0;
//	kinematics.parameters.bodyPitch=3.3;//sensors_->values_[angleY];

	kinematics.updateDHChain();

	// Resetting generators and moving to intermediate stance
        if (standing){
          clipper->reset();
	  if(DEBUG_OUTPUT) cout << "Resetting clipper" << endl;
          request.body = ActionCommand::Body::INITIAL;
          odo.clear();
        }


	// 5.2 update bodyModel
	bodyModel.kinematics = &kinematics;
	bodyModel.update(&odo, sensors);
	
	// 6. Get ball position relative to robot
	// This is for lining up to the ball
	WorldObject* ball = &(world_objects_->objects_[WO_BALL]); //ball->loc.x (after localization)
	double ballX = ball->relPos.x;
	double ballY = ball->relPos.y;
	if(DEBUG_OUTPUT) cout << "BallX: "<<ballX<<" BallY: "<<ballY<<endl;

	if (request.body.actionType == ActionCommand::Body::WALK && odometry_->walkDisabled){
		static int delay_ct = 0;
		if (delay_ct % 100 == 0)//prev_command != walk_request_->motion_)
		  speech_->say("Not Calibrated");
	        delay_ct ++;
		if(DEBUG_OUTPUT) cout << "Not calibrated" << endl;
		if (odometry_->standing)
			request.body = ActionCommand::Body::STAND;
		else
			request.body = ActionCommand::Body::NONE;
	}

	  if(DEBUG_OUTPUT) printf("motion request: %s, prev: %s, body request: %i\n", WalkRequestBlock::getName(walk_request_->motion_), WalkRequestBlock::getName(prev_command), request.body);
        prev_command = walk_request_->motion_;


	// Call the clipped generator which calls the distributed generator to produce walks, stands, etc.
	JointValues joints = clipper->makeJoints(&request, &odo, sensors, bodyModel, ballX, ballY);
	

	// Printing out current_generator for debugging
/*	ActionCommand::Body::ActionType action_type = ((DistributedGenerator*)(clipper->generator))->current_generator;


	switch(action_type){
		case ActionCommand::Body::ActionType::STAND_STRAIGHT:
			cout << "STAND_STRAIGHT"<<endl; break;
		case ActionCommand::Body::ActionType::STAND:
			cout << "STAND"<<endl; break;
		case ActionCommand::Body::ActionType::WALK:
			cout << "WALK"<<endl; break;
		case ActionCommand::Body::ActionType::INITIAL:
			cout << "INITIAL" << endl; break;
		case ActionCommand::Body::ActionType::NONE:
			cout << "NULL GEN" <<endl; break;
		default:
			cout << "Another Generator: "<<action_type<<endl;

	}		
*/
	// We aren't setting arms any more so this isn't important unless we go back to it
	// Setting arms behind back makes us get caught less but then we can't counter balance rswalk
	if ((frame_info_->seconds_since_start - last_walk_or_stand_) > 0.3) {
		arm_state_ = -1;
	}

        // Update odometry
	static double cum_f=0, cum_l=0, cum_t=0;
        odometry_->displacement.translation.x = odo.forward;
        odometry_->displacement.translation.y = odo.left;
        odometry_->displacement.rotation = odo.turn;
	//if (walk_request_->speed_.translation.x > 0.5)
		//odometry_->displacement.rotation -= (walk_params_->bh_params_.rs_turn_angle_offset / 100.0);
        odometry_->standing = clipper->isStanding();
	Odometry delta = Odometry(odo - prev);
	cum_f += delta.forward;
	cum_l += delta.left;
	cum_t += delta.turn;
	// For debugging odometry
/*	if (body.turn != 0){
		cout << "Odometry Prev: " << prev.turn;
		cout << " Odometry Delta: " << delta.turn;
		cout << " Odometry: "  << odo.turn;
		cout << " Cumulative Odometry: " << cum_t << endl;
	}
*/
      // Update walk_info_
        //walk_info_->walk_is_active_ = !(clipper->isStanding());
        walk_info_->instability_ = avg_gyroX;
        if (avg_gyroX < -2 or avg_gyroX > 2) {
          walk_info_->instable_ = true;
        } else {
          walk_info_->instable_ = false;
        }

        // Update
        wasKicking = kick_request_->kick_running_;

	if (request.body.actionType == ActionCommand::Body::ActionType::NONE){
		return;
	}

	// For setting arms
	last_walk_or_stand_ = frame_info_->seconds_since_start;

	// Convert RS joints to UT joints and write to commands memory block
	for (int ut_ind = BODY_JOINT_OFFSET; ut_ind < NUM_JOINTS; ut_ind++) {
		if (ut_ind != RHipYawPitch){ // RS does not have this joint - Josiah
     			int rs_ind = utJointToRSJoint[ut_ind];
        		commands_->angles_[ut_ind] = robot_joint_signs[ut_ind] * joints.angles[rs_ind];
    	    		commands_->stiffness_[ut_ind] = joints.stiffnesses[rs_ind];
		} 
   	}

	// Ruohan: setting head stiffness was commented out in bhuman. Should ask Jake about this	
	commands_->stiffness_[HeadPitch] = 1.0;
	commands_->stiffness_[HeadYaw] = 1.0;
	commands_->stiffness_[RHipYawPitch] = commands_->stiffness_[LHipYawPitch]; // RHYP will be overwritten with LHYP anyway but for completeness
	commands_->angles_[RHipYawPitch] = commands_->angles_[LHipYawPitch];

	// Walk keeps falling backwards when knees over 100. Leaning forward helps some
	if (request.body.actionType == ActionCommand::Body::STAND && (bodyModel.isLeftLegHot || bodyModel.isRightLegHot)){
		commands_->angles_[RHipPitch] -= DEG_T_RAD * 2;
		commands_->angles_[LHipPitch] -= DEG_T_RAD * 2;
	}
	
	// if the robot has walked, listen to arm command from WalkGenerator, else do stiffness
	if (request.body.actionType != ActionCommand::Body::WALK) {
		for (int ind = ARM_JOINT_FIRST; ind <= ARM_JOINT_LAST; ind ++)
			commands_->stiffness_[ind] = 1.0; //0.75;
	}
	
	selectivelySendStiffness(); // Only send stiffness if at least one joint has changed stiffness values by at least 0.01
//	setArms(commands_->angles_,0.01); // Arms getting stuck on robot front with rswalk. Needs tuning

	commands_->send_body_angles_ = true; // So commands will execute
	commands_->send_stiffness_ = true;

	if (request.body.actionType == ActionCommand::Body::STAND){
		commands_->body_angle_time_ = 10; // Increase these if standing?
		commands_->stiffness_time_ = 10; 
	} else {
		commands_->body_angle_time_ = 10;
		commands_->stiffness_time_ = 10;

	}

	// Not doing anything with these now. Could be used in future for smoothing
	prevForward = body.forward;
	prevLeft = body.left;
	prevTurn = body.turn;

	// Agent Effector code?
	// This is in rswalk agent effecter. Not sure what standing is but it works now.
	static bool kill_standing = false;
	standing = kill_standing;
	if (kill_standing) {
		kill_standing = false;
		standing = false;
	}
	else{
		kill_standing = true;
	}

}