Project

General

Profile

Revision 5d0687a9

ID5d0687a9991d806c94fc7c21405334e6717e3611

Added by Priya over 7 years ago

Small changes to wireless, and starting turning with line following.

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/sim_line.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/sim_line.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)
36
set(CONTROL_CLASSES src/MotorControl.cpp src/SonarControl.cpp src/HeadlightControl.cpp src/ButtonControl.cpp src/WirelessSender.cpp src/EncodersControl.cpp src/LinesensorControl.cpp)
36
set(CONTROL_CLASSES src/MotorControl.cpp src/SonarControl.cpp src/HeadlightControl.cpp src/ButtonControl.cpp src/WirelessSender.cpp src/WirelessReceiver.cpp src/EncodersControl.cpp src/LinesensorControl.cpp)
37 37

  
38 38
rosbuild_add_executable(libscout ${MAIN_FILES} ${BEHAVIOR_FILES} ${TEST_BEHAVIOR_FILES} ${CONTROL_CLASSES} ${HELPER_FILES})
39 39
rosbuild_add_executable(test_encoders src/test_encoders.cpp)
scout/libscout/src/Behavior.cpp
58 58
    //cliffsensor = new CliffsensorControl(node, scoutname);
59 59
    encoders = new EncodersControl(node, scoutname);
60 60
    linesensor = new LinesensorControl(node, scoutname);
61
    wl_sender = new WirelessSender(node);
62
    wl_receiver = new WirelessReceiver(node);
61 63

  
62 64
    loop_rate = new ros::Rate(10);
63 65
}
scout/libscout/src/Behavior.h
47 47
//#include "CliffsensorControl.h"
48 48
#include "EncodersControl.h"
49 49
#include "LinesensorControl.h"
50
#include "WirelessSender.h"
51
#include "WirelessReceiver.h"
50 52
#include "constants.h"
51 53

  
52 54
typedef ros::Time Time;
......
77 79
        //CliffsensorControl * cliffsensor;
78 80
        EncodersControl * encoders;
79 81
        LinesensorControl * linesensor;
82
        WirelessSender * wl_sender;
83
        WirelessReceiver * wl_receiver;
80 84

  
81 85
        // Wrappers for ROS functions
82 86
        bool ok();
scout/libscout/src/BehaviorList.cpp
9 9
  behavior_list.push_back((Behavior*)new Scheduler(scoutname));
10 10
  behavior_list.push_back((Behavior*)new WH_Robot(scoutname));
11 11
  behavior_list.push_back((Behavior*)new sim_line(scoutname));
12
  behavior_list.push_back((Behavior*)new wl_test(scoutname));
12 13
  return;
13 14
}
14 15

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

  
13 14
class BehaviorList
14 15
{
scout/libscout/src/LinesensorControl.cpp
35 35

  
36 36
using namespace std;
37 37

  
38
static double last_ret;
39

  
38 40
/**
39 41
 * @brief Initialize the line following module of libscout.
40 42
 */
......
62 64
    return srv.response.readings;
63 65
}
64 66

  
67
/** 
68
 *  @brief Returns the average readings of the line following sensors. 
69
 */
70
double LinesensorControl::readline()
71
{
72
    unsigned int total_read = 0;
73
    unsigned int weighted_total = 0;
74

  
75
    vector<uint32_t> readings = query();
76

  
77
    for (int i = 0; i < 8; i++)
78
    {
79
        total_read += readings[i];
80
        weighted_total += i * readings[i];
81
    }
82

  
83
    if (total_read == 0)
84
    {
85
        return last_ret;
86
    }
87
    
88
    double ret_val = last_ret = ((double) weighted_total / total_read) - 4;
89
    return ret_val;
90
}
91

  
92
bool LinesensorControl::fullline()
93
{
94
    return false; 
95
}
96

  
97
void LinesensorControl::turnLeft()
98
{
99
    vector<uint32_t> readings = query();
100
}
101

  
102
void LinesensorControl::turnRight()
103
{
104
    vector<uint32_t> readings = query();
105

  
106
}
scout/libscout/src/LinesensorControl.h
49 49
        /** Use ROS to get the current readings. */
