C++ (Cpp) printGoal Examples

Example #1

File: draw_something.cpp Project: apojomovsky/draw_something

// funcion donde se ejecuta la logica, se llama cada vez que se cumple el tiempo configurado en timer
void timerCallback(const ros::TimerEvent&, ros::Publisher twist_pub)
{
  if (!g_pose)	// si g_pose==0, termina la funcion
  {
    return;
  }

  if (!g_first_goal_set) // si no hubiese una meta, ejecuta:
  {
    g_first_goal_set = true;
    g_state = FORWARD;
    g_goal.x = distance[d]*cos(g_pose->theta) + g_pose->x;
    g_goal.y = distance[d]*sin(g_pose->theta) + g_pose->y;
    g_goal.theta = g_pose->theta;
		d++;
    printGoal();
  }

  if (g_state == FORWARD)	// si el estado es FORWARD, ejecuta:
  {
    forward(twist_pub);	// envia la orden de movimiento a la funcion forward
  }
  else if (g_state == STOP_FORWARD)	// si el estado es STOP_FORWARD, ejecuta:
  {
    stopForward(twist_pub); // envia la orden de detenerse a la funcion stop_forward
  }
  else if (g_state == TURN) // si el estado es TURN, ejecuta:
  {
    turn(twist_pub); // envia la orden de giro a la funcion turn
  }
  else if (g_state == STOP_TURN) // si el estado es STOP_TURN, ejecuta:
  {
    stopTurn(twist_pub); // envia la orden de detener giro a la funcion stop_turn
  }
}

Example #2

File: draw_square.cpp Project: florisvb/Flyatar

void timerCallback(const ros::TimerEvent&, ros::Publisher vel_pub)
{
  if (!g_pose)
    {
      return;
    }

  if (!g_first_goal_set)
    {
      g_first_goal_set = true;
      g_state = FORWARD;
      g_goal.x = cos(g_pose->theta) * 2 + g_pose->x;
      g_goal.y = sin(g_pose->theta) * 2 + g_pose->y;
      g_goal.theta = g_pose->theta;
      printGoal();
    }

  if (g_state == FORWARD)
    {
      forward(vel_pub);
    }
  else if (g_state == STOP_FORWARD)
    {
      stopForward(vel_pub);
    }
  else if (g_state == TURN)
    {
      turn(vel_pub);
    }
  else if (g_state == STOP_TURN)
    {
      stopTurn(vel_pub);
    }
}

Example #3

File: draw_square.cpp Project: florisvb/Flyatar

void stopTurn(ros::Publisher vel_pub)
{
  if (hasStopped())
    {
      ROS_INFO("Reached goal");
      g_state = FORWARD;
      g_goal.x = cos(g_pose->theta) * 2 + g_pose->x;
      g_goal.y = sin(g_pose->theta) * 2 + g_pose->y;
      g_goal.theta = g_pose->theta;
      printGoal();
    }
  else
    {
      commandFly(vel_pub, 0, 0);
    }
}

Example #4

File: draw_square.cpp Project: florisvb/Flyatar

void stopForward(ros::Publisher vel_pub)
{
  if (hasStopped())
    {
      ROS_INFO("Reached goal");
      g_state = TURN;
      g_goal.x = g_pose->x;
      g_goal.y = g_pose->y;
      g_goal.theta = fmod(g_pose->theta + PI/2.0, 2*PI);
      printGoal();
    }
  else
    {
      commandFly(vel_pub, 0, 0);
    }
}

Example #5

File: draw_something.cpp Project: apojomovsky/draw_something

// detiene el movimiento de giro
void stopTurn(ros::Publisher twist_pub)
{
  if (hasStopped()) // si el giro se ha detenido:
  {
    ROS_INFO("Reached goal");


    g_state = FORWARD;
    g_goal.x = distance[d]* cos(g_pose->theta) + g_pose->x;
    g_goal.y = distance[d]* sin(g_pose->theta) + g_pose->y;
    g_goal.theta = g_pose->theta;
		d++;
    printGoal();
  }
  else	// sino, da la orden de detener el giro:
  {
    commandTurtle(twist_pub, 0, 0);
  }
}

Example #6

File: draw_something.cpp Project: apojomovsky/draw_something

// detiene el movimiento forward
void stopForward(ros::Publisher twist_pub)
{
	double temp;
  if (hasStopped()) // si la tortuga se ha detenido:
  {
    ROS_INFO("Reached goal");
    g_state = TURN;
    g_goal.x = g_pose->x;	// mantiene los valores de x e y
    g_goal.y = g_pose->y;
		if((g_pose->theta + angle[a]) >= 2*PI)  {
    	g_goal.theta =g_pose->theta + angle[a] - 2*PI;		// mantiene el valor de theta menor a 2*PI
		}
		else g_goal.theta =g_pose->theta + angle[a];
		a++;
    printGoal();
  }
  else	// sino, da la orden de detenerla
  {
    commandTurtle(twist_pub, 0, 0);
  }
}