depthai-ros
depthai-ros copied to clipboard
[BUG] undefined symbol lookup error at the function - getCameraIntrinsics(CameraBoardSocket, Point2f)
I was trying to run a pipeline with imu, rgb, stereo and depth enabled. I created a left and right image converters using the classes provided in dai namespace. I called the readCalibration() function from the device pointer and calling the calibrationToCameraInfo from the left & right converter nodes. I think here is where the getCameraIntrinsics() is being called wherein the symbol lookup error is popping up.
Sharing the code I used -
{
plugin_name_ = plugin_name;
node_ = node;
RCLCPP_INFO(
node_->get_logger(), "Initialising Multiple Camera Interface Plugin for '%s'",
plugin_name_.c_str());
loadParams();
configureDevice(camera_params_.mx_id);
RCLCPP_INFO(
node_->get_logger(), "%s device for camera '%s' with MXID : %s, is configured!",
camera_params_.device_name.c_str(), plugin_name_.c_str(), camera_params_.mx_id.c_str());
// Create data queue objects
auto left_queue = device_->getOutputQueue("left", 30, false);
auto right_queue = device_->getOutputQueue("right", 30, false);
auto calibration_handler = device_->readCalibration();
// Add Left Camera Publisher
left_converter_ = std::make_unique<dai::rosBridge::ImageConverter>(
plugin_name_ + "_left_camera_optical_frame", true);
auto left_camera_info = left_converter_->calibrationToCameraInfo(
calibration_handler, dai::CameraBoardSocket::LEFT,
camera_params_.resolution_map[camera_params_.mono_resolution][0],
camera_params_.resolution_map[camera_params_.mono_resolution][1]);
left_publisher_ =
std::make_unique<dai::rosBridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame>>(
left_queue, node_, plugin_name_ + std::string("/left/image"),
std::bind(
&dai::rosBridge::ImageConverter::toRosMsg, left_converter_.get(), std::placeholders::_1,
std::placeholders::_2),
kQueueSize, left_camera_info, plugin_name_ + "/left");
// Add Right Camera Publisher
right_converter_ = std::make_unique<dai::rosBridge::ImageConverter>(
plugin_name_ + "_right_camera_optical_frame", true);
auto right_camera_info = right_converter_->calibrationToCameraInfo(
calibration_handler, dai::CameraBoardSocket::RIGHT,
camera_params_.resolution_map[camera_params_.mono_resolution][0],
camera_params_.resolution_map[camera_params_.mono_resolution][1]);
right_publisher_ =
std::make_unique<dai::rosBridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame>>(
right_queue, node_, plugin_name_ + std::string("/right/image"),
std::bind(
&dai::rosBridge::ImageConverter::toRosMsg, right_converter_.get(), std::placeholders::_1,
std::placeholders::_2),
kQueueSize, right_camera_info, plugin_name_ + "/right");
if (camera_params_.enable_infra) {
left_publisher_->addPublisherCallback();
right_publisher_->addPublisherCallback();
}
// Add rgb publisher
if (camera_params_.enable_rgb) {
auto rgb_queue = device_->getOutputQueue("rgb", 30, false);
rgb_converter_ =
std::make_unique<dai::rosBridge::ImageConverter>(plugin_name_ + "_rgb_frame", true);
auto rgb_camera_info = rgb_converter_->calibrationToCameraInfo(
calibration_handler, dai::CameraBoardSocket::RGB,
camera_params_.resolution_map[camera_params_.mono_resolution][0],
camera_params_.resolution_map[camera_params_.mono_resolution][1]);
rgb_publisher_ =
std::make_unique<dai::rosBridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame>>(
rgb_queue, node_, plugin_name_ + std::string("/rgb"),
std::bind(
&dai::rosBridge::ImageConverter::toRosMsg, rgb_converter_.get(), std::placeholders::_1,
std::placeholders::_2),
kQueueSize, rgb_camera_info, plugin_name_ + "/rgb");
rgb_publisher_->addPublisherCallback();
}
// Add Depth Publisher
std::shared_ptr<dai::DataOutputQueue> stereo_queue;
if (camera_params_.enable_depth) {
stereo_queue = device_->getOutputQueue("depth", 30, false);
depth_publisher_ =
std::make_unique<dai::rosBridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame>>(
stereo_queue, node_, plugin_name_ + std::string("/stereo/depth"),
std::bind(
&dai::rosBridge::ImageConverter::toRosMsg, right_converter_.get(), std::placeholders::_1,
std::placeholders::_2),
kQueueSize, right_camera_info,
plugin_name_ + "/stereo"); // Reusing camera info name
depth_publisher_->addPublisherCallback();
} else {
stereo_queue = device_->getOutputQueue("disparity", 30, false);
depth_publisher_ =
std::make_unique<dai::rosBridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame>>(
stereo_queue, node_, plugin_name_ + std::string("/stereo/image"),
std::bind(
&dai::rosBridge::ImageConverter::toRosMsg, right_converter_.get(), std::placeholders::_1,
std::placeholders::_2),
kQueueSize, right_camera_info, plugin_name_ + "/stereo");
depth_publisher_->addPublisherCallback();
}
// Add IMU publisher
if (camera_params_.enable_imu) {
auto imu_queue = device_->getOutputQueue("imu", 30, false);
imu_converter_ = std::make_unique<dai::rosBridge::ImuConverter>(plugin_name_ + "_imu_frame");
imu_publisher_ =
std::make_unique<dai::rosBridge::BridgePublisher<sensor_msgs::msg::Imu, dai::IMUData>>(
imu_queue, node_, plugin_name_ + std::string("/imu"),
std::bind(
&dai::rosBridge::ImuConverter::toRosMsg, imu_converter_.get(), std::placeholders::_1,
std::placeholders::_2),
kQueueSize, "", plugin_name_ + "/imu");
imu_publisher_->addPublisherCallback();
}
}
void MultipleCameraInterface::configureDevice(const std::string & cam_id)
{
auto pipeline = getPipeline();
auto mx_id = cam_id;
get_device_by_mx_id_ = dai::XLinkConnection::getDeviceByMxId(mx_id);
device_found_ = false;
device_found_ = std::get<0>(get_device_by_mx_id_);
device_info_ = std::get<1>(get_device_by_mx_id_);
if (device_found_) {
RCLCPP_INFO(node_->get_logger(), "Camera device with MXID %s found!", mx_id.c_str());
} else {
throw std::runtime_error(
std::string(plugin_name_) + " device with MXID " + std::string(mx_id) + " not found!");
}
device_ =
std::make_shared<dai::Device>(dai::OpenVINO::Version::VERSION_2021_4, device_info_, false);
device_->startPipeline(pipeline);
}
dai::Pipeline MultipleCameraInterface::getPipeline()
{
dai::Pipeline pipeline;
auto monoLeft = pipeline.create<dai::node::MonoCamera>();
auto monoRight = pipeline.create<dai::node::MonoCamera>();
// auto camRgb = pipeline.create<dai::node::ColorCamera>();
// auto imu = pipeline.create<dai::node::IMU>();
// auto xoutDepth = pipeline.create<dai::node::XLinkOut>();
// auto xoutImu = pipeline.create<dai::node::XLinkOut>();
// auto xoutRgb = pipeline.create<dai::node::XLinkOut>();
auto xoutLeft = pipeline.create<dai::node::XLinkOut>();
auto xoutRight = pipeline.create<dai::node::XLinkOut>();
// XLinkOut
xoutLeft->setStreamName("left");
xoutRight->setStreamName("right");
// Depth
// MonoCamera
auto resolution = dai::MonoCameraProperties::SensorResolution::THE_400_P;
if (camera_params_.mono_resolution == "480p") {
resolution = dai::MonoCameraProperties::SensorResolution::THE_480_P;
} else if (camera_params_.mono_resolution == "720p") {
resolution = dai::MonoCameraProperties::SensorResolution::THE_720_P;
} else if (camera_params_.mono_resolution == "800p") {
resolution = dai::MonoCameraProperties::SensorResolution::THE_800_P;
}
monoLeft->setResolution(resolution);
monoRight->setResolution(resolution);
monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT);
monoLeft->setFps(camera_params_.stereo_fps);
monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT);
monoRight->setFps(camera_params_.stereo_fps);
// Stereo
if (camera_params_.enable_stereo) {
auto stereo = pipeline.create<dai::node::StereoDepth>();
xoutDepth_ = pipeline.create<dai::node::XLinkOut>();
// StereoDepthConfig
stereo->initialConfig.setConfidenceThreshold(camera_params_.disparity_confidence);
stereo->setRectifyEdgeFillColor(0);
stereo->setLeftRightCheck(camera_params_.enable_leftright_check);
stereo->initialConfig.setLeftRightCheckThreshold(camera_params_.leftright_check_thr);
stereo->setExtendedDisparity(camera_params_.extended_depth);
stereo->setSubpixel(camera_params_.subpixel_depth);
if (camera_params_.rectify) {
stereo->rectifiedLeft.link(xoutLeft->input);
stereo->rectifiedRight.link(xoutRight->input);
} else {
stereo->syncedLeft.link(xoutLeft->input);
stereo->syncedRight.link(xoutRight->input);
}
monoLeft->out.link(stereo->left);
monoRight->out.link(stereo->right);
if (camera_params_.enable_depth) {
xoutDepth_->setStreamName("depth");
stereo->depth.link(xoutDepth_->input);
} else {
xoutDepth_->setStreamName("disparity");
stereo->disparity.link(xoutDepth_->input);
}
}
// IMU
if (camera_params_.enable_imu) {
imu_ = pipeline.create<dai::node::IMU>();
xoutImu_ = pipeline.create<dai::node::XLinkOut>();
xoutImu_->setStreamName("imu");
imu_->enableIMUSensor(
{dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, camera_params_.imu_rate);
imu_->setBatchReportThreshold(1);
imu_->setMaxBatchReports(5); // Hardcoded for now
imu_->out.link(xoutImu_->input);
}
// RGB image
if (camera_params_.enable_rgb) {
camRgb_ = pipeline.create<dai::node::ColorCamera>();
xoutRgb_ = pipeline.create<dai::node::XLinkOut>();
xoutRgb_->setStreamName("rgb");
camRgb_->setBoardSocket(dai::CameraBoardSocket::RGB);
camRgb_->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P);
// the ColorCamera is downscaled from 1080p to 720p.
// Otherwise, the aligned depth is automatically upscaled to 1080p
camRgb_->setIspScale(2, 3);
// For now, RGB needs fixed focus to properly align with depth.
// This value was used during calibration
camRgb_->initialControl.setManualFocus(135);
camRgb_->isp.link(xoutRgb_->input);
} else {
// Stereo imges
std::cerr << "RGB is disbaled!";
}
if (camera_params_.enable_infra) {
monoLeft->out.link(xoutLeft->input);
monoRight->out.link(xoutRight->input);
}
return pipeline;
}
Expected behavior The pipeline to start without any errors and give out the respective feeds from the camera.
Attach system log [INFO] [launch]: All log files can be found below /home/akshay/.ros/log/2022-10-10-12-58-41-076591-bot30pc-60109 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [robot_state_publisher-1]: process started with pid [60121] [INFO] [component_container-2]: process started with pid [60123] [robot_state_publisher-1] Link front_camera_frame had 5 children [robot_state_publisher-1] Link front_camera_imu_frame had 0 children [robot_state_publisher-1] Link front_camera_left_camera_frame had 1 children [robot_state_publisher-1] Link front_camera_left_camera_optical_frame had 0 children [robot_state_publisher-1] Link front_camera_model_origin had 0 children [robot_state_publisher-1] Link front_camera_rgb_camera_frame had 1 children [robot_state_publisher-1] Link front_camera_rgb_camera_optical_frame had 0 children [robot_state_publisher-1] Link front_camera_right_camera_frame had 1 children [robot_state_publisher-1] Link front_camera_right_camera_optical_frame had 0 children [robot_state_publisher-1] [INFO] [1665386921.293591546] [oak_state_publisher]: got segment front_camera_frame [robot_state_publisher-1] [INFO] [1665386921.293653141] [oak_state_publisher]: got segment front_camera_imu_frame [robot_state_publisher-1] [INFO] [1665386921.293659398] [oak_state_publisher]: got segment front_camera_left_camera_frame [robot_state_publisher-1] [INFO] [1665386921.293663110] [oak_state_publisher]: got segment front_camera_left_camera_optical_frame [robot_state_publisher-1] [INFO] [1665386921.293666601] [oak_state_publisher]: got segment front_camera_model_origin [robot_state_publisher-1] [INFO] [1665386921.293670181] [oak_state_publisher]: got segment front_camera_oak-d-base-frame [robot_state_publisher-1] [INFO] [1665386921.293673771] [oak_state_publisher]: got segment front_camera_rgb_camera_frame [robot_state_publisher-1] [INFO] [1665386921.293677108] [oak_state_publisher]: got segment front_camera_rgb_camera_optical_frame [robot_state_publisher-1] [INFO] [1665386921.293680595] [oak_state_publisher]: got segment front_camera_right_camera_frame [robot_state_publisher-1] [INFO] [1665386921.293684124] [oak_state_publisher]: got segment front_camera_right_camera_optical_frame [component_container-2] [INFO] [1665386921.502091933] [depthai_container]: Load Library: /home/akshay/cleaning_peppermint_ws/install/depthai_ros_ppmt/lib/libmulti_cam_interface_comp_node.so [component_container-2] [INFO] [1665386921.503405640] [depthai_container]: Found class: rclcpp_components::NodeFactoryTemplate<ppmt_multi_cam_driver::MultiCamInterfaceCompNode> [component_container-2] [INFO] [1665386921.503455780] [depthai_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ppmt_multi_cam_driver::MultiCamInterfaceCompNode> [component_container-2] [INFO] [1665386921.513378916] [multi_cam_interface_composable]: Multi Cam Interface Comp Node Initialised [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/multi_cam_interface_composable' in container '/depthai_container' [component_container-2] [INFO] [1665386921.514811681] [depthai_container]: Load Library: /opt/ros/humble/lib/libdepth_image_proc.so [component_container-2] [INFO] [1665386921.554824832] [depthai_container]: Found class: rclcpp_components::NodeFactoryTemplate<depth_image_proc::ConvertMetricNode> [component_container-2] [INFO] [1665386921.554872440] [depthai_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<depth_image_proc::ConvertMetricNode> [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/front_camera_convert_metric_node' in container '/depthai_container' [component_container-2] [INFO] [1665386921.578150799] [depthai_container]: Found class: rclcpp_components::NodeFactoryTemplate<depth_image_proc::ConvertMetricNode> [component_container-2] [INFO] [1665386921.578178966] [depthai_container]: Found class: rclcpp_components::NodeFactoryTemplate<depth_image_proc::CropForemostNode> [component_container-2] [INFO] [1665386921.578183887] [depthai_container]: Found class: rclcpp_components::NodeFactoryTemplate<depth_image_proc::DisparityNode> [component_container-2] [INFO] [1665386921.578188094] [depthai_container]: Found class: rclcpp_components::NodeFactoryTemplate<depth_image_proc::PointCloudXyzNode> [component_container-2] [INFO] [1665386921.578192010] [depthai_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<depth_image_proc::PointCloudXyzNode> [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/front_camera_point_cloud_xyz_node' in container '/depthai_container' [component_container-2] Multiple camera interface plugin loaded! [component_container-2] [INFO] [1665386922.523556377] [multi_cam_interface_composable]: Initialising Multiple Camera Interface Plugin for 'front_camera' [component_container-2] [INFO] [1665386922.527422413] [multi_cam_interface_composable]: Camera device with MXID 19443010E1D4F91200 found! [component_container-2] [INFO] [1665386925.838804488] [multi_cam_interface_composable]: OAK-D device for camera 'front_camera' with MXID : 19443010E1D4F91200, is configured! [component_container-2] /opt/ros/humble/lib/rclcpp_components/component_container: symbol lookup error: /home/akshay/cleaning_peppermint_ws/install/depthai_bridge/lib/libdepthai_bridge.so: undefined symbol: ZN3dai18CalibrationHandler19getCameraIntrinsicsENS_17CameraBoardSocketEiiNS_7Point2fES2 [ERROR] [component_container-2]: process has died [pid 60123, exit code 127, cmd '/opt/ros/humble/lib/rclcpp_components/component_container --ros-args -r __node:=depthai_container -r __ns:=/'].
- Which OS/OS version are you using? - Ubuntu 22.04
- Which ROS version are you using? - ROS2
Please append the RME. aka the package in ROS case.
https://docs.luxonis.com/en/latest/pages/support/#creating-minimal-reproducible-example
Basing on this description, I would check if all necessary headers are included, verify CMakeLists.txt and package.xml in your example