50 50
        std::vector<uint32_t> query();
51 51

  
52
        /** Get line position */
53
        double readline();
54

  
55
        /** Implement turn functions */
56
        void turnLeft();
57
        void turnRight();
58

  
52 59
    private:
53 60
        /* ROS publisher and client declaration */
54 61
        ros::ServiceClient query_client;
55 62
        ros::NodeHandle node;
63

  
64
        /** Check if we are at an intersecetion */
65
        bool fullline();
56 66
};
57 67

  
58 68
#endif /* _LINESENSOR_CONTORL_H_ */
scout/libscout/src/WirelessReceiver.cpp
42 42
/**
43 43
 * @brief Initializes wireless receiver
44 44
 * 
45
 * @param libscout_node Node handle of the ROS node used by libscout.
46
 **/
47
WirelessReceiver::WirelessReceiver(const ros::NodeHandle& libscout_node)
48
    : node(libscout_node)
49
{
50
    sub = node.subscribe("/wireless/receive", QUEUE_SIZE,
51
          &WirelessReceiver::receive, this);
52
}
53

  
54
/**
55
 * @brief Registers a function to be called upon receipt of a packet.
56
 *
45 57
 * @param callback A function pointer to a function which is called whenever a
46 58
 * wireless packet is received. It should be of form:
47 59
 *     void callback(const WirelessPacketConstPtr& packet);
48 60
 **/
49
WirelessReceiver::WirelessReceiver(WirelessCallback callback)
50
    : callback(callback)
61
void WirelessReceiver::register_callback(WirelessCallback new_callback)
51 62
{
52
    sub = nh.subscribe("/wireless/receive", QUEUE_SIZE,
53
        &WirelessReceiver::receive, this);
63
    callback = new_callback;
54 64
}
55 65

  
56 66
/**
......
60 70
 **/
61 71
void WirelessReceiver::receive(const ::messages::WirelessPacketConstPtr& packet)
62 72
{
63
    (*callback)(packet);
73
    if (callback == NULL)
74
    {
75
        ROS_WARN("Warning: Ignored wireless packet.");
76
    }
77
    else
78
    {
79
        (*callback)(packet->data);
80
    }
64 81
}
scout/libscout/src/WirelessReceiver.h
43 43
#include "messages/WirelessPacket.h"
44 44
#include "constants.h"
45 45

  
46
typedef void (*WirelessCallback)(const ::messages::WirelessPacketConstPtr&);
46
typedef void (*WirelessCallback)(std::vector<uint8_t>);
47 47

  
48 48
class WirelessReceiver
49 49
{
50
public:
51
    WirelessReceiver(WirelessCallback callback);
52
private:
53
    ros::NodeHandle nh;
54
    ros::Subscriber sub;
55
    WirelessCallback callback;
56
    void receive(const ::messages::WirelessPacketConstPtr&);
50
    public:
51
        /** Set up the class and prepare to communicate over ROS */
52
        WirelessReceiver(const ros::NodeHandle& libscout_node);
53

  
54
        /** Register a callback to be called every time a packet is received */
55
        void register_callback(WirelessCallback new_callback);
56

  
57
    private:
58
        ros::Subscriber sub;
59
        ros::NodeHandle node;
60
        WirelessCallback callback;
61
        void receive(const ::messages::WirelessPacketConstPtr&);
57 62
};
58 63

  
59 64
#endif
scout/libscout/src/WirelessSender.cpp
40 40
using namespace std;
41 41
using namespace ::messages;
42 42

  
43
WirelessSender::WirelessSender(uint16_t type, uint16_t dest, uint16_t pan,
44
    uint16_t channel)
43
WirelessSender::WirelessSender(const ros::NodeHandle& libscout_node,
44
                               uint16_t type, uint16_t pan, uint16_t channel)
45
    : node(libscout_node)
