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