camera.h

Go to the documentation of this file.

#pragma once

#include <image_transport/image_transport.h>
#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>

#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <vector>

#include <ensenso_camera_msgs/AccessTreeAction.h>
#include <ensenso_camera_msgs/CalibrateHandEyeAction.h>
#include <ensenso_camera_msgs/CalibrateWorkspaceAction.h>
#include <ensenso_camera_msgs/ExecuteCommandAction.h>
#include <ensenso_camera_msgs/FitPrimitiveAction.h>
#include <ensenso_camera_msgs/GetParameterAction.h>
#include <ensenso_camera_msgs/LocatePatternAction.h>
#include <ensenso_camera_msgs/ProjectPatternAction.h>
#include <ensenso_camera_msgs/RequestDataAction.h>
#include <ensenso_camera_msgs/SetParameterAction.h>

#include "ensenso_camera/calibration_pattern.h"
#include "ensenso_camera/point_cloud_utilities.h"
#include "ensenso_camera/queued_action_server.h"

#include "nxLib.h"

using AccessTreeServer = QueuedActionServer<ensenso_camera_msgs::AccessTreeAction>;
using CalibrateHandEyeServer = QueuedActionServer<ensenso_camera_msgs::CalibrateHandEyeAction>;
using CalibrateWorkspaceServer = QueuedActionServer<ensenso_camera_msgs::CalibrateWorkspaceAction>;
using ExecuteCommandServer = QueuedActionServer<ensenso_camera_msgs::ExecuteCommandAction>;
using FitPrimitiveServer = QueuedActionServer<ensenso_camera_msgs::FitPrimitiveAction>;
using GetParameterServer = QueuedActionServer<ensenso_camera_msgs::GetParameterAction>;
using LocatePatternServer = QueuedActionServer<ensenso_camera_msgs::LocatePatternAction>;
using ProjectPatternServer = QueuedActionServer<ensenso_camera_msgs::ProjectPatternAction>;
using RequestDataServer = QueuedActionServer<ensenso_camera_msgs::RequestDataAction>;
using SetParameterServer = QueuedActionServer<ensenso_camera_msgs::SetParameterAction>;

struct ParameterSet
{
  NxLibItem node;

  bool useROI = false;

  PointCloudROI roi;

  bool autoProjector = true;

  ParameterSet(std::string const& name, NxLibItem const& defaultParameters);
};

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 Camera
{
private:
  std::string serial;
  NxLibItem cameraNode;

  bool isFileCamera;
  std::string fileCameraPath;
  // Whether the camera was created by this node. If that is the case, we will
  // delete it again after it got closed.
  bool createdFileCamera;

  // Whether the camera is fixed in the world or moves with a robot.
  bool fixed;

  std::string cameraFrame;
  std::string targetFrame;
  std::string robotFrame;
  std::string wristFrame;

  // Controls parallel access to the NxLib.
  mutable std::mutex nxLibMutex;

  sensor_msgs::CameraInfoPtr leftCameraInfo;
  sensor_msgs::CameraInfoPtr rightCameraInfo;
  sensor_msgs::CameraInfoPtr leftRectifiedCameraInfo;
  sensor_msgs::CameraInfoPtr rightRectifiedCameraInfo;

  tf::TransformListener transformListener;
  tf::TransformBroadcaster transformBroadcaster;

  std::unique_ptr<AccessTreeServer> accessTreeServer;
  std::unique_ptr<ExecuteCommandServer> executeCommandServer;
  std::unique_ptr<GetParameterServer> getParameterServer;
  std::unique_ptr<SetParameterServer> setParameterServer;
  std::unique_ptr<RequestDataServer> requestDataServer;
  std::unique_ptr<LocatePatternServer> locatePatternServer;
  std::unique_ptr<ProjectPatternServer> projectPatternServer;
  std::unique_ptr<CalibrateHandEyeServer> calibrateHandEyeServer;
  std::unique_ptr<CalibrateWorkspaceServer> calibrateWorkspaceServer;
  std::unique_ptr<FitPrimitiveServer> fitPrimitiveServer;

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

  ros::Publisher pointCloudPublisher;

  ros::Publisher statusPublisher;
  ros::Timer statusTimer;

  // Contains a parameter tree that is used for creating new parameter sets.
  NxLibItem defaultParameters;

  std::map<std::string, ParameterSet> parameterSets;
  std::string currentParameterSet;

  mutable std::map<std::string, tf::StampedTransform> transformationCache;

  // 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<tf::Pose> handEyeCalibrationRobotPoses;

public:
  Camera(ros::NodeHandle nh, std::string const& serial, std::string const& fileCameraPath, bool fixed,
         std::string const& cameraFrame, std::string const& targetFrame, std::string const& robotFrame,
         std::string const& wristFrame);

  bool open();
  void close();

  void startServers() const;

  bool loadSettings(std::string const& jsonFile, bool saveAsDefaultParameters = false);

  void onAccessTree(ensenso_camera_msgs::AccessTreeGoalConstPtr const& goal);
  void onExecuteCommand(ensenso_camera_msgs::ExecuteCommandGoalConstPtr const& goal);

  void onGetParameter(ensenso_camera_msgs::GetParameterGoalConstPtr const& goal);
  void onSetParameter(ensenso_camera_msgs::SetParameterGoalConstPtr const& goal);

  void onRequestData(ensenso_camera_msgs::RequestDataGoalConstPtr const& goal);
  void onLocatePattern(ensenso_camera_msgs::LocatePatternGoalConstPtr const& goal);
  void onProjectPattern(ensenso_camera_msgs::ProjectPatternGoalConstPtr const& goal);
  void onCalibrateHandEye(ensenso_camera_msgs::CalibrateHandEyeGoalConstPtr const& goal);
  void onCalibrateWorkspace(ensenso_camera_msgs::CalibrateWorkspaceGoalConstPtr const& goal);
  void onFitPrimitive(ensenso_camera_msgs::FitPrimitiveGoalConstPtr const& goal);

private:
  bool cameraIsAvailable() const;

  bool cameraIsOpen() const;
  void publishStatus(ros::TimerEvent const& event) const;

  void saveDefaultParameterSet();

  void saveParameterSet(std::string name, bool projectorWritten = false);

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

  ros::Time capture() const;

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

  tf::Stamped<tf::Pose> estimatePatternPose(ros::Time imageTimestamp = ros::Time::now(),
                                            std::string const& targetFrame = "", bool latestPatternOnly = false) const;

  std::vector<tf::Stamped<tf::Pose>> estimatePatternPoses(ros::Time imageTimestamp = ros::Time::now(),
                                                          std::string const& targetFrame = "") const;

  void updateTransformations(ros::Time time = ros::Time::now(), std::string targetFrame = "",
                             bool useCachedTransformation = false) const;

  void updateTransformations(tf::Pose const& targetFrameTransformation) const;

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

  ensenso_camera_msgs::ParameterPtr readParameter(std::string const& key) const;
  void writeParameter(ensenso_camera_msgs::Parameter const& parameter);
};