Colony

/** @file colonet_wireless.c

 *

 * @brief Implementation of server-side colonet wireless library

 *

 * @author Eugene Marinelli

 */

/********************************* Includes **********************************/

#include <string>

#include <pthread.h>

#include <iostream>

#include <fstream>

#include <unistd.h>

#include <fcntl.h>

#include <signal.h>

#include <ColonetServer.h>

#include <wireless.h> // Colonet wireless library.

#include <wl_token_ring.h>

#include <colonet_defs.h>

#include <colonet_wireless.h>

/******************************* Definitions *********************************/

//Enable debug printouts

#define DEBUG 1

#ifdef DEBUG

#define dbg_printf(...) printf(__VA_ARGS__)

#define dbg_fprintf(...) fprintf(__VA_ARGS__)

#else

#define dbg_printf(...)

#define dbg_fprintf(...)

#endif

extern ColonetServer colonet_server;

static bool logging_enabled;

static char log_filename[80];

static pthread_t listener_thread;

static char wl_port[40];

/************************* Internal prototypes *******************************/

static void* listen(void* args);

static void timeout_handler(void);

static void handle_response(int frame, int received);

static void handle_receive(char type, int source, unsigned char* packet, int len);

static void unregister(void);

static int log_packet(unsigned char* packet, int len);

/**************************** Public functions *******************************/

/**

 * @brief Initializes the wireless library

 *

 * @param wl_port_ The port to listen for wireless messages on

 * @param log_filename_ The name of the log file

 *

 * @return 0 on success, negative error code on failure

 */

int colonet_wl_init(char* wl_port_, char* log_filename_) {

  if (log_filename_ != NULL) {

    logging_enabled = true;

    strcpy(log_filename, log_filename_);

  }

  strncpy(wl_port, wl_port_, 40);

  wl_set_com_port(wl_port);

  fprintf(stderr, "Calling wl_init(%s)...\n", wl_port);

  if (wl_init() != 0) {

    fprintf(stderr, "wl_init failed.\n");

    return -1;

  }

  wl_token_ring_register();

  fprintf(stderr, "Joining token ring...\n");

  if (wl_token_ring_join() != 0) {

    fprintf(stderr, "Failed to join token ring.\n");

    return -1;

  }

  fprintf(stderr, "Joined token ring.\n");

  return 0;

}

/**

 * @brief This function will kill the thread that is listening for wireless messages

 */

void colonet_wl_kill_listener_thread() {

  pthread_kill(listener_thread, 9);

}

/**

 * @brief This function spawns a thread to listen for wireless messages

 *

 * @return 0 on success, negative error code on failure

 */

int colonet_wl_run_listener_thread() {

  dbg_printf("Spawning listener thread...\n");

  if (pthread_create(&listener_thread, NULL, listen, NULL)) {

    perror("pthread_create");

    return -1;

  }

  return 0;

}

/**

 * @brief This function sends a message out on the wireless

 *

 * @param client_source The index in the connection pool of the client that is sending the message

 * @param dest The robot that is meant to receive this message

 * @param msg_type The type of the message

 * @param msg_code The message code

 * @param args The arguments to send with the message

 *

 * @return 0 on success, a negative error code on failure

 */

int colonet_wl_send(short client_source, short dest, ColonetRobotMessageType msg_type, unsigned char msg_code,

                    unsigned char* args) {

  printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", client_source, dest, msg_code);

  //create and set up new packet to be sent.

  ColonetRobotServerPacket pkt;

  pkt.client_id = client_source;

  pkt.msg_code = msg_code;

  //copy over the args into the data portion of the packet

  for (int i = 0; i < PACKET_DATA_LEN; i++) {

    //printf("data %i: %u\n", i, args[i]);

    pkt.data[i] = args[i];

  }

  if (dest == GLOBAL_DEST) {

    //printf("sending to global dest\n");

    int ret = wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt),

                                    sizeof(ColonetRobotServerPacket), 0);

    if (ret != 0) {

      fprintf(stderr, "%s: wl_send_global_packet failed.\n", __FUNCTION__);

      return -1;

    }

  } else {

    //("sending to specific robot: %d.\n", dest);

    int ret = wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt),

                                                   sizeof(ColonetRobotServerPacket), dest,

                                                   COLONET_RESPONSE_PACKET_FRAME_ID);

    if (ret != 0) {

      fprintf(stderr, "%s: wl_send_robot_to_robot_global_packet failed.\n", __FUNCTION__);

      return -1;

    }

  }

  return 0;

}

/**

 * @brief Used to get the number of robots in the token ring

 *

 * @return The result of wl_token_get_num_robots()

 */

int colonet_get_num_robots(void) {

  return wl_token_get_num_robots();

}

/**

 * @brief Gets a list of the xbee ids of the ones in the token ring

 *

 * @param numrobots A pointer to the variable where you want the number of robots to be stored

 *

 * @return A dynamically allocated piece of memory that contains the list of xbee ids.

 *   The function calling this function must free this memory when done with it.

 */

