Project

General

Profile

Revision d140fd71

IDd140fd71ddd19ef38a0124cbbea1165874a5474d
Parent 8a4624e7
Child 479d25d8

Added by Yuyang over 11 years ago

Added Sensors class that abstracts away sensors from behaviors. Stops creating
duplicated sensors for each scout.

View differences:

scout/libscout/CMakeLists.txt
29 29
#rosbuild_add_executable(example examples/example.cpp)
30 30
#target_link_libraries(example ${PROJECT_NAME})
31 31

  
32
set(MAIN_FILES src/Behavior.cpp src/BehaviorList.cpp src/BehaviorProcess.cpp)
32
set(MAIN_FILES src/Sensors.cpp src/Behavior.cpp src/BehaviorList.cpp src/BehaviorProcess.cpp)
33 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/line_follow.cpp src/behaviors/wl_test.cpp)
34 34
set(TEST_BEHAVIOR_FILES src/behaviors/Scheduler.cpp src/behaviors/WH_Robot.cpp)
35 35
set(HELPER_FILES src/helper_classes/Order.cpp src/helper_classes/PQWrapper.cpp)
scout/libscout/src/Behavior.cpp
48 48
 * @param scoutname If nonempty, specifies which scout in the simulator
49 49
 *                  will be controlled by this behavior.
50 50
 */
