Skip to content

Commit 362f350

Browse files
dylanzemlindylanzemlin
committed
Added vectornav support
1 parent d98a8eb commit 362f350

File tree

7 files changed

+216
-3
lines changed

7 files changed

+216
-3
lines changed

‎.vscode/c_cpp_properties.json‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,10 @@
1212
"/usr/include/opencv4/**",
1313

1414
// ONNX includes
15-
"/opt/onnxruntime/include/**"
15+
"/opt/onnxruntime/include/**",
16+
17+
// VectorNav includes
18+
"/usr/local/vectornav/cpp/include/**"
1619
],
1720
"defines": [],
1821
"compilerPath": "/usr/bin/gcc",

‎igvc_ws/src/igvc/include/igvc/topics.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ namespace IGVC
66
{
77
// real robot stuff
88
constexpr const char *GPS = "/igvc/gps";
9+
constexpr const char *IMU = "/igvc/imu";
910
constexpr const char *CAMERA = "/igvc/camera/{dynamic}/raw";
1011
constexpr const char *PROCESSED_IMAGE = "/igvc/camera/{dynamic}/processed";
1112
constexpr const char *MOTOR_INPUT = "/igvc/motor_input";

‎igvc_ws/src/igvc_hardware/CMakeLists.txt‎

Lines changed: 14 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,13 @@ endif()
2323
find_package(OpenCV REQUIRED)
2424
include_directories(${OpenCV_INCLUDE_DIRS} /usr/include/opencv4)
2525

26+
# Vectornav
27+
set(VN_DIR "/usr/local/vectornav/cpp")
28+
29+
file(GLOB VN_SOURCES "${VN_DIR}/src/**/*.cpp")
30+
add_library(vectornav SHARED ${VN_SOURCES})
31+
target_include_directories(vectornav PUBLIC "${VN_DIR}/include")
32+
2633
if(BUILD_TESTING)
2734
find_package(ament_lint_auto REQUIRED)
2835
set(ament_cmake_copyright_FOUND TRUE)
@@ -33,14 +40,20 @@ endif()
3340
# local package include directories
3441
include_directories(include)
3542

36-
# creating executables
43+
# create executable (can)
3744
add_executable(igvc_hardware_can src/can.cpp)
3845
ament_target_dependencies(igvc_hardware_can igvc rclcpp std_msgs nav_msgs sensor_msgs cv_bridge image_transport OpenCV)
3946
target_link_libraries(igvc_hardware_can ${OpenCV_LIBS} ${SOCKETCAN_LIB} opencv_core opencv_imgproc opencv_highgui)
4047

48+
# create executable (vn)
49+
add_executable(igvc_hardware_vn src/vn.cpp)
50+
ament_target_dependencies(igvc_hardware_vn igvc rclcpp std_msgs nav_msgs sensor_msgs)
51+
target_link_libraries(igvc_hardware_vn vectornav)
52+
4153
# installing executables
4254
install(TARGETS
4355
igvc_hardware_can
56+
igvc_hardware_vn
4457
DESTINATION lib/${PROJECT_NAME}
4558
)
4659

‎igvc_ws/src/igvc_hardware/src/vn.cpp‎

Lines changed: 161 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,161 @@
1+
#include <chrono>
2+
#include <map>
3+
#include <thread>
4+
5+
#include "igvc/node.hpp"
6+
#include "rclcpp/rclcpp.hpp"
7+
#include "igvc_messages/msg/gps_feedback.hpp"
8+
#include "igvc_messages/msg/imu_feedback.hpp"
9+
10+
#include "vectornav/Interface/Sensor.hpp"
11+
12+
using namespace std::chrono_literals;
13+
14+
class IGVCHardwareVectornavNode : public IGVC::Node
15+
{
16+
public:
17+
IGVCHardwareVectornavNode() : IGVC::Node("igvc_vectornav") {}
18+
19+
void init() override
20+
{
21+
mGpsFeedbackPublisher = this->create_publisher<igvc_messages::msg::GPSFeedback>(IGVC::Topics::GPS, 10);
22+
mImuFeedbackPublisher = this->create_publisher<igvc_messages::msg::IMUFeedback>(IGVC::Topics::IMU, 10);
23+
24+
setDeviceState(IGVC::DeviceState::READY);
25+
26+
mSensorConnectionThread = std::thread(&IGVCHardwareVectornavNode::sensorThreadFunction, this);
27+
mSensorMonitorThread = std::thread(&IGVCHardwareVectornavNode::monitorThreadFunction, this);
28+
}
29+
30+
void sensorThreadFunction()
31+
{
32+
// setup output register
33+
mOutputRegister.rateDivisor = 5; // 20 Hz
34+
mOutputRegister.asyncMode = VN::Registers::System::BinaryOutput::AsyncMode{};
35+
mOutputRegister.asyncMode->serial1 = 1;
36+
37+
// gnss/lla
38+
mOutputRegister.gnss = VN::Registers::System::BinaryOutput::Gnss{};
39+
mOutputRegister.gnss.gnss1Fix = 1;
40+
mOutputRegister.gnss.gnss1NumSats = 1;
41+
mOutputRegister.gnss.gnss1PosLla = 1;
42+
43+
// ypr
44+
mOutputRegister.common = VN::Registers::System::BinaryOutput::Common{};
45+
mOutputRegister.common.ypr = 1;
46+
47+
while (rclcpp::ok())
48+
{
49+
// first, if we have a sensor check if its still connected
50+
if (mSensor && !mSensor->verifySensorConnectivity())
51+
{
52+
RCLCPP_WARN(get_logger(), "VectorNav sensor disconnected, attempting to reconnect");
53+
mSensor.reset();
54+
setDeviceState(IGVC::DeviceState::READY);
55+
}
56+
57+
// if we are already connected, just sleep and continue
58+
if (mSensor)
59+
{
60+
std::this_thread::sleep_for(1s);
61+
continue;
62+
}
63+
64+
// try to connect
65+
mSensor = std::make_shared<VN::Sensor>();
66+
VN::Error err = mSensor->autoConnect("/dev/igvc_vn");
67+
if (err != VN::Error::None)
68+
{
69+
std::string errString = VN::errorCodeToString(err);
70+
RCLCPP_WARN(get_logger(), "Failed to connect to VectorNav sensor: %s", errString.c_str());
71+
mSensor.reset();
72+
std::this_thread::sleep_for(3s);
73+
continue;
74+
}
75+
76+
// setup
77+
mSensor->asyncOutputEnable(VN::AsyncOutputEnable::State::Disable);
78+
79+
auto writeCmdOpt = mOutputRegister.toWriteCommand();
80+
if (!writeCmdOpt)
81+
{
82+
RCLCPP_ERROR(get_logger(), "Failed to create VectorNav output register write command");
83+
mSensor.reset();
84+
std::this_thread::sleep_for(3s);
85+
continue;
86+
}
87+
88+
mSensor->asyncOutputEnable(VN::AsyncOutputEnable::State::Enable);
89+
90+
// connected
91+
RCLCPP_INFO(get_logger(), "Connected to VectorNav sensor");
92+
setDeviceState(IGVC::DeviceState::OPERATING);
93+
mIsConnected.store(true);
94+
}
95+
}
96+
97+
void monitorThreadFunction()
98+
{
99+
while (rclcpp::ok())
100+
{
101+
if (!mIsConnected.load())
102+
{
103+
std::this_thread::sleep_for(100ms);
104+
continue;
105+
}
106+
107+
// read the registers
108+
VN::Sensor::CompositeDataQueueReturn compositeData = mSensor->getNextMeasurement();
109+
if (!compositeData)
110+
{
111+
// TOOD: Log?
112+
continue;
113+
}
114+
115+
if (!compositeData->matchesMessage(mOutputRegister))
116+
{
117+
// we don't care about this
118+
continue;
119+
}
120+
121+
// gps
122+
igvc_messages::msg::GPSFeedback gpsMsg;
123+
gpsMsg.latitude = compositeData->gnss.gnss1PosLla->lat;
124+
gpsMsg.longitude = compositeData->gnss.gnss1PosLla->lon;
125+
gpsMsg.altitude = compositeData->gnss.gnss1PosLla->alt;
126+
gpsMsg.num_sats = compositeData->gnss.gnss1NumSats.value_or(0);
127+
gpsMsg.gps_fix = compositeData->gnss.gnss1Fix.value_or(0);
128+
mGpsFeedbackPublisher->publish(gpsMsg);
129+
130+
// imu
131+
igvc_messages::msg::IMUFeedback imuMsg;
132+
imuMsg.yaw = compositeData->attitude.ypr->yaw;
133+
imuMsg.pitch = compositeData->attitude.ypr->pitch;
134+
imuMsg.roll = compositeData->attitude.ypr->roll;
135+
mImuFeedbackPublisher->publish(imuMsg);
136+
137+
std::this_thread::sleep_for(10ms);
138+
}
139+
}
140+
141+
private:
142+
// threads
143+
std::thread mSensorConnectionThread;
144+
std::thread mSensorMonitorThread;
145+
146+
// shared data
147+
std::atomic<bool> mIsConnected{false};
148+
149+
// vn stuff
150+
std::shared_ptr<VN::Sensor> mSensor;
151+
VN::Registers::System::BinaryOutput1 mOutputRegister;
152+
153+
// publishers
154+
rclcpp::Publisher<igvc_messages::msg::GPSFeedback>::SharedPtr mGpsFeedbackPublisher;
155+
rclcpp::Publisher<igvc_messages::msg::IMUFeedback>::SharedPtr mImuFeedbackPublisher;
156+
};
157+
158+
int main(int argc, char *argv[])
159+
{
160+
IGVC::Node::create_and_run<IGVCHardwareVectornavNode>(argc, argv);
161+
}

‎igvc_ws/src/igvc_messages/msg/GPSFeedback.msg‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,5 @@
11
float64 latitude
2-
float64 longitude
2+
float64 longitude
3+
float64 altitude
4+
int16 num_sats
5+
int16 gps_fix

‎igvc_ws/src/igvc_messages/msg/IMUFeedback.msg‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
float64 yaw
2+
float64 pitch
3+
float64 roll

‎setup/install_vn.sh‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
#!/bin/bash
2+
set -e
3+
4+
ZIP_URL="https://cdn.soonerrobotics.org/scr/vn.zip"
5+
INSTALL_DIR="/usr/local/vectornav"
6+
TMP_DIR="/tmp/vectornav_install"
7+
8+
# check if /usr/local/vectornav exists
9+
if [ -d "$INSTALL_DIR" ]; then
10+
echo "$INSTALL_DIR already exists. Skipping installation."
11+
exit 0
12+
fi
13+
14+
# create the directory
15+
sudo mkdir -p "$INSTALL_DIR"
16+
17+
# clone the zip
18+
mkdir -p "$TMP_DIR"
19+
cd "$TMP_DIR"
20+
wget "$ZIP_URL" -O vn.zip
21+
22+
# unzip the contents
23+
unzip vn.zip -d vn_contents
24+
cd vn_contents
25+
sudo cp -r * "$INSTALL_DIR/"
26+
cd ..
27+
28+
# cleanup
29+
rm -rf "$TMP_DIR"

0 commit comments

Comments
 (0)