diff --git a/continuous-integration/setup/setup_code_analysis.sh b/continuous-integration/setup/setup_code_analysis.sh index 0c222537..b0f6a911 100755 --- a/continuous-integration/setup/setup_code_analysis.sh +++ b/continuous-integration/setup/setup_code_analysis.sh @@ -10,6 +10,14 @@ function apt-yes { apt-get --assume-yes "$@" } +apt-yes install curl gnupg2 lsb-release || exit $? + +curl -fsSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc \ + | gpg --dearmor -o /usr/share/keyrings/ros-archive-keyring.gpg + +echo "deb [signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" \ + > /etc/apt/sources.list.d/ros-latest.list + apt-yes update || exit $? apt-yes dist-upgrade || exit $? diff --git a/zivid_camera/include/zivid_camera.h b/zivid_camera/include/zivid_camera.h index a44d9805..6d1c60f4 100644 --- a/zivid_camera/include/zivid_camera.h +++ b/zivid_camera/include/zivid_camera.h @@ -76,6 +76,7 @@ class ZividCamera void publishPointCloudXYZ(const std_msgs::Header& header, const Zivid::PointCloud& point_cloud); void publishPointCloudXYZRGBA(const std_msgs::Header& header, const Zivid::PointCloud& point_cloud, ColorSpace color_space); + void publishColorImageFromFrame2D(const Zivid::Frame2D& frame2D); void publishColorImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info, const Zivid::PointCloud& point_cloud, ColorSpace color_space); void publishColorImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info, @@ -90,6 +91,7 @@ class ZividCamera sensor_msgs::CameraInfoConstPtr makeCameraInfo(const std_msgs::Header& header, std::size_t width, std::size_t height, const Zivid::CameraIntrinsics& intrinsics); ColorSpace colorSpace() const; + void updateIntrinsics2D(const Zivid::CameraIntrinsics& intrinsics); IntrinsicsSource intrinsicsSource() const; // using Capture3DSettingsController = @@ -133,6 +135,7 @@ class ZividCamera Zivid::Camera camera_; Zivid::Settings current_settings_; Zivid::Settings2D current_settings_2d_; + Zivid::CameraIntrinsics current2Dintrinsics_; std::string frame_id_; unsigned int header_seq_; diff --git a/zivid_camera/src/node.cpp b/zivid_camera/src/node.cpp index de736280..12a23c89 100644 --- a/zivid_camera/src/node.cpp +++ b/zivid_camera/src/node.cpp @@ -37,7 +37,11 @@ int main(int argc, char** argv) } ROS_INFO("Successfully loaded nodelet '%s'", nodelet_name); - ros::spin(); + // ros::spin(); + ros::AsyncSpinner spinner(3); // Use 2 threads or more, depends on workload + spinner.start(); + + ros::waitForShutdown(); return EXIT_SUCCESS; } catch (const std::exception& e) diff --git a/zivid_camera/src/zivid_camera.cpp b/zivid_camera/src/zivid_camera.cpp index 3b959719..1ac58596 100644 --- a/zivid_camera/src/zivid_camera.cpp +++ b/zivid_camera/src/zivid_camera.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -317,6 +318,8 @@ ZividCamera::ZividCamera(ros::NodeHandle& nh, ros::NodeHandle& priv) ROS_INFO_STREAM("Connected to camera '" << camera_.info().serialNumber() << "'"); setCameraStatus(CameraStatus::Connected); + updateIntrinsics2D(Zivid::Experimental::Calibration::intrinsics(camera_, current_settings_2d_)); + camera_connection_keepalive_timer_ = nh_.createTimer(ros::Duration(10), &ZividCamera::onCameraConnectionKeepAliveTimeout, this); @@ -471,7 +474,6 @@ bool ZividCamera::capture2DServiceHandler(Capture2D::Request&, Capture2D::Respon serviceHandlerHandleCameraConnectionLoss(); - const auto color_space = colorSpace(); const auto settings2D = current_settings_2d_; // capture_2d_settings_controller_->zividSettings(); if (settings2D.acquisitions().isEmpty()) @@ -482,34 +484,14 @@ bool ZividCamera::capture2DServiceHandler(Capture2D::Request&, Capture2D::Respon ROS_INFO("Capturing 2D with %zd acquisition(s)", settings2D.acquisitions().size()); ROS_DEBUG_STREAM(settings2D); auto frame2D = camera_.capture(settings2D); + updateIntrinsics2D(Zivid::Experimental::Calibration::intrinsics(camera_, settings2D)); if (shouldPublishAcquisitionDone()) { publishAcquisitionDone(makeHeader()); } if (shouldPublishColorImg()) { - const auto header = makeHeader(); - const auto intrinsics = Zivid::Experimental::Calibration::intrinsics(camera_, settings2D); - switch (color_space) - { - case ColorSpace::sRGB: { - auto image = frame2D.imageRGBA_SRGB(); - const auto camera_info = makeCameraInfo(header, image.width(), image.height(), intrinsics); - ROS_DEBUG("Publishing image with 'srgb' color space"); - publishColorImage(header, camera_info, image); - } - break; - case ColorSpace::LinearRGB: { - auto image = frame2D.imageRGBA(); - const auto camera_info = makeCameraInfo(header, image.width(), image.height(), intrinsics); - ROS_DEBUG("Publishing image with 'linear' color space"); - publishColorImage(header, camera_info, image); - } - break; - default: - throw std::runtime_error("Internal error: Unknown color space value " + - std::to_string(static_cast(color_space))); - } + publishColorImageFromFrame2D(frame2D); } return true; } @@ -596,13 +578,12 @@ void ZividCamera::publishFrame(const Zivid::Frame& frame) const bool publish_acquisition_done = shouldPublishAcquisitionDone(); const bool publish_points_xyz = shouldPublishPointsXYZ(); const bool publish_points_xyzrgba = shouldPublishPointsXYZRGBA(); - const bool publish_color_img = shouldPublishColorImg(); const bool publish_depth_img = shouldPublishDepthImg(); const bool publish_snr_img = shouldPublishSnrImg(); const bool publish_normals_xyz = shouldPublishNormalsXYZ(); - if (publish_acquisition_done || publish_points_xyz || publish_points_xyzrgba || publish_color_img || - publish_depth_img || publish_snr_img || publish_normals_xyz) + if (publish_acquisition_done || publish_points_xyz || publish_points_xyzrgba || publish_depth_img || + publish_snr_img || publish_normals_xyz) { const auto color_space = colorSpace(); const auto header = makeHeader(); @@ -627,17 +608,17 @@ void ZividCamera::publishFrame(const Zivid::Frame& frame) { publishPointCloudXYZRGBA(header, point_cloud, color_space); } - if (publish_color_img || publish_depth_img || publish_snr_img) + if (publish_depth_img || publish_snr_img) { auto intrinsics = [&] { const auto intrinsics_source = intrinsicsSource(); switch (intrinsics_source) { case IntrinsicsSource::Camera: - ROS_INFO("Using camera intrinsics for publishing images"); + ROS_DEBUG("Using camera intrinsics for publishing images"); return Zivid::Experimental::Calibration::intrinsics(camera_, frame.settings()); case IntrinsicsSource::Frame: - ROS_INFO("Using frame intrinsics for publishing images"); + ROS_DEBUG("Using frame intrinsics for publishing images"); return Zivid::Experimental::Calibration::estimateIntrinsics(frame); default: throw std::runtime_error("Internal error: Unknown intrinsics source value " + @@ -646,10 +627,6 @@ void ZividCamera::publishFrame(const Zivid::Frame& frame) }(); const auto camera_info = makeCameraInfo(header, point_cloud.width(), point_cloud.height(), intrinsics); - if (publish_color_img) - { - publishColorImage(header, camera_info, point_cloud, color_space); - } if (publish_depth_img) { publishDepthImage(header, camera_info, point_cloud); @@ -785,6 +762,34 @@ void ZividCamera::publishColorImage(const std_msgs::Header& header, const sensor color_image_publisher_.publish(image, camera_info); } +void ZividCamera::publishColorImageFromFrame2D(const Zivid::Frame2D& frame2D) +{ + ROS_DEBUG("Publishing color image from Zivid::Frame2D"); + const auto color_space = colorSpace(); + const auto header = makeHeader(); + const auto settings2D = frame2D.settings(); + switch (color_space) + { + case ColorSpace::sRGB: { + auto image = frame2D.imageRGBA_SRGB(); + const auto camera_info = makeCameraInfo(header, image.width(), image.height(), current2Dintrinsics_); + ROS_DEBUG("Publishing image with 'srgb' color space"); + publishColorImage(header, camera_info, image); + } + break; + case ColorSpace::LinearRGB: { + auto image = frame2D.imageRGBA(); + const auto camera_info = makeCameraInfo(header, image.width(), image.height(), current2Dintrinsics_); + ROS_DEBUG("Publishing image with 'linear' color space"); + publishColorImage(header, camera_info, image); + } + break; + default: + throw std::runtime_error("Internal error: Unknown color space value " + + std::to_string(static_cast(color_space))); + } +} + void ZividCamera::publishColorImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info, const Zivid::Image& image) { @@ -897,9 +902,30 @@ Zivid::Frame ZividCamera::invokeCaptureAndPublishFrame() throw std::runtime_error("capture called with 0 enabled acquisitions!"); } - ROS_INFO("Capturing with %zd acquisition(s)", settings.acquisitions().size()); - ROS_DEBUG_STREAM(settings); - const auto frame = camera_.capture(settings); + ROS_DEBUG("Capturing with %zd acquisition(s)", settings.acquisitions().size()); + Zivid::Frame2D frame2D; + bool should_publish_color_img = shouldPublishColorImg(); + if (settings.color().hasValue() && should_publish_color_img) + { + ROS_DEBUG_STREAM("Capturing 2D as part of 2D+3D. " << color_image_publisher_.getNumSubscribers() + << " subscribers."); + frame2D = camera_.capture2D(settings); + updateIntrinsics2D(Zivid::Experimental::Calibration::intrinsics(camera_, settings)); + } + else if (should_publish_color_img) + { + ROS_DEBUG("No color image to publish from Zivid::Frame2D, since settings.color() is not set."); + should_publish_color_img = false; + } + ROS_DEBUG("Capturing 3D as part of 2D+3D"); + std::future frame3D_future = + std::async(std::launch::async, &decltype(camera_)::capture3D, &camera_, settings); + if (should_publish_color_img) + { + ROS_DEBUG("Publishing color image from Zivid::Frame2D in separate thread"); + publishColorImageFromFrame2D(frame2D); + } + const auto frame = frame3D_future.get(); publishFrame(frame); return frame; } @@ -916,6 +942,12 @@ ColorSpace ZividCamera::colorSpace() const return parameterStringToEnum(ParamNames::color_space, color_space_str, color_space_name_value_map_); } +void ZividCamera::updateIntrinsics2D(const Zivid::CameraIntrinsics& intrinsics) +{ + ROS_DEBUG_STREAM(__func__); + current2Dintrinsics_ = intrinsics; +} + IntrinsicsSource ZividCamera::intrinsicsSource() const { ROS_DEBUG_STREAM(__func__); diff --git a/zivid_samples/settings/camera_settings.yml b/zivid_samples/settings/camera_settings.yml index 635c095e..966cf37f 100644 --- a/zivid_samples/settings/camera_settings.yml +++ b/zivid_samples/settings/camera_settings.yml @@ -1,48 +1,100 @@ -__version__: - serializer: 1 - data: 10 +__version__: 27 Settings: - Acquisitions: - - Acquisition: - Aperture: 5.66 - Brightness: 1.5 - ExposureTime: 6500 - Gain: 1 - Diagnostics: - Enabled: no - Experimental: - Engine: phase - Processing: + Acquisitions: + - Acquisition: + Aperture: 2.83 + Brightness: 1.8 + ExposureTime: 40000 + Gain: 1 + - Acquisition: + Aperture: 2.83 + Brightness: 1.8 + ExposureTime: 40000 + Gain: 1 Color: - Balance: - Blue: 1 - Green: 1 - Red: 1 - Experimental: - ToneMapping: - Enabled: hdrOnly - Gamma: 1 - Filters: - Experimental: - ContrastDistortion: - Correction: + __version__: 7 + Settings2D: + Acquisitions: + - Acquisition: + Aperture: 4.0 + Brightness: 1.8 + ExposureTime: 2000 + Gain: 1 + Processing: + Color: + Balance: + Blue: 1 + Green: 1 + Red: 1 + Experimental: + Mode: automatic + Gamma: 1 + Sampling: + Color: rgb + Pixel: all + Diagnostics: + Enabled: no + Engine: stripe + Processing: + Color: + Balance: + Blue: 1 + Green: 1 + Red: 1 + Experimental: + Mode: automatic + Gamma: 1 + Filters: + Cluster: + Removal: + Enabled: yes + MaxNeighborDistance: 7 + MinArea: 500 + Experimental: + ContrastDistortion: + Correction: + Enabled: yes + Strength: 0.3 + Removal: + Enabled: no + Threshold: 0.3 + Hole: + Repair: + Enabled: yes + HoleSize: 0.2 + Strictness: 2 + Noise: + Removal: + Enabled: yes + Threshold: 3 + Repair: + Enabled: no + Suppression: + Enabled: no + Outlier: + Removal: + Enabled: yes + Threshold: 20 + Reflection: + Removal: + Enabled: yes + Mode: global + Smoothing: + Gaussian: + Enabled: yes + Sigma: 4 + Resampling: + Mode: __not_set__ + RegionOfInterest: + Box: Enabled: no - Strength: 0.4 - Removal: + Extents: [-10, 100] + PointA: [0, 0, 0] + PointB: [0, 0, 0] + PointO: [0, 0, 0] + Depth: Enabled: no - Threshold: 0.5 - Noise: - Removal: - Enabled: yes - Threshold: 7 - Outlier: - Removal: - Enabled: yes - Threshold: 5 - Reflection: - Removal: - Enabled: no - Smoothing: - Gaussian: - Enabled: no - Sigma: 1.5 + Range: [300, 1300] + Sampling: + Color: rgb + Pixel: all diff --git a/zivid_samples/src/sample_capture_with_settings_from_yml.cpp b/zivid_samples/src/sample_capture_with_settings_from_yml.cpp index b59e7387..64aaa396 100644 --- a/zivid_samples/src/sample_capture_with_settings_from_yml.cpp +++ b/zivid_samples/src/sample_capture_with_settings_from_yml.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include @@ -19,6 +20,21 @@ namespace { const ros::Duration default_wait_duration{ 30 }; +auto waitForPublisher = [](const ros::Subscriber& sub, ros::Duration timeout) { + ros::Time start = ros::Time::now(); + ros::Rate rate(10); + while ((ros::Time::now() - start) < timeout) + { + if (sub.getNumPublishers() > 0) + { + return true; + } + ros::spinOnce(); + rate.sleep(); + } + return false; +}; + void capture() { ROS_INFO("Calling capture service"); @@ -26,6 +42,20 @@ void capture() CHECK(ros::service::call("/zivid_camera/capture", capture)); } +void on_points(const sensor_msgs::PointCloud2ConstPtr&) +{ + ROS_INFO("PointCloud received"); +} + +void on_image_color(const sensor_msgs::ImageConstPtr&) +{ + ROS_INFO("2D color image received"); +} + +void on_acquisition_done(const std_msgs::HeaderConstPtr& header) +{ + ROS_INFO("Acquisition done: %s", header->frame_id.c_str()); +} } // namespace int main(int argc, char** argv) @@ -37,9 +67,16 @@ int main(int argc, char** argv) CHECK(ros::service::waitForService("/zivid_camera/capture", default_wait_duration)); - ros::AsyncSpinner spinner(1); + ros::AsyncSpinner spinner(3); spinner.start(); + auto points_sub = n.subscribe("/zivid_camera/points/xyzrgba", 1, on_points); + CHECK(waitForPublisher(points_sub, default_wait_duration)); + auto image_color_sub = n.subscribe("/zivid_camera/color/image_color", 1, on_image_color); + CHECK(waitForPublisher(image_color_sub, default_wait_duration)); + auto acquisition_done_sub = n.subscribe("/zivid_camera/acquisition_done", 1, on_acquisition_done); + CHECK(waitForPublisher(acquisition_done_sub, default_wait_duration)); + std::string samples_path = ros::package::getPath("zivid_samples"); std::string settings_path = samples_path + "/settings/camera_settings.yml"; ROS_INFO("Loading settings from: %s", settings_path.c_str());