void GrabPuckServer::controlLoop()
{
double vel_x = 0, vel_y = 0, vel_phi = 0;
ROS_DEBUG("%s", GrabPuckStates::toString(state_).c_str());
double percentage = 0;
if (!loaded_ && (state_ == grab_puck_states::ALIGNING_LATERAL || state_ == grab_puck_states::HEADING_TOWARD_PUCK || state_ == grab_puck_states::GRABBING_PUCK))
{
robotino_vision::FindObjects srv;
srv.request.color = Colors::toCode(color_);
if (!find_objects_cli_.call(srv))
{
ROS_ERROR("Puck not found!!!");
state_ = grab_puck_states::LOST;
return;
}
std::vector<float> distances = srv.response.distances;
std::vector<float> directions = srv.response.directions;
int num_objects = srv.response.distances.size();
if (num_objects > 0 && state_ != grab_puck_states::GRABBING_PUCK)
{
int closest_index = 0;
for (int i = 0; i < num_objects; i++)
{
if (distances[i] < distances[closest_index])
{
closest_index = i;
}
}
double max_error_lateral = 50, max_error_frontal = 40;
double error_lateral = directions[closest_index];
if (error_lateral > max_error_lateral)
{
error_lateral = max_error_lateral;
}
double error_frontal = distances[closest_index];
if (error_frontal > max_error_frontal)
{
error_frontal = max_error_frontal;
}
float tolerance_lateral = 0.1, tolerance_frontal = 35;
double K_error_lateral = 0.3, K_error_frontal = 0.005;
double percentage_f, percentage_0, tolerance, max_error, error;
if (fabs(error_lateral) > tolerance_lateral) // 0% a 49%
{
state_ = grab_puck_states::ALIGNING_LATERAL;
vel_y = -K_error_lateral * error_lateral;
percentage_0 = 0;
percentage_f = 49;
tolerance = tolerance_lateral;
max_error = max_error_lateral;
error = fabs(error_lateral);
}
else if (fabs(error_frontal) > tolerance_frontal) // 50% a 69%
{
state_ = grab_puck_states::HEADING_TOWARD_PUCK;
vel_x = K_error_frontal * error_frontal;
percentage_0 = 50;
percentage_f = 69;
tolerance = tolerance_frontal;
max_error = max_error_frontal;
error = fabs(error_frontal);
}
else
{
vel_x = .2;
state_ = grab_puck_states::GRABBING_PUCK;
grabbing_start_ = ros::Time::now();
}
percentage = percentage_0 + (percentage_f - percentage_0) * (max_error - error) / (max_error - tolerance);
}
else if (state_ == grab_puck_states::GRABBING_PUCK) // 70% a 79%
{
state_ = grab_puck_states::GRABBING_PUCK;
vel_x = .2;
double percentage_0 = 70, percentage_f = 79;
double elapsed_time = (ros::Time::now() - grabbing_start_).toSec();
if (elapsed_time > GRABBING_DEADLINE)
{
state_ = grab_puck_states::LOST;
ROS_ERROR("Grabbing state deadline expired!!!");
}
percentage = percentage_0 + (percentage_f - percentage_0) * elapsed_time / GRABBING_DEADLINE;
}
}
else
{
if (state_ == grab_puck_states::GRABBING_PUCK)
{
ros::Duration d(.5);
d.sleep();
delta_x_ = -getOdometry_X();
resetOdometry();
state_ = grab_puck_states::ROTATING;
percentage_ = 79;
}
double max_error_linear = 1.25;
double error_linear = getOdometry_X() - delta_x_;
if (error_linear > max_error_linear)
{
error_linear = max_error_linear;
}
double max_error_angular = PI;
double error_angular = PI - getOdometry_PHI();
if (error_angular > max_error_angular)
{
error_angular = max_error_angular;
}
float tolerance_linear = 0.1, tolerance_angular = 0.2;
double K_error_linear = 1, K_error_angular = 0.6;
double percentage_f, percentage_0, tolerance, max_error, error;
if (fabs(error_angular) > tolerance_angular) // 80% a 89%
{
state_ = grab_puck_states::ROTATING;
vel_phi = K_error_angular * error_angular;
percentage_0 = 80;
percentage_f = 89;
tolerance = tolerance_angular;
max_error = max_error_angular;
error = fabs(error_angular);
}
else if (fabs(error_linear) > tolerance_linear) // 90% a 99%
{
state_ = grab_puck_states::GOING_BACK_TO_ORIGIN;
vel_x = K_error_linear * error_linear;
percentage_0 = 91;
percentage_f = 99;
tolerance = tolerance_linear;
max_error = max_error_linear;
error = fabs(error_linear);
}
percentage = percentage_0 + (percentage_f - percentage_0) * (max_error - error) / (max_error - tolerance);
}
if (vel_x == 0 && vel_y == 0 && vel_phi == 0) // 100%
{
if(!loaded_ramp_)
{
result_.goal_achieved = false;
result_.message = "Grab fail!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("%s", result_.message.c_str());
}
else
{
state_ = grab_puck_states::FINISHED;
percentage_ = 100;
}
}
if (percentage > percentage_)
{
percentage_ = percentage;
}
setVelocity(vel_x, vel_y, vel_phi);
publishVelocity();
publishFeedback();
}
void StorePuckServer::controlLoop()
{
double vel_x = 0, vel_y = 0, vel_phi = 0;
ROS_DEBUG("%s", StorePuckStates::toString(state_).c_str());
double percentage = 0;
if (mode_ == store_modes::LASER_SCAN)
{
robotino_motion::AlignGoal frontal_alignment, lateral_alignment;
frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
frontal_alignment.distance_mode = 1; // NORMAL distance mode
//lateral_alignment.alignment_mode = 9; // RIGHT_LASER alignment mode
//lateral_alignment.distance_mode = 1; // NORMAL distance mode
align_client_.sendGoal(frontal_alignment);
state_ = store_puck_states::ALIGNING_FRONTAL;
align_client_.waitForResult();
ROS_INFO("Store_number_: %s", StoreStoreNumbers::toString(store_number_).c_str());
switch(store_number_)
{
case store_store_numbers::NEAR:
{
while(laser_front_ > 0.13)
{
setVelocity(0.15, 0, 0);
publishVelocity();
}
//setVelocity(0.0, 0.0, 0.0);
//publishVelocity();
}
break;
case store_store_numbers::MIDDLE:
{
printf("entrou no MIDDLE");
while(laser_front_ > 0.78)
{
setVelocity(0.15, 0, 0);
publishVelocity();
}
//setVelocity(0.0, 0.0, 0.0);
//publishVelocity();
}
break;
case store_store_numbers::FAR_AWAY:
{
printf("entrou no FAR_AWAY");
while(laser_front_ > 1.43)
{
setVelocity(0.15, 0, 0);
publishVelocity();
}
//setVelocity(0.0, 0.0, 0.0);
//publishVelocity();
}
break;
case store_store_numbers::VOLTAR_PRA_CASA:
{
frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
frontal_alignment.distance_mode = 1; // NORMAL distance mode
align_client_.sendGoal(frontal_alignment);
state_ = store_puck_states::ALIGNING_FRONTAL;
align_client_.waitForResult();
while(laser_front_ > 0.13)
{
setVelocity(0.15, 0, 0);
publishVelocity();
}
while(laser_left_ > 0.3)
{
setVelocity(0, 0.1, 0);
publishVelocity();
}
flag_aux_ = true;
}
break;
default:
ROS_ERROR("Store Number not supported yet!!!");
}
//laserDistanceFront(store_number_);
if(flag_aux_ == false)
{
frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
frontal_alignment.distance_mode = 1; // NORMAL distance mode
align_client_.sendGoal(frontal_alignment);
state_ = store_puck_states::ALIGNING_FRONTAL;
align_client_.waitForResult();
resetOdometry();
while(getOdometry_PHI() < PI / 2)
{
setVelocity(0, 0, 0.5);
publishVelocity();
}
frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
frontal_alignment.distance_mode = 1; // NORMAL distance mode
align_client_.sendGoal(frontal_alignment);
state_ = store_puck_states::ALIGNING_FRONTAL;
align_client_.waitForResult();
state_ = store_puck_states::STORING_PUCK;
while(laser_front_ > 0.33)
{
setVelocity(0.15, 0, 0);
publishVelocity();
}
state_ = store_puck_states::LEAVING_PUCK;
while(laser_front_ < 0.8)
{
setVelocity(-0.2, 0, 0);
publishVelocity();
}
if(laser_right_ < 0.6 || laser_left_ < 0.6)
{
while(laser_right_ < 0.6)
{
setVelocity(0, 0.2, 0);
publishVelocity();
}
while(laser_left_ < 0.6)
{
setVelocity(0, -0.2, 0);
publishVelocity();
}
}
}
}
else if (mode_ == store_modes::VISION)
{
if (state_ != store_puck_states::LEAVING_PUCK && state_ != store_puck_states::GOING_BACK_TO_ORIGIN)
{
robotino_vision::FindInsulatingTapeAreas srv;
find_areas_cli_.waitForExistence();
if (!find_areas_cli_.call(srv))
{
ROS_ERROR("Area not found!!!");
state_ = store_puck_states::LOST;
return;
}
std::vector<float> distances = srv.response.distances;
std::vector<float> directions = srv.response.directions;
int num_areas = srv.response.distances.size();
if (num_areas > 0 && state_ != store_puck_states::STORING_PUCK)
{
int closest_area_index = 0;
for (int i = 0; i < num_areas; i++)
{
if (distances[i] < distances[closest_area_index] && fabs(directions[i]) <fabs(directions[closest_area_index]))
{
closest_area_index = i;
}
}
double max_error_lateral = 50, max_error_frontal = 40;
double error_lateral = directions[closest_area_index];
if (error_lateral > max_error_lateral)
{
error_lateral = max_error_lateral;
}
double error_frontal = distances[closest_area_index];
if (error_frontal > max_error_frontal)
{
error_frontal = max_error_frontal;
}
float tolerance_lateral = 0.1, tolerance_frontal = 30;
double K_error_lateral = 0.3, K_error_frontal = 0.003;
double percentage_f, percentage_0, tolerance, max_error, error;
if (fabs(error_lateral) > tolerance_lateral) // 0% a 49%
{
state_ = store_puck_states::ALIGNING_LATERAL;
vel_y = -K_error_lateral * error_lateral;
percentage_0 = 0;
percentage_f = 49;
tolerance = tolerance_lateral;
max_error = max_error_lateral;
error = fabs(error_lateral);
}
else if (fabs(error_frontal) > tolerance_frontal) // 50% a 69%
{
state_ = store_puck_states::HEADING_TOWARD_AREA;
vel_x = K_error_frontal * error_frontal;
percentage_0 = 50;
percentage_f = 69;
tolerance = tolerance_frontal;
max_error = max_error_frontal;
error = fabs(error_frontal);
}
else
{
vel_x = .14;
state_ = store_puck_states::STORING_PUCK;
storing_start_ = ros::Time::now();
}
percentage = percentage_0 + (percentage_f - percentage_0) * (max_error - error) / (max_error - tolerance);
}
else if (state_ == store_puck_states::STORING_PUCK) // 70% a 89%
{
vel_x = .1;
double percentage_0 = 70, percentage_f = 89;
double elapsed_time = (ros::Time::now() - storing_start_).toSec();
if (elapsed_time > STORING_DEADLINE)
{
state_ = store_puck_states::LEAVING_PUCK;
}
percentage = percentage_0 + (percentage_f - percentage_0) * elapsed_time / STORING_DEADLINE;
}
}
else
{
if (state_ == store_puck_states::LEAVING_PUCK)
{
delta_x_ = -getOdometry_X();
vel_x = -.14;
resetOdometry();
state_ = store_puck_states::GOING_BACK_TO_ORIGIN;
percentage_ = 89;
}
double max_error_linear = 1.25;
double error_linear = delta_x_ - getOdometry_X();
if (error_linear > max_error_linear)
{
error_linear = max_error_linear;
}
float tolerance_linear = 0.01;
double K_error_linear = 1.2;
double percentage_f, percentage_0, tolerance, max_error, error;
if (fabs(error_linear) > tolerance_linear) // 90% a 99%
{
state_ = store_puck_states::GOING_BACK_TO_ORIGIN;
vel_x = K_error_linear * error_linear;
percentage_0 = 91;
percentage_f = 99;
tolerance = tolerance_linear;
max_error = max_error_linear;
error = fabs(error_linear);
}
percentage = percentage_0 + (percentage_f - percentage_0) * (max_error - error) / (max_error - tolerance);
}
}
else
{
ROS_ERROR("Storage Mode not supported yet!!!");
return;
}
if (vel_x == 0 && vel_y == 0 && vel_phi == 0) // 100%
{
state_ = store_puck_states::FINISHED;
percentage_ = 100;
}
if (percentage > percentage_)
{
percentage_ = percentage;
}
setVelocity(vel_x, vel_y, vel_phi);
publishVelocity();
publishFeedback();
}