void ReadPuckServer::executeCallback(const robotino_motion::ReadPuckGoalConstPtr& goal)
{
ros::Rate loop_rate(20);
if(!validateNewGoal(goal))
{
ROS_WARN("Goal not accepted!!!");
return;
}
while(nh_.ok())
{
if(server_.isPreemptRequested())
{
if(server_.isNewGoalAvailable() && !validateNewGoal(server_.acceptNewGoal()))
{
ROS_WARN("Goal not accepted!!!");
return;
}
ROS_INFO("Cancel request!!!");
server_.setPreempted();
state_ = read_puck_states::IDLE;
setVelocity(0, 0, 0);
publishVelocity();
return;
}
controlLoop();
if(state_ == read_puck_states::FINISHED)
{
state_ = read_puck_states::IDLE;
setVelocity(0, 0, 0);
publishVelocity();
result_.goal_achieved = true;
result_.message = "Goal achieved with success!!!";
server_.setSucceeded(result_);
ROS_INFO("%s goal reached!!!", name_.c_str());
return;
}
else if (state_ == read_puck_states::LOST)
{
state_ = read_puck_states::IDLE;
setVelocity(0, 0, 0);
publishVelocity();
result_.goal_achieved = false;
result_.message = "Robotino got lost!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("%s", result_.message.c_str());
return;
}
ros::spinOnce();
loop_rate.sleep();
}
state_ = read_puck_states::IDLE;
setVelocity(0, 0, 0);
publishVelocity();
result_.goal_achieved = false;
result_.message = "Aborting on the goal because the node has been killed!!!";
server_.setAborted(result_, result_.message);
}
/**
* 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;
}
/** Try to advance towards the current target pose.
*
* Queries the current position of the base in the odometry frame and
* passes it to the @ref VelocityController object, which calculates the
* velocities that are needed to move in the target pose direction.
*
* @return false if the goal pose was reached, true if we need to proceed. */
bool moveTowardsGoal()
{
tf::StampedTransform transform;
try
{
ros::Time time;
std::string str;
transform_listener_.getLatestCommonTime("odom", "base_footprint", time, &str);
transform_listener_.lookupTransform("odom", "base_footprint", time, transform);
}
catch (tf::TransformException& ex)
{
ROS_WARN("Transform lookup failed (\\odom -> \\base_footprint). Reason: %s.", ex.what());
return true; // goal was not reached
}
Pose current_pose(transform.getOrigin().getX(), transform.getOrigin().getY(), tf::getYaw(transform.getRotation()));
Velocity velocity = velocity_controller_->computeVelocity(current_pose);
if (velocity_controller_->isTargetReached())
{
ROS_INFO("The goal was reached.");
return false;
}
else
{
publishVelocity(velocity);
return true;
}
}
void ReadPuckServer::stop()
{
setVelocity(0, 0, 0);
publishVelocity();
state_ = read_puck_states::IDLE;
result_.goal_achieved = false;
result_.message = "Unexpected emergency stop request!!!";
server_.setAborted(result_, result_.message);
}
void rosaria_motion::RosariaMotionNode::calculateVelocity()
{
if (prev_disp_x_ < 0 && disp_x_ > 0)
{
vel_phi_ = -vel_phi_;
resetOdometry();
publishVelocity();
}
}
void StorePuckServer::laserDistanceFront(StoreStoreNumber mode)
{
printf("Entrou no laserDistanceFront!!!!");
if (mode == store_store_numbers::NEAR)
{
while(laser_front_ > 0.13)
{
setVelocity(0.1, 0, 0);
publishVelocity();
}
setVelocity(0.0, 0.0, 0.0);
publishVelocity();
}
if (mode == store_store_numbers::MIDDLE)
{
printf("entrou no MIDDLE");
while(laser_front_ > 0.78)
{
setVelocity(0.1, 0, 0);
publishVelocity();
}
setVelocity(0.0, 0.0, 0.0);
publishVelocity();
}
if (mode == store_store_numbers::FAR_AWAY)
{
printf("entrou no FAR_AWAY");
while(laser_front_ > 1.43)
{
setVelocity(0.1, 0, 0);
publishVelocity();
}
setVelocity(0.0, 0.0, 0.0);
publishVelocity();
}
}
void ReadPuckServer::controlLoop()
{
double vel_x = 0, vel_y = 0, vel_phi = 0;
ROS_INFO("%s", ReadPuckStates::toString(state_).c_str());
double percentage = 0;
if (state_ == read_puck_states::ALIGNING_FRONTAL)
{
/////////////////////////////ALINHAR COM A FRENTE NEM SEMPRE É BOM
robotino_motion::AlignGoal frontal_alignment;
frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
frontal_alignment.distance_mode = 1; // NORMAL distance mode
align_client_.sendGoal(frontal_alignment);
align_client_.waitForResult(ros::Duration(1.0));
state_ = read_puck_states::HEADING_TOWARD_PUCK;
}
else if (state_ == read_puck_states::HEADING_TOWARD_PUCK || state_ == read_puck_states::HEADING_BACKWARD_PUCK)
{
if (state_ == read_puck_states::HEADING_TOWARD_PUCK)
{
vel_x = .1;
}
else
{
vel_x = -.1;
}
robotino_vision::FindObjects srv;
srv.request.specific_number_of_markers = 0;
if (verify_markers_)
{
srv.request.verify_markers = true;
}
else
{
srv.request.verify_markers = false;
}
for (int i = 0; i < valid_colors_.size(); i++)
{
srv.request.color = valid_colors_[i];
find_objects_cli_.waitForExistence();
Color color = Colors::toColor(valid_colors_[i]);
if (!find_objects_cli_.call(srv))
{
ROS_WARN("There is no %s object", Colors::toString(color).c_str());
continue;
}
std::vector<int> number_of_markers;
for (int j = 0; j < srv.response.number_of_markers.size(); j++)
{
number_of_markers.push_back(srv.response.number_of_markers[i]);
}
if (!number_of_markers.empty())
{
for (int j = 0; j < number_of_markers.size(); i++)
{
Puck puck;
puck.color = color;
puck.number_of_markers = number_of_markers[j];
updatePucks(puck);
}
}
}
}
if ((ros::Time::now() - reading_start_).toSec() > READING_DEADLINE) // 100%
{
if (state_ == read_puck_states::HEADING_TOWARD_PUCK)
{
reading_start_ = ros::Time::now();
state_ == read_puck_states::HEADING_BACKWARD_PUCK;
}
else
{
vel_x = 0;
if (state_ == read_puck_states::HEADING_TOWARD_PUCK && pucks_.empty())
{
result_.goal_achieved = false;
result_.message = "No puck!!!";
server_.setAborted(result_, result_.message);
ROS_ERROR("%s", result_.message.c_str());
return;
}
int sum_parts = 0;
int sum_total = 0;
for (int i = 0; i < pucks_.size(); i++)
{
ROS_INFO("Index: %d, Counter: %d", i, pucks_[i].counter);
sum_parts += i * pucks_[i].counter;
sum_total += pucks_[i].counter;
}
int index = round(sum_parts / sum_total);
result_.color = Colors::toCode(pucks_[index].color);
result_.number_of_markers = pucks_[index].number_of_markers;
ROS_INFO("----------------------------");
ROS_INFO("Final Puck:/n Color:%s, Number_of_markers: %d", Colors::toString(pucks_[index].color).c_str(), pucks_[index].number_of_markers);
state_ = read_puck_states::FINISHED;
percentage_ = 100;
}
}
if (percentage > percentage_)
{
percentage_ = percentage;
}
setVelocity(vel_x, vel_y, vel_phi);
publishVelocity();
}
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();
}
/** Publish a velocity command which will stop the robot. */
inline void publishZeroVelocity()
{
publishVelocity(Velocity());
}
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();
}
