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;
};