From a35d6a742161ba249c558c01e97971333d0c098b Mon Sep 17 00:00:00 2001 From: Pekka Rantalankila Date: Fri, 24 Nov 2023 13:53:43 +0200 Subject: [PATCH 1/9] Use parameterSets in SDK configuration. --- spectacularai_ros2/src/ros2_plugin.cpp | 37 ++++++++------------------ 1 file changed, 11 insertions(+), 26 deletions(-) diff --git a/spectacularai_ros2/src/ros2_plugin.cpp b/spectacularai_ros2/src/ros2_plugin.cpp index b3b2189..7324fb8 100644 --- a/spectacularai_ros2/src/ros2_plugin.cpp +++ b/spectacularai_ros2/src/ros2_plugin.cpp @@ -58,30 +58,6 @@ static const rclcpp::Duration CAMERA_SYNC_INTERVAL = rclcpp::Duration(0, 10 * 1e static const rclcpp::QoS CAMERA_QOS = rclcpp::QoS(rclcpp::KeepLast(CAM_QUEUE_SIZE)).best_effort().durability_volatile(); static const rclcpp::QoS IMU_QOS = rclcpp::QoS(rclcpp::KeepLast(IMU_QUEUE_SIZE)).best_effort().durability_volatile(); -const char *oakYaml = -R"(# set: wrapper-base -trackChiTestOutlierR: 3 -trackOutlierThresholdGrowthFactor: 1.3 -hybridMapSize: 2 -sampleSyncLag: 1 -trackingStatusInitMinSeconds: 0.5 -cameraTrailLength: 12 -cameraTrailHanoiLength: 8 -delayFrames: 2 -maxSlamResultQueueSize: 2 -# set: oak-d -imuAnomalyFilterGyroEnabled: true -imuStationaryEnabled: true -visualR: 0.01 -skipFirstNFrames: 10 -# set: live -noWaitingForResults: true -# custom -useSlam: True -ffmpegVideoCodec: "libx264 -crf 15 -preset ultrafast" -)"; - - using StereoCameraPolicy = message_filters::sync_policies::ApproximateTime< sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::Image, sensor_msgs::msg::Image>; @@ -221,14 +197,23 @@ class Node : public rclcpp::Node { private: std::string createConfigYaml() { std::ostringstream oss; - oss << oakYaml; + const bool isRae = stringStartsWith(deviceModel, "RAE") || stringStartsWith(deviceModel, "rae"); + + if (isRae) { + oss << "parameterSets: [\"wrapper-base\", \"live\", \"rae\"]\n"; + } + else { + oss << "parameterSets: [\"wrapper-base\", \"live\", \"oak-d\"]\n"; + } oss << "alreadyRectified: True\n"; + oss << "useSlam: True\n"; + oss << "ffmpegVideoCodec: \"libx264 -crf 15 -preset ultrafast\"\n"; bool useFeatureTracker = cameraInputType == "stereo_depth_features"; if (useFeatureTracker) { // Not good with non-depth grayscale video inputs. - oss << "depthErrorScale: 0.1\n"; + if (!isRae) oss << "depthErrorScale: 0.1\n"; oss << "keyframeCandidateInterval: 0\n"; } else { oss << "keyframeCandidateInterval: 6\n"; From ca38a79d963fc097918360f4397e01d2af174910 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 30 Nov 2023 14:40:53 +0000 Subject: [PATCH 2/9] changes for python bindings --- scripts/build_all_static.sh | 3 +- scripts/download_and_build_static.sh | 2 +- spectacularai_ros2/CMakeLists.txt | 37 +- .../spectacularai_ros2}/device_imu_calib.hpp | 0 .../spectacularai_ros2}/helpers.hpp | 0 .../spectacularai_ros2}/occupancy_grid.hpp | 0 .../spectacularai_ros2}/pose_helper.hpp | 0 .../spectacularai_ros2/ros2_plugin.hpp | 683 +++++++++++++++++ spectacularai_ros2/src/ros2_plugin.cpp | 684 +----------------- 9 files changed, 715 insertions(+), 694 deletions(-) rename spectacularai_ros2/{src => include/spectacularai_ros2}/device_imu_calib.hpp (100%) rename spectacularai_ros2/{src => include/spectacularai_ros2}/helpers.hpp (100%) rename spectacularai_ros2/{src => include/spectacularai_ros2}/occupancy_grid.hpp (100%) rename spectacularai_ros2/{src => include/spectacularai_ros2}/pose_helper.hpp (100%) create mode 100644 spectacularai_ros2/include/spectacularai_ros2/ros2_plugin.hpp diff --git a/scripts/build_all_static.sh b/scripts/build_all_static.sh index d4ef917..cf69fc6 100755 --- a/scripts/build_all_static.sh +++ b/scripts/build_all_static.sh @@ -14,13 +14,14 @@ cp "$SDK_PACKAGE" "$TMP/unpack/" cd "$TMP/unpack" tar xvf spectacularAI_*.tar.gz make PREFIX="$TMP/install" test +make install cd "$ROOT" ./scripts/build_all.sh -DspectacularAI_DIR="$TMP/install/lib/cmake/spectacularAI" INSTALL_DIR="spectacularai_ros2/install/spectacularai_ros2/" # Strip the executable (just in case the CMake build failed to do that) -strip "$INSTALL_DIR/lib/libvislam.so" +strip "$INSTALL_DIR/lib/libspectacularai_ros2.so" # Copy license/notice file form the SDK package LICENSE_OUTPUT_DIR="$INSTALL_DIR" diff --git a/scripts/download_and_build_static.sh b/scripts/download_and_build_static.sh index de17aa9..3dd85e9 100755 --- a/scripts/download_and_build_static.sh +++ b/scripts/download_and_build_static.sh @@ -30,7 +30,7 @@ cd .. # Build ROS node . /opt/ros/${ROS_DISTRO}/setup.sh -. $DEPTHAI_WS/install/setup.sh +# . $DEPTHAI_WS/install/setup.sh ./scripts/build_all_static.sh sai_sdk/spectacularAI_*_static_rae.tar.gz # Remove temp files diff --git a/spectacularai_ros2/CMakeLists.txt b/spectacularai_ros2/CMakeLists.txt index e0c3468..267007f 100644 --- a/spectacularai_ros2/CMakeLists.txt +++ b/spectacularai_ros2/CMakeLists.txt @@ -6,7 +6,7 @@ set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -O2") set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") -set(DYNAMIC_SAI_LIBRARY OFF CACHE STRING "Use dynamic Spectacular AI SDK") +set(DYNAMIC_SAI_LIBRARY ON ) set(PLUGIN_SRC src/ros2_plugin.cpp) @@ -25,23 +25,23 @@ find_package(message_filters REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(depthai_ros_msgs REQUIRED) # Depth AI features - -# add_executable(vislam ${PLUGIN_SRC}) -add_library(vislam SHARED ${PLUGIN_SRC}) +include_directories(include) +# add_executable(${PROJECT_NAME} ${PLUGIN_SRC}) +add_library(${PROJECT_NAME} SHARED ${PLUGIN_SRC}) # Dynamic dependencies of the static library SDK variant set(SAI_STATIC_MANUAL_LINK_LIBS z) -target_link_libraries(vislam PRIVATE spectacularAI::spectacularAI ${SAI_STATIC_MANUAL_LINK_LIBS}) +target_link_libraries(${PROJECT_NAME} PRIVATE spectacularAI::spectacularAI ${SAI_STATIC_MANUAL_LINK_LIBS}) # Strip the output executable -set_target_properties(vislam PROPERTIES LINK_FLAGS -s) +set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS -s) -rclcpp_components_register_nodes(vislam "spectacularAI::ros2::Node") +rclcpp_components_register_nodes(${PROJECT_NAME} "spectacularAI::ros2::Node") # ROS dependencies -ament_target_dependencies(vislam PUBLIC rclcpp rclcpp_components sensor_msgs nav_msgs tf2_geometry_msgs image_transport message_filters tf2 tf2_ros depthai_ros_msgs) +ament_target_dependencies(${PROJECT_NAME} PUBLIC spectacularAI rclcpp rclcpp_components sensor_msgs nav_msgs tf2_geometry_msgs image_transport message_filters tf2 tf2_ros depthai_ros_msgs) install( - TARGETS vislam DESTINATION lib/ + TARGETS ${PROJECT_NAME} DESTINATION lib/ RUNTIME DESTINATION lib/ ARCHIVE DESTINATION lib/ LIBRARY DESTINATION lib/ @@ -52,5 +52,24 @@ install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) if (DYNAMIC_SAI_LIBRARY) install(IMPORTED_RUNTIME_ARTIFACTS spectacularAI::spectacularAI) endif() +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include + +) +install(EXPORT ${PROJECT_NAME}Targets + DESTINATION share/${PROJECT_NAME}/cmake) +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${dependencies}) ament_package() diff --git a/spectacularai_ros2/src/device_imu_calib.hpp b/spectacularai_ros2/include/spectacularai_ros2/device_imu_calib.hpp similarity index 100% rename from spectacularai_ros2/src/device_imu_calib.hpp rename to spectacularai_ros2/include/spectacularai_ros2/device_imu_calib.hpp diff --git a/spectacularai_ros2/src/helpers.hpp b/spectacularai_ros2/include/spectacularai_ros2/helpers.hpp similarity index 100% rename from spectacularai_ros2/src/helpers.hpp rename to spectacularai_ros2/include/spectacularai_ros2/helpers.hpp diff --git a/spectacularai_ros2/src/occupancy_grid.hpp b/spectacularai_ros2/include/spectacularai_ros2/occupancy_grid.hpp similarity index 100% rename from spectacularai_ros2/src/occupancy_grid.hpp rename to spectacularai_ros2/include/spectacularai_ros2/occupancy_grid.hpp diff --git a/spectacularai_ros2/src/pose_helper.hpp b/spectacularai_ros2/include/spectacularai_ros2/pose_helper.hpp similarity index 100% rename from spectacularai_ros2/src/pose_helper.hpp rename to spectacularai_ros2/include/spectacularai_ros2/pose_helper.hpp diff --git a/spectacularai_ros2/include/spectacularai_ros2/ros2_plugin.hpp b/spectacularai_ros2/include/spectacularai_ros2/ros2_plugin.hpp new file mode 100644 index 0000000..37c035d --- /dev/null +++ b/spectacularai_ros2/include/spectacularai_ros2/ros2_plugin.hpp @@ -0,0 +1,683 @@ +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "message_filters/subscriber.h" +#include "message_filters/time_synchronizer.h" +#include "message_filters/sync_policies/approximate_time.h" + +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/image_encodings.hpp" + +#include "image_transport/image_transport.hpp" + +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.h" + +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2/LinearMath/Quaternion.h" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/path.hpp" + +// Depth AI +#include + +// Spectacular AI +#include + +// Helpers +#include "helpers.hpp" +#include "pose_helper.hpp" +#include "device_imu_calib.hpp" +#include "occupancy_grid.hpp" + +namespace { +constexpr size_t IMU_QUEUE_SIZE = 1000; +constexpr size_t ODOM_QUEUE_SIZE = 1000; +constexpr uint32_t CAM_QUEUE_SIZE = 30; +constexpr size_t TRAJECTORY_LENGTH = 1000; +constexpr size_t UPDATE_TRAJECTORY_EVERY_NTH_POSE = 5; + +// Group window for camera frames, should always be smaller than time between two frames +static const rclcpp::Duration CAMERA_SYNC_INTERVAL = rclcpp::Duration(0, 10 * 1e6); + +// https://docs.ros.org/en/humble/Concepts/Intermediate/About-Quality-of-Service-Settings.html +static const rclcpp::QoS CAMERA_QOS = rclcpp::QoS(rclcpp::KeepLast(CAM_QUEUE_SIZE)).best_effort().durability_volatile(); +static const rclcpp::QoS IMU_QOS = rclcpp::QoS(rclcpp::KeepLast(IMU_QUEUE_SIZE)).best_effort().durability_volatile(); + +using StereoCameraPolicy = message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo, + sensor_msgs::msg::Image, sensor_msgs::msg::Image>; + +using StereoDepthCameraPolicy = message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo, + sensor_msgs::msg::Image, sensor_msgs::msg::Image, + sensor_msgs::msg::Image, depthai_ros_msgs::msg::TrackedFeatures>; + +using StereoCameraSynchronizer = message_filters::Synchronizer; +using StereoDepthCameraSynchronizer = message_filters::Synchronizer; + +#pragma pack(push) +#pragma pack(1) +struct RosPointWithColor { + spectacularAI::Vector3f pos; + uint8_t rgba[4]; +}; +#pragma pack(pop) + +class TrajectoryPublisher { + rclcpp::Publisher::SharedPtr publisher; + std::deque poses; + std::string frameId; + int counter = 0; +public: + TrajectoryPublisher(rclcpp::Publisher::SharedPtr publisher, std::string frameId) + : publisher(publisher), frameId(frameId) {} + + void addPose(spectacularAI::Pose pose) { + poses.push_front(poseToPoseStampped(pose, frameId)); + while (poses.size() > TRAJECTORY_LENGTH) poses.pop_back(); + if (counter % UPDATE_TRAJECTORY_EVERY_NTH_POSE == 0) { + nav_msgs::msg::Path path; + path.header.stamp = secondsToStamp(pose.time); + path.header.frame_id = frameId; + path.poses = std::vector(poses.begin(), poses.end()); + publisher->publish(path); + } + counter++; + } +}; + + +} // namespace + +namespace spectacularAI { +namespace ros2 { + +class Node : public rclcpp::Node { +public: + Node(const rclcpp::NodeOptions& options) : rclcpp::Node("spetacularAI", options), vioInitDone(false), receivedFrames(false) { + + deviceModel = declareAndReadParameterString("device_model", ""); + + // Used for IMU-Camera extrinsics calibration + imuFrameId = declareAndReadParameterString("imu_frame_id", ""); + cam0FrameId = declareAndReadParameterString("cam0_optical_frame_id", ""); + cam1FrameId = declareAndReadParameterString("cam1_optical_frame_id", ""); + + // Output frames + fixedFrameId = declareAndReadParameterString("fixed_frame_id", "map"); + odometryFrameId = declareAndReadParameterString("odometry_frame_id", "odom"); + baseLinkFrameId = declareAndReadParameterString("base_link_frame_id", "base_link"); + + depthScale = declareAndReadParameterDouble("depth_scale", 1.0 / 1000.0); + recordingFolder = declareAndReadParameterString("recording_folder", ""); + enableMapping = declareAndReadParameterBool("enable_mapping", true); + enableOccupancyGrid = declareAndReadParameterBool("enable_occupancy_grid", false); + cameraInputType = declareAndReadParameterString("camera_input_type", "stereo_depth_features"); + + outputOnImuSamples = declareAndReadParameterBool("output_on_imu_samples", false); + recordingOnly = declareAndReadParameterBool("recording_only", false); + + maxOdomFreq = declareAndReadParameterDouble("max_odom_freq", 100); + maxOdomCorrectionFreq = declareAndReadParameterDouble("max_odom_correction_freq", 10); + + overrideImuToCam0 = declareAndReadParameterString("imu_to_cam0", ""); + overrideImuToCam1 = declareAndReadParameterString("imu_to_cam1", ""); + + bool separateOdomTf = declareAndReadParameterBool("separate_odom_tf", false); + + bool publishPaths = declareAndReadParameterBool("publish_paths", false); + + transformListenerBuffer = std::make_unique(this->get_clock()); + transformListener = std::make_shared(*transformListenerBuffer); + transformBroadcaster = std::make_unique(*this); + + imuSubscription = this->create_subscription("input/imu", IMU_QOS, + std::bind(&Node::imuCallback, this, std::placeholders::_1)); + + if (separateOdomTf) { + poseHelper = std::make_unique(1.0 / maxOdomCorrectionFreq); + } + + // odometryPublisher = this->create_publisher("output/odometry", ODOM_QUEUE_SIZE); + if (enableOccupancyGrid) occupancyGridPublisher = this->create_publisher("output/occupancyGrid", ODOM_QUEUE_SIZE); + if (enableMapping) pointCloudPublisher = this->create_publisher("output/pointcloud", ODOM_QUEUE_SIZE); + if (publishPaths) { + odometryPathPublisher = std::make_unique(this->create_publisher("output/odometry_path", ODOM_QUEUE_SIZE), fixedFrameId); + correctedPathPublisher = std::make_unique(this->create_publisher("output/corrected_path", ODOM_QUEUE_SIZE), fixedFrameId); + vioPathPublisher = std::make_unique(this->create_publisher("output/vio_path", ODOM_QUEUE_SIZE), fixedFrameId); + } + + if (cameraInputType == "stereo") { + cameraInfo0Subscription.subscribe(this, "/input/cam0/camera_info", CAMERA_QOS.get_rmw_qos_profile()); + cameraInfo1Subscription.subscribe(this, "/input/cam1/camera_info", CAMERA_QOS.get_rmw_qos_profile()); + cameraImage0Subscription.subscribe(this, "/input/cam0/image_rect", CAMERA_QOS.get_rmw_qos_profile()); + cameraImage1Subscription.subscribe(this, "/input/cam1/image_rect", CAMERA_QOS.get_rmw_qos_profile()); + + stereoCameraSynchronizer = std::make_unique(StereoCameraPolicy(CAM_QUEUE_SIZE), + cameraInfo0Subscription, cameraInfo1Subscription, + cameraImage0Subscription, cameraImage1Subscription); + stereoCameraSynchronizer->setMaxIntervalDuration(CAMERA_SYNC_INTERVAL); + stereoCameraSynchronizer->registerCallback(std::bind(&Node::stereoCameraCallback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); + } else if (cameraInputType == "stereo_depth_features") { + cameraInfo0Subscription.subscribe(this, "/input/cam0/camera_info", CAMERA_QOS.get_rmw_qos_profile()); + cameraInfo1Subscription.subscribe(this, "/input/cam1/camera_info", CAMERA_QOS.get_rmw_qos_profile()); + cameraImage0Subscription.subscribe(this, "/input/cam0/image_rect", CAMERA_QOS.get_rmw_qos_profile()); + cameraImage1Subscription.subscribe(this, "/input/cam1/image_rect", CAMERA_QOS.get_rmw_qos_profile()); + cameraDepthSubscription.subscribe(this, "/input/depth/image", CAMERA_QOS.get_rmw_qos_profile()); + depthaiTrackedFeaturesCam0Subscription.subscribe(this, "/input/cam0/features", CAMERA_QOS.get_rmw_qos_profile()); + + stereoDepthCameraSynchronizer = std::make_unique(StereoDepthCameraPolicy(CAM_QUEUE_SIZE), + cameraInfo0Subscription, cameraInfo1Subscription, + cameraImage0Subscription, cameraImage1Subscription, + cameraDepthSubscription, depthaiTrackedFeaturesCam0Subscription); + stereoDepthCameraSynchronizer->setMaxIntervalDuration(CAMERA_SYNC_INTERVAL); + stereoDepthCameraSynchronizer->registerCallback(std::bind(&Node::stereoDepthCameraCallback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); + } else { + RCLCPP_WARN(this->get_logger(), "Unsupported camera_input_type: %s", cameraInputType.c_str()); + } + } + +private: + std::string createConfigYaml() { + std::ostringstream oss; + const bool isRae = stringStartsWith(deviceModel, "RAE") || stringStartsWith(deviceModel, "rae"); + + if (isRae) { + oss << "parameterSets: [\"wrapper-base\", \"live\", \"rae\"]\n"; + } + else { + oss << "parameterSets: [\"wrapper-base\", \"live\", \"oak-d\"]\n"; + } + oss << "alreadyRectified: True\n"; + oss << "useSlam: True\n"; + oss << "ffmpegVideoCodec: \"libx264 -crf 15 -preset ultrafast\"\n"; + + bool useFeatureTracker = cameraInputType == "stereo_depth_features"; + + if (useFeatureTracker) { + // Not good with non-depth grayscale video inputs. + if (!isRae) oss << "depthErrorScale: 0.1\n"; + oss << "keyframeCandidateInterval: 0\n"; + } else { + oss << "keyframeCandidateInterval: 6\n"; + } + + if (enableMapping) { + oss << "computeStereoPointCloud: True\n"; + oss << "computeDenseStereoDepthKeyFramesOnly: True\n"; + oss << "stereoPointCloudStride: 5\n"; + oss << "stereoPointCloudMinDepth: " << minDepth << "\n"; + } + + if (outputOnImuSamples) { + oss << "outputOnFrames: False\n"; + } + + return oss.str(); + } + + std::string declareAndReadParameterString(std::string name, std::string defaultValue) { + this->declare_parameter(name, defaultValue); + return this->get_parameter(name).as_string(); + } + + double declareAndReadParameterDouble(std::string name, double defaultValue) { + this->declare_parameter(name, defaultValue); + return this->get_parameter(name).as_double(); + } + + bool declareAndReadParameterBool(std::string name, bool defaultValue) { + this->declare_parameter(name, defaultValue); + return this->get_parameter(name).as_bool(); + } + + void imuCallback(const sensor_msgs::msg::Imu &msg) { + // RCLCPP_INFO(this->get_logger(), "Received: %s", msgToString(msg).c_str()); + double time = stampToSeconds(msg.header.stamp); + if (imuStartTime < 0) { + imuStartTime = time; + } else if (!receivedFrames && time - imuStartTime > 5.0 && time - lastCameraWarning > 5.0) { + RCLCPP_WARN(this->get_logger(), "Received IMU data for %f seconds, but haven't received any camera data. Are camera streams properly configured and synchronized?", time - imuStartTime); + lastCameraWarning = time; + } + + if (!vioInitDone || !vioApi) return; + if (time < previousImuTime) { + RCLCPP_WARN(this->get_logger(), "Received IMU sample (%f) that's older than previous (%f), skipping it.", time, previousImuTime); + return; + } + vioApi->addGyro(time, {msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z}); + vioApi->addAcc(time, {msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z}); + if (outputOnImuSamples && (time - previousTriggerTime) > (1. / maxOdomFreq)) { + vioApi->addTrigger(time, triggerCounter++); + previousTriggerTime = time; + } + previousImuTime = time; + } + + bool getStereoCameraExtrinsics(spectacularAI::Matrix4d &imuToCam0, spectacularAI::Matrix4d &imuToCam1, spectacularAI::Matrix4d &imuToOutput) { + if ((!deviceModel.empty() || !overrideImuToCam0.empty() || !overrideImuToCam1.empty()) + && !cam0FrameId.empty() + && !cam1FrameId.empty()) { + + spectacularAI::Matrix4d imuToCameraX; + int camIndex = 0; + + if (!overrideImuToCam0.empty()) { + camIndex = 0; + if (!matrixFromJson(overrideImuToCam0, imuToCameraX)) { + RCLCPP_WARN(this->get_logger(), "Failed to parse imuToCam0 matrix from given JSON string: %s", overrideImuToCam0.c_str()); + return false; + } + RCLCPP_INFO(this->get_logger(), "Using given imuToCam0 calibration"); + } else if (!overrideImuToCam1.empty()) { + camIndex = 1; + if (!matrixFromJson(overrideImuToCam1, imuToCameraX)) { + RCLCPP_WARN(this->get_logger(), "Failed to parse imuToCam0 matrix from given JSON string: %s", overrideImuToCam0.c_str()); + return false; + } + RCLCPP_INFO(this->get_logger(), "Using given imuToCam1 calibration"); + } else if (!deviceModel.empty()) { + if (!getDeviceImuToCameraMatrix(deviceModel, imuToCameraX, camIndex)) { + RCLCPP_WARN(this->get_logger(), "No stored camera calibration for %s", deviceModel.c_str()); + return false; + } + RCLCPP_INFO(this->get_logger(), "Using imuToCamera calibration based on device model"); + } else { + RCLCPP_WARN(this->get_logger(), "Failed to find any imuToCamera calibration"); + return false; + } + + if (camIndex == 0) { + spectacularAI::Matrix4d cam0ToCam1; + try { + cam0ToCam1 = matrixConvert(transformListenerBuffer->lookupTransform(cam1FrameId, cam0FrameId, tf2::TimePointZero)); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "Could not get camera transforms: %s", ex.what()); + return false; + } + imuToCam0 = imuToCameraX; + imuToCam1 = matrixMul(cam0ToCam1, imuToCam0); + } else { + spectacularAI::Matrix4d cam1ToCam0; + try { + cam1ToCam0 = matrixConvert(transformListenerBuffer->lookupTransform(cam0FrameId, cam1FrameId, tf2::TimePointZero)); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "Could not get camera transforms: %s", ex.what()); + return false; + } + imuToCam1 = imuToCameraX; + imuToCam0 = matrixMul(cam1ToCam0, imuToCam1); + } + + + } else if (!imuFrameId.empty() && !cam0FrameId.empty() && !cam1FrameId.empty()) { + try { + imuToCam0 = matrixConvert(transformListenerBuffer->lookupTransform(cam0FrameId, imuFrameId, tf2::TimePointZero)); + imuToCam1 = matrixConvert(transformListenerBuffer->lookupTransform(cam1FrameId, imuFrameId, tf2::TimePointZero)); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "Could not get camera transforms: %s", ex.what()); + return false; + } + } else { + RCLCPP_WARN(this->get_logger(), + "Failed to obtain camera extrinsics, cannot start tracking. You must provide a known device_model or imu frame ID"); + return false; + } + + spectacularAI::Matrix4d cam0ToBaseLink; + + try { + cam0ToBaseLink = matrixConvert(transformListenerBuffer->lookupTransform(baseLinkFrameId, cam0FrameId, tf2::TimePointZero)); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "Could not get camera transforms for computing imuToOutput: %s", ex.what()); + return false; + } + imuToOutput = matrixMul(cam0ToBaseLink, imuToCam0); + return true; + } + + void stereoCameraCallback( + const sensor_msgs::msg::CameraInfo::ConstSharedPtr &camInfo0, const sensor_msgs::msg::CameraInfo::ConstSharedPtr &camInfo1, + const sensor_msgs::msg::Image::ConstSharedPtr &img0, const sensor_msgs::msg::Image::ConstSharedPtr &img1) { + receivedFrames = true; + + if (!vioInitDone) { + spectacularAI::Matrix4d imuToCam0, imuToCam1, imuToOutput; + if (!getStereoCameraExtrinsics(imuToCam0, imuToCam1, imuToOutput)) return; + // TODO: Support non rectified images, by ROS convention "image" topic is unrectified, "image_rect" is rectified + constexpr bool IS_RECTIFIED = true; + startVio({camInfo0, camInfo1}, {imuToCam0, imuToCam1}, imuToOutput, IS_RECTIFIED); + } + + if (vioInitDone && !vioApi) return; + + if (img0->width != img1->width || img0->height != img0->height) { + RCLCPP_WARN(this->get_logger(), "Stereo image resolutions don't match"); + return; + } + + spectacularAI::ColorFormat colorFormat; + if (!rosEncodingToColorFormat(img0->encoding, colorFormat)) { + RCLCPP_WARN(this->get_logger(), "Unsupported image encoding"); + return; + } + + // RCLCPP_INFO(this->get_logger(), "Timestamp: %d, %d", img0->header.stamp.sec, img0->header.stamp.nanosec); + + double time = stampToSeconds(img0->header.stamp); + if (time < previousFrameTime) { + RCLCPP_WARN(this->get_logger(), "Received frame (%f) that's older than previous (%f), skipping it.", time, previousFrameTime); + return; + } + previousFrameTime = time; + vioApi->addFrameStereo( + time, + img0->width, img0->height, + img0->data.data(), img1->data.data(), + colorFormat + ); + } + + void stereoDepthCameraCallback( + const sensor_msgs::msg::CameraInfo::ConstSharedPtr &camInfo0, const sensor_msgs::msg::CameraInfo::ConstSharedPtr &camInfo1, + const sensor_msgs::msg::Image::ConstSharedPtr &img0, const sensor_msgs::msg::Image::ConstSharedPtr &img1, + const sensor_msgs::msg::Image::ConstSharedPtr &depth, const depthai_ros_msgs::msg::TrackedFeatures::ConstSharedPtr &features) { + + receivedFrames = true; + //RCLCPP_INFO(this->get_logger(), "Received all data: %f", stampToSeconds(img0->header.stamp)); + + if (!vioInitDone) { + spectacularAI::Matrix4d imuToCam0, imuToCam1, imuToOutput; + if (!getStereoCameraExtrinsics(imuToCam0, imuToCam1, imuToOutput)) return; + // TODO: Support non rectified images, by ROS convention "image" topic is unrectified, "image_rect" is rectified + constexpr bool IS_RECTIFIED = true; + startVio({camInfo0, camInfo1}, {imuToCam0, imuToCam1}, imuToOutput, IS_RECTIFIED); + } + + if (vioInitDone && !vioApi) return; + + spectacularAI::ColorFormat colorFormat; + spectacularAI::ColorFormat depthFormat; + if (!rosEncodingToColorFormat(img0->encoding, colorFormat)) { + RCLCPP_WARN(this->get_logger(), "Unsupported image encoding: %s", img0->encoding.c_str()); + return; + } + if (!rosEncodingToColorFormat(depth->encoding, depthFormat)) { + RCLCPP_WARN(this->get_logger(), "Unsupported depth image encoding: %s", depth->encoding.c_str()); + return; + } + + monoFeatures.clear(); + monoFeatures.resize(features->features.size()); + for (auto& ft : features->features) { + monoFeatures.push_back({ int(ft.id), { float(ft.position.x), float(ft.position.y) } }); + } + + double time = stampToSeconds(img0->header.stamp); + if (time < previousFrameTime) { + RCLCPP_WARN(this->get_logger(), "Received frame (%f) that's older than previous (%f), skipping it.", time, previousFrameTime); + return; + } + previousFrameTime = time; + + bool isSlamKf = frameNumber % 6 == 0; // TODO: Make configurable + vioApi->addFrameStereoDepth( + time, + img0->width, img0->height, + monoFeatures.data(), + monoFeatures.size(), + depth->data.data(), + depthFormat, + isSlamKf ? img0->data.data() : nullptr, + isSlamKf ? img1->data.data() : nullptr, + isSlamKf ? colorFormat : spectacularAI::ColorFormat::NONE, + depthScale); + + frameNumber++; + } + + void vioOutputCallback(spectacularAI::VioOutputPtr vioOutput) { + if (vioOutput->status == spectacularAI::TrackingStatus::TRACKING) { + if (poseHelper) { + spectacularAI::Pose odomPose; + if (poseHelper->computeContinousTrajectory(vioOutput, odomPose, odomCorrection)) { + transformBroadcaster->sendTransform(poseToTransformStampped(odomCorrection, fixedFrameId, odometryFrameId)); + } + transformBroadcaster->sendTransform(poseToTransformStampped(odomPose, odometryFrameId, baseLinkFrameId)); + if (odometryPathPublisher) odometryPathPublisher->addPose(odomPose); + if (correctedPathPublisher) { + correctedPathPublisher->addPose(spectacularAI::Pose::fromMatrix(odomPose.time, matrixMul(odomCorrection.asMatrix(), odomPose.asMatrix()))); + } + } else { + transformBroadcaster->sendTransform(poseToTransformStampped(vioOutput->pose, fixedFrameId, baseLinkFrameId)); + } + if (vioPathPublisher) vioPathPublisher->addPose(vioOutput->pose); + + // odometryPublisher->publish(outputToOdometryMsg(vioOutput, vioOutputParentFrameId, vioOutputChildFrameId)); + // RCLCPP_INFO(this->get_logger(), "Output: %s", vioOutput->asJson().c_str()); + } + } + + void mappingOutputCallback(spectacularAI::mapping::MapperOutputPtr mappingOutput) { + if (latestKeyFrame == mappingOutput->updatedKeyFrames.back()) return; + latestKeyFrame = mappingOutput->updatedKeyFrames.back(); + const auto &kf = mappingOutput->map->keyFrames.at(latestKeyFrame); + if (!kf->pointCloud || kf->pointCloud->empty()) return; + const auto &primary = kf->frameSet->primaryFrame; + + sensor_msgs::msg::PointCloud2 pc; + pc.header.stamp = secondsToStamp(primary->cameraPose.pose.time); + pc.header.frame_id = fixedFrameId; + + std::string fieldNames[] = {"x", "y", "z", "rgb"}; + uint32_t totalOffset = 0; + for (int i = 0; i < 4; i++) { + sensor_msgs::msg::PointField f; + f.name = fieldNames[i]; + f.offset = totalOffset; + f.datatype = sensor_msgs::msg::PointField::FLOAT32; // 1 float == rgba8 + f.count = 1; + pc.fields.push_back(f); + totalOffset += sizeof(uint32_t); + } + + const std::uint8_t* rgb24Data = kf->pointCloud->getRGB24Data(); + const spectacularAI::Vector3f* positionData = kf->pointCloud->getPositionData(); + Matrix4f camToWorld = matrixToFloat(primary->cameraPose.getCameraToWorldMatrix()); + bool hasColors = kf->pointCloud->hasColors(); + + spectacularAI::Vector3f center = { + (float)primary->cameraPose.pose.position.x, (float)primary->cameraPose.pose.position.y, (float)primary->cameraPose.pose.position.z + }; + if (enableOccupancyGrid) { + if (occupancyGrid.empty()) { + occupancyGrid.reset(center, minDepth, maxDepth, cellSizeMeters, occupiedThreshold); + } + tempGrid.reset(center, minDepth, maxDepth, cellSizeMeters, occupiedThreshold); // TODO: Could use bounding box for visible area + } + + pc.data.resize(sizeof(RosPointWithColor) * kf->pointCloud->size()); + RosPointWithColor* points = (RosPointWithColor*) pc.data.data(); + for (size_t i = 0; i < kf->pointCloud->size(); i++) { + auto &p = points[i]; + p.pos = matVectorMult(camToWorld, positionData[i]); + if (hasColors) { + p.rgba[0] = rgb24Data[i * 3 + 0]; + p.rgba[1] = rgb24Data[i * 3 + 1]; + p.rgba[2] = rgb24Data[i * 3 + 2]; + p.rgba[3] = 255; + } else { + p.rgba[0] = 0; + p.rgba[1] = 0; + p.rgba[2] = 0; + p.rgba[3] = 0; + } + if (enableOccupancyGrid) { + float relativeZ = p.pos.z - center.z; + if (relativeZ > minOccupancyHeight && relativeZ < maxOccupancyHeight) { + tempGrid.addPoint(p.pos); + // p.rgba[0] = 255; + // p.rgba[1] = 0; + // p.rgba[2] = 0; + } + } + } + + pc.point_step = sizeof(RosPointWithColor); + pc.height = 1; + pc.width = kf->pointCloud->size(); + pc.row_step = pc.point_step * kf->pointCloud->size(); + pc.is_bigendian = false; + pc.is_dense = true; + pointCloudPublisher->publish(pc); + + if (enableOccupancyGrid && primary->image) { + // Trace to all edge points to find unoccupied cells + float cameraTestHeight = center.z + (maxOccupancyHeight + minOccupancyHeight) * 0.5; + tempGrid.traceFreeSpace(center, *primary, cameraTestHeight); + occupancyGrid.merge(tempGrid); + + nav_msgs::msg::OccupancyGrid og; + og.header.stamp = pc.header.stamp; + og.header.frame_id = pc.header.frame_id; + + og.info.map_load_time = og.header.stamp; // Seems redundant, duplicating header + og.info.resolution = occupancyGrid.cellSize; + og.info.width = occupancyGrid.width; + og.info.height = occupancyGrid.height; + + og.info.origin.position.x = occupancyGrid.originX * occupancyGrid.cellSize; + og.info.origin.position.y = occupancyGrid.originY * occupancyGrid.cellSize; + og.info.origin.position.z = center.z; + og.info.origin.orientation.x = 0.0f; + og.info.origin.orientation.y = 0.0f; + og.info.origin.orientation.z = 0.0f; + og.info.origin.orientation.w = 1.0f; + og.data = occupancyGrid.cells; // TODO: Copying entire map, perhaps occupancyGrid could internally wrap nav_msgs::msg::OccupancyGrid? + occupancyGridPublisher->publish(og); + } + } + + void startVio(std::vector intrinsics, std::vector extrinsics, spectacularAI::Matrix4d imuToOutput, bool isRectified) { + std::lock_guard lock(vioStartup); + if (vioInitDone) return; + + std::string calibrationJson; + if (!cameraInfoToCalibrationJson(intrinsics, extrinsics, imuToOutput, isRectified, calibrationJson)) { + vioInitDone = true; + return; + } + + std::string config = createConfigYaml(); + + // RCLCPP_INFO(this->get_logger(), "Calibration: %s", calibrationJson.c_str()); + // RCLCPP_INFO(this->get_logger(), "Configuration: %s", config.c_str()); + + spectacularAI::Vio::Builder vioBuilder = spectacularAI::Vio::builder(); + vioBuilder.setCalibrationJSON(calibrationJson); + vioBuilder.setConfigurationYAML(config); + if (!recordingFolder.empty()) { + vioBuilder.setRecordingFolder(recordingFolder); + vioBuilder.setRecordingOnly(recordingOnly); + } + if (enableMapping) vioBuilder.setMapperCallback(std::bind(&Node::mappingOutputCallback, this, std::placeholders::_1)); + vioApi = vioBuilder.build(); + vioApi->setOutputCallback(std::bind(&Node::vioOutputCallback, this, std::placeholders::_1)); + vioInitDone = true; + } + + std::unique_ptr odometryPathPublisher; + std::unique_ptr correctedPathPublisher; + std::unique_ptr vioPathPublisher; + + // rclcpp::Publisher::SharedPtr odometryPublisher; + rclcpp::Publisher::SharedPtr pointCloudPublisher; + rclcpp::Publisher::SharedPtr occupancyGridPublisher; + + rclcpp::Subscription::SharedPtr imuSubscription; + + message_filters::Subscriber cameraInfo0Subscription; + message_filters::Subscriber cameraInfo1Subscription; + message_filters::Subscriber cameraImage0Subscription; + message_filters::Subscriber cameraImage1Subscription; + message_filters::Subscriber cameraDepthSubscription; + message_filters::Subscriber depthaiTrackedFeaturesCam0Subscription; + + std::unique_ptr transformListenerBuffer; + std::shared_ptr transformListener; + std::shared_ptr transformBroadcaster; + + std::unique_ptr stereoCameraSynchronizer; + std::unique_ptr stereoDepthCameraSynchronizer; + + std::unique_ptr vioApi; + std::atomic_bool vioInitDone; + std::mutex vioStartup; + std::unique_ptr poseHelper; + + double previousFrameTime = 0.0; + double previousImuTime = 0.0; + double previousTriggerTime = 0.0; + + int frameNumber = 0; + int64_t latestKeyFrame = -1; + std::vector monoFeatures; + bool outputOnImuSamples; + bool recordingOnly; + uint64_t triggerCounter = 1; + + std::atomic_bool receivedFrames; + double imuStartTime = -1.; + double lastCameraWarning = -1.; + spectacularAI::Pose odomCorrection; + + // Params + std::string fixedFrameId; + std::string odometryFrameId; + std::string baseLinkFrameId; + + std::string deviceModel; + std::string imuFrameId; + std::string cam0FrameId; + std::string cam1FrameId; + double depthScale; + std::string cameraInputType; + std::string recordingFolder; + bool enableMapping; + double maxOdomFreq; + double maxOdomCorrectionFreq; + std::string overrideImuToCam0; + std::string overrideImuToCam1; + + // Map + bool enableOccupancyGrid; + OccupancyGrid occupancyGrid; + OccupancyGrid tempGrid; + float maxDepth = 3.0; // Max distance from camera for occupancy map + float minDepth = 0.2; + float maxOccupancyHeight = 0.3; // Max height above device for point to be counted in occupancy map + float minOccupancyHeight = 0.05; // Min height above device for point to be counted in occupancy map + float cellSizeMeters = 0.07; + int occupiedThreshold = 2; +}; + +} // namespace ros2 +} // namespace spectacularai diff --git a/spectacularai_ros2/src/ros2_plugin.cpp b/spectacularai_ros2/src/ros2_plugin.cpp index 7324fb8..51cc98c 100644 --- a/spectacularai_ros2/src/ros2_plugin.cpp +++ b/spectacularai_ros2/src/ros2_plugin.cpp @@ -1,686 +1,4 @@ -#include -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" - -#include "message_filters/subscriber.h" -#include "message_filters/time_synchronizer.h" -#include "message_filters/sync_policies/approximate_time.h" - -#include "sensor_msgs/msg/imu.hpp" -#include "sensor_msgs/msg/camera_info.hpp" -#include "sensor_msgs/msg/image.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "sensor_msgs/image_encodings.hpp" - -#include "image_transport/image_transport.hpp" - -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/buffer.h" - -#include "tf2/LinearMath/Matrix3x3.h" -#include "tf2/LinearMath/Quaternion.h" - -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - -#include "nav_msgs/msg/odometry.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" -#include "nav_msgs/msg/path.hpp" - -// Depth AI -#include - -// Spectacular AI -#include - -// Helpers -#include "helpers.hpp" -#include "pose_helper.hpp" -#include "device_imu_calib.hpp" -#include "occupancy_grid.hpp" - -namespace { -constexpr size_t IMU_QUEUE_SIZE = 1000; -constexpr size_t ODOM_QUEUE_SIZE = 1000; -constexpr uint32_t CAM_QUEUE_SIZE = 30; -constexpr size_t TRAJECTORY_LENGTH = 1000; -constexpr size_t UPDATE_TRAJECTORY_EVERY_NTH_POSE = 5; - -// Group window for camera frames, should always be smaller than time between two frames -static const rclcpp::Duration CAMERA_SYNC_INTERVAL = rclcpp::Duration(0, 10 * 1e6); - -// https://docs.ros.org/en/humble/Concepts/Intermediate/About-Quality-of-Service-Settings.html -static const rclcpp::QoS CAMERA_QOS = rclcpp::QoS(rclcpp::KeepLast(CAM_QUEUE_SIZE)).best_effort().durability_volatile(); -static const rclcpp::QoS IMU_QOS = rclcpp::QoS(rclcpp::KeepLast(IMU_QUEUE_SIZE)).best_effort().durability_volatile(); - -using StereoCameraPolicy = message_filters::sync_policies::ApproximateTime< - sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo, - sensor_msgs::msg::Image, sensor_msgs::msg::Image>; - -using StereoDepthCameraPolicy = message_filters::sync_policies::ApproximateTime< - sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo, - sensor_msgs::msg::Image, sensor_msgs::msg::Image, - sensor_msgs::msg::Image, depthai_ros_msgs::msg::TrackedFeatures>; - -using StereoCameraSynchronizer = message_filters::Synchronizer; -using StereoDepthCameraSynchronizer = message_filters::Synchronizer; - -#pragma pack(push) -#pragma pack(1) -struct RosPointWithColor { - spectacularAI::Vector3f pos; - uint8_t rgba[4]; -}; -#pragma pack(pop) - -class TrajectoryPublisher { - rclcpp::Publisher::SharedPtr publisher; - std::deque poses; - std::string frameId; - int counter = 0; -public: - TrajectoryPublisher(rclcpp::Publisher::SharedPtr publisher, std::string frameId) - : publisher(publisher), frameId(frameId) {} - - void addPose(spectacularAI::Pose pose) { - poses.push_front(poseToPoseStampped(pose, frameId)); - while (poses.size() > TRAJECTORY_LENGTH) poses.pop_back(); - if (counter % UPDATE_TRAJECTORY_EVERY_NTH_POSE == 0) { - nav_msgs::msg::Path path; - path.header.stamp = secondsToStamp(pose.time); - path.header.frame_id = frameId; - path.poses = std::vector(poses.begin(), poses.end()); - publisher->publish(path); - } - counter++; - } -}; - - -} // namespace - -namespace spectacularAI { -namespace ros2 { - -class Node : public rclcpp::Node { -public: - Node(const rclcpp::NodeOptions& options) : rclcpp::Node("spetacularAI", options), vioInitDone(false), receivedFrames(false) { - - deviceModel = declareAndReadParameterString("device_model", ""); - - // Used for IMU-Camera extrinsics calibration - imuFrameId = declareAndReadParameterString("imu_frame_id", ""); - cam0FrameId = declareAndReadParameterString("cam0_optical_frame_id", ""); - cam1FrameId = declareAndReadParameterString("cam1_optical_frame_id", ""); - - // Output frames - fixedFrameId = declareAndReadParameterString("fixed_frame_id", "map"); - odometryFrameId = declareAndReadParameterString("odometry_frame_id", "odom"); - baseLinkFrameId = declareAndReadParameterString("base_link_frame_id", "base_link"); - - depthScale = declareAndReadParameterDouble("depth_scale", 1.0 / 1000.0); - recordingFolder = declareAndReadParameterString("recording_folder", ""); - enableMapping = declareAndReadParameterBool("enable_mapping", true); - enableOccupancyGrid = declareAndReadParameterBool("enable_occupancy_grid", false); - cameraInputType = declareAndReadParameterString("camera_input_type", "stereo_depth_features"); - - outputOnImuSamples = declareAndReadParameterBool("output_on_imu_samples", false); - recordingOnly = declareAndReadParameterBool("recording_only", false); - - maxOdomFreq = declareAndReadParameterDouble("max_odom_freq", 100); - maxOdomCorrectionFreq = declareAndReadParameterDouble("max_odom_correction_freq", 10); - - overrideImuToCam0 = declareAndReadParameterString("imu_to_cam0", ""); - overrideImuToCam1 = declareAndReadParameterString("imu_to_cam1", ""); - - bool separateOdomTf = declareAndReadParameterBool("separate_odom_tf", false); - - bool publishPaths = declareAndReadParameterBool("publish_paths", false); - - transformListenerBuffer = std::make_unique(this->get_clock()); - transformListener = std::make_shared(*transformListenerBuffer); - transformBroadcaster = std::make_unique(*this); - - imuSubscription = this->create_subscription("input/imu", IMU_QOS, - std::bind(&Node::imuCallback, this, std::placeholders::_1)); - - if (separateOdomTf) { - poseHelper = std::make_unique(1.0 / maxOdomCorrectionFreq); - } - - // odometryPublisher = this->create_publisher("output/odometry", ODOM_QUEUE_SIZE); - if (enableOccupancyGrid) occupancyGridPublisher = this->create_publisher("output/occupancyGrid", ODOM_QUEUE_SIZE); - if (enableMapping) pointCloudPublisher = this->create_publisher("output/pointcloud", ODOM_QUEUE_SIZE); - if (publishPaths) { - odometryPathPublisher = std::make_unique(this->create_publisher("output/odometry_path", ODOM_QUEUE_SIZE), fixedFrameId); - correctedPathPublisher = std::make_unique(this->create_publisher("output/corrected_path", ODOM_QUEUE_SIZE), fixedFrameId); - vioPathPublisher = std::make_unique(this->create_publisher("output/vio_path", ODOM_QUEUE_SIZE), fixedFrameId); - } - - if (cameraInputType == "stereo") { - cameraInfo0Subscription.subscribe(this, "/input/cam0/camera_info", CAMERA_QOS.get_rmw_qos_profile()); - cameraInfo1Subscription.subscribe(this, "/input/cam1/camera_info", CAMERA_QOS.get_rmw_qos_profile()); - cameraImage0Subscription.subscribe(this, "/input/cam0/image_rect", CAMERA_QOS.get_rmw_qos_profile()); - cameraImage1Subscription.subscribe(this, "/input/cam1/image_rect", CAMERA_QOS.get_rmw_qos_profile()); - - stereoCameraSynchronizer = std::make_unique(StereoCameraPolicy(CAM_QUEUE_SIZE), - cameraInfo0Subscription, cameraInfo1Subscription, - cameraImage0Subscription, cameraImage1Subscription); - stereoCameraSynchronizer->setMaxIntervalDuration(CAMERA_SYNC_INTERVAL); - stereoCameraSynchronizer->registerCallback(std::bind(&Node::stereoCameraCallback, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); - } else if (cameraInputType == "stereo_depth_features") { - cameraInfo0Subscription.subscribe(this, "/input/cam0/camera_info", CAMERA_QOS.get_rmw_qos_profile()); - cameraInfo1Subscription.subscribe(this, "/input/cam1/camera_info", CAMERA_QOS.get_rmw_qos_profile()); - cameraImage0Subscription.subscribe(this, "/input/cam0/image_rect", CAMERA_QOS.get_rmw_qos_profile()); - cameraImage1Subscription.subscribe(this, "/input/cam1/image_rect", CAMERA_QOS.get_rmw_qos_profile()); - cameraDepthSubscription.subscribe(this, "/input/depth/image", CAMERA_QOS.get_rmw_qos_profile()); - depthaiTrackedFeaturesCam0Subscription.subscribe(this, "/input/cam0/features", CAMERA_QOS.get_rmw_qos_profile()); - - stereoDepthCameraSynchronizer = std::make_unique(StereoDepthCameraPolicy(CAM_QUEUE_SIZE), - cameraInfo0Subscription, cameraInfo1Subscription, - cameraImage0Subscription, cameraImage1Subscription, - cameraDepthSubscription, depthaiTrackedFeaturesCam0Subscription); - stereoDepthCameraSynchronizer->setMaxIntervalDuration(CAMERA_SYNC_INTERVAL); - stereoDepthCameraSynchronizer->registerCallback(std::bind(&Node::stereoDepthCameraCallback, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); - } else { - RCLCPP_WARN(this->get_logger(), "Unsupported camera_input_type: %s", cameraInputType.c_str()); - } - } - -private: - std::string createConfigYaml() { - std::ostringstream oss; - const bool isRae = stringStartsWith(deviceModel, "RAE") || stringStartsWith(deviceModel, "rae"); - - if (isRae) { - oss << "parameterSets: [\"wrapper-base\", \"live\", \"rae\"]\n"; - } - else { - oss << "parameterSets: [\"wrapper-base\", \"live\", \"oak-d\"]\n"; - } - oss << "alreadyRectified: True\n"; - oss << "useSlam: True\n"; - oss << "ffmpegVideoCodec: \"libx264 -crf 15 -preset ultrafast\"\n"; - - bool useFeatureTracker = cameraInputType == "stereo_depth_features"; - - if (useFeatureTracker) { - // Not good with non-depth grayscale video inputs. - if (!isRae) oss << "depthErrorScale: 0.1\n"; - oss << "keyframeCandidateInterval: 0\n"; - } else { - oss << "keyframeCandidateInterval: 6\n"; - } - - if (enableMapping) { - oss << "computeStereoPointCloud: True\n"; - oss << "computeDenseStereoDepthKeyFramesOnly: True\n"; - oss << "stereoPointCloudStride: 5\n"; - oss << "stereoPointCloudMinDepth: " << minDepth << "\n"; - } - - if (outputOnImuSamples) { - oss << "outputOnFrames: False\n"; - } - - return oss.str(); - } - - std::string declareAndReadParameterString(std::string name, std::string defaultValue) { - this->declare_parameter(name, defaultValue); - return this->get_parameter(name).as_string(); - } - - double declareAndReadParameterDouble(std::string name, double defaultValue) { - this->declare_parameter(name, defaultValue); - return this->get_parameter(name).as_double(); - } - - bool declareAndReadParameterBool(std::string name, bool defaultValue) { - this->declare_parameter(name, defaultValue); - return this->get_parameter(name).as_bool(); - } - - void imuCallback(const sensor_msgs::msg::Imu &msg) { - // RCLCPP_INFO(this->get_logger(), "Received: %s", msgToString(msg).c_str()); - double time = stampToSeconds(msg.header.stamp); - if (imuStartTime < 0) { - imuStartTime = time; - } else if (!receivedFrames && time - imuStartTime > 5.0 && time - lastCameraWarning > 5.0) { - RCLCPP_WARN(this->get_logger(), "Received IMU data for %f seconds, but haven't received any camera data. Are camera streams properly configured and synchronized?", time - imuStartTime); - lastCameraWarning = time; - } - - if (!vioInitDone || !vioApi) return; - if (time < previousImuTime) { - RCLCPP_WARN(this->get_logger(), "Received IMU sample (%f) that's older than previous (%f), skipping it.", time, previousImuTime); - return; - } - vioApi->addGyro(time, {msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z}); - vioApi->addAcc(time, {msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z}); - if (outputOnImuSamples && (time - previousTriggerTime) > (1. / maxOdomFreq)) { - vioApi->addTrigger(time, triggerCounter++); - previousTriggerTime = time; - } - previousImuTime = time; - } - - bool getStereoCameraExtrinsics(spectacularAI::Matrix4d &imuToCam0, spectacularAI::Matrix4d &imuToCam1, spectacularAI::Matrix4d &imuToOutput) { - if ((!deviceModel.empty() || !overrideImuToCam0.empty() || !overrideImuToCam1.empty()) - && !cam0FrameId.empty() - && !cam1FrameId.empty()) { - - spectacularAI::Matrix4d imuToCameraX; - int camIndex = 0; - - if (!overrideImuToCam0.empty()) { - camIndex = 0; - if (!matrixFromJson(overrideImuToCam0, imuToCameraX)) { - RCLCPP_WARN(this->get_logger(), "Failed to parse imuToCam0 matrix from given JSON string: %s", overrideImuToCam0.c_str()); - return false; - } - RCLCPP_INFO(this->get_logger(), "Using given imuToCam0 calibration"); - } else if (!overrideImuToCam1.empty()) { - camIndex = 1; - if (!matrixFromJson(overrideImuToCam1, imuToCameraX)) { - RCLCPP_WARN(this->get_logger(), "Failed to parse imuToCam0 matrix from given JSON string: %s", overrideImuToCam0.c_str()); - return false; - } - RCLCPP_INFO(this->get_logger(), "Using given imuToCam1 calibration"); - } else if (!deviceModel.empty()) { - if (!getDeviceImuToCameraMatrix(deviceModel, imuToCameraX, camIndex)) { - RCLCPP_WARN(this->get_logger(), "No stored camera calibration for %s", deviceModel.c_str()); - return false; - } - RCLCPP_INFO(this->get_logger(), "Using imuToCamera calibration based on device model"); - } else { - RCLCPP_WARN(this->get_logger(), "Failed to find any imuToCamera calibration"); - return false; - } - - if (camIndex == 0) { - spectacularAI::Matrix4d cam0ToCam1; - try { - cam0ToCam1 = matrixConvert(transformListenerBuffer->lookupTransform(cam1FrameId, cam0FrameId, tf2::TimePointZero)); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "Could not get camera transforms: %s", ex.what()); - return false; - } - imuToCam0 = imuToCameraX; - imuToCam1 = matrixMul(cam0ToCam1, imuToCam0); - } else { - spectacularAI::Matrix4d cam1ToCam0; - try { - cam1ToCam0 = matrixConvert(transformListenerBuffer->lookupTransform(cam0FrameId, cam1FrameId, tf2::TimePointZero)); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "Could not get camera transforms: %s", ex.what()); - return false; - } - imuToCam1 = imuToCameraX; - imuToCam0 = matrixMul(cam1ToCam0, imuToCam1); - } - - - } else if (!imuFrameId.empty() && !cam0FrameId.empty() && !cam1FrameId.empty()) { - try { - imuToCam0 = matrixConvert(transformListenerBuffer->lookupTransform(cam0FrameId, imuFrameId, tf2::TimePointZero)); - imuToCam1 = matrixConvert(transformListenerBuffer->lookupTransform(cam1FrameId, imuFrameId, tf2::TimePointZero)); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "Could not get camera transforms: %s", ex.what()); - return false; - } - } else { - RCLCPP_WARN(this->get_logger(), - "Failed to obtain camera extrinsics, cannot start tracking. You must provide a known device_model or imu frame ID"); - return false; - } - - spectacularAI::Matrix4d cam0ToBaseLink; - - try { - cam0ToBaseLink = matrixConvert(transformListenerBuffer->lookupTransform(baseLinkFrameId, cam0FrameId, tf2::TimePointZero)); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "Could not get camera transforms for computing imuToOutput: %s", ex.what()); - return false; - } - imuToOutput = matrixMul(cam0ToBaseLink, imuToCam0); - return true; - } - - void stereoCameraCallback( - const sensor_msgs::msg::CameraInfo::ConstSharedPtr &camInfo0, const sensor_msgs::msg::CameraInfo::ConstSharedPtr &camInfo1, - const sensor_msgs::msg::Image::ConstSharedPtr &img0, const sensor_msgs::msg::Image::ConstSharedPtr &img1) { - receivedFrames = true; - - if (!vioInitDone) { - spectacularAI::Matrix4d imuToCam0, imuToCam1, imuToOutput; - if (!getStereoCameraExtrinsics(imuToCam0, imuToCam1, imuToOutput)) return; - // TODO: Support non rectified images, by ROS convention "image" topic is unrectified, "image_rect" is rectified - constexpr bool IS_RECTIFIED = true; - startVio({camInfo0, camInfo1}, {imuToCam0, imuToCam1}, imuToOutput, IS_RECTIFIED); - } - - if (vioInitDone && !vioApi) return; - - if (img0->width != img1->width || img0->height != img0->height) { - RCLCPP_WARN(this->get_logger(), "Stereo image resolutions don't match"); - return; - } - - spectacularAI::ColorFormat colorFormat; - if (!rosEncodingToColorFormat(img0->encoding, colorFormat)) { - RCLCPP_WARN(this->get_logger(), "Unsupported image encoding"); - return; - } - - // RCLCPP_INFO(this->get_logger(), "Timestamp: %d, %d", img0->header.stamp.sec, img0->header.stamp.nanosec); - - double time = stampToSeconds(img0->header.stamp); - if (time < previousFrameTime) { - RCLCPP_WARN(this->get_logger(), "Received frame (%f) that's older than previous (%f), skipping it.", time, previousFrameTime); - return; - } - previousFrameTime = time; - vioApi->addFrameStereo( - time, - img0->width, img0->height, - img0->data.data(), img1->data.data(), - colorFormat - ); - } - - void stereoDepthCameraCallback( - const sensor_msgs::msg::CameraInfo::ConstSharedPtr &camInfo0, const sensor_msgs::msg::CameraInfo::ConstSharedPtr &camInfo1, - const sensor_msgs::msg::Image::ConstSharedPtr &img0, const sensor_msgs::msg::Image::ConstSharedPtr &img1, - const sensor_msgs::msg::Image::ConstSharedPtr &depth, const depthai_ros_msgs::msg::TrackedFeatures::ConstSharedPtr &features) { - - receivedFrames = true; - //RCLCPP_INFO(this->get_logger(), "Received all data: %f", stampToSeconds(img0->header.stamp)); - - if (!vioInitDone) { - spectacularAI::Matrix4d imuToCam0, imuToCam1, imuToOutput; - if (!getStereoCameraExtrinsics(imuToCam0, imuToCam1, imuToOutput)) return; - // TODO: Support non rectified images, by ROS convention "image" topic is unrectified, "image_rect" is rectified - constexpr bool IS_RECTIFIED = true; - startVio({camInfo0, camInfo1}, {imuToCam0, imuToCam1}, imuToOutput, IS_RECTIFIED); - } - - if (vioInitDone && !vioApi) return; - - spectacularAI::ColorFormat colorFormat; - spectacularAI::ColorFormat depthFormat; - if (!rosEncodingToColorFormat(img0->encoding, colorFormat)) { - RCLCPP_WARN(this->get_logger(), "Unsupported image encoding: %s", img0->encoding.c_str()); - return; - } - if (!rosEncodingToColorFormat(depth->encoding, depthFormat)) { - RCLCPP_WARN(this->get_logger(), "Unsupported depth image encoding: %s", depth->encoding.c_str()); - return; - } - - monoFeatures.clear(); - monoFeatures.resize(features->features.size()); - for (auto& ft : features->features) { - monoFeatures.push_back({ int(ft.id), { float(ft.position.x), float(ft.position.y) } }); - } - - double time = stampToSeconds(img0->header.stamp); - if (time < previousFrameTime) { - RCLCPP_WARN(this->get_logger(), "Received frame (%f) that's older than previous (%f), skipping it.", time, previousFrameTime); - return; - } - previousFrameTime = time; - - bool isSlamKf = frameNumber % 6 == 0; // TODO: Make configurable - vioApi->addFrameStereoDepth( - time, - img0->width, img0->height, - monoFeatures.data(), - monoFeatures.size(), - depth->data.data(), - depthFormat, - isSlamKf ? img0->data.data() : nullptr, - isSlamKf ? img1->data.data() : nullptr, - isSlamKf ? colorFormat : spectacularAI::ColorFormat::NONE, - depthScale); - - frameNumber++; - } - - void vioOutputCallback(spectacularAI::VioOutputPtr vioOutput) { - if (vioOutput->status == spectacularAI::TrackingStatus::TRACKING) { - if (poseHelper) { - spectacularAI::Pose odomPose; - if (poseHelper->computeContinousTrajectory(vioOutput, odomPose, odomCorrection)) { - transformBroadcaster->sendTransform(poseToTransformStampped(odomCorrection, fixedFrameId, odometryFrameId)); - } - transformBroadcaster->sendTransform(poseToTransformStampped(odomPose, odometryFrameId, baseLinkFrameId)); - if (odometryPathPublisher) odometryPathPublisher->addPose(odomPose); - if (correctedPathPublisher) { - correctedPathPublisher->addPose(spectacularAI::Pose::fromMatrix(odomPose.time, matrixMul(odomCorrection.asMatrix(), odomPose.asMatrix()))); - } - } else { - transformBroadcaster->sendTransform(poseToTransformStampped(vioOutput->pose, fixedFrameId, baseLinkFrameId)); - } - if (vioPathPublisher) vioPathPublisher->addPose(vioOutput->pose); - - // odometryPublisher->publish(outputToOdometryMsg(vioOutput, vioOutputParentFrameId, vioOutputChildFrameId)); - // RCLCPP_INFO(this->get_logger(), "Output: %s", vioOutput->asJson().c_str()); - } - } - - void mappingOutputCallback(spectacularAI::mapping::MapperOutputPtr mappingOutput) { - if (latestKeyFrame == mappingOutput->updatedKeyFrames.back()) return; - latestKeyFrame = mappingOutput->updatedKeyFrames.back(); - const auto &kf = mappingOutput->map->keyFrames.at(latestKeyFrame); - if (!kf->pointCloud || kf->pointCloud->empty()) return; - const auto &primary = kf->frameSet->primaryFrame; - - sensor_msgs::msg::PointCloud2 pc; - pc.header.stamp = secondsToStamp(primary->cameraPose.pose.time); - pc.header.frame_id = fixedFrameId; - - std::string fieldNames[] = {"x", "y", "z", "rgb"}; - uint32_t totalOffset = 0; - for (int i = 0; i < 4; i++) { - sensor_msgs::msg::PointField f; - f.name = fieldNames[i]; - f.offset = totalOffset; - f.datatype = sensor_msgs::msg::PointField::FLOAT32; // 1 float == rgba8 - f.count = 1; - pc.fields.push_back(f); - totalOffset += sizeof(uint32_t); - } - - const std::uint8_t* rgb24Data = kf->pointCloud->getRGB24Data(); - const spectacularAI::Vector3f* positionData = kf->pointCloud->getPositionData(); - Matrix4f camToWorld = matrixToFloat(primary->cameraPose.getCameraToWorldMatrix()); - bool hasColors = kf->pointCloud->hasColors(); - - spectacularAI::Vector3f center = { - (float)primary->cameraPose.pose.position.x, (float)primary->cameraPose.pose.position.y, (float)primary->cameraPose.pose.position.z - }; - if (enableOccupancyGrid) { - if (occupancyGrid.empty()) { - occupancyGrid.reset(center, minDepth, maxDepth, cellSizeMeters, occupiedThreshold); - } - tempGrid.reset(center, minDepth, maxDepth, cellSizeMeters, occupiedThreshold); // TODO: Could use bounding box for visible area - } - - pc.data.resize(sizeof(RosPointWithColor) * kf->pointCloud->size()); - RosPointWithColor* points = (RosPointWithColor*) pc.data.data(); - for (size_t i = 0; i < kf->pointCloud->size(); i++) { - auto &p = points[i]; - p.pos = matVectorMult(camToWorld, positionData[i]); - if (hasColors) { - p.rgba[0] = rgb24Data[i * 3 + 0]; - p.rgba[1] = rgb24Data[i * 3 + 1]; - p.rgba[2] = rgb24Data[i * 3 + 2]; - p.rgba[3] = 255; - } else { - p.rgba[0] = 0; - p.rgba[1] = 0; - p.rgba[2] = 0; - p.rgba[3] = 0; - } - if (enableOccupancyGrid) { - float relativeZ = p.pos.z - center.z; - if (relativeZ > minOccupancyHeight && relativeZ < maxOccupancyHeight) { - tempGrid.addPoint(p.pos); - // p.rgba[0] = 255; - // p.rgba[1] = 0; - // p.rgba[2] = 0; - } - } - } - - pc.point_step = sizeof(RosPointWithColor); - pc.height = 1; - pc.width = kf->pointCloud->size(); - pc.row_step = pc.point_step * kf->pointCloud->size(); - pc.is_bigendian = false; - pc.is_dense = true; - pointCloudPublisher->publish(pc); - - if (enableOccupancyGrid && primary->image) { - // Trace to all edge points to find unoccupied cells - float cameraTestHeight = center.z + (maxOccupancyHeight + minOccupancyHeight) * 0.5; - tempGrid.traceFreeSpace(center, *primary, cameraTestHeight); - occupancyGrid.merge(tempGrid); - - nav_msgs::msg::OccupancyGrid og; - og.header.stamp = pc.header.stamp; - og.header.frame_id = pc.header.frame_id; - - og.info.map_load_time = og.header.stamp; // Seems redundant, duplicating header - og.info.resolution = occupancyGrid.cellSize; - og.info.width = occupancyGrid.width; - og.info.height = occupancyGrid.height; - - og.info.origin.position.x = occupancyGrid.originX * occupancyGrid.cellSize; - og.info.origin.position.y = occupancyGrid.originY * occupancyGrid.cellSize; - og.info.origin.position.z = center.z; - og.info.origin.orientation.x = 0.0f; - og.info.origin.orientation.y = 0.0f; - og.info.origin.orientation.z = 0.0f; - og.info.origin.orientation.w = 1.0f; - og.data = occupancyGrid.cells; // TODO: Copying entire map, perhaps occupancyGrid could internally wrap nav_msgs::msg::OccupancyGrid? - occupancyGridPublisher->publish(og); - } - } - - void startVio(std::vector intrinsics, std::vector extrinsics, spectacularAI::Matrix4d imuToOutput, bool isRectified) { - std::lock_guard lock(vioStartup); - if (vioInitDone) return; - - std::string calibrationJson; - if (!cameraInfoToCalibrationJson(intrinsics, extrinsics, imuToOutput, isRectified, calibrationJson)) { - vioInitDone = true; - return; - } - - std::string config = createConfigYaml(); - - // RCLCPP_INFO(this->get_logger(), "Calibration: %s", calibrationJson.c_str()); - // RCLCPP_INFO(this->get_logger(), "Configuration: %s", config.c_str()); - - spectacularAI::Vio::Builder vioBuilder = spectacularAI::Vio::builder(); - vioBuilder.setCalibrationJSON(calibrationJson); - vioBuilder.setConfigurationYAML(config); - if (!recordingFolder.empty()) { - vioBuilder.setRecordingFolder(recordingFolder); - vioBuilder.setRecordingOnly(recordingOnly); - } - if (enableMapping) vioBuilder.setMapperCallback(std::bind(&Node::mappingOutputCallback, this, std::placeholders::_1)); - vioApi = vioBuilder.build(); - vioApi->setOutputCallback(std::bind(&Node::vioOutputCallback, this, std::placeholders::_1)); - vioInitDone = true; - } - - std::unique_ptr odometryPathPublisher; - std::unique_ptr correctedPathPublisher; - std::unique_ptr vioPathPublisher; - - // rclcpp::Publisher::SharedPtr odometryPublisher; - rclcpp::Publisher::SharedPtr pointCloudPublisher; - rclcpp::Publisher::SharedPtr occupancyGridPublisher; - - rclcpp::Subscription::SharedPtr imuSubscription; - - message_filters::Subscriber cameraInfo0Subscription; - message_filters::Subscriber cameraInfo1Subscription; - message_filters::Subscriber cameraImage0Subscription; - message_filters::Subscriber cameraImage1Subscription; - message_filters::Subscriber cameraDepthSubscription; - message_filters::Subscriber depthaiTrackedFeaturesCam0Subscription; - - std::unique_ptr transformListenerBuffer; - std::shared_ptr transformListener; - std::shared_ptr transformBroadcaster; - - std::unique_ptr stereoCameraSynchronizer; - std::unique_ptr stereoDepthCameraSynchronizer; - - std::unique_ptr vioApi; - std::atomic_bool vioInitDone; - std::mutex vioStartup; - std::unique_ptr poseHelper; - - double previousFrameTime = 0.0; - double previousImuTime = 0.0; - double previousTriggerTime = 0.0; - - int frameNumber = 0; - int64_t latestKeyFrame = -1; - std::vector monoFeatures; - bool outputOnImuSamples; - bool recordingOnly; - uint64_t triggerCounter = 1; - - std::atomic_bool receivedFrames; - double imuStartTime = -1.; - double lastCameraWarning = -1.; - spectacularAI::Pose odomCorrection; - - // Params - std::string fixedFrameId; - std::string odometryFrameId; - std::string baseLinkFrameId; - - std::string deviceModel; - std::string imuFrameId; - std::string cam0FrameId; - std::string cam1FrameId; - double depthScale; - std::string cameraInputType; - std::string recordingFolder; - bool enableMapping; - double maxOdomFreq; - double maxOdomCorrectionFreq; - std::string overrideImuToCam0; - std::string overrideImuToCam1; - - // Map - bool enableOccupancyGrid; - OccupancyGrid occupancyGrid; - OccupancyGrid tempGrid; - float maxDepth = 3.0; // Max distance from camera for occupancy map - float minDepth = 0.2; - float maxOccupancyHeight = 0.3; // Max height above device for point to be counted in occupancy map - float minOccupancyHeight = 0.05; // Min height above device for point to be counted in occupancy map - float cellSizeMeters = 0.07; - int occupiedThreshold = 2; -}; - -} // namespace ros2 -} // namespace spectacularai +#include "spectacularai_ros2/ros2_plugin.hpp" #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(spectacularAI::ros2::Node) From ede7d83dcb316278dc95d300f1bda91ed0ec0735 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Fri, 1 Dec 2023 14:14:39 +0000 Subject: [PATCH 3/9] move to hpp --- spectacularai_ros2/src/ros2_plugin.cpp | 684 +------------------------ 1 file changed, 1 insertion(+), 683 deletions(-) diff --git a/spectacularai_ros2/src/ros2_plugin.cpp b/spectacularai_ros2/src/ros2_plugin.cpp index 7324fb8..51cc98c 100644 --- a/spectacularai_ros2/src/ros2_plugin.cpp +++ b/spectacularai_ros2/src/ros2_plugin.cpp @@ -1,686 +1,4 @@ -#include -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" - -#include "message_filters/subscriber.h" -#include "message_filters/time_synchronizer.h" -#include "message_filters/sync_policies/approximate_time.h" - -#include "sensor_msgs/msg/imu.hpp" -#include "sensor_msgs/msg/camera_info.hpp" -#include "sensor_msgs/msg/image.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "sensor_msgs/image_encodings.hpp" - -#include "image_transport/image_transport.hpp" - -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/buffer.h" - -#include "tf2/LinearMath/Matrix3x3.h" -#include "tf2/LinearMath/Quaternion.h" - -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - -#include "nav_msgs/msg/odometry.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" -#include "nav_msgs/msg/path.hpp" - -// Depth AI -#include - -// Spectacular AI -#include - -// Helpers -#include "helpers.hpp" -#include "pose_helper.hpp" -#include "device_imu_calib.hpp" -#include "occupancy_grid.hpp" - -namespace { -constexpr size_t IMU_QUEUE_SIZE = 1000; -constexpr size_t ODOM_QUEUE_SIZE = 1000; -constexpr uint32_t CAM_QUEUE_SIZE = 30; -constexpr size_t TRAJECTORY_LENGTH = 1000; -constexpr size_t UPDATE_TRAJECTORY_EVERY_NTH_POSE = 5; - -// Group window for camera frames, should always be smaller than time between two frames -static const rclcpp::Duration CAMERA_SYNC_INTERVAL = rclcpp::Duration(0, 10 * 1e6); - -// https://docs.ros.org/en/humble/Concepts/Intermediate/About-Quality-of-Service-Settings.html -static const rclcpp::QoS CAMERA_QOS = rclcpp::QoS(rclcpp::KeepLast(CAM_QUEUE_SIZE)).best_effort().durability_volatile(); -static const rclcpp::QoS IMU_QOS = rclcpp::QoS(rclcpp::KeepLast(IMU_QUEUE_SIZE)).best_effort().durability_volatile(); - -using StereoCameraPolicy = message_filters::sync_policies::ApproximateTime< - sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo, - sensor_msgs::msg::Image, sensor_msgs::msg::Image>; - -using StereoDepthCameraPolicy = message_filters::sync_policies::ApproximateTime< - sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo, - sensor_msgs::msg::Image, sensor_msgs::msg::Image, - sensor_msgs::msg::Image, depthai_ros_msgs::msg::TrackedFeatures>; - -using StereoCameraSynchronizer = message_filters::Synchronizer; -using StereoDepthCameraSynchronizer = message_filters::Synchronizer; - -#pragma pack(push) -#pragma pack(1) -struct RosPointWithColor { - spectacularAI::Vector3f pos; - uint8_t rgba[4]; -}; -#pragma pack(pop) - -class TrajectoryPublisher { - rclcpp::Publisher::SharedPtr publisher; - std::deque poses; - std::string frameId; - int counter = 0; -public: - TrajectoryPublisher(rclcpp::Publisher::SharedPtr publisher, std::string frameId) - : publisher(publisher), frameId(frameId) {} - - void addPose(spectacularAI::Pose pose) { - poses.push_front(poseToPoseStampped(pose, frameId)); - while (poses.size() > TRAJECTORY_LENGTH) poses.pop_back(); - if (counter % UPDATE_TRAJECTORY_EVERY_NTH_POSE == 0) { - nav_msgs::msg::Path path; - path.header.stamp = secondsToStamp(pose.time); - path.header.frame_id = frameId; - path.poses = std::vector(poses.begin(), poses.end()); - publisher->publish(path); - } - counter++; - } -}; - - -} // namespace - -namespace spectacularAI { -namespace ros2 { - -class Node : public rclcpp::Node { -public: - Node(const rclcpp::NodeOptions& options) : rclcpp::Node("spetacularAI", options), vioInitDone(false), receivedFrames(false) { - - deviceModel = declareAndReadParameterString("device_model", ""); - - // Used for IMU-Camera extrinsics calibration - imuFrameId = declareAndReadParameterString("imu_frame_id", ""); - cam0FrameId = declareAndReadParameterString("cam0_optical_frame_id", ""); - cam1FrameId = declareAndReadParameterString("cam1_optical_frame_id", ""); - - // Output frames - fixedFrameId = declareAndReadParameterString("fixed_frame_id", "map"); - odometryFrameId = declareAndReadParameterString("odometry_frame_id", "odom"); - baseLinkFrameId = declareAndReadParameterString("base_link_frame_id", "base_link"); - - depthScale = declareAndReadParameterDouble("depth_scale", 1.0 / 1000.0); - recordingFolder = declareAndReadParameterString("recording_folder", ""); - enableMapping = declareAndReadParameterBool("enable_mapping", true); - enableOccupancyGrid = declareAndReadParameterBool("enable_occupancy_grid", false); - cameraInputType = declareAndReadParameterString("camera_input_type", "stereo_depth_features"); - - outputOnImuSamples = declareAndReadParameterBool("output_on_imu_samples", false); - recordingOnly = declareAndReadParameterBool("recording_only", false); - - maxOdomFreq = declareAndReadParameterDouble("max_odom_freq", 100); - maxOdomCorrectionFreq = declareAndReadParameterDouble("max_odom_correction_freq", 10); - - overrideImuToCam0 = declareAndReadParameterString("imu_to_cam0", ""); - overrideImuToCam1 = declareAndReadParameterString("imu_to_cam1", ""); - - bool separateOdomTf = declareAndReadParameterBool("separate_odom_tf", false); - - bool publishPaths = declareAndReadParameterBool("publish_paths", false); - - transformListenerBuffer = std::make_unique(this->get_clock()); - transformListener = std::make_shared(*transformListenerBuffer); - transformBroadcaster = std::make_unique(*this); - - imuSubscription = this->create_subscription("input/imu", IMU_QOS, - std::bind(&Node::imuCallback, this, std::placeholders::_1)); - - if (separateOdomTf) { - poseHelper = std::make_unique(1.0 / maxOdomCorrectionFreq); - } - - // odometryPublisher = this->create_publisher("output/odometry", ODOM_QUEUE_SIZE); - if (enableOccupancyGrid) occupancyGridPublisher = this->create_publisher("output/occupancyGrid", ODOM_QUEUE_SIZE); - if (enableMapping) pointCloudPublisher = this->create_publisher("output/pointcloud", ODOM_QUEUE_SIZE); - if (publishPaths) { - odometryPathPublisher = std::make_unique(this->create_publisher("output/odometry_path", ODOM_QUEUE_SIZE), fixedFrameId); - correctedPathPublisher = std::make_unique(this->create_publisher("output/corrected_path", ODOM_QUEUE_SIZE), fixedFrameId); - vioPathPublisher = std::make_unique(this->create_publisher("output/vio_path", ODOM_QUEUE_SIZE), fixedFrameId); - } - - if (cameraInputType == "stereo") { - cameraInfo0Subscription.subscribe(this, "/input/cam0/camera_info", CAMERA_QOS.get_rmw_qos_profile()); - cameraInfo1Subscription.subscribe(this, "/input/cam1/camera_info", CAMERA_QOS.get_rmw_qos_profile()); - cameraImage0Subscription.subscribe(this, "/input/cam0/image_rect", CAMERA_QOS.get_rmw_qos_profile()); - cameraImage1Subscription.subscribe(this, "/input/cam1/image_rect", CAMERA_QOS.get_rmw_qos_profile()); - - stereoCameraSynchronizer = std::make_unique(StereoCameraPolicy(CAM_QUEUE_SIZE), - cameraInfo0Subscription, cameraInfo1Subscription, - cameraImage0Subscription, cameraImage1Subscription); - stereoCameraSynchronizer->setMaxIntervalDuration(CAMERA_SYNC_INTERVAL); - stereoCameraSynchronizer->registerCallback(std::bind(&Node::stereoCameraCallback, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); - } else if (cameraInputType == "stereo_depth_features") { - cameraInfo0Subscription.subscribe(this, "/input/cam0/camera_info", CAMERA_QOS.get_rmw_qos_profile()); - cameraInfo1Subscription.subscribe(this, "/input/cam1/camera_info", CAMERA_QOS.get_rmw_qos_profile()); - cameraImage0Subscription.subscribe(this, "/input/cam0/image_rect", CAMERA_QOS.get_rmw_qos_profile()); - cameraImage1Subscription.subscribe(this, "/input/cam1/image_rect", CAMERA_QOS.get_rmw_qos_profile()); - cameraDepthSubscription.subscribe(this, "/input/depth/image", CAMERA_QOS.get_rmw_qos_profile()); - depthaiTrackedFeaturesCam0Subscription.subscribe(this, "/input/cam0/features", CAMERA_QOS.get_rmw_qos_profile()); - - stereoDepthCameraSynchronizer = std::make_unique(StereoDepthCameraPolicy(CAM_QUEUE_SIZE), - cameraInfo0Subscription, cameraInfo1Subscription, - cameraImage0Subscription, cameraImage1Subscription, - cameraDepthSubscription, depthaiTrackedFeaturesCam0Subscription); - stereoDepthCameraSynchronizer->setMaxIntervalDuration(CAMERA_SYNC_INTERVAL); - stereoDepthCameraSynchronizer->registerCallback(std::bind(&Node::stereoDepthCameraCallback, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); - } else { - RCLCPP_WARN(this->get_logger(), "Unsupported camera_input_type: %s", cameraInputType.c_str()); - } - } - -private: - std::string createConfigYaml() { - std::ostringstream oss; - const bool isRae = stringStartsWith(deviceModel, "RAE") || stringStartsWith(deviceModel, "rae"); - - if (isRae) { - oss << "parameterSets: [\"wrapper-base\", \"live\", \"rae\"]\n"; - } - else { - oss << "parameterSets: [\"wrapper-base\", \"live\", \"oak-d\"]\n"; - } - oss << "alreadyRectified: True\n"; - oss << "useSlam: True\n"; - oss << "ffmpegVideoCodec: \"libx264 -crf 15 -preset ultrafast\"\n"; - - bool useFeatureTracker = cameraInputType == "stereo_depth_features"; - - if (useFeatureTracker) { - // Not good with non-depth grayscale video inputs. - if (!isRae) oss << "depthErrorScale: 0.1\n"; - oss << "keyframeCandidateInterval: 0\n"; - } else { - oss << "keyframeCandidateInterval: 6\n"; - } - - if (enableMapping) { - oss << "computeStereoPointCloud: True\n"; - oss << "computeDenseStereoDepthKeyFramesOnly: True\n"; - oss << "stereoPointCloudStride: 5\n"; - oss << "stereoPointCloudMinDepth: " << minDepth << "\n"; - } - - if (outputOnImuSamples) { - oss << "outputOnFrames: False\n"; - } - - return oss.str(); - } - - std::string declareAndReadParameterString(std::string name, std::string defaultValue) { - this->declare_parameter(name, defaultValue); - return this->get_parameter(name).as_string(); - } - - double declareAndReadParameterDouble(std::string name, double defaultValue) { - this->declare_parameter(name, defaultValue); - return this->get_parameter(name).as_double(); - } - - bool declareAndReadParameterBool(std::string name, bool defaultValue) { - this->declare_parameter(name, defaultValue); - return this->get_parameter(name).as_bool(); - } - - void imuCallback(const sensor_msgs::msg::Imu &msg) { - // RCLCPP_INFO(this->get_logger(), "Received: %s", msgToString(msg).c_str()); - double time = stampToSeconds(msg.header.stamp); - if (imuStartTime < 0) { - imuStartTime = time; - } else if (!receivedFrames && time - imuStartTime > 5.0 && time - lastCameraWarning > 5.0) { - RCLCPP_WARN(this->get_logger(), "Received IMU data for %f seconds, but haven't received any camera data. Are camera streams properly configured and synchronized?", time - imuStartTime); - lastCameraWarning = time; - } - - if (!vioInitDone || !vioApi) return; - if (time < previousImuTime) { - RCLCPP_WARN(this->get_logger(), "Received IMU sample (%f) that's older than previous (%f), skipping it.", time, previousImuTime); - return; - } - vioApi->addGyro(time, {msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z}); - vioApi->addAcc(time, {msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z}); - if (outputOnImuSamples && (time - previousTriggerTime) > (1. / maxOdomFreq)) { - vioApi->addTrigger(time, triggerCounter++); - previousTriggerTime = time; - } - previousImuTime = time; - } - - bool getStereoCameraExtrinsics(spectacularAI::Matrix4d &imuToCam0, spectacularAI::Matrix4d &imuToCam1, spectacularAI::Matrix4d &imuToOutput) { - if ((!deviceModel.empty() || !overrideImuToCam0.empty() || !overrideImuToCam1.empty()) - && !cam0FrameId.empty() - && !cam1FrameId.empty()) { - - spectacularAI::Matrix4d imuToCameraX; - int camIndex = 0; - - if (!overrideImuToCam0.empty()) { - camIndex = 0; - if (!matrixFromJson(overrideImuToCam0, imuToCameraX)) { - RCLCPP_WARN(this->get_logger(), "Failed to parse imuToCam0 matrix from given JSON string: %s", overrideImuToCam0.c_str()); - return false; - } - RCLCPP_INFO(this->get_logger(), "Using given imuToCam0 calibration"); - } else if (!overrideImuToCam1.empty()) { - camIndex = 1; - if (!matrixFromJson(overrideImuToCam1, imuToCameraX)) { - RCLCPP_WARN(this->get_logger(), "Failed to parse imuToCam0 matrix from given JSON string: %s", overrideImuToCam0.c_str()); - return false; - } - RCLCPP_INFO(this->get_logger(), "Using given imuToCam1 calibration"); - } else if (!deviceModel.empty()) { - if (!getDeviceImuToCameraMatrix(deviceModel, imuToCameraX, camIndex)) { - RCLCPP_WARN(this->get_logger(), "No stored camera calibration for %s", deviceModel.c_str()); - return false; - } - RCLCPP_INFO(this->get_logger(), "Using imuToCamera calibration based on device model"); - } else { - RCLCPP_WARN(this->get_logger(), "Failed to find any imuToCamera calibration"); - return false; - } - - if (camIndex == 0) { - spectacularAI::Matrix4d cam0ToCam1; - try { - cam0ToCam1 = matrixConvert(transformListenerBuffer->lookupTransform(cam1FrameId, cam0FrameId, tf2::TimePointZero)); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "Could not get camera transforms: %s", ex.what()); - return false; - } - imuToCam0 = imuToCameraX; - imuToCam1 = matrixMul(cam0ToCam1, imuToCam0); - } else { - spectacularAI::Matrix4d cam1ToCam0; - try { - cam1ToCam0 = matrixConvert(transformListenerBuffer->lookupTransform(cam0FrameId, cam1FrameId, tf2::TimePointZero)); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "Could not get camera transforms: %s", ex.what()); - return false; - } - imuToCam1 = imuToCameraX; - imuToCam0 = matrixMul(cam1ToCam0, imuToCam1); - } - - - } else if (!imuFrameId.empty() && !cam0FrameId.empty() && !cam1FrameId.empty()) { - try { - imuToCam0 = matrixConvert(transformListenerBuffer->lookupTransform(cam0FrameId, imuFrameId, tf2::TimePointZero)); - imuToCam1 = matrixConvert(transformListenerBuffer->lookupTransform(cam1FrameId, imuFrameId, tf2::TimePointZero)); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "Could not get camera transforms: %s", ex.what()); - return false; - } - } else { - RCLCPP_WARN(this->get_logger(), - "Failed to obtain camera extrinsics, cannot start tracking. You must provide a known device_model or imu frame ID"); - return false; - } - - spectacularAI::Matrix4d cam0ToBaseLink; - - try { - cam0ToBaseLink = matrixConvert(transformListenerBuffer->lookupTransform(baseLinkFrameId, cam0FrameId, tf2::TimePointZero)); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "Could not get camera transforms for computing imuToOutput: %s", ex.what()); - return false; - } - imuToOutput = matrixMul(cam0ToBaseLink, imuToCam0); - return true; - } - - void stereoCameraCallback( - const sensor_msgs::msg::CameraInfo::ConstSharedPtr &camInfo0, const sensor_msgs::msg::CameraInfo::ConstSharedPtr &camInfo1, - const sensor_msgs::msg::Image::ConstSharedPtr &img0, const sensor_msgs::msg::Image::ConstSharedPtr &img1) { - receivedFrames = true; - - if (!vioInitDone) { - spectacularAI::Matrix4d imuToCam0, imuToCam1, imuToOutput; - if (!getStereoCameraExtrinsics(imuToCam0, imuToCam1, imuToOutput)) return; - // TODO: Support non rectified images, by ROS convention "image" topic is unrectified, "image_rect" is rectified - constexpr bool IS_RECTIFIED = true; - startVio({camInfo0, camInfo1}, {imuToCam0, imuToCam1}, imuToOutput, IS_RECTIFIED); - } - - if (vioInitDone && !vioApi) return; - - if (img0->width != img1->width || img0->height != img0->height) { - RCLCPP_WARN(this->get_logger(), "Stereo image resolutions don't match"); - return; - } - - spectacularAI::ColorFormat colorFormat; - if (!rosEncodingToColorFormat(img0->encoding, colorFormat)) { - RCLCPP_WARN(this->get_logger(), "Unsupported image encoding"); - return; - } - - // RCLCPP_INFO(this->get_logger(), "Timestamp: %d, %d", img0->header.stamp.sec, img0->header.stamp.nanosec); - - double time = stampToSeconds(img0->header.stamp); - if (time < previousFrameTime) { - RCLCPP_WARN(this->get_logger(), "Received frame (%f) that's older than previous (%f), skipping it.", time, previousFrameTime); - return; - } - previousFrameTime = time; - vioApi->addFrameStereo( - time, - img0->width, img0->height, - img0->data.data(), img1->data.data(), - colorFormat - ); - } - - void stereoDepthCameraCallback( - const sensor_msgs::msg::CameraInfo::ConstSharedPtr &camInfo0, const sensor_msgs::msg::CameraInfo::ConstSharedPtr &camInfo1, - const sensor_msgs::msg::Image::ConstSharedPtr &img0, const sensor_msgs::msg::Image::ConstSharedPtr &img1, - const sensor_msgs::msg::Image::ConstSharedPtr &depth, const depthai_ros_msgs::msg::TrackedFeatures::ConstSharedPtr &features) { - - receivedFrames = true; - //RCLCPP_INFO(this->get_logger(), "Received all data: %f", stampToSeconds(img0->header.stamp)); - - if (!vioInitDone) { - spectacularAI::Matrix4d imuToCam0, imuToCam1, imuToOutput; - if (!getStereoCameraExtrinsics(imuToCam0, imuToCam1, imuToOutput)) return; - // TODO: Support non rectified images, by ROS convention "image" topic is unrectified, "image_rect" is rectified - constexpr bool IS_RECTIFIED = true; - startVio({camInfo0, camInfo1}, {imuToCam0, imuToCam1}, imuToOutput, IS_RECTIFIED); - } - - if (vioInitDone && !vioApi) return; - - spectacularAI::ColorFormat colorFormat; - spectacularAI::ColorFormat depthFormat; - if (!rosEncodingToColorFormat(img0->encoding, colorFormat)) { - RCLCPP_WARN(this->get_logger(), "Unsupported image encoding: %s", img0->encoding.c_str()); - return; - } - if (!rosEncodingToColorFormat(depth->encoding, depthFormat)) { - RCLCPP_WARN(this->get_logger(), "Unsupported depth image encoding: %s", depth->encoding.c_str()); - return; - } - - monoFeatures.clear(); - monoFeatures.resize(features->features.size()); - for (auto& ft : features->features) { - monoFeatures.push_back({ int(ft.id), { float(ft.position.x), float(ft.position.y) } }); - } - - double time = stampToSeconds(img0->header.stamp); - if (time < previousFrameTime) { - RCLCPP_WARN(this->get_logger(), "Received frame (%f) that's older than previous (%f), skipping it.", time, previousFrameTime); - return; - } - previousFrameTime = time; - - bool isSlamKf = frameNumber % 6 == 0; // TODO: Make configurable - vioApi->addFrameStereoDepth( - time, - img0->width, img0->height, - monoFeatures.data(), - monoFeatures.size(), - depth->data.data(), - depthFormat, - isSlamKf ? img0->data.data() : nullptr, - isSlamKf ? img1->data.data() : nullptr, - isSlamKf ? colorFormat : spectacularAI::ColorFormat::NONE, - depthScale); - - frameNumber++; - } - - void vioOutputCallback(spectacularAI::VioOutputPtr vioOutput) { - if (vioOutput->status == spectacularAI::TrackingStatus::TRACKING) { - if (poseHelper) { - spectacularAI::Pose odomPose; - if (poseHelper->computeContinousTrajectory(vioOutput, odomPose, odomCorrection)) { - transformBroadcaster->sendTransform(poseToTransformStampped(odomCorrection, fixedFrameId, odometryFrameId)); - } - transformBroadcaster->sendTransform(poseToTransformStampped(odomPose, odometryFrameId, baseLinkFrameId)); - if (odometryPathPublisher) odometryPathPublisher->addPose(odomPose); - if (correctedPathPublisher) { - correctedPathPublisher->addPose(spectacularAI::Pose::fromMatrix(odomPose.time, matrixMul(odomCorrection.asMatrix(), odomPose.asMatrix()))); - } - } else { - transformBroadcaster->sendTransform(poseToTransformStampped(vioOutput->pose, fixedFrameId, baseLinkFrameId)); - } - if (vioPathPublisher) vioPathPublisher->addPose(vioOutput->pose); - - // odometryPublisher->publish(outputToOdometryMsg(vioOutput, vioOutputParentFrameId, vioOutputChildFrameId)); - // RCLCPP_INFO(this->get_logger(), "Output: %s", vioOutput->asJson().c_str()); - } - } - - void mappingOutputCallback(spectacularAI::mapping::MapperOutputPtr mappingOutput) { - if (latestKeyFrame == mappingOutput->updatedKeyFrames.back()) return; - latestKeyFrame = mappingOutput->updatedKeyFrames.back(); - const auto &kf = mappingOutput->map->keyFrames.at(latestKeyFrame); - if (!kf->pointCloud || kf->pointCloud->empty()) return; - const auto &primary = kf->frameSet->primaryFrame; - - sensor_msgs::msg::PointCloud2 pc; - pc.header.stamp = secondsToStamp(primary->cameraPose.pose.time); - pc.header.frame_id = fixedFrameId; - - std::string fieldNames[] = {"x", "y", "z", "rgb"}; - uint32_t totalOffset = 0; - for (int i = 0; i < 4; i++) { - sensor_msgs::msg::PointField f; - f.name = fieldNames[i]; - f.offset = totalOffset; - f.datatype = sensor_msgs::msg::PointField::FLOAT32; // 1 float == rgba8 - f.count = 1; - pc.fields.push_back(f); - totalOffset += sizeof(uint32_t); - } - - const std::uint8_t* rgb24Data = kf->pointCloud->getRGB24Data(); - const spectacularAI::Vector3f* positionData = kf->pointCloud->getPositionData(); - Matrix4f camToWorld = matrixToFloat(primary->cameraPose.getCameraToWorldMatrix()); - bool hasColors = kf->pointCloud->hasColors(); - - spectacularAI::Vector3f center = { - (float)primary->cameraPose.pose.position.x, (float)primary->cameraPose.pose.position.y, (float)primary->cameraPose.pose.position.z - }; - if (enableOccupancyGrid) { - if (occupancyGrid.empty()) { - occupancyGrid.reset(center, minDepth, maxDepth, cellSizeMeters, occupiedThreshold); - } - tempGrid.reset(center, minDepth, maxDepth, cellSizeMeters, occupiedThreshold); // TODO: Could use bounding box for visible area - } - - pc.data.resize(sizeof(RosPointWithColor) * kf->pointCloud->size()); - RosPointWithColor* points = (RosPointWithColor*) pc.data.data(); - for (size_t i = 0; i < kf->pointCloud->size(); i++) { - auto &p = points[i]; - p.pos = matVectorMult(camToWorld, positionData[i]); - if (hasColors) { - p.rgba[0] = rgb24Data[i * 3 + 0]; - p.rgba[1] = rgb24Data[i * 3 + 1]; - p.rgba[2] = rgb24Data[i * 3 + 2]; - p.rgba[3] = 255; - } else { - p.rgba[0] = 0; - p.rgba[1] = 0; - p.rgba[2] = 0; - p.rgba[3] = 0; - } - if (enableOccupancyGrid) { - float relativeZ = p.pos.z - center.z; - if (relativeZ > minOccupancyHeight && relativeZ < maxOccupancyHeight) { - tempGrid.addPoint(p.pos); - // p.rgba[0] = 255; - // p.rgba[1] = 0; - // p.rgba[2] = 0; - } - } - } - - pc.point_step = sizeof(RosPointWithColor); - pc.height = 1; - pc.width = kf->pointCloud->size(); - pc.row_step = pc.point_step * kf->pointCloud->size(); - pc.is_bigendian = false; - pc.is_dense = true; - pointCloudPublisher->publish(pc); - - if (enableOccupancyGrid && primary->image) { - // Trace to all edge points to find unoccupied cells - float cameraTestHeight = center.z + (maxOccupancyHeight + minOccupancyHeight) * 0.5; - tempGrid.traceFreeSpace(center, *primary, cameraTestHeight); - occupancyGrid.merge(tempGrid); - - nav_msgs::msg::OccupancyGrid og; - og.header.stamp = pc.header.stamp; - og.header.frame_id = pc.header.frame_id; - - og.info.map_load_time = og.header.stamp; // Seems redundant, duplicating header - og.info.resolution = occupancyGrid.cellSize; - og.info.width = occupancyGrid.width; - og.info.height = occupancyGrid.height; - - og.info.origin.position.x = occupancyGrid.originX * occupancyGrid.cellSize; - og.info.origin.position.y = occupancyGrid.originY * occupancyGrid.cellSize; - og.info.origin.position.z = center.z; - og.info.origin.orientation.x = 0.0f; - og.info.origin.orientation.y = 0.0f; - og.info.origin.orientation.z = 0.0f; - og.info.origin.orientation.w = 1.0f; - og.data = occupancyGrid.cells; // TODO: Copying entire map, perhaps occupancyGrid could internally wrap nav_msgs::msg::OccupancyGrid? - occupancyGridPublisher->publish(og); - } - } - - void startVio(std::vector intrinsics, std::vector extrinsics, spectacularAI::Matrix4d imuToOutput, bool isRectified) { - std::lock_guard lock(vioStartup); - if (vioInitDone) return; - - std::string calibrationJson; - if (!cameraInfoToCalibrationJson(intrinsics, extrinsics, imuToOutput, isRectified, calibrationJson)) { - vioInitDone = true; - return; - } - - std::string config = createConfigYaml(); - - // RCLCPP_INFO(this->get_logger(), "Calibration: %s", calibrationJson.c_str()); - // RCLCPP_INFO(this->get_logger(), "Configuration: %s", config.c_str()); - - spectacularAI::Vio::Builder vioBuilder = spectacularAI::Vio::builder(); - vioBuilder.setCalibrationJSON(calibrationJson); - vioBuilder.setConfigurationYAML(config); - if (!recordingFolder.empty()) { - vioBuilder.setRecordingFolder(recordingFolder); - vioBuilder.setRecordingOnly(recordingOnly); - } - if (enableMapping) vioBuilder.setMapperCallback(std::bind(&Node::mappingOutputCallback, this, std::placeholders::_1)); - vioApi = vioBuilder.build(); - vioApi->setOutputCallback(std::bind(&Node::vioOutputCallback, this, std::placeholders::_1)); - vioInitDone = true; - } - - std::unique_ptr odometryPathPublisher; - std::unique_ptr correctedPathPublisher; - std::unique_ptr vioPathPublisher; - - // rclcpp::Publisher::SharedPtr odometryPublisher; - rclcpp::Publisher::SharedPtr pointCloudPublisher; - rclcpp::Publisher::SharedPtr occupancyGridPublisher; - - rclcpp::Subscription::SharedPtr imuSubscription; - - message_filters::Subscriber cameraInfo0Subscription; - message_filters::Subscriber cameraInfo1Subscription; - message_filters::Subscriber cameraImage0Subscription; - message_filters::Subscriber cameraImage1Subscription; - message_filters::Subscriber cameraDepthSubscription; - message_filters::Subscriber depthaiTrackedFeaturesCam0Subscription; - - std::unique_ptr transformListenerBuffer; - std::shared_ptr transformListener; - std::shared_ptr transformBroadcaster; - - std::unique_ptr stereoCameraSynchronizer; - std::unique_ptr stereoDepthCameraSynchronizer; - - std::unique_ptr vioApi; - std::atomic_bool vioInitDone; - std::mutex vioStartup; - std::unique_ptr poseHelper; - - double previousFrameTime = 0.0; - double previousImuTime = 0.0; - double previousTriggerTime = 0.0; - - int frameNumber = 0; - int64_t latestKeyFrame = -1; - std::vector monoFeatures; - bool outputOnImuSamples; - bool recordingOnly; - uint64_t triggerCounter = 1; - - std::atomic_bool receivedFrames; - double imuStartTime = -1.; - double lastCameraWarning = -1.; - spectacularAI::Pose odomCorrection; - - // Params - std::string fixedFrameId; - std::string odometryFrameId; - std::string baseLinkFrameId; - - std::string deviceModel; - std::string imuFrameId; - std::string cam0FrameId; - std::string cam1FrameId; - double depthScale; - std::string cameraInputType; - std::string recordingFolder; - bool enableMapping; - double maxOdomFreq; - double maxOdomCorrectionFreq; - std::string overrideImuToCam0; - std::string overrideImuToCam1; - - // Map - bool enableOccupancyGrid; - OccupancyGrid occupancyGrid; - OccupancyGrid tempGrid; - float maxDepth = 3.0; // Max distance from camera for occupancy map - float minDepth = 0.2; - float maxOccupancyHeight = 0.3; // Max height above device for point to be counted in occupancy map - float minOccupancyHeight = 0.05; // Min height above device for point to be counted in occupancy map - float cellSizeMeters = 0.07; - int occupiedThreshold = 2; -}; - -} // namespace ros2 -} // namespace spectacularai +#include "spectacularai_ros2/ros2_plugin.hpp" #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(spectacularAI::ros2::Node) From c3a8ffb2158ff6762191d61df1d1a3022f7fea6f Mon Sep 17 00:00:00 2001 From: Aniel Alexa Date: Tue, 6 Feb 2024 11:06:20 +0200 Subject: [PATCH 4/9] 2D mode & new params & launch change --- .../include/spectacularai_ros2/helpers.hpp | 4 +-- .../spectacularai_ros2/pose_helper.hpp | 4 +-- .../spectacularai_ros2/ros2_plugin.hpp | 21 ++++++++---- spectacularai_ros2/launch/rae.launch.py | 34 ++++++++++++++----- spectacularai_ros2/launch/rae.yaml | 25 ++++++++++---- 5 files changed, 62 insertions(+), 26 deletions(-) diff --git a/spectacularai_ros2/include/spectacularai_ros2/helpers.hpp b/spectacularai_ros2/include/spectacularai_ros2/helpers.hpp index b6174bd..6125799 100644 --- a/spectacularai_ros2/include/spectacularai_ros2/helpers.hpp +++ b/spectacularai_ros2/include/spectacularai_ros2/helpers.hpp @@ -197,8 +197,8 @@ geometry_msgs::msg::TransformStamped poseToTransformStampped(spectacularAI::Pose tf.transform.translation.x = pose.position.x; tf.transform.translation.y = pose.position.y; tf.transform.translation.z = pose.position.z; - tf.transform.rotation.x = pose.orientation.x; - tf.transform.rotation.y = pose.orientation.y; + tf.transform.rotation.x = 0; + tf.transform.rotation.y = 0; tf.transform.rotation.z = pose.orientation.z; tf.transform.rotation.w = pose.orientation.w; return tf; diff --git a/spectacularai_ros2/include/spectacularai_ros2/pose_helper.hpp b/spectacularai_ros2/include/spectacularai_ros2/pose_helper.hpp index cfdeb14..d2645c6 100644 --- a/spectacularai_ros2/include/spectacularai_ros2/pose_helper.hpp +++ b/spectacularai_ros2/include/spectacularai_ros2/pose_helper.hpp @@ -32,10 +32,10 @@ class PoseHelper { double deltaT = vioOutput->pose.time - lastTime; odomPosition.x += vioOutput->velocity.x * deltaT; odomPosition.y += vioOutput->velocity.y * deltaT; - odomPosition.z += vioOutput->velocity.z * deltaT; + odomPosition.z = 0; } lastTime = vioOutput->pose.time; - + odomPose = spectacularAI::Pose { .time = vioOutput->pose.time, .position = odomPosition, diff --git a/spectacularai_ros2/include/spectacularai_ros2/ros2_plugin.hpp b/spectacularai_ros2/include/spectacularai_ros2/ros2_plugin.hpp index 37c035d..a16201e 100644 --- a/spectacularai_ros2/include/spectacularai_ros2/ros2_plugin.hpp +++ b/spectacularai_ros2/include/spectacularai_ros2/ros2_plugin.hpp @@ -121,7 +121,7 @@ class Node : public rclcpp::Node { // Output frames fixedFrameId = declareAndReadParameterString("fixed_frame_id", "map"); odometryFrameId = declareAndReadParameterString("odometry_frame_id", "odom"); - baseLinkFrameId = declareAndReadParameterString("base_link_frame_id", "base_link"); + baseLinkFrameId = declareAndReadParameterString("base_link_frame_id", "base_footprint"); depthScale = declareAndReadParameterDouble("depth_scale", 1.0 / 1000.0); recordingFolder = declareAndReadParameterString("recording_folder", ""); @@ -153,7 +153,7 @@ class Node : public rclcpp::Node { poseHelper = std::make_unique(1.0 / maxOdomCorrectionFreq); } - // odometryPublisher = this->create_publisher("output/odometry", ODOM_QUEUE_SIZE); + odometryPublisher = this->create_publisher("output/odometry", ODOM_QUEUE_SIZE); if (enableOccupancyGrid) occupancyGridPublisher = this->create_publisher("output/occupancyGrid", ODOM_QUEUE_SIZE); if (enableMapping) pointCloudPublisher = this->create_publisher("output/pointcloud", ODOM_QUEUE_SIZE); if (publishPaths) { @@ -195,6 +195,7 @@ class Node : public rclcpp::Node { } private: + static size_t outputCounter; std::string createConfigYaml() { std::ostringstream oss; const bool isRae = stringStartsWith(deviceModel, "RAE") || stringStartsWith(deviceModel, "rae"); @@ -304,7 +305,6 @@ class Node : public rclcpp::Node { RCLCPP_WARN(this->get_logger(), "Failed to find any imuToCamera calibration"); return false; } - if (camIndex == 0) { spectacularAI::Matrix4d cam0ToCam1; try { @@ -453,8 +453,11 @@ class Node : public rclcpp::Node { frameNumber++; } - + void vioOutputCallback(spectacularAI::VioOutputPtr vioOutput) { + if (outputCounter++ % 2 != 0){ + return; + } if (vioOutput->status == spectacularAI::TrackingStatus::TRACKING) { if (poseHelper) { spectacularAI::Pose odomPose; @@ -465,15 +468,19 @@ class Node : public rclcpp::Node { if (odometryPathPublisher) odometryPathPublisher->addPose(odomPose); if (correctedPathPublisher) { correctedPathPublisher->addPose(spectacularAI::Pose::fromMatrix(odomPose.time, matrixMul(odomCorrection.asMatrix(), odomPose.asMatrix()))); + RCLCPP_INFO(this->get_logger(),std::to_string(odomPose.time).c_str()); } } else { transformBroadcaster->sendTransform(poseToTransformStampped(vioOutput->pose, fixedFrameId, baseLinkFrameId)); } if (vioPathPublisher) vioPathPublisher->addPose(vioOutput->pose); - // odometryPublisher->publish(outputToOdometryMsg(vioOutput, vioOutputParentFrameId, vioOutputChildFrameId)); + odometryPublisher->publish(outputToOdometryMsg(vioOutput, fixedFrameId, baseLinkFrameId)); // RCLCPP_INFO(this->get_logger(), "Output: %s", vioOutput->asJson().c_str()); } + else{ + RCLCPP_INFO(this->get_logger(),"not in tracking state !"); + } } void mappingOutputCallback(spectacularAI::mapping::MapperOutputPtr mappingOutput) { @@ -608,7 +615,7 @@ class Node : public rclcpp::Node { std::unique_ptr correctedPathPublisher; std::unique_ptr vioPathPublisher; - // rclcpp::Publisher::SharedPtr odometryPublisher; + rclcpp::Publisher::SharedPtr odometryPublisher; rclcpp::Publisher::SharedPtr pointCloudPublisher; rclcpp::Publisher::SharedPtr occupancyGridPublisher; @@ -678,6 +685,6 @@ class Node : public rclcpp::Node { float cellSizeMeters = 0.07; int occupiedThreshold = 2; }; - +size_t Node::outputCounter = 0; } // namespace ros2 } // namespace spectacularai diff --git a/spectacularai_ros2/launch/rae.launch.py b/spectacularai_ros2/launch/rae.launch.py index 7c2b7f3..a4b00ae 100644 --- a/spectacularai_ros2/launch/rae.launch.py +++ b/spectacularai_ros2/launch/rae.launch.py @@ -2,9 +2,10 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction,ExecuteProcess, RegisterEventHandler, TimerAction, LogInfo from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration +from launch.event_handlers import OnProcessStart from launch_ros.actions import ComposableNodeContainer, Node from launch.conditions import IfCondition from launch_ros.descriptions import ComposableNode @@ -14,13 +15,11 @@ def launch_setup(context, *args, **kwargs): params_file = LaunchConfiguration("params_file") name = LaunchConfiguration('name').perform(context) - return [ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('rae_description'), 'launch', 'rsp.launch.py')), - launch_arguments={'sim': 'false'}.items() - ), - ComposableNodeContainer( + reset_pwm = ExecuteProcess( + cmd=[['busybox devmem 0x20320180 32 0x00000000']], + shell=True + ) + sai_slam = ComposableNodeContainer( name=name+"_container", namespace="", package="rclcpp_components", @@ -70,6 +69,24 @@ def launch_setup(context, *args, **kwargs): arguments=['--ros-args', '--log-level', 'info'], output="both", ) + return [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('rae_description'), 'launch', 'rsp.launch.py')), + launch_arguments={'sim': 'false'}.items() + ), + sai_slam, + RegisterEventHandler( + OnProcessStart( + target_action=sai_slam, + on_start=[ + TimerAction( + period=15.0, + actions=[reset_pwm, LogInfo(msg='Resetting PWM.'),], + ) + ] + ) + ), ] @@ -88,3 +105,4 @@ def generate_launch_description(): + diff --git a/spectacularai_ros2/launch/rae.yaml b/spectacularai_ros2/launch/rae.yaml index 8d17cdf..f46b8a4 100644 --- a/spectacularai_ros2/launch/rae.yaml +++ b/spectacularai_ros2/launch/rae.yaml @@ -8,6 +8,8 @@ i_usb_speed: SUPER_PLUS i_nn_type: none left: + i_fsync_continuous: true + i_fsync_mode: INPUT i_resolution: '800P' i_publish_topic: false i_output_isp: false @@ -15,9 +17,11 @@ i_set_isp_scale: false i_width: 640 i_height: 400 - i_fps: 30.0 + i_fps: 10.0 i_sensor_img_orientation: AUTO right: + i_fsync_continuous: true + i_fsync_mode: OUTPUT i_publish_topic: false i_output_isp: false i_board_socket_id: 2 @@ -25,17 +29,24 @@ i_set_isp_scale: false i_width: 640 i_height: 400 - i_fps: 30.0 + i_fps: 10.0 + # imu: + # i_batch_report_threshold: 15 + # i_max_batch_reports: 15 + # i_enable_rotation: true + # i_message_type: IMU_WITH_MAG_SPLIT + # i_sync_method: COPY imu: - i_batch_report_threshold: 15 - i_max_batch_reports: 15 - i_enable_rotation: true - i_message_type: IMU_WITH_MAG_SPLIT + i_batch_report_threshold: 5 + i_max_batch_reports: 5 + i_message_type: IMU i_sync_method: COPY + i_gyro_freq: 100 + i_acc_freq: 100 stereo: i_align_depth: true i_board_socket_id: 2 i_subpixel: true i_publish_right_rect: true i_publish_left_rect: true - i_right_rect_enable_feature_tracker: true \ No newline at end of file + i_right_rect_enable_feature_tracker: true From 84722596ae79b0b0e7479b09d8074388468ee2ff Mon Sep 17 00:00:00 2001 From: Aniel Alexa Date: Tue, 6 Feb 2024 15:08:03 +0200 Subject: [PATCH 5/9] fixed source env --- scripts/download_and_build_static.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) mode change 100755 => 100644 scripts/download_and_build_static.sh diff --git a/scripts/download_and_build_static.sh b/scripts/download_and_build_static.sh old mode 100755 new mode 100644 index 535e5ec..13daace --- a/scripts/download_and_build_static.sh +++ b/scripts/download_and_build_static.sh @@ -30,7 +30,7 @@ cd .. # Build ROS node . /opt/ros/${ROS_DISTRO}/setup.sh -# . $DEPTHAI_WS/install/setup.sh +. $DEPTHAI_WS/install/setup.sh ./scripts/build_all_static.sh sai_sdk/spectacularAI_*_static_rae.tar.gz # Remove temp files From 63dd27c1e01590fe4527bd4d0978073313f09d05 Mon Sep 17 00:00:00 2001 From: Aniel Alexa Date: Fri, 9 Feb 2024 10:56:48 +0200 Subject: [PATCH 6/9] colcon symlink-install --- scripts/build_all.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) mode change 100755 => 100644 scripts/build_all.sh diff --git a/scripts/build_all.sh b/scripts/build_all.sh old mode 100755 new mode 100644 index 4257b45..90e4589 --- a/scripts/build_all.sh +++ b/scripts/build_all.sh @@ -8,4 +8,4 @@ cd $ROOT/spectacularai_ros2 # sudo rosdep init rosdep update rosdep install -i --from-path src --rosdistro humble -y -colcon build --cmake-args "$@" +colcon build --cmake-args "$@" --symlink-install From a315121b5e9743cee87532d7f2fbdcbe015543f2 Mon Sep 17 00:00:00 2001 From: Aniel Alexa Date: Fri, 9 Feb 2024 18:36:36 +0200 Subject: [PATCH 7/9] Undo colcon symlink --- scripts/build_all.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/build_all.sh b/scripts/build_all.sh index 90e4589..4257b45 100644 --- a/scripts/build_all.sh +++ b/scripts/build_all.sh @@ -8,4 +8,4 @@ cd $ROOT/spectacularai_ros2 # sudo rosdep init rosdep update rosdep install -i --from-path src --rosdistro humble -y -colcon build --cmake-args "$@" --symlink-install +colcon build --cmake-args "$@" From 8c5c93de1205180a2468ea7b02f536da689d28da Mon Sep 17 00:00:00 2001 From: Aniel Alexa Date: Fri, 9 Feb 2024 18:46:14 +0200 Subject: [PATCH 8/9] permission fix --- scripts/build_all_static.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/build_all_static.sh b/scripts/build_all_static.sh index cf69fc6..9b74a58 100755 --- a/scripts/build_all_static.sh +++ b/scripts/build_all_static.sh @@ -17,7 +17,7 @@ make PREFIX="$TMP/install" test make install cd "$ROOT" -./scripts/build_all.sh -DspectacularAI_DIR="$TMP/install/lib/cmake/spectacularAI" +sudo ./scripts/build_all.sh -DspectacularAI_DIR="$TMP/install/lib/cmake/spectacularAI" INSTALL_DIR="spectacularai_ros2/install/spectacularai_ros2/" # Strip the executable (just in case the CMake build failed to do that) From 5bbaa0866b20cd6b4947cbda507a1e3723ef6087 Mon Sep 17 00:00:00 2001 From: Aniel Alexa Date: Fri, 9 Feb 2024 18:52:55 +0200 Subject: [PATCH 9/9] revert permission --- scripts/build_all_static.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/build_all_static.sh b/scripts/build_all_static.sh index 9b74a58..cf69fc6 100755 --- a/scripts/build_all_static.sh +++ b/scripts/build_all_static.sh @@ -17,7 +17,7 @@ make PREFIX="$TMP/install" test make install cd "$ROOT" -sudo ./scripts/build_all.sh -DspectacularAI_DIR="$TMP/install/lib/cmake/spectacularAI" +./scripts/build_all.sh -DspectacularAI_DIR="$TMP/install/lib/cmake/spectacularAI" INSTALL_DIR="spectacularai_ros2/install/spectacularai_ros2/" # Strip the executable (just in case the CMake build failed to do that)