Project

General

Profile

Revision 6761a531

ID6761a53177d5f8d6771f18a6f948abccc4365076

Added by Priya almost 12 years ago

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)

View differences:

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