Skip to content

Commit 61caaa5

Browse files
Alex-BehAlex-Beh
authored
Merge branch 'main' into main
2 parents c1abda8 + 0b4d4f8 commit 61caaa5

8 files changed

Lines changed: 280 additions & 0 deletions

File tree

‎CMakeLists.txt‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,5 +11,44 @@ if (NOT CMAKE_BUILD_TYPE)
1111
endif ()
1212

1313
add_subdirectory(infinite_sense_core)
14+
1415
add_subdirectory(tools)
1516
add_subdirectory(example)
17+
18+
find_package(ZeroMQ QUIET)
19+
if (ZeroMQ_FOUND)
20+
message(STATUS "ZMQ found, Compiling ZMQ nodes")
21+
find_package(ZeroMQ REQUIRED)
22+
if (NOT ZeroMQ_LIBRARIES)
23+
message(WARNING "ZeroMQ_LIBRARIES is empty. Using manual linking fallback.")
24+
set(ZeroMQ_LIBRARIES zmq)
25+
endif()
26+
27+
find_package(Threads REQUIRED)
28+
29+
message(STATUS "ZeroMQ_LIBRARIES: ${ZeroMQ_LIBRARIES}")
30+
message(STATUS "ZeroMQ_INCLUDE_DIRS: ${ZeroMQ_INCLUDE_DIRS}")
31+
32+
add_executable(${PROJECT_NAME}_zmq_node
33+
example/ZMQ/zmq_main.cpp
34+
infinite_sense_core/src/messenger.cpp
35+
)
36+
target_include_directories(${PROJECT_NAME}_zmq_node PRIVATE
37+
${ZeroMQ_INCLUDE_DIRS}
38+
)
39+
target_link_libraries(${PROJECT_NAME}_zmq_node PRIVATE
40+
Threads::Threads
41+
${ZeroMQ_LIBRARIES}
42+
infinite_sense_core
43+
)
44+
endif ()
45+
46+
# Conditionally add ROS 2 components
47+
find_package(rclcpp QUIET)
48+
find_package(rclcpp_components QUIET)
49+
if(rclcpp_FOUND AND rclcpp_components_FOUND)
50+
message(STATUS "ROS 2 detected, adding example/ros2_components components...")
51+
add_subdirectory(example/ros2_components)
52+
else()
53+
message(STATUS "ROS 2 not found. Skipping ROS 2 components.")
54+
endif()

‎example/gprmc_streaming/data.log‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
sprintf(gmrc_buffer,
2+
"GPRMC,
3+
%02d%02d%02d, <---- hhmmss
4+
A,
5+
%s, <---- 25.04776
6+
N,
7+
%s, <---- 121.53185
8+
E,
9+
022.4,
10+
084.4,
11+
%02d%02d%02d, <---- ddmmyy
12+
,
13+
A", <---- A XX
14+
15+
hh, mm, ss, "25.04776", "121.53185", day, month, year);
16+
17+
$GPRMC,hhmmss,A,25.04776,N,121.53185,E,022.4,084.4,ddmmyy,,A XX
18+
19+
void SendHex(uart_inst_t uart, uint8_t crc) {
20+
char high = (crc >> 4) & 0x0F;
21+
char low = crc & 0x0F;
22+
high = (high < 10) ? ('0' + high) : ('A' + high - 10);
23+
low = (low < 10) ? ('0' + low) : ('A' + low - 10);
24+
uart_putc(uart, high);
25+
uart_putc(uart, low);
26+
uart_puts(uart, "\n");
27+
}
28+
29+
主函数：
30+
sprintf(gmrc_buffer, "GPRMC,%02d%02d%02d,A,%s,N,%s,E,022.4,084.4,%02d%02d%02d,,A", hh, mm, ss, "25.04776", "121.53185", day, month, year);
31+
uart_puts(uart, "$");
32+
uart_puts(uart, gmrc_buffer);
33+
uart_puts(uart, "");
34+
byte crc = 0;
35+
for (byte x = 0; x < strlen(gmrc_buffer); x++) {
36+
crc = crc ^ gmrc_buffer[x];
37+
}
38+
SendHex(uart, crc);
39+
40+
65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 32 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 30 0a
41+
65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 33 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 31 0a
42+
65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 34 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 36 0a
43+
65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 35 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 37 0a
44+
65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 36 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 34 0a
45+
65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 37 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 35 0a
46+
65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 38 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 41 0a
47+
65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 39 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 42 0a

