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