stereo_camera.h

Go to the documentation of this file.

#pragma once

#include "ensenso_camera/camera.h"

#include "ensenso_camera/ros2/pcl.h"

#include "ensenso_camera/ros2/ensenso_msgs/calibrate_hand_eye.h"
#include "ensenso_camera/ros2/ensenso_msgs/calibrate_workspace.h"
#include "ensenso_camera/ros2/ensenso_msgs/fit_primitive.h"
#include "ensenso_camera/ros2/ensenso_msgs/locate_pattern.h"
#include "ensenso_camera/ros2/ensenso_msgs/primitive.h"
#include "ensenso_camera/ros2/ensenso_msgs/project_pattern.h"
#include "ensenso_camera/ros2/ensenso_msgs/request_data.h"
#include "ensenso_camera/ros2/ensenso_msgs/telecentric_projection.h"
#include "ensenso_camera/ros2/ensenso_msgs/textured_point_cloud.h"

enum ProjectorState
{
  projectorDontCare,  //@< Inherit the projector settings from the parameter set.
  projectorOn,        //@< Enable the projector and disable the front light.
  projectorOff        //@< Enable the front light and disable the projector.
};

class StereoCamera : public Camera
{
private:
  sensor_msgs::msg::CameraInfoPtr leftCameraInfo;
  sensor_msgs::msg::CameraInfoPtr rightCameraInfo;
  sensor_msgs::msg::CameraInfoPtr leftRectifiedCameraInfo;
  sensor_msgs::msg::CameraInfoPtr rightRectifiedCameraInfo;

  std::unique_ptr<CalibrateHandEyeServer> calibrateHandEyeServer;
  std::unique_ptr<CalibrateWorkspaceServer> calibrateWorkspaceServer;
  std::unique_ptr<FitPrimitiveServer> fitPrimitiveServer;
  std::unique_ptr<LocatePatternServer> locatePatternServer;
  std::unique_ptr<ProjectPatternServer> projectPatternServer;
  std::unique_ptr<RequestDataServer> requestDataServer;
  std::unique_ptr<TexturedPointCloudServer> texturedPointCloudServer;
  std::unique_ptr<TelecentricProjectionServer> telecentricProjectionServer;

  image_transport::CameraPublisher leftRawImagePublisher;
  image_transport::CameraPublisher rightRawImagePublisher;
  image_transport::CameraPublisher leftRectifiedImagePublisher;
  image_transport::CameraPublisher rightRectifiedImagePublisher;
  image_transport::CameraPublisher disparityMapPublisher;
  image_transport::CameraPublisher depthImagePublisher;
  image_transport::Publisher projectedImagePublisher;

  PointCloudPublisher<ensenso::pcl::PointCloud> pointCloudPublisher;
  PointCloudPublisher<ensenso::pcl::PointCloudNormals> pointCloudNormalsPublisher;
  PointCloudPublisher<ensenso::pcl::PointCloudColored> pointCloudColoredPublisher;
  PointCloudPublisher<ensenso::pcl::PointCloud> pointCloudProjectedPublisher;

  // Information that we remember between the different steps of the hand-eye calibration. We save the pattern buffer
  // outside of the NxLib, because otherwise we could not use the LocatePattern action while a hand-eye calibration is
  // in progress.
  std::string handEyeCalibrationPatternBuffer;
  std::vector<tf2::Transform> handEyeCalibrationRobotTransforms;

public:
  StereoCamera(ensenso::ros::NodeHandle& nh, CameraParameters params);

  void init() override;

  void onSetParameter(ensenso::action::SetParameterGoalConstPtr const& goal) override;

  void onRequestData(ensenso::action::RequestDataGoalConstPtr const& goal);

  void onLocatePattern(ensenso::action::LocatePatternGoalConstPtr const& goal);

  void onProjectPattern(ensenso::action::ProjectPatternGoalConstPtr const& goal);

  void onCalibrateHandEye(ensenso::action::CalibrateHandEyeGoalConstPtr const& goal);

  void onCalibrateWorkspace(ensenso::action::CalibrateWorkspaceGoalConstPtr const& goal);

  void onFitPrimitive(ensenso::action::FitPrimitiveGoalConstPtr const& goal);

  void onTexturedPointCloud(ensenso::action::TexturedPointCloudGoalConstPtr const& goal);

  void onTelecentricProjection(ensenso::action::TelecentricProjectionGoalConstPtr const& goal);

private:
  void updateCameraTypeSpecifics() override;

  void startServers() const override;

  void updateCameraInfo() override;

  geometry_msgs::msg::PoseStamped estimatePatternPose(ensenso::ros::Time imageTimestamp,
                                                      std::string const& targetFrame = "",
                                                      bool latestPatternOnly = false) const override;

  std::vector<geometry_msgs::msg::PoseStamped> estimatePatternPoses(ensenso::ros::Time imageTimestamp,
                                                                    std::string const& targetFrame = "") const override;

  ensenso::ros::Time capture() const override;

  ensenso::msg::ParameterPtr readParameter(std::string const& key) const override;

  void writeParameter(ensenso::msg::Parameter const& parameter) override;

  void advertiseTopics();

  void saveParameterSet(std::string name, bool projectorWasSet);

  void loadParameterSet(std::string name, ProjectorState projector = projectorDontCare);

  ensenso::ros::Time timestampOfCapturedImage() const;

  std::vector<StereoCalibrationPattern> collectPattern(bool clearBuffer = false) const;

  void fillCameraInfoFromNxLib(sensor_msgs::msg::CameraInfoPtr const& info, bool right, bool rectified = false) const;

  bool isSSeries() const;

  bool isXrSeries() const;

  bool hasRightCamera() const;

  bool hasRawImages() const;

  bool hasDownloadedImages() const;

  bool hasDisparityMap() const;

  void addDisparityMapOffset(sensor_msgs::msg::CameraInfoPtr const& info) const;
};