Colony » Colony Scout » Colony Scout OS

#include "WH_Robot.h"

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

/** @Brief: warehouse robot constructor **/

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

{

    ROS_INFO("WH_ROBOT: Creating a WH_Robot...");

    nav_map = new navigationMap(scoutname);

    curr_task = DEFAULT_TASK;

    name = scoutname;

    reg_failed = 1;

    robot_to_sched = n.advertise<std_msgs::String>(name + "_topic", 1000);

    sched_to_robot = n.subscribe("sched_to_robot", 1000, msg_callback);

    while(reg_failed)

    {

      std_msgs::String msg;

      std::stringstream ss;

      ss << "REGISTER "<<name;

      msg.data = ss.str();

      robot_to_sched.publish(msg);

    }

    ROS_INFO("WH_Robot: I am a created WH robot");

}

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

}

int WH_Robot::exec_task()

{

    ROS_INFO("WH_robot: Executing a task");

    assert(curr_task != DEFAULT_TASK);

    //TODO: do task

    srand(0xDEADBEEF);

    int error = rand() % 12;

    if(error < 9) //Fail with 1/4 probability

    {

      ROS_INFO("WH_robot: TASK COMPLETE!");

      return TASK_COMPLETED;

    }

    else

    {

      ROS_INFO("WH_robot: TASK FAILED!");

      return TASK_FAILED;

    }

}

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

{

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

  {

    reg_failed = 0;

  }

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

  {

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

    char* data;

    data = strtok(string, " ");

    int 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 = NULL;

    Duration est_time(0);

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

  }

  else

  {

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

  }

}

void WH_Robot::run ()

{

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

        while(curr_task == DEFAULT_TASK)

          continue;

        int error = exec_task();

        if(error == TASK_COMPLETED)

        {

        ss << "SUCCESS "<<curr_task->id;

        msg.data = ss.str();

        robot_to_sched.publish(msg);

        }

        else //error == TASK_FAILED

        {

        ss << "FAILED "<<curr_task->id;

        msg.data = ss.str();

        robot_to_sched.publish(msg);

       }

        delete curr_task;

        curr_task = DEFAULT_TASK;

    }

}

void WH_Robot::set_task(Order order)

{

    curr_task = new Order(order.getid(), order.get_source(), order.get_dest(), 

        order.get_start_time(), order.get_path(), order.get_est_time()); 

    return;

}