Revision 6761a531
ID | 6761a53177d5f8d6771f18a6f948abccc4365076 |
Pushing modified behavior process that requires less arguments and appends scoutname to ros node name. and a compiling but not tested version of test_behaviors (scheduler and wh_robot)
scout/libscout/CMakeLists.txt | ||
---|---|---|
30 | 30 |
#target_link_libraries(example ${PROJECT_NAME}) |
31 | 31 |
|
32 | 32 |
set(MAIN_FILES src/Behavior.cpp src/BehaviorList.cpp src/BehaviorProcess.cpp) |
33 |
set(BEHAVIOR_FILES src/behaviors/Odometry.cpp src/behaviors/draw_cw_circle.cpp src/behaviors/draw_ccw_circle.cpp src/behaviors/navigationMap.cpp src/behaviors/Scheduler.cpp src/behaviors/WH_Robot.cpp) |
|
33 |
set(BEHAVIOR_FILES src/behaviors/Odometry.cpp src/behaviors/draw_cw_circle.cpp src/behaviors/draw_ccw_circle.cpp src/behaviors/navigationMap.cpp) |
|
34 |
set(TEST_BEHAVIOR_FILES src/behaviors/Scheduler.cpp src/behaviors/WH_Robot.cpp) |
|
35 |
set(HELPER_FILES src/helper_classes/Order.cpp src/helper_classes/PQWrapper.cpp) |
|
34 | 36 |
set(CONTROL_CLASSES src/MotorControl.cpp src/SonarControl.cpp src/HeadlightControl.cpp src/ButtonControl.cpp src/WirelessSender.cpp src/EncodersControl.cpp) |
35 | 37 |
|
36 |
rosbuild_add_executable(libscout ${MAIN_FILES} ${BEHAVIOR_FILES} ${CONTROL_CLASSES})
|
|
38 |
rosbuild_add_executable(libscout ${MAIN_FILES} ${BEHAVIOR_FILES} ${TEST_BEHAVIOR_FILES} ${CONTROL_CLASSES} ${HELPER_FILES})
|
|
37 | 39 |
rosbuild_add_executable(test_encoders src/test_encoders.cpp) |
scout/libscout/src/BehaviorList.cpp | ||
---|---|---|
6 | 6 |
behavior_list.push_back((Behavior*)new draw_ccw_circle(scoutname)); |
7 | 7 |
behavior_list.push_back((Behavior*)new Odometry(scoutname)); |
8 | 8 |
behavior_list.push_back((Behavior*)new navigationMap(scoutname)); |
9 |
behavior_list.push_back((Behavior*)new Scheduler(scoutname)); |
|
9 |
// behavior_list.push_back((Behavior*)new Scheduler(scoutname));
|
|
10 | 10 |
return; |
11 | 11 |
} |
12 | 12 |
|
scout/libscout/src/BehaviorProcess.cpp | ||
---|---|---|
43 | 43 |
|
44 | 44 |
// Running with no arguments only supports one scout. Check in case |
45 | 45 |
// the user meant to specify a scout in the arguments. |
46 |
if (argc != 4)
|
|
46 |
if (argc != 3)
|
|
47 | 47 |
{ |
48 | 48 |
cout << "You have started this behavior in hardware mode." << endl |
49 | 49 |
<< "To start in software mode, use: " << argv[0] |
50 |
<< " <scoutname> <behavior#> <behaviorname>" << endl;
|
|
50 |
<< " <scoutname> <behavior#> " << endl; |
|
51 | 51 |
} |
52 | 52 |
else |
53 | 53 |
{ |
54 | 54 |
// Use the provided scoutname for simulator messages |
55 | 55 |
scoutname = argv[1]; |
56 | 56 |
behavior_num = atoi(argv[2]); |
57 |
behavior_name = argv[3]; |
|
58 | 57 |
|
59 |
ros::init(argc, argv, behavior_name);
|
|
58 |
ros::init(argc, argv, scoutname + "_behavior");
|
|
60 | 59 |
|
61 | 60 |
BehaviorList* list = new BehaviorList(scoutname); |
62 | 61 |
vector<Behavior*> behavior_list = list->behavior_list; |
scout/libscout/src/MotorControl.cpp | ||
---|---|---|
56 | 56 |
node.serviceClient<motors::query_motors>(scoutname + "/query_motors"); |
57 | 57 |
} |
58 | 58 |
|
59 |
MotorControl::~MotorControl() |
|
60 |
{ |
|
61 |
set_sides(0, 0, MOTOR_ABSOLUTE); |
|
62 |
} |
|
63 |
|
|
59 | 64 |
/** |
60 | 65 |
* @brief Sets all the speeds according to the specificiation of which motors |
61 | 66 |
* to set. |
scout/libscout/src/MotorControl.h | ||
---|---|---|
79 | 79 |
MotorControl(const ros::NodeHandle& libscout_node, |
80 | 80 |
std::string scoutname); |
81 | 81 |
|
82 |
~MotorControl(); |
|
83 |
|
|
82 | 84 |
/** Uses which to specify what to change, and sets all to same speed */ |
83 | 85 |
void set(int which, float speed, char units=MOTOR_DEFAULT_UNIT); |
84 | 86 |
|
scout/libscout/src/behaviors/WH_Robot.cpp | ||
---|---|---|
1 | 1 |
#include "WH_Robot.h" |
2 | 2 |
#include "Scheduler.h" |
3 |
|
|
3 |
#include "../helper_classes/Order.h" |
|
4 | 4 |
|
5 | 5 |
/** @Brief: warehouse robot constructor **/ |
6 | 6 |
WH_Robot::WH_Robot(std::string scoutname, void* sched):Behavior(scoutname, "WH_Robot") |
7 | 7 |
{ |
8 |
nav_map = navigationMap(scoutname); |
|
8 |
nav_map = new navigationMap(scoutname);
|
|
9 | 9 |
curr_task = DEFAULT_TASK; |
10 | 10 |
scheduler = sched; |
11 | 11 |
} |
12 | 12 |
|
13 |
Duration WH_Robot::get_wc_time(State start_state, State target_state) |
|
13 |
WH_Robot::~WH_Robot() |
|
14 |
{ |
|
15 |
delete(nav_map); |
|
16 |
} |
|
17 |
|
|
18 |
Duration WH_Robot::get_worst_case_time(State start_state, State target_state) |
|
14 | 19 |
{ |
15 |
return nav_map.get_worst_case_time(start_state, target_state);
|
|
20 |
return nav_map->get_worst_case_time(start_state, target_state);
|
|
16 | 21 |
} |
17 | 22 |
|
18 | 23 |
int WH_Robot::exec_task() |
... | ... | |
32 | 37 |
} |
33 | 38 |
|
34 | 39 |
void WH_Robot::run (){ |
35 |
((Scheduler*)scheduler).get_task(this);
|
|
40 |
((Scheduler*)scheduler)->get_task(this);
|
|
36 | 41 |
while(curr_task == DEFAULT_TASK) |
37 | 42 |
continue; |
38 | 43 |
int error = exec_task(); |
39 |
if(error == TASK_COMPLETE) |
|
44 |
if(error == TASK_COMPLETED)
|
|
40 | 45 |
{ |
41 |
((Scheduler*)scheduler).task_complete(*curr_task);
|
|
46 |
((Scheduler*)scheduler)->task_complete(*curr_task);
|
|
42 | 47 |
} |
43 | 48 |
else //error == TASK_FAILED |
44 | 49 |
{ |
45 |
((Scheduler*)scheduler).task_failed(*curr_task);
|
|
50 |
((Scheduler*)scheduler)->task_failed(*curr_task);
|
|
46 | 51 |
} |
47 | 52 |
curr_task = DEFAULT_TASK; |
48 | 53 |
} |
scout/libscout/src/behaviors/WH_Robot.h | ||
---|---|---|
1 | 1 |
#ifndef _WH_ROBOT_ |
2 | 2 |
#define _WH_ROBOT_ |
3 | 3 |
|
4 |
#define DEFAULT_TASK NULL;
|
|
4 |
#define DEFAULT_TASK NULL |
|
5 | 5 |
#define TASK_COMPLETED 0 |
6 | 6 |
#define TASK_FAILED -1 |
7 | 7 |
|
... | ... | |
14 | 14 |
class WH_Robot : Behavior{ |
15 | 15 |
|
16 | 16 |
Order* curr_task; |
17 |
navigationMap nav_map; |
|
17 |
navigationMap* nav_map;
|
|
18 | 18 |
void* scheduler; |
19 | 19 |
|
20 | 20 |
Duration get_worst_case_time(State start_state, State target_state); |
scout/libscout/src/helper_classes/Order.cpp | ||
---|---|---|
45 | 45 |
return est_time; |
46 | 46 |
} |
47 | 47 |
|
48 |
int Order::get_priority() const |
|
49 |
{ |
|
50 |
return start_time.toSec() + MAX_WAIT_TIME - est_time.toSec(); |
|
51 |
} |
|
52 |
|
|
48 | 53 |
bool Order::operator==(Order& order) |
49 | 54 |
{ |
50 | 55 |
return this->id == order.id; |
... | ... | |
62 | 67 |
*/ |
63 | 68 |
bool CompareOrder::operator()(Order& o1, Order& o2) |
64 | 69 |
{ |
65 |
int pq_value1 = o1.get_start_time + MAX_WAIT_TIME - o1.get_est_time(); |
|
66 |
int pq_value2 = o2.get_start_time + MAX_WAIT_TIME - o2.get_est_time(); |
|
67 |
return pq_value1 > pq_value2; |
|
70 |
return o1.get_priority() > o2.get_priority(); |
|
68 | 71 |
} |
69 | 72 |
|
scout/libscout/src/helper_classes/Order.h | ||
---|---|---|
13 | 13 |
Address source, dest; |
14 | 14 |
Time start_time; |
15 | 15 |
Path path; |
16 |
Time est_time;
|
|
16 |
Duration est_time;
|
|
17 | 17 |
public: |
18 | 18 |
Order(int order_id, Address order_source, Address order_dest, |
19 | 19 |
Time order_start_time, Path order_path, Duration order_est_time); |
20 |
|
|
21 |
Order() {}; |
|
20 | 22 |
|
21 | 23 |
int getid() const; |
22 | 24 |
Address get_source() const; |
... | ... | |
25 | 27 |
Path get_path() const; |
26 | 28 |
Duration get_est_time() const; |
27 | 29 |
void set_path(Path order_path); |
30 |
int get_priority() const; |
|
28 | 31 |
|
29 | 32 |
bool operator==(Order& order); |
30 | 33 |
|
scout/libscout/src/helper_classes/PQWrapper.cpp | ||
---|---|---|
24 | 24 |
bool foundPosition = false; |
25 | 25 |
while (!foundPosition && position < arrContents) |
26 | 26 |
{ |
27 |
if (o.getpriority() < minElems[position].getpriority())
|
|
27 |
if (o.get_priority() < minElems[position].get_priority())
|
|
28 | 28 |
{ |
29 | 29 |
foundPosition=true; |
30 | 30 |
} |
Also available in: Unified diff