‎example/gprmc_streaming/gprmc_streaming.py‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
import serial
2+
from time import sleep
3+
4+
ser = serial.Serial('/dev/ttyUSB0', # or /dev/ttyUSB0
5+
# baudrate=9600, # <-- set to the same value you used
6+
baudrate=115200, # <-- set to the same value you used
7+
bytesize=serial.EIGHTBITS,
8+
parity=serial.PARITY_NONE,
9+
stopbits=serial.STOPBITS_ONE,
10+
timeout=1)
11+
12+
def read_vendor_nmea(ser):
13+
raw = ser.readline() # blocks until LF
14+
# if raw:
15+
# print("RAW :", raw.hex(" "))
16+
17+
if not raw or raw[:1] != b'$' or len(raw) < 6:
18+
return None, False
19+
20+
raw = raw.rstrip(b'\r\n') # drop LF/CR
21+
22+
star = raw.rfind(b'*') # <─ find the asterisk
23+
if star == -1 or len(raw) - star < 3: # need at least '*XX'
24+
return None, False
25+
26+
payload = raw[1:star] # bytes between $ and *
27+
cs_hex = raw[star+1:star+3] # the two ASCII hex digits
28+
29+
try:
30+
cs_rx = int(cs_hex, 16)
31+
except ValueError:
32+
return None, False
33+
34+
cs_calc = 0
35+
for b in payload:
36+
cs_calc ^= b
37+
38+
nmea = b'$' + payload + b'*' + cs_hex + b'\r\n'
39+
return nmea.decode('ascii', errors='replace'), cs_calc == cs_rx
40+
41+
42+
while True:
43+
nmea, ok = read_vendor_nmea(ser)
44+
if nmea:
45+
print("✓" if ok else "✗", nmea)
46+
else:
47+
print("waiting …", end='\r')
48+
sleep(0.1)

‎example/gprmc_streaming/loopback_test.py‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
# Loopback Test
2+
3+
import serial
4+
import time
5+
6+
ser = serial.Serial('/dev/ttyUSB0', # or /dev/ttyUSB0
7+
# baudrate=9600, # <-- set to the same value you used
8+
baudrate=115200, # <-- set to the same value you used
9+
bytesize=serial.EIGHTBITS,
10+
parity=serial.PARITY_NONE,
11+
stopbits=serial.STOPBITS_ONE,
12+
timeout=1)
13+
print(f"Listening on {ser.port} @ {ser.baudrate} baud – Ctrl‑C to quit")
14+
15+
# --- Read forever -----------------------------------------------------
16+
try:
17+
while True:
18+
ser.write(b'>') # send the character '>'
19+
time.sleep(1.00)
20+
print("echo:", ser.read(1))
21+
except KeyboardInterrupt:
22+
pass
23+
finally:
24+
ser.close()
25+
print("\nSerial port closed – bye.")

