// 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 } }
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); } }
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); } }
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); } }
// 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); } }
// 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); } }