Skip to content

Commit 6d3fa63

Browse files
adthomsadthoms
committed
move Handle_dev branch from beam_camera_sync intoig_handle repo
2 parents 1996f3f + 0a27580 commit 6d3fa63

2 files changed

Lines changed: 252 additions & 0 deletions

File tree

‎main/install_instructions.md‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
### 1. Install Arduino + Teensyduino (https://www.pjrc.com/teensy/td_download.html)
2+
```
3+
wget https://downloads.arduino.cc/arduino-1.8.13-linux64.tar.xz
4+
wget https://www.pjrc.com/teensy/td_153/TeensyduinoInstall.linux64
5+
wget https://www.pjrc.com/teensy/00-teensy.rules
6+
sudo cp 00-teensy.rules /etc/udev/rules.d/
7+
tar -xf arduino-1.8.13-linux64.tar.xz
8+
chmod 755 TeensyduinoInstall.linux64
9+
./TeensyduinoInstall.linux64 --dir=arduino-1.8.13
10+
```
11+
12+
### 2. Install rosserial
13+
```
14+
sudo apt-get install ros-kinetic-rosserial-arduino
15+
sudo apt-get install ros-kinetic-rosserial
16+
```
17+
18+
### 3. Instal ros_lib
19+
```
20+
cd <sketchbook>/libraries
21+
rm -rf ros_lib
22+
rosrun rosserial_arduino make_libraries.py .
23+
```
24+
### 4. Upload firmware to Teensy (if the Teensy doesn't already have it installed already)
25+
Open `main.ino` with Arduino and press upload. It should compile and then open the Teensyduino uploader. There should a status bar as the firmware is written. Make sure the settings for the board match those below (the port will vary but it's usually /usb/ttyACM0 on linux machines).
26+
<img width="826" alt="image" src="https://user-images.githubusercontent.com/32364356/112694118-2d1be080-8e58-11eb-87a6-f3feb7064fae.png">
27+
28+
### 5. Run the pass-through node.
29+
Start `roscore` then run the rosserial client application that forwards your Arduino messages to the rest of ROS. Make sure to use the correct serial port:
30+
```
31+
rosrun rosserial_python serial_node.py /dev/ttyACM0
32+
```
33+
Once this node is running all topics that the Teensy publishes to are available to ros. `rostopic list` should reveal /F1/cam_time, /F1/toggle, etc.

‎main/main.ino‎

Lines changed: 219 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,219 @@
1+
#include <ros.h>
2+
#include <sensor_msgs/TimeReference.h>
3+
#include <FrequencyTimer2.h>
4+
#include <TimeLib.h>
5+
6+
#define GPSERIAL Serial1
7+
#define PPS_PIN 6
8+
#define IMU_IN 12
9+
#define CAM1_OUT 2
10+
#define CAM2_OUT 7
11+
#define CAM3_OUT 8
12+
#define CAM4_OUT 3
13+
#define CAM1_IN 9
14+
#define CAM2_IN 10
15+
#define CAM3_IN 11
16+
#define CAM4_IN 24
17+
#define IMU_START 23
18+
19+
/* ROS node handler */
20+
ros::NodeHandle nh;
21+
/* Trigger variables for camera */
22+
sensor_msgs::TimeReference time_msg;
23+
ros::Publisher F1_time("/F1/cam_time", &time_msg);
24+
ros::Publisher F2_time("/F2/cam_time", &time_msg);
25+
ros::Publisher F3_time("/F3/cam_time", &time_msg);
26+
ros::Publisher F4_time("/F4/cam_time", &time_msg);
27+
ros::Publisher IMU_time("/imu/imu_time", &time_msg);
28+
volatile bool sendNMEA = false, F1_closed = false, F2_closed = false,
29+
F3_closed = false, F4_closed = false, IMU_sampled = false;
30+
ros::Time F1_close_stamp, F2_close_stamp, F3_close_stamp, F4_close_stamp, IMU_stamp;
31+
32+
///* Forward function declarations */
33+
void cam1_ISR(void);
34+
void cam2_ISR(void);
35+
void cam3_ISR(void);
36+
void cam4_ISR(void);
37+
void IMU_ISR(void);
38+
void setSendNMEA(void);
39+
void enableTriggers();
40+
String checksum(String msg);
41+
42+
/*
43+
* Initial setup for the arduino sketch
44+
* This function performs:
45+
* - Configure timers for LiDAR and camera triggering
46+
* - Advertisement and subscribing to ROS topics
47+
* - UART Serial setup for NMEA strings
48+
* - Holds until rosserial is connected
49+
*/
50+
void setup() {
51+
/* Setup outputs to LiDAR
52+
* - PPS but don't start it yet
53+
- Attach */
54+
pinMode(PPS_PIN, OUTPUT); // pin 5 is still driven by default but has a 50% duty cycle
55+
FrequencyTimer2::setPeriod(1000000); // 10^6 microseconds, 1 second
56+
FrequencyTimer2::setOnOverflow(setSendNMEA_ISR); // sets the function that runs when the timer overflows
57+
58+
// node initialization
59+
nh.initNode();
60+
nh.advertise(F1_time);
61+
nh.advertise(F2_time);
62+
nh.advertise(F3_time);
63+
nh.advertise(F4_time);
64+
nh.advertise(IMU_time);
65+
66+
67+
/* configure input pins */
68+
pinMode(CAM1_IN, INPUT_PULLUP);
69+
pinMode(CAM2_IN, INPUT_PULLUP);
70+
pinMode(CAM3_IN, INPUT_PULLUP);
71+
pinMode(CAM4_IN, INPUT_PULLUP);
72+
pinMode(IMU_IN, INPUT);
73+
74+
/* enable interrupts */
75+
attachInterrupt(digitalPinToInterrupt(CAM1_IN), cam1_ISR, RISING);
76+
attachInterrupt(digitalPinToInterrupt(CAM2_IN), cam2_ISR, RISING);
77+
attachInterrupt(digitalPinToInterrupt(CAM3_IN), cam3_ISR, RISING);
78+
attachInterrupt(digitalPinToInterrupt(CAM4_IN), cam4_ISR, RISING);
79+
attachInterrupt(digitalPinToInterrupt(IMU_IN), IMU_ISR, RISING);
80+
81+
/* set up the camera triggers but don't start them yet either */
82+
analogWriteFrequency(CAM1_OUT, 20.0); // 20.0 Hz base frequency for the PWM signal
83+
analogWriteFrequency(CAM2_OUT, 20.0); // We're using a PWM signal because it's a way of offloading
84+
analogWriteFrequency(CAM3_OUT, 20.0); // the task to free up the main loop
85+
analogWriteFrequency(CAM4_OUT, 20.0);
86+
/* setup IMU_START ping as output */
87+
pinMode(IMU_START, OUTPUT);
88+
digitalWrite(IMU_START, LOW); // make sure that the pin is low before we send a rising edge
89+
90+
// await nodehandle time sync
91+
while (!nh.connected()) {
92+
nh.spinOnce();
93+
}
94+
95+
/* start sampling */
96+
nh.loginfo("Setup complete, enabling triggers");
97+
enableTriggers();
98+
}
99+
100+
101+
/*
102+
* Continously looping function performs the following:
103+
* - Triggering camera line at certain frequency and publishes the timestamp to
104+
* /cam_time
105+
* - Trigger lidar line (PPS) and transmits NMEA string over Serial1
106+
*/
107+
void loop() {
108+
if (sendNMEA == true) {
109+
char time_now[7], date_now[7];
110+
time_t t = nh.now().sec;
111+
sprintf(time_now, "%02i%02i%02i", hour(t), minute(t), second(t));
112+
sprintf(date_now, "%02i%02i%02i", day(t), month(t), year(t) % 100);
113+
String nmea_string = F("GPRMC,") + String(time_now) +
114+
F(",A,4365.107,N,79347.702,E,022.4,084.4,") +
115+
String(date_now) + ",003.1,W";
116+
String chk = checksum(nmea_string);
117+
nmea_string = "$" + nmea_string + "*" + chk + "\n";
118+
GPSERIAL.print(nmea_string);
119+
sendNMEA = false;
120+
digitalWriteFast(PPS_PIN,
121+
LOW); // minimum pulse duration required by LiDAR is 10 us
122+
123+
124+
// nh.loginfo(nmea_string.c_str()); //debug nmea string in rosconsole
125+
}
126+
if (F1_closed == true) {
127+
F1_closed = false;
128+
time_msg.time_ref = F1_close_stamp;
129+
F1_time.publish(&time_msg);
130+
}
131+
if (F2_closed == true) {
132+
F2_closed = false;
133+
time_msg.time_ref = F2_close_stamp;
134+
F2_time.publish(&time_msg);
135+
}
136+
if (F3_closed == true) {
137+
F3_closed = false;
138+
time_msg.time_ref = F3_close_stamp;
139+
F3_time.publish(&time_msg);
140+
}
141+
if (F4_closed == true) {
142+
F4_closed = false;
143+
time_msg.time_ref = F4_close_stamp;
144+
F4_time.publish(&time_msg);
145+
}
146+
if (IMU_sampled == true) {
147+
IMU_sampled = false;
148+
time_msg.time_ref = IMU_stamp;
149+
IMU_time.publish(&time_msg);
150+
}
151+
152+
nh.spinOnce();
153+
}
154+
155+
/***********************************************
156+
* Helper functions *
157+
***********************************************/
158+
void setSendNMEA_ISR(void) {
159+
/* It's not really recommended to write to pins from an interrupt as it can
160+
* take a relatively long time to execute. However, one of the main purposes
161+
* of this code is to output accurate PPS signal and this is the most accurate
162+
* way that abides by the format constraints on the signal.
163+
*/
164+
digitalWriteFast(PPS_PIN, HIGH);
165+
sendNMEA = true;
166+
// microsSincePPS = 0;
167+
}
168+
169+
// Timestamp creation interrupts
170+
void cam1_ISR(void) {
171+
F1_close_stamp = nh.now();
172+
F1_closed = true;
173+
}
174+
void cam2_ISR(void) {
175+
F2_close_stamp = nh.now();
176+
F2_closed = true;
177+
}
178+
void cam3_ISR(void) {
179+
F3_close_stamp = nh.now();
180+
F3_closed = true;
181+
}
182+
void cam4_ISR(void) {
183+
F4_close_stamp = nh.now();
184+
F4_closed = true;
185+
}
186+
void IMU_ISR(void) {
187+
IMU_stamp = nh.now();
188+
IMU_sampled = true;
189+
}
190+
191+
/* Computes XOR checksum of NMEA sentence */
192+
String checksum(String msg) {
193+
byte chksum = 0;
194+
int l = msg.length();
195+
for (int i = 0; i < l; i++) {
196+
chksum ^= msg[i];
197+
}
198+
String result = String(chksum, HEX);
199+
result.toUpperCase();
200+
if (result.length() < 2) {
201+
result = "0" + result;
202+
}
203+
return result;
204+
}
205+
206+
void enableTriggers() {
207+
/* await even second */
208+
// By switching to nh.now() we shouldn't need this wait
209+
// while (!(micros() % 1000000) {
210+
// }
211+
FrequencyTimer2::enable();
212+
213+
/* start sampling */
214+
analogWrite(CAM1_OUT, 5); // 5% duty cycle @ 20 Hz = 2.5 ms pulse
215+
analogWrite(CAM2_OUT, 5);
216+
analogWrite(CAM3_OUT, 5);
217+
analogWrite(CAM4_OUT, 5);
218+
digitalWrite(IMU_START, HIGH);
219+
}

0 commit comments

Comments
 (0)