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;
}
Exemplo n.º 3
0
  /** 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();
}
Exemplo n.º 9
0
 /** 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();
}