mono_camera.h

Go to the documentation of this file.

#pragma once

#include "ensenso_camera/camera.h"

#include <ensenso_camera_msgs/RequestDataMonoAction.h>
#include <ensenso_camera_msgs/LocatePatternMonoAction.h>

using RequestDataMonoServer = QueuedActionServer<ensenso_camera_msgs::RequestDataMonoAction>;
using LocatePatternMonoServer = QueuedActionServer<ensenso_camera_msgs::LocatePatternMonoAction>;

class MonoCamera : public Camera
{
private:
  sensor_msgs::CameraInfoPtr cameraInfo;
  sensor_msgs::CameraInfoPtr rectifiedCameraInfo;

  std::unique_ptr<RequestDataMonoServer> requestDataServer;
  std::unique_ptr<LocatePatternMonoServer> locatePatternServer;

  image_transport::CameraPublisher rawImagePublisher;
  image_transport::CameraPublisher rectifiedImagePublisher;

public:
  MonoCamera(ros::NodeHandle nh, std::string serial, std::string fileCameraPath, bool fixed, std::string cameraFrame,
             std::string targetFrame, std::string linkFrame);

  bool open() override;

  void startServers() const override;

  void onRequestData(ensenso_camera_msgs::RequestDataMonoGoalConstPtr const& goal);

  void onLocatePattern(ensenso_camera_msgs::LocatePatternMonoGoalConstPtr const& goal);

  void onSetParameter(ensenso_camera_msgs::SetParameterGoalConstPtr const& goal) override;

  ros::Time capture() const override;

private:
  void updateCameraInfo() override;
  void fillCameraInfoFromNxLib(sensor_msgs::CameraInfoPtr const& info, bool rectified = false);

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

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

  std::vector<geometry_msgs::TransformStamped> estimatePatternPoses(ros::Time imageTimestamp = ros::Time::now(),
                                                                    std::string const& targetFrame = "") const override;
};