6 |
6 |
#include "stepper.h"
|
7 |
7 |
#include "orb.h"
|
8 |
8 |
#include "cliffSensor.h"
|
9 |
|
#include <bom/bom.h>
|
10 |
|
#include <sonar/sonar_distance.h>
|
11 |
|
#include <sonar/sonar_toggle.h>
|
12 |
|
#include <sonar/sonar_set_scan.h>
|
13 |
|
#include <headlights/set_headlights.h>
|
14 |
|
#include <cliffsensor/cliff_status_changed.h>
|
|
9 |
#include <messages/bom.h>
|
|
10 |
#include <messages/sonar_distance.h>
|
|
11 |
#include <messages/sonar_toggle.h>
|
|
12 |
#include <messages/sonar_set_scan.h>
|
|
13 |
#include <messages/set_headlights.h>
|
|
14 |
#include <messages/cliff_status_changed.h>
|
15 |
15 |
#include <util/delay.h>
|
16 |
16 |
|
17 |
17 |
/* Period of main loop in ms */
|
... | ... | |
23 |
23 |
{
|
24 |
24 |
}
|
25 |
25 |
|
26 |
|
void orb_callback(const headlights::set_headlights& msg)
|
|
26 |
void orb_callback(const messages::set_headlights& msg)
|
27 |
27 |
{
|
28 |
28 |
orb_set0(msg.left_red, msg.left_green, msg.left_blue);
|
29 |
29 |
orb_set1(msg.right_red, msg.right_green, msg.right_blue);
|
30 |
30 |
}
|
31 |
31 |
|
32 |
32 |
/* dammit, Priya, this capitalization just looks ridiculous */
|
33 |
|
void range_toggle_cb(const sonar::sonar_toggleRequest& req,
|
34 |
|
sonar::sonar_toggleResponse& resp)
|
|
33 |
void range_toggle_cb(const messages::sonar_toggleRequest& req,
|
|
34 |
messages::sonar_toggleResponse& resp)
|
35 |
35 |
{
|
36 |
36 |
range_enabled = req.set_on;
|
37 |
37 |
if (range_enabled)
|
... | ... | |
41 |
41 |
resp.ack = true;
|
42 |
42 |
}
|
43 |
43 |
|
44 |
|
void range_set_scan_cb(const sonar::sonar_set_scanRequest& req,
|
45 |
|
sonar::sonar_set_scanResponse& resp)
|
|
44 |
void range_set_scan_cb(const messages::sonar_set_scanRequest& req,
|
|
45 |
messages::sonar_set_scanResponse& resp)
|
46 |
46 |
{
|
47 |
47 |
step_sweep_bounds(req.stop_l, req.stop_r);
|
48 |
48 |
resp.ack = true;
|
... | ... | |
53 |
53 |
unsigned long now, next;
|
54 |
54 |
unsigned int ranges[2];
|
55 |
55 |
char i, id;
|
|
56 |
int cliff_front, cliff_left, cliff_right;
|
56 |
57 |
|
57 |
58 |
ros::NodeHandle nh;
|
58 |
59 |
nh.initNode();
|
... | ... | |
66 |
67 |
|
67 |
68 |
/* Range */
|
68 |
69 |
range_init();
|
69 |
|
sonar::sonar_distance range_msg;
|
|
70 |
messages::sonar_distance range_msg;
|
70 |
71 |
ros::Publisher range_pub("sonar_distance", &range_msg);
|
71 |
72 |
ros::ServiceServer
|
72 |
|
<sonar::sonar_toggleRequest, sonar::sonar_toggleResponse>
|
|
73 |
<messages::sonar_toggleRequest, messages::sonar_toggleResponse>
|
73 |
74 |
range_toggle("sonar_toggle", range_toggle_cb);
|
74 |
75 |
ros::ServiceServer
|
75 |
|
<sonar::sonar_set_scanRequest, sonar::sonar_set_scanResponse>
|
|
76 |
<messages::sonar_set_scanRequest, messages::sonar_set_scanResponse>
|
76 |
77 |
range_set_scan("sonar_set_scan", range_set_scan_cb);
|
77 |
78 |
nh.advertise(range_pub);
|
78 |
79 |
nh.advertiseService(range_toggle);
|
... | ... | |
80 |
81 |
|
81 |
82 |
/* BOM */
|
82 |
83 |
bom_init();
|
83 |
|
bom::bom bom_msg;
|
|
84 |
messages::bom bom_msg;
|
84 |
85 |
ros::Publisher bom_pub("bom", &bom_msg);
|
85 |
86 |
nh.advertise(bom_pub);
|
86 |
87 |
|
87 |
88 |
/* Headlights (aka Orbs) */
|
88 |
|
//orb_init();
|
89 |
|
ros::Subscriber<headlights::set_headlights> orb_sub("set_headlights",
|
|
89 |
orb_init();
|
|
90 |
/*ros::Subscriber<messages::set_headlights> orb_sub("set_headlights",
|
90 |
91 |
orb_callback);
|
91 |
|
nh.subscribe(orb_sub);
|
|
92 |
nh.subscribe(orb_sub);*/
|
92 |
93 |
|
93 |
94 |
/* Cliff sensors */
|
94 |
95 |
cliffSensor_init();
|
95 |
|
cliffsensor::cliff_status_changed cliff_msg;
|
|
96 |
messages::cliff_status_changed cliff_msg;
|
96 |
97 |
ros::Publisher cliff_pub("cliff", &cliff_msg);
|
97 |
98 |
nh.advertise(cliff_pub);
|
98 |
99 |
|
... | ... | |
143 |
144 |
}
|
144 |
145 |
|
145 |
146 |
/* TODO remove raw values and have single bitmask */
|
146 |
|
cliff_msg.front_raw = read_cliffSensor_front();
|
147 |
|
cliff_msg.left_raw = read_cliffSensor_left();
|
148 |
|
cliff_msg.right_raw = read_cliffSensor_right();
|
149 |
|
cliff_msg.cliff_status = cliff_msg.front_raw | cliff_msg.left_raw
|
150 |
|
| cliff_msg.right_raw;
|
|
147 |
cliff_front = read_cliffSensor_front();
|
|
148 |
cliff_left = read_cliffSensor_left();
|
|
149 |
cliff_right = read_cliffSensor_right();
|
|
150 |
if (cliff_front != cliff_msg.front_raw || cliff_left != cliff_msg.left_raw
|
|
151 |
|| cliff_right != cliff_msg.right_raw) {
|
|
152 |
cliff_msg.front_raw = cliff_front;
|
|
153 |
cliff_msg.left_raw = cliff_left;
|
|
154 |
cliff_msg.right_raw = cliff_right;
|
|
155 |
cliff_msg.cliff_status = cliff_front || cliff_left || cliff_right;
|
|
156 |
cliff_pub.publish(&cliff_msg);
|
|
157 |
}
|
151 |
158 |
|
152 |
159 |
}
|
153 |
160 |
|