Revision 8741d18c scout_avr/src/main.cpp

View differences:

scout_avr/src/main.cpp
5 5
#include "range.h"
6 6
#include "stepper.h"
7 7
#include "orb.h"
8
#include "cliffSensor.h"
8 9
#include <bom/bom.h>
9 10
#include <sonar/sonar_distance.h>
10 11
#include <sonar/sonar_toggle.h>
11 12
#include <sonar/sonar_set_scan.h>
12 13
#include <headlights/set_headlights.h>
14
#include <cliffsensor/cliff_status_changed.h>
13 15
#include <util/delay.h>
14 16

  
15 17
/* Period of main loop in ms */
......
88 90
      orb_callback);
89 91
  nh.subscribe(orb_sub);
90 92

  
93
  /* Cliff sensors */
94
  cliffSensor_init();
95
  cliffsensor::cliff_status_changed cliff_msg;
96
  ros::Publisher cliff_pub("cliff", &cliff_msg);
97
  nh.advertise(cliff_pub);
98

  
91 99
  id = 0;
92 100
  next = 0;
93 101
  while (1)
......
126 134
    if (range_enabled) {
127 135
      step_sweep();
128 136
      range_measure(ranges);
129
      range_msg.header.stamp = nh.now();
130
      range_msg.header.seq++;
137
      range_msg.stamp = nh.now();
131 138
      range_msg.pos = step_get_pos();
132 139
      range_msg.distance0 = ranges[0];
133 140
      range_msg.distance1 = ranges[1];
......
135 142
      range_pub.publish(&range_msg);
136 143
    }
137 144

  
145
    /* 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;
151

  
138 152
  }
139 153

  
140 154
  return 0;

Also available in: Unified diff