Colony

#include "../helper_classes/Order.h"

WH_Robot::WH_Robot(std::string scoutname):Behavior(scoutname, "WH_Robot")

    nav_map = new navigationMap(scoutname);

    name = scoutname;

    reg_failed = 1;

    robot_to_sched = node.advertise<std_msgs::String>("robot_to_sched", 1000);

    sched_to_robot = node.subscribe(name + "_topic", 1000, &WH_Robot::robot_callback, this);

    ROS_INFO("A WH_Robot has been created");

WH_Robot::~WH_Robot()

    delete(nav_map);

Duration WH_Robot::get_worst_case_time(State start_state, State target_state)

{

  return nav_map->get_worst_case_time(start_state, target_state);

}

    ROS_INFO("WH_robot: Executing a task");

      ROS_INFO("WH_robot: TASK COMPLETE!");

      ROS_INFO("WH_robot: TASK FAILED!");

void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg)

{

  if(msg->data.compare(0, 11, "REG_SUCCESS") == 0)

  {

    id = atoi(msg->data.substr(12).c_str());

    reg_failed = 0;

  }

  else if(msg->data.compare(0, 8, "SET_TASK") == 0)

  {

    char* string = (char*)msg->data.c_str();

    char* data;

    data = strtok(string, " ");

    int order_id = atoi(data);

    data = strtok(string, " ");

    Address source = atoi(data);

    data = strtok(string, " ");

    Address dest = atoi(data);

    Time start_time = ros::Time::now();

    Path path;

    path.len = 0;

    path.path = NULL;

    Duration est_time(0);

    curr_task = new Order(order_id, source, dest, start_time, path, est_time);

  }

  else

  {

    ROS_INFO("I got a bad message: %s", msg->data.c_str());

  }

}

void WH_Robot::run ()

{

    ROS_INFO("I am a robot. Registering with scheduler...");

    while(reg_failed)

      std_msgs::String msg;

      std::stringstream ss;

      ss << "REGISTER "<<name;

      msg.data = ss.str();

      robot_to_sched.publish(msg);

      spinOnce();

    while(ok())

        ROS_INFO("WH_ROBOT: Running main loop");

        std_msgs::String msg;

        std::stringstream ss;

        ss << "GET_TASK "<<id;

        msg.data = ss.str();

        robot_to_sched.publish(msg);

        spinOnce();

        while(curr_task == DEFAULT_TASK)

          continue;

        int error = exec_task();

        if(error == TASK_COMPLETED)

        {

        ss << "SUCCESS "<<curr_task->getid();

        msg.data = ss.str();

        robot_to_sched.publish(msg);

        }

        else //error == TASK_FAILED

        {

        ss << "FAILED "<<curr_task->getid();

        msg.data = ss.str();

        robot_to_sched.publish(msg);

       }

        delete curr_task;

        curr_task = DEFAULT_TASK;

#define DEFAULT_TASK NULL

/*Deleted Behaviors.h*/

#include "navigationMap.h"/*Do we do something here?*/

#include "helper_classes/Order.h"

/* ~~Begin Variable Declarations~~ */

int id;

int reg_failed;

navigationMap* nav_map;

/* ^ Is this going to be changed? Do we need to conv this to c?*/

/* ~~The End of all declarations : variable related~~*/

/*On colony 3 bots time is just an int (being ms || 1/16 of a sec)*/

int get_worst_case_time(State start_state, State target_state);

void robot_callback(const std_msgs::String::ConstPtr& msg);

/*WH_Robot(std::string scoutname); ->Coalesce into the main function*/

/*~WH_Robot(); ->Deconstructors don't exist in C*/

/*void run(); ->Coalesce into the main function*/

/* These subscribers must be dealt with through wireless checking

ros::Publisher robot_to_sched;

ros::Subscriber sched_to_robot;

*/

#ifndef _ORDER_

#define _ORDER_

#include "../Behavior.h"

#include "../behaviors/navigationMap.h"

#define MAX_WAIT_TIME 120 //2 minutes in seconds

typedef unsigned int Address;

class Order {

    int id;

    Address source, dest;

    Time start_time;

    Path path;

    Duration est_time;

public:

    Order(int order_id, Address order_source, Address order_dest, 

        Time order_start_time, Path order_path, Duration order_est_time);

    Order() {};

    int getid() const;

    Address get_source() const;

    Address get_dest() const;

    Time get_start_time() const;

    Path get_path() const;

    Duration get_est_time() const;

    void set_path(Path order_path);

    int get_priority() const;

    bool operator==(Order& order);

};

class CompareOrder {

  public:

    bool operator()(Order& o1, Order& o2);   

};

#endif

    Time order_start_time, Path order_path, Duration order_est_time) 

Duration Order::get_est_time() const

int Order::get_priority() const

    return start_time.toSec() + MAX_WAIT_TIME - est_time.toSec();

bool Order::operator==(Order& order)

{

  return this->id == order.id;

}

  return o1.get_priority() > o2.get_priority();