45 46
{
46 47
    packet.type = type;
47
    packet.dest = dest;
48 48
    packet.pan = pan;
49 49
    packet.channel = channel;
50
    pub = nh.advertise< ::messages::WirelessPacket>("/wireless/send",
51
        QUEUE_SIZE);
50
    pub = node.advertise< ::messages::WirelessPacket>("/wireless/send",
51
                                                      QUEUE_SIZE);
52 52
}
53 53

  
54 54
void WirelessSender::setType(uint16_t type)
55 55
{
56 56
    packet.type = type;
57 57
}
58
void WirelessSender::setDest(uint16_t dest)
59
{
60
    packet.dest = dest;
61
}
62 58
void WirelessSender::setPan(uint16_t pan)
63 59
{
64 60
    packet.pan = pan;
......
68 64
    packet.channel = channel;
69 65
}
70 66

  
71
void WirelessSender::send(uint8_t data[], int length)
67
void WirelessSender::send(uint8_t data[], int length, uint16_t dest)
72 68
{
69
    packet.dest = dest;
73 70
    packet.data.clear();
74 71
    for (int i = 0; i < length; i++)
75 72
    {
76 73
        packet.data.push_back(data[i]);
77 74
    }
78
    publish();
75
    pub.publish(packet);
79 76
}
80 77

  
81
void WirelessSender::send(vector<uint8_t>& data)
78
void WirelessSender::send(vector<uint8_t>& data, uint16_t dest)
82 79
{
80
    packet.dest = dest;
83 81
    packet.data = data;
84
    publish();
85
}
86

  
87
void WirelessSender::publish()
88
{
89 82
    pub.publish(packet);
90 83
}
scout/libscout/src/WirelessSender.h
31 31
 * Contains functions and definitions for the use of
32 32
 * XBee wireless
33 33
 *
34
 * Uses the dest field of the wireless packet to specify which
35
 * scout the message is intended for, or SCOUT_ALL to send to
36
 * all scouts
37
 *
34 38
 * @author Colony Project, CMU Robotics Club
35 39
 * @author Tom Mullins
36 40
 **/
......
43 47
#include "messages/WirelessPacket.h"
44 48
#include "constants.h"
45 49

  
50
/// A macro to allow sending to all the scouts.
51
#define SCOUT_ALL (messages::WirelessPacket::DEST_BROADCAST)
52

  
46 53
class WirelessSender
47 54
{
48 55
    public:
49
        WirelessSender(uint16_t type = 0,
50
                uint16_t dest = messages::WirelessPacket::DEST_BROADCAST,
56
        WirelessSender(const ros::NodeHandle& libscout_node,
57
                uint16_t type = 0,
51 58
                uint16_t pan = messages::WirelessPacket::PAN_DEFAULT,
52 59
                uint16_t channel = messages::WirelessPacket::CHANNEL_DEFAULT);
53 60
        
54 61
        void setType(uint16_t type);
55
        void setDest(uint16_t dest);
56 62
        void setPan(uint16_t pan);
57 63
        void setChannel(uint16_t channel);
58 64
        
59
        void send(uint8_t data[], int length);
60
        void send(std::vector<uint8_t>& data);
65
        void send(uint8_t data[], int length, uint16_t dest = SCOUT_ALL);
66
        void send(std::vector<uint8_t>& data, uint16_t dest = SCOUT_ALL);
61 67
        
62 68
    private:
63
        ros::NodeHandle nh;
69
        ros::NodeHandle node;
64 70
        ros::Publisher pub;
65 71
        messages::WirelessPacket packet;
66
        
67
        void publish();
68 72
};
69 73

  
70 74
#endif
scout/libscout/src/behaviors/Scheduler.cpp
45 45
  p.path = NULL;
46 46
  Time t = ros::Time::now();
47 47
  Duration end(0);
48
	Order a(1,0,1,t,p,end);
49
  Order b(2,0,1,t,p,end);
50
  Order c(3,0,9,t,p,end);
48
  Duration five(5);
49
  Duration three(3);
50
  Duration ten(10);
51
	Order a(1,0,1,t+ten,p,end);
52
  Order b(2,0,1,t+five,p,end);
53
  Order c(3,0,9,t+three,p,end);
51 54
  Order d(4,0,2,t,p,end);
52 55
  Order e(5,0,4,t,p,end);
53 56
	
......
94 97
  double time = t.toSec();
95 98

  
96 99
  Order* best;
100
  int best_index;
97 101
  for(unsigned int i=0; i<unassignedOrders->arraySize(); i++)
98 102
  {
99 103
    Order order = unassignedOrders->peek(i);
......
103 107
    if(end_time.toSec() + MAX_WAIT_TIME < time) //TODO: use another function
104 108
    {
105 109
      best = &order;
110
      best_index = i;
106 111
      time = end_time.toSec() + MAX_WAIT_TIME;
107 112
    }
108 113
  }
109
  return *best;
114

  
115
  Order ret;
116
  ret = unassignedOrders->remove(best_index);
117
  assignedOrders.push_back(ret);
118
  return ret;
110 119
}
111 120

  
112 121
void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
......
137 146
  }