int* colonet_get_xbee_ids(int* numrobots) {

  int num_robots = wl_token_get_num_robots();

  int* ids = (int*)malloc(num_robots * sizeof(int));

  wl_token_iterator_begin();

  int i = 0;

  while (wl_token_iterator_has_next()) {

    ids[i] = wl_token_iterator_next();

    i++;

  }

  *numrobots = num_robots;

  return ids;

}

/**

 * @brief Gets the sensor matrix from the wireless library

 *

 * @param numrobots A pointer to the variable where you want the number of robots to be stored

 * @param ids_ A pointer to a pointer that you want to point to the list of xbee ids.  This memory is dynamically

 * allocated and should be freed by the calling process when it is done with it

 *

 * @return The 2d matrix reprsenting the sensor matrix.  This memory is dynamically allocated and should be freed by

 * the calling process when it is done with it

 */

int** colonet_get_sensor_matrix(int* numrobots, int** ids_) {

  int num_robots;

  int* ids = colonet_get_xbee_ids(&num_robots);

  int** m = (int**)malloc(num_robots * sizeof(int*));

  for (int i = 0; i < num_robots; i++) {

    m[i] = (int*)malloc(num_robots * sizeof(int*));

  }

  for (int i = 0; i < num_robots; i++) {

    for (int j = 0; j < num_robots; j++) {

      m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]);

    }

  }

  *numrobots = num_robots;

  *ids_ = ids;

  return m;

}

/**************************** Private functions ******************************/

static void timeout_handler() {

  // printf("colonet wireless - timeout!\n");

}

static void handle_response(int frame, int received) {

  //TODO: These are just here to get rid of compiler warnings

  frame = frame;

  received = received;

  printf("handle_response\n");

}

static void handle_receive(char type, int source, unsigned char* data, int len) {

  type = type; //TODO: This is just here to get rid of compiler warnings.

  printf("***Got packet from robot***\n");

  ColonetRobotServerPacket* pkt = (ColonetRobotServerPacket*)data;

  if (logging_enabled) {

    log_packet(data, len);

  }

  //see if a robot wants the server to tell it where it is

  if (pkt->msg_code == ROBOT_REQUEST_POSITION_FROM_SERVER) {

    /* Robot has requested its position. */

    int robot_x, robot_y;

    PositionMonitor* pm = colonet_server.getPositionMonitor();

    int num_robots = pm->getNumVisibleRobots();

    //ask the position monitor for where that robot is.

    int ret = pm->getRobotPosition(source, &robot_x, &robot_y);

    if (ret != 0 && num_robots != 1) {

      fprintf(stderr, "Robot %d requested position, but its position is not known.\n", source);

    } else {

      if (ret != 0 /* && num_robots == 1 */) {

        VisionPosition pos = pm->getFirstPosition();

        robot_x = pos.x;

        robot_y = pos.y;

      }

      unsigned char response[80];

      response[0] = (robot_x >> 8) & 0xFF;

      response[1] = robot_x & 0xFF;

      response[2] = (robot_y >> 8) & 0xFF;

      response[3] = robot_y & 0xFF;

      fprintf(stderr, "Robot %d requested position.  Its position is %d,%d.\n", source, robot_x, robot_y);

      if (colonet_wl_send(-1, source, COLONET_COMMAND, SERVER_REPORT_POSITION_TO_ROBOT, response) != 0) {

        fprintf(stderr, "colonet_wl_send failed!\n");

        exit(1);

      }

    }

  } else {

    printf("Calling colonet_server.process_received_wireless_message\n");

    char processed_data[80];

    sprintf(processed_data, "%d %d %d %s\n", RESPONSE_TO_CLIENT_REQUEST, pkt->msg_code, source, pkt->data);

    fprintf(stderr, "client_id=%d; processed_data: %s\n", pkt->client_id, processed_data);

    colonet_server.process_received_wireless_message(pkt->client_id, processed_data, strlen(processed_data));

  }

}

static void unregister(void) {

  printf("unregister\n");

}

/** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.

 * Listens for bytes on the wireless port and constructs packets based on them.

 * Not part of the ColonetWireless class since a function pointer must be

 * passed in starting the thread that runs it (tricky or impossible if it's

 * a class function).

 *

 * @param args Pointer to arguments.  Can be safely casted to ListenerArgs type

 * @return NULL

 */

static void* listen(void* args) {

  args = args; //TODO: These are just here to get rid of compiler warnings.

  fprintf(stderr, "Called listen.\n");

  PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID, timeout_handler, handle_response, handle_receive, unregister};

  wl_register_packet_group(&pgh);

  while (1) {

    wl_do();

    usleep(1000);

  }

  fprintf(stderr, "Listen is returning.\n");

  wl_terminate();

  return NULL;

}

/** @brief

 *

 * @param pkt Packet to be logged

 *

 * @return 0 on success, -1 on failure

 */

int log_packet(unsigned char* packet, int len) {

  FILE* logfile = fopen(log_filename, "a");

  if (logfile == NULL) {

    dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);

    return -1;

  }

  for (int i = 0; i < len; i++) {

    fprintf(logfile, "%d ", *((unsigned char*)packet + i));

  }

  fprintf(logfile, "\n");

  fclose(logfile);

  return 0;

}