Revision 0970d303 scout_avr/src/main.cpp

View differences:

scout_avr/src/main.cpp
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

  

Also available in: Unified diff