Colony » Colony Scout

#include "WH_Robot.h"

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

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

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

{

  nav_map = new navigationMap(scoutname);

  curr_task = DEFAULT_TASK;

  name = scoutname;

  reg_failed = 1;

}

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

}

/** @brief Based on the given path, make

 *         the series of turns to follow this path,

 *         updating the current state in the navigation map as we do so   

 *  @param the Path to follow

 */

void WH_Robot::follow_path(Path path_to_follow){

  ROS_INFO("Path length: %d", path_to_follow.len);

  for(int i=0; i<path_to_follow.len; i++)

  {

    Turn t = path_to_follow.path[i];

    ROS_INFO("making turn %d at state %d", (int)t, (int)nav_map->get_state());

    nav_map->update_state(t);

    Duration time = nav_map->get_time_remaining();

    while(time.sec > 0)

      time = nav_map->get_time_remaining();

  }  

  ROS_INFO("Path complete at state %d", (int)nav_map->get_state());

}

int WH_Robot::exec_task()

{

  ROS_INFO("WH_robot: Executing a task");

  assert(curr_task != DEFAULT_TASK);

  // remember where you started doing this task so we know where to return

  State home_state = nav_map->get_state();

  ROS_INFO("WH_robot: starting from home state %d", (int)home_state);

  ROS_INFO("WH_robot: aiming for state %d", (int)curr_task->get_dest());

  Path new_path = nav_map->shortest_path(curr_task->get_dest());

  curr_task->set_path(new_path);

  follow_path(new_path);

  //TODO: forklift yaaaay

  int did_task_complete;

  srand(0xDEADBEEF);

  int error = rand() % 12;

  if(error < 6) //Fail with 1/2 probability

  {

    ROS_INFO("WH_robot: TASK COMPLETE!");

    did_task_complete = TASK_COMPLETED;

  }

  else

  {

    ROS_INFO("WH_robot: TASK FAILED!");

    did_task_complete = TASK_FAILED;

  }

  ROS_INFO("WH_robot: Now I am attempting to go home from state %d to state %d", 

        (int)nav_map->get_state(), (int)home_state);

  Path return_path = nav_map->shortest_path(home_state);

  curr_task->set_path(return_path);

  follow_path(return_path);

  return did_task_complete;

}

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

{

  std::cout << "robot callback called." << std::endl;

  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)

  {

    ROS_INFO("WH_ROBOT: Setting robot task");

    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("A WH_Robot has been created");

  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("I am a robot. Registering with scheduler...");

  while(reg_failed && ok())

  {

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

    while(curr_task == DEFAULT_TASK && ok()){

      spinOnce();

    }

    int error = exec_task();

    if(error == TASK_COMPLETED)

    {

      std_msgs::String msg;

      std::stringstream ss;

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

      msg.data = ss.str();

      robot_to_sched.publish(msg);

    }

    else //error == TASK_FAILED

    {

      std_msgs::String msg;

      std::stringstream ss;

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

      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;

}