138 147
  else if(msg->data.compare(0, 8, "GET_TASK") == 0)
139 148
  {
149
    ROS_INFO("SCHEDULER: got get task message");
140 150
    int robot = atoi(msg->data.substr(9).c_str());
141 151
    get_task(robot);
142 152
  }
......
164 174
    ss<<"REG_SUCCESS "<<id;
165 175
    msg.data = ss.str();
166 176
    new_robot.topic.publish(msg);
167
    ros::spinOnce();
177
    spinOnce();
168 178

  
169 179
    ROS_INFO("Registration a success");
170 180

  
......
194 204
	{
195 205
    ROS_INFO("Scheduler running");
196 206
    spinOnce();
197
		while(waitingRobots.empty() || unassignedOrders->arraySize()==0) 
207
		while((waitingRobots.empty() || unassignedOrders->arraySize()==0) && ok())
208
    {
198 209
      waiting_dance();
210
      spinOnce();
211
    }
199 212
  
200 213
    ROS_INFO("SCHEDULER: Setting task");
201 214
    Order next = get_next_item();
......
207 220
    ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest();
208 221
    msg.data = ss.str();
209 222
    r.topic.publish(msg);
223
  
224
    spinOnce();
210 225

  
211 226
    waitingRobots.front().sched_status = ORDERED_ROBOT;
212 227
		waitingRobots.pop();
scout/libscout/src/behaviors/WH_Robot.cpp
24 24
{
25 25
  ROS_INFO("WH_robot: Executing a task");
26 26
  assert(curr_task != DEFAULT_TASK);
27
  //TODO: do task
27
  
28
  Path new_path = nav_map->shortest_path(curr_task->get_dest());
29
  curr_task->set_path(new_path);
30
  for(int i=0; i<new_path.len; i++)
31
  {
32
    Turn t = new_path.path[i];
33
    ROS_INFO("making turn %d at state %d", (int)t, (int)nav_map->get_state());
34
    nav_map->update_state(t);
35
    Duration time = nav_map->get_time_remaining();
36
    while(time.sec > 0)
37
      time = nav_map->get_time_remaining();
38
  }
39
  ROS_INFO("Path complete at state %d", (int)nav_map->get_state());
28 40
  srand(0xDEADBEEF);
29 41
  int error = rand() % 12;
30
  if(error < 9) //Fail with 1/4 probability
42
  if(error < 6) //Fail with 1/2 probability
31 43
  {
32 44
    ROS_INFO("WH_robot: TASK COMPLETE!");
33 45
    return TASK_COMPLETED;
scout/libscout/src/behaviors/sim_line.cpp
31 31
static int motor_r;
32 32
static double last_ret;
33 33

  
34

  
35 34
/** TODO: This should be part of the line sensor library */
36 35
double sim_line::get_line_pos(vector<uint32_t> readings)
37 36
{
......
60 59

  
61 60
    while(ok())
62 61
    {
63
        double line_loc = get_line_pos(linesensor->query());
62
        double line_loc = linesensor->readline();
64 63

  
65 64
        cout << "line_loc: " << line_loc << endl;
66 65

  

Also available in: Unified diff