/**
* Seta os valores de velocidade passados através dos requisitos e retorna uma mensagem
*/
bool rosaria_motion::RosariaMotionNode::setVelocity(rosaria_motion::SetVelocity::Request &req, rosaria_motion::SetVelocity::Response &res)
{
if (req.linear == 0 && req.angular == 0)
{
res.message = "Jah jah que para. rsrsrs";
}
else if (req.linear == 0)
{
res.message = "Vai ficar rodopiando.";
}
else if (req.angular == 0)
{
res.message = "Ao infinito e aleinh!!!";
}
else if (req.linear < 0)
{
res.message = "Fala serio!! Andar de reh??? Sinto muito, soh vale pra frente.";
}
else
{
res.message = "Xah comigo!!!";
}
vel_x_ = fabs(req.linear);
vel_phi_ = req.angular;
ROS_INFO("New linear velocity: %f", vel_x_);
ROS_INFO("New angular velocity: %f", vel_phi_);
resetOdometry();
publishVelocity();
return true;
}
void rosaria_motion::RosariaMotionNode::calculateVelocity()
{
if (prev_disp_x_ < 0 && disp_x_ > 0)
{
vel_phi_ = -vel_phi_;
resetOdometry();
publishVelocity();
}
}
bool StorePuckServer::validateNewGoal(const robotino_motion::StorePuckGoalConstPtr& goal)
{
if(state_ == store_puck_states::UNINITIALIZED)
{
result_.goal_achieved = false;
result_.message = "Odometry not initialized yet!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("%s", result_.message.c_str());
return false;
}
if(!loaded_)
{
result_.goal_achieved = false;
result_.message = "Robotino is not loaded!!!";
//server_.setAborted(result_, result_.message);
ROS_ERROR("%s", result_.message.c_str());
//return false;
}
mode_ = StoreModes::newInstance(goal->mode);
store_number_ = StoreStoreNumbers::newInstance(goal->store_number);
if (mode_ == store_modes::NONE)
{
result_.goal_achieved = false;
result_.message = "Invalid storage mode code!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("Invalid storage mode code: %d!!!", goal->mode);
return false;
}
if (store_number_ == store_store_numbers::NONE)
{
result_.goal_achieved = false;
result_.message = "Invalid storage number code!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("Invalid storage number code: %d!!!", goal->store_number);
return false;
}
// seria bom verificar se tem peça na área (se a area é muito pequena)
/*robotino_vision::FindObjects srv;
srv.request.color = Colors::toCode(color_);
if (!find_objects_cli_.call(srv))
{
result_.goal_achieved = false;
result_.message = Colors::toString(color_) + " puck not found!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("%s", result_.message.c_str());
return false;
}*/
percentage_ = 0;
resetOdometry();
state_ = store_puck_states::ALIGNING_LATERAL;
ROS_INFO("Goal accepted, storing puck in %s mode!!!", StoreModes::toString(mode_).c_str());
return true;
}
bool GrabPuckServer::validateNewGoal(const robotino_motion::GrabPuckGoalConstPtr& goal)
{
if(state_ == grab_puck_states::UNINITIALIZED)
{
result_.goal_achieved = false;
result_.message = "Odometry not initialized yet!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("%s", result_.message.c_str());
return false;
}
if(loaded_)
{
result_.goal_achieved = false;
result_.message = "Robotino is already loaded!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("%s", result_.message.c_str());
return false;
}
color_ = Colors::toColor(goal->color);
if (color_ == colors::NONE)
{
result_.goal_achieved = false;
result_.message = "Invalid color code!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("Invalid color code: %d!!!", goal->color);
return false;
}
if (color_ != colors::YELLOW && color_ != colors::BLUE && color_ != colors::GREEN && color_ != colors::RED)
{
result_.goal_achieved = false;
result_.message = Colors::toString(color_) + " color is not working because it was not calibrated yet in robotino_vision package!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("%s", result_.message.c_str());
return false;
}
robotino_vision::FindObjects srv;
srv.request.color = Colors::toCode(color_);
if (!find_objects_cli_.call(srv))
{
result_.goal_achieved = false;
result_.message = Colors::toString(color_) + " puck not found!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("%s", result_.message.c_str());
return false;
}
loaded_ramp_ = false;
percentage_ = 0;
resetOdometry();
state_ = grab_puck_states::ALIGNING_LATERAL;
ROS_INFO("Goal accepted, grabbing %s puck!!!", Colors::toString(color_).c_str());
return true;
}
void Syrotek::activate()
{
_chasis->activate();
_wheel[0]->activate();
_wheel[1]->activate();
_ball[0]->activate();
_ball[1]->activate();
_jball[0]->activate();
_jball[1]->activate();
_jwheel[0]->activate();
_jwheel[1]->activate();
resetOdometry();
}
bool ReadPuckServer::validateNewGoal(const robotino_motion::ReadPuckGoalConstPtr& goal)
{
if(state_ == read_puck_states::UNINITIALIZED)
{
result_.goal_achieved = false;
result_.message = "Odometry not initialized yet!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("%s", result_.message.c_str());
return false;
}
valid_colors_.clear();
for (int i = 0; i < goal->valid_colors.size(); i++)
{
int counter = 0;
Color color = Colors::toColor(goal->valid_colors[i]);
if (color == colors::NONE)
{
result_.goal_achieved = false;
result_.message = "Invalid color code!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("Invalid color code: %d!!!", color);
return false;
}
if (color != colors::YELLOW && color != colors::BLUE && color != colors::GREEN && color != colors::RED)
{
result_.goal_achieved = false;
result_.message = Colors::toString(color) + " color is not working because it was not calibrated yet in robotino_vision package!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("%s", result_.message.c_str());
return false;
}
/*robotino_vision::FindObjects srv;
srv.request.color = Colors::toCode(color);
if (find_objects_cli_.call(srv))
{
find_objects_cli_.
}*/
valid_colors_.push_back(color);
for (int j = 0; j < valid_colors_.size(); j++)
{
if (color == valid_colors_[j])
{
counter++;
if (counter > 1)
{
valid_colors_.erase(valid_colors_.begin() + j);
break;
}
}
}
}
if(valid_colors_.empty())
{
result_.goal_achieved = false;
result_.message = "There is no valid color setted!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("%s", result_.message.c_str());
return false;
}
verify_markers_ = goal->verify_markers;
ROS_INFO("Verify markers: %s, %d", verify_markers_?"true":"false", verify_markers_);
for (int i = 0; i < goal->valid_number_of_markers.size(); i++)
{
valid_number_of_markers_.push_back(goal->valid_number_of_markers[i]);
}
/*robotino_vision::FindObjects srv;
srv.request.color = Colors::toCode(color_);
if (!find_objects_cli_.call(srv))
{
result_.goal_achieved = false;
result_.message = Colors::toString(color_) + " puck not found!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("%s", result_.message.c_str());
return false;
}*/
reading_start_ = ros::Time::now();
percentage_ = 0;
resetOdometry();
state_ = read_puck_states::HEADING_TOWARD_PUCK;
ROS_INFO("Goal accepted, reading puck!!!");
return true;
}
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();
}