‎example/ros2_components/CMakeLists.txt‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
find_package(ament_cmake REQUIRED)
2+
find_package(rclcpp REQUIRED)
3+
find_package(rclcpp_components REQUIRED)
4+
5+
add_library(synchronizer_component SHARED
6+
synchronizer_component.cpp
7+
)
8+
9+
target_link_libraries(synchronizer_component
10+
infinite_sense_core # Assuming this is defined in your top-level CMake
11+
)
12+
ament_target_dependencies(synchronizer_component
13+
"rclcpp"
14+
"rclcpp_components"
15+
"std_msgs"
16+
)
17+
18+
# Include directories (not targets)
19+
target_include_directories(synchronizer_component
20+
PRIVATE
21+
${CMAKE_SOURCE_DIR}/infinite_sense_core/include
22+
)
23+
24+
# Register the component
25+
rclcpp_components_register_nodes(synchronizer_component "SynchronizerComponent")
26+
27+
# Install targets
28+
install(TARGETS synchronizer_component
29+
ARCHIVE DESTINATION lib
30+
LIBRARY DESTINATION lib
31+
RUNTIME DESTINATION bin
32+
)

‎example/ros2_components/synchronizer_component.cpp‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
// synchronizer_component.cpp
2+
#include "infinite_sense.h"
3+
#include <chrono>
4+
#include <thread>
5+
#include "rclcpp/rclcpp.hpp"
6+
#include "rclcpp_components/register_node_macro.hpp"
7+
8+
using namespace std::chrono_literals;
9+
using namespace infinite_sense;
10+
11+
class SynchronizerComponent : public rclcpp::Node
12+
{
13+
public:
14+
SynchronizerComponent(const rclcpp::NodeOptions & options)
15+
: Node("synchronizer_node", options)
16+
{
17+
RCLCPP_INFO(get_logger(), "Initializing SynchronizerComponent...");
18+
19+
// Set up the serial connection
20+
synchronizer_.SetSerialLink("/dev/ttyACM0", 460800);
21+
22+
// Optionally: SetNetLink or UseMvCam
23+
// synchronizer_.SetNetLink("192.168.1.188", 8888);
24+
// synchronizer_.UseMvCam();
25+
26+
synchronizer_.Start();
27+
Synchronizer::PrintSummary();
28+
29+
// Setup a timer instead of while loop
30+
timer_ = this->create_wall_timer(1s, [this]() {
31+
RCLCPP_INFO(this->get_logger(), "Running...");
32+
// You can add status logging or health check here
33+
});
34+
}
35+
36+
~SynchronizerComponent()
37+
{
38+
synchronizer_.Stop();
39+
}
40+
41+
private:
42+
Synchronizer synchronizer_;
43+
rclcpp::TimerBase::SharedPtr timer_;
44+
};
45+
46+
RCLCPP_COMPONENTS_REGISTER_NODE(SynchronizerComponent)

‎example/send_cfg/cfg.json‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
{
2+
"f": "cfg",
3+
"port": 8888,
4+
"ip": [
5+
192,
6+
168,
7+
1,
8+
188
9+
],
10+
"subnet": [
11+
255,
12+
255,
13+
255,
14+
0
15+
],
16+
"hz_cam_1": 10,
17+
"uart_0_baud_rate": 921600,
18+
"uart_1_baud_rate": 115200,
19+
"uart_2_baud_rate": 115200,
20+
"use_gps": true,
21+
"use_pps": true,
22+
"version": 300
23+
}

‎example/send_cfg/send_cfg.sh‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
#!/bin/bash
2+
3+
SERIAL_PORT="/dev/ttyACM0"
4+
BAUD_RATE="921600"
5+
CFG_FILE="cfg.json"
6+
7+
if [ ! -f "$CFG_FILE" ]; then
8+
echo "Error: $CFG_FILE not found!"
9+
exit 1
10+
fi
11+
12+
echo "Sending cfg to $SERIAL_PORT at $BAUD_RATE baud..."
13+
14+
# Configure the serial port
15+
stty -F "$SERIAL_PORT" raw speed "$BAUD_RATE" cs8 -cstopb -parenb
16+
17+
# Send the content + newline
18+
cat "$CFG_FILE" | sed 's/$/\n/' > "$SERIAL_PORT"
19+
20+
echo "Done."

0 commit comments

Comments
 (0)