51
Behavior::Behavior(string scoutname, string my_name)
51
Behavior::Behavior(string scoutname, string my_name, Sensors * sensors)
52 52
{
53
    name = my_name;
54

  
55
    motors = new MotorControl(node, scoutname);
56
    buttons = new ButtonControl(node, scoutname);
57
    sonar = new SonarControl(node, scoutname);
58
    //cliffsensor = new CliffsensorControl(node, scoutname);
59
    encoders = new EncodersControl(node, scoutname);
60
    linesensor = new LinesensorControl(node, scoutname);
61
    wl_sender = new WirelessSender(node);
62
    wl_receiver = new WirelessReceiver(node);
63 53

  
54
    name = my_name;
55
    sensors->init(&motors, &buttons, &sonar, &encoders, &linesensor,
56
                        &wl_sender, &wl_receiver);
64 57
    loop_rate = new ros::Rate(10);
65 58
}
66 59

  
scout/libscout/src/Behavior.h
50 50
#include "WirelessSender.h"
51 51
#include "WirelessReceiver.h"
52 52
#include "constants.h"
53
#include "Sensors.h"
53 54

  
54 55
typedef ros::Time Time;
55 56
typedef ros::Duration Duration;
......
58 59
{
59 60
    public:
60 61
        // Initializes ROS for behavior
61
        Behavior(std::string scoutname, std::string name);
62
        // name stands for behavior name
63
        Behavior(std::string scoutname, std::string name,
64
                 Sensors * sensor);
62 65

  
63 66
        /// Extended by subclasses to actually run the behavior.
64 67
        virtual void run() = 0;
65 68

  
66 69
        // Name of behaviour
67 70
        std::string name;
68

  
71
        
69 72
    protected:
70 73
        ros::NodeHandle node;
71 74

  
scout/libscout/src/BehaviorList.cpp
1 1
#include "BehaviorList.h"
2 2

  
3
BehaviorList::BehaviorList(std::string scoutname)
3

  
4
BehaviorList::BehaviorList(std::string scoutname, Sensors * sensor)
4 5
{
5
  behavior_list.push_back((Behavior*)new draw_cw_circle(scoutname));
6
  behavior_list.push_back((Behavior*)new draw_ccw_circle(scoutname));
7
  behavior_list.push_back((Behavior*)new Odometry(scoutname));
8
  behavior_list.push_back((Behavior*)new navigationMap(scoutname));
9
  behavior_list.push_back((Behavior*)new Scheduler(scoutname));
10
  behavior_list.push_back((Behavior*)new WH_Robot(scoutname));
11
  behavior_list.push_back((Behavior*)new line_follow(scoutname));
12
  behavior_list.push_back((Behavior*)new wl_test(scoutname));
6
  behavior_list.push_back((Behavior*)new draw_cw_circle(scoutname,sensor));
7
  behavior_list.push_back((Behavior*)new draw_ccw_circle(scoutname, sensor));
8
  behavior_list.push_back((Behavior*)new Odometry(scoutname, sensor));
9
  behavior_list.push_back((Behavior*)new navigationMap(scoutname, sensor));
10
  behavior_list.push_back((Behavior*)new Scheduler(scoutname, sensor));
11
  behavior_list.push_back((Behavior*)new WH_Robot(scoutname, sensor));
12
  behavior_list.push_back((Behavior*)new line_follow(scoutname, sensor));
13
  behavior_list.push_back((Behavior*)new wl_test(scoutname, sensor));
13 14
  return;
14 15
}
15 16

  
scout/libscout/src/BehaviorList.h
10 10
#include "behaviors/Scheduler.h"
11 11
#include "behaviors/WH_Robot.h"
12 12
#include "behaviors/wl_test.h"
13

  
13
#include "Sensors.h"
14 14
class BehaviorList
15 15
{
16 16
  public:
17 17
  std::vector<Behavior*> behavior_list;
18 18

  
19 19
  //Constructor. Initializes behavior_list
20
  BehaviorList(std::string scoutname);
20
  BehaviorList(std::string scoutname, Sensors* sensor);
21 21
  //Destructor. Frees behavior_list;
22 22
  ~BehaviorList();
23 23
};
scout/libscout/src/BehaviorProcess.cpp
1

  
1 2
/**
2 3
 * Copyright (c) 2011 Colony Project
3 4
 * 
......
34 35
 */
35 36

  
36 37
#include "BehaviorProcess.h"
38
#include "Sensors.h"
39
#include "assert.h"
37 40

  
38 41
int main (int argc, char **argv)
39 42
{
......
56 59
        behavior_num = atoi(argv[2]);
57 60

  
58 61
        ros::init(argc, argv, scoutname + "_behavior");
59

  
60
        BehaviorList* list = new BehaviorList(scoutname);
62
        // one Sensor instance per-class
63
        Sensors* sensors = new Sensors(scoutname);
64
        BehaviorList* list = new BehaviorList(scoutname, sensors);
61 65
        vector<Behavior*> behavior_list = list->behavior_list;
62

  
63 66
        if (behavior_num < (int)behavior_list.size())
64 67
        {
65 68
            (behavior_list[behavior_num])->run();
scout/libscout/src/MotorControl.h
65 65
#define MOTOR_LEFT_SPIN MOTOR_LEFT_REV | MOTOR_RIGHT
66 66
#define MOTOR_RIGHT_SPIN MOTOR_LEFT | MOTOR_RIGHT_REV
67 67

  
68
#define MAXSPEED 100
68
#define MAXSPEED 255
69 69
#define MOTOR_PERCENT 'p'
70 70
#define MOTOR_MMS 'm'
71 71
#define MOTOR_CMS 'c'
scout/libscout/src/Sensors.cpp
1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25

  
26
/**
27
 * @file Behavior.cpp
28
 * @brief Contains basic functions for the structure of all behaviors.
29
 * 
30
 * Contains function implementations needed for all behavior.
31
 *
32
 * @author Colony Project, CMU Robotics Club
33
 * @author Priyanka Deo
34
 * @author Alex Zirbel
35
 **/
36

  
37
#include "Sensors.h"
38

  
39
using namespace std;
40

  
41
Sensors::Sensors(string scoutname)
42
{
43
    name = scoutname;
44
    motors = new MotorControl(node,scoutname);
45
    buttons = new ButtonControl(node, scoutname);
46
    sonar = new SonarControl(node, scoutname);
47
    //cliffsensor = new CliffsensorControl(node, scoutname);
48
    encoders = new EncodersControl(node, scoutname);
49
    linesensor = new LinesensorControl(node, scoutname);
50
    wl_sender = new WirelessSender(node);
51
}
52

  
53
// b_stuff stand for behavior control class
54
void Sensors::init(MotorControl** b_motors, ButtonControl ** b_buttons,
55
                  SonarControl ** b_sonar, EncodersControl ** b_encoders,
56
                  LinesensorControl ** b_linesensor,
57
                  WirelessSender ** b_wl_sender,
58
                  WirelessReceiver ** b_wl_receiver)
59
{
60
    //(MotorControl *)(* b_motors) = motors;
61
    *b_motors = motors;
62
    *b_buttons = buttons;
63
    *b_sonar = sonar;
64
    *b_encoders = encoders;
65
    *b_linesensor = linesensor;
66
    *b_wl_sender = wl_sender;
67
    *b_wl_receiver = new WirelessReceiver(node);
68
}
scout/libscout/src/Sensors.h
1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25

  
26
/**
27
 * @file Behavior.h
28
 * @brief Contains declarations for the structure of all behaviours.
29
 * 
30
 * Contains functions and definitions for a generic behavior.
31
 *
32
 * @author Colony Project, CMU Robotics Club
33
 * @author Priyanka Deo
34
 * @author Alex Zirbel
35
 **/
36

  
37
#ifndef _SENSORS_H_
38
#define _SENSORS_H_
39

  
40
#include <ros/ros.h>
41
#include <std_msgs/String.h>
42

  
43
#include "MotorControl.h"
44
#include "HeadlightControl.h"
45
#include "ButtonControl.h"
46
#include "SonarControl.h"
47
//#include "CliffsensorControl.h"
48
#include "EncodersControl.h"
49
#include "LinesensorControl.h"
50
#include "WirelessSender.h"
51
#include "WirelessReceiver.h"
52

  
53
class Sensors
54
{
55
    public:
56
        Sensors(std::string scoutname);
57
        // b_stuff stand for behavior control class
58
        void init(MotorControl ** b_motors, ButtonControl ** b_buttons,
59
                  SonarControl ** b_sonar, EncodersControl ** b_encoders,
60
                  LinesensorControl ** b_linesensor,
61
                  WirelessSender ** b_wl_sender,
62
                  WirelessReceiver ** b_wl_receiver);
63
        std::string name;
64
    private:
65
        ros::NodeHandle node;
66
        MotorControl * motors;
67
        ButtonControl * buttons;
68
        SonarControl * sonar;
69
        //CliffsensorControl * cliffsensor;
70
        EncodersControl * encoders;
71
        LinesensorControl * linesensor;
72
        WirelessSender * wl_sender;
73
        WirelessReceiver * wl_receiver;
74
        
75
};
76

  
77

  
78
#endif
scout/libscout/src/behaviors/Odometry.cpp
4 4
using namespace std;
5 5

  
6 6
/** Set up the odometry node and prepare communcations over ROS */
7
Odometry::Odometry(string scoutname):Behavior(scoutname, "odometry")
7
Odometry::Odometry(string scoutname, Sensors* sensors):Behavior(scoutname, 
8
                                                "odometry", sensors)
8 9
{
9 10
  scout_pos = new pos;
10 11
}
scout/libscout/src/behaviors/Odometry.h
2 2
#define _ODOMETRY_H_
3 3

  
4 4
#include "../Behavior.h"
5
#include "../Sensors.h"
5 6

  
6 7

  
7 8
#define WHEEL_RADIUS  5
......
21 22
  public:
22 23

  
23 24
  /** Set up the odometry node and prepare communcations over ROS */
24
  Odometry(std::string scoutname);
25
  Odometry(std::string scoutname, Sensors* sensors);
25 26

  
26 27
  /** Query encoders and estimate position based on encoder reading */
27 28
  void get_position();
scout/libscout/src/behaviors/Scheduler.cpp
4 4
using namespace std;
5 5

  
6 6
/** @Brief: Initialize data structures for the scheduler. */
7
Scheduler::Scheduler(std::string scoutname):Behavior(scoutname, "Scheduler")
7
Scheduler::Scheduler(std::string scoutname, Sensors* sensors):
8
            Behavior(scoutname, "Scheduler", sensors)
8 9
{
9 10
  unassignedOrders = new PQWrapper(NUM_TASKS);
10 11

  
scout/libscout/src/behaviors/Scheduler.h
30 30
  void msg_callback(const std_msgs::String::ConstPtr& msg);
31 31

  
32 32
public:
33
  Scheduler(std::string scoutname);
33
  Scheduler(std::string scoutname, Sensors* sensors);
34 34
	~Scheduler();
35 35
	
36 36
	void get_task(int robot);
scout/libscout/src/behaviors/WH_Robot.cpp
2 2
#include "../helper_classes/Order.h"
3 3

  
4 4
/** @Brief: warehouse robot constructor **/
5
WH_Robot::WH_Robot(std::string scoutname):line_follow(scoutname) 
5
WH_Robot::WH_Robot(std::string scoutname, Sensors* sensors):
6
            line_follow(scoutname, sensors) 
6 7
{
7
  nav_map = new navigationMap(scoutname);
8
  nav_map = new navigationMap(scoutname, sensors);
8 9
  curr_task = DEFAULT_TASK;
9 10
  name = scoutname;
10 11
  reg_failed = 1;
scout/libscout/src/behaviors/WH_Robot.h
30 30
        void leave_home();
31 31

  
32 32
    public:
33
        WH_Robot(std::string scoutname);
33
        WH_Robot(std::string scoutname, Sensors* sensors);
34 34
        ~WH_Robot();
35 35
        void run();
36 36

  
scout/libscout/src/behaviors/draw_ccw_circle.cpp
29 29
{
30 30
    while(ok())
31 31
    {
32
        motors->set_sides(20, 80, MOTOR_ABSOLUTE);
32
        motors->set_sides(80, 20, MOTOR_ABSOLUTE);
33 33

  
34 34
        spinOnce();
35 35
        loop_rate->sleep();
scout/libscout/src/behaviors/draw_ccw_circle.h
27 27
#define _DRAW_CCW_CIRCLE_H_
28 28

  
29 29
#include "../Behavior.h"
30

  
30
#include "../Sensors.h"
31 31
class draw_ccw_circle : Behavior
32 32
{
33 33
    public:
34 34
        /// @todo Is this the best way to inherit the Behavior constructor?
35
        draw_ccw_circle(std::string scoutname) : Behavior(scoutname, "draw_ccw_circle"){ };
35
        draw_ccw_circle(std::string scoutname, Sensors* sensors) : 
36
                        Behavior(scoutname, "draw_ccw_circle", sensors){ };
36 37

  
37 38
        /** Actually executes the behavior. */
38 39
        void run();
scout/libscout/src/behaviors/draw_cw_circle.cpp
29 29
{
30 30
    while(ok())
31 31
    {
32
        motors->set_sides(80, 20, MOTOR_ABSOLUTE);
32
        motors->set_sides(20, 80, MOTOR_ABSOLUTE);
33 33

  
34 34
        encoder_readings readings = encoders->query();
35 35

  
scout/libscout/src/behaviors/draw_cw_circle.h
27 27
#define _DRAW_CW_CIRCLE_H_
28 28

  
29 29
#include "../Behavior.h"
30
#include "../Sensors.h"
30 31

  
31 32
class draw_cw_circle : Behavior
32 33
{
33 34
    public:
34 35
        /// @todo Is this the best way to inherit the Behavior constructor?
35
        draw_cw_circle(std::string scoutname) : Behavior(scoutname, "draw_cw_circle") {};
36
        draw_cw_circle(std::string scoutname, Sensors* sensors) : 
37
                Behavior(scoutname, "draw_cw_circle", sensors) {};
36 38

  
37 39
        /** Actually executes the behavior. */
38 40
        void run();
scout/libscout/src/behaviors/line_follow.h
27 27
#define _LINE_FOLLOW_H_
28 28

  
29 29
#include "../Behavior.h"
30
#include "../Sensors.h"
30 31

  
31 32
#define MOTOR_BASE 50
32 33
#define SCALE 20
......
34 35
class line_follow : public Behavior
35 36
{
36 37
    public:
37
        line_follow(std::string scoutname) : Behavior(scoutname, "line_follow") {};
38
        line_follow(std::string scoutname, Sensors* sensors) : 
39
                Behavior(scoutname, "line_follow", sensors) {};
38 40

  
39 41
        /** Actually executes the behavior. */
40 42
        void run();
scout/libscout/src/behaviors/navigationMap.cpp
48 48
 * The map itself is represented as a dynamic array of edge arrays
49 49
 * @param the string name of the scout
50 50
 */
51
navigationMap::navigationMap(string scoutname) : Behavior(scoutname, "navigationMap")
51
navigationMap::navigationMap(string scoutname, Sensors* sensors) : 
52
            Behavior(scoutname, "navigationMap", sensors)
52 53
{
53 54
  /** Initialize Map 
54 55
   *                     _____
scout/libscout/src/behaviors/navigationMap.h
108 108

  
109 109
  public:
110 110
    /** Initializes the navigation map */
111
    navigationMap(std::string scoutname);
111
    navigationMap(std::string scoutname, Sensors* sensors);
112 112
    /** Goes through and frees all allocated memory */
113 113
    ~navigationMap();
114 114

  
scout/libscout/src/behaviors/trafficNavigation.h
136 136
class trafficNavigation : Behavior
137 137
{
138 138
    public:
139
        trafficNavigation(std::string scoutname) : Behavior(scoutname, "traffic_nav") {};
139
        trafficNavigation(std::string scoutname, Sensors* sensors) : 
140
                Behavior(scoutname, "traffic_nav", sensors) {};
140 141

  
141 142
        void run();
142 143
		int wirelessPacketHandle(int state);
scout/libscout/src/behaviors/wl_test.h
27 27
#define _WL_TEST_H_
28 28

  
29 29
#include "../Behavior.h"
30
#include "../Sensors.h"
30 31

  
31 32
class wl_test : Behavior
32 33
{
33 34
    public:
34
        wl_test(std::string scoutname) : Behavior(scoutname, "wl_test"),
35
        wl_test(std::string scoutname, Sensors* sensors) : 
36
                Behavior(scoutname, "wl_test", sensors),
35 37
            no_response(true), scout_name(scoutname) {};
36 38
        /** Actually executes the behavior. */
37 39
        void data_callback(std::vector<uint8_t> data);

Also available in: Unified diff