Project

General

Profile

Revision 3f72678f

ID3f72678f171cbcb5499524f6cbfe994f4a416364

Added by Priya over 6 years ago

Changing scheduler and WH robot to ROS messages

View differences:

scout/libscout/src/behaviors/WH_Robot.cpp
1 1
#include "WH_Robot.h"
2
#include "Scheduler.h" 
3 2
#include "../helper_classes/Order.h"
4 3

  
5 4
/** @Brief: warehouse robot constructor **/
6
WH_Robot::WH_Robot(std::string scoutname, void* sched):Behavior(scoutname, "WH_Robot")
5
WH_Robot::WH_Robot(std::string scoutname):Behavior(scoutname, "WH_Robot")
7 6
{
7
    ROS_INFO("WH_ROBOT: Creating a WH_Robot...");
8 8
    nav_map = new navigationMap(scoutname);
9 9
    curr_task = DEFAULT_TASK;
10
    scheduler = sched;
10
    name = scoutname;
11
    reg_failed = 1;
12

  
13
    robot_to_sched = n.advertise<std_msgs::String>(name + "_topic", 1000);
14
    sched_to_robot = n.subscribe("sched_to_robot", 1000, msg_callback);
15

  
16
    while(reg_failed)
17
    {
18
      std_msgs::String msg;
19
      std::stringstream ss;
20
      ss << "REGISTER "<<name;
21
      msg.data = ss.str();
22
      robot_to_sched.publish(msg);
23
    }
24

  
25
    ROS_INFO("WH_Robot: I am a created WH robot");
11 26
}
12 27

  
13 28
WH_Robot::~WH_Robot()
......
22 37

  
23 38
int WH_Robot::exec_task()
24 39
{
40
    ROS_INFO("WH_robot: Executing a task");
25 41
    assert(curr_task != DEFAULT_TASK);
26 42
    //TODO: do task
27 43
    srand(0xDEADBEEF);
28 44
    int error = rand() % 12;
29 45
    if(error < 9) //Fail with 1/4 probability
30 46
    {
47
      ROS_INFO("WH_robot: TASK COMPLETE!");
31 48
      return TASK_COMPLETED;
32 49
    }
33 50
    else
34 51
    {
52
      ROS_INFO("WH_robot: TASK FAILED!");
35 53
      return TASK_FAILED;
36 54
    }
37 55
}
38 56

  
39
void WH_Robot::run (){
40
    ((Scheduler*)scheduler)->get_task(this);
41
    while(curr_task == DEFAULT_TASK)
42
      continue;
43
    int error = exec_task();
44
    if(error == TASK_COMPLETED)
45
    {
46
        ((Scheduler*)scheduler)->task_complete(*curr_task);
47
    }
48
    else //error == TASK_FAILED
57
void WH_Robot::msg_callback(const std_msgs::String::ConstPtr& msg)
58
{
59
  if(msg->data.compare(0, 11, "REG_SUCCESS") == 0)
60
  {
61
    reg_failed = 0;
62
  }
63
  else if(msg->data.compare(0, 8, "SET_TASK") == 0)
64
  {
65
    char* string = msg->data.c_str();
66
    char* data;
67
    data = strtok(string, " ");
68
    int id = atoi(data);
69
    data = strtok(string, " ");
70
    Address source = atoi(data);
71
    data = strtok(string, " ");
72
    Address dest = atoi(data);
73
    Time start_time = ros::Time::now();
74
    Path path = NULL;
75
    Duration est_time(0);
76

  
77
    curr_task = new Order(id, source, dest, start_time, path, est_time);
78
  }
79
  else
80
  {
81
    ROS_INFO("I got a bad message: %s", msg->data);
82
  }
83
}
84

  
85
void WH_Robot::run ()
86
{
87

  
88
    while(ok())
49 89
    {
50
        ((Scheduler*)scheduler)->task_failed(*curr_task);
90
        ROS_INFO("WH_ROBOT: Running main loop");
91

  
92

  
93
        std_msgs::String msg;
94
        std::stringstream ss;
95
        ss << "GET_TASK "<<id;
96
        msg.data = ss.str();
97
        robot_to_sched.publish(msg);
98

  
99

  
100
        while(curr_task == DEFAULT_TASK)
101
          continue;
102
        int error = exec_task();
103
        if(error == TASK_COMPLETED)
104
        {
105

  
106

  
107
        ss << "SUCCESS "<<curr_task->id;
108
        msg.data = ss.str();
109
        robot_to_sched.publish(msg);
110

  
111

  
112
        }
113
        else //error == TASK_FAILED
114
        {
115
 
116

  
117
        ss << "FAILED "<<curr_task->id;
118
        msg.data = ss.str();
119
        robot_to_sched.publish(msg);
120

  
121

  
122
       }
123
        delete curr_task;
124
        curr_task = DEFAULT_TASK;
51 125
    }
52
    curr_task = DEFAULT_TASK;
53 126
}
54 127

  
55 128
void WH_Robot::set_task(Order order)

Also available in: Unified diff