diff --git a/vectornav/CMakeLists.txt b/vectornav/CMakeLists.txt index 0cf42c8d..2af957a2 100644 --- a/vectornav/CMakeLists.txt +++ b/vectornav/CMakeLists.txt @@ -35,7 +35,7 @@ target_link_libraries(${PROJECT_NAME} vncxx ) # vn_sensor_msgs add_executable(vn_sensor_msgs src/vn_sensor_msgs.cc) -ament_target_dependencies(vn_sensor_msgs rclcpp sensor_msgs vectornav_msgs) +ament_target_dependencies(vn_sensor_msgs rclcpp sensor_msgs vectornav_msgs tf2_geometry_msgs) # install install(TARGETS @@ -52,4 +52,4 @@ endif() ament_export_dependencies(rosidl_default_runtime) -ament_package() \ No newline at end of file +ament_package() diff --git a/vectornav/src/vectornav.cc b/vectornav/src/vectornav.cc index a0284177..5ec09cc1 100755 --- a/vectornav/src/vectornav.cc +++ b/vectornav/src/vectornav.cc @@ -363,14 +363,17 @@ class Vectornav : public rclcpp::Node { // GPS Compass Baseline // 8.2.3 - auto gps_baseline = vs_.readGpsCompassBaseline(); - RCLCPP_INFO(get_logger(), "GPS Baseline : (%f, %f, %f), (%f, %f, %f)", - gps_baseline.position[0], gps_baseline.position[1], gps_baseline.position[2], - gps_baseline.uncertainty[0], gps_baseline.uncertainty[1], gps_baseline.uncertainty[2]); + try { + auto gps_baseline = vs_.readGpsCompassBaseline(); + RCLCPP_INFO(get_logger(), "GPS Baseline : (%f, %f, %f), (%f, %f, %f)", + gps_baseline.position[0], gps_baseline.position[1], gps_baseline.position[2], + gps_baseline.uncertainty[0], gps_baseline.uncertainty[1], gps_baseline.uncertainty[2]); + } catch (vn::sensors::sensor_error e) { + RCLCPP_ERROR(get_logger(), "readGpsCompassBaseline: %s", e.what()); + } // Register Binary Data Callback vs_.registerAsyncPacketReceivedHandler(this, Vectornav::AsyncPacketReceivedHandler); - // Connection Successful return true; }