camera.h

Go to the documentation of this file.

#pragma once

#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/SetParameterAction.h"
#include "ensenso_camera/calibration_pattern.h"
#include "ensenso_camera/point_cloud_utilities.h"
#include "ensenso_camera/queued_action_server.h"
#include "ensenso_camera/image_utilities.h"
#include "ensenso_camera/nxLibVersionInfo.h"

#include "nxLib.h"

#include <tf2/LinearMath/Transform.h>
#include <image_transport/image_transport.h>
#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Quaternion.h>
#include <diagnostic_msgs/DiagnosticArray.h>

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

double const STATUS_INTERVAL = 3.0;  // Seconds.

double const POSE_TF_INTERVAL = 1;  // Seconds.

double const TRANSFORMATION_REQUEST_TIMEOUT = 10.;  // Seconds.

std::string const DEFAULT_PARAMETER_SET = "default";

std::string const TARGET_FRAME_LINK = "Workspace";

// The ROS node gives back error codes from the NxLib. Additionally, we use the
// following error codes to indicate errors from the ROS node itself.
int const ERROR_CODE_UNKNOWN_EXCEPTION = 100;
int const ERROR_CODE_TF = 101;

#define LOG_NXLIB_EXCEPTION(EXCEPTION)                                                                                 \
  try                                                                                                                  \
  {                                                                                                                    \
    if (EXCEPTION.getErrorCode() == NxLibExecutionFailed)                                                              \
    {                                                                                                                  \
      NxLibItem executionNode(EXCEPTION.getItemPath());                                                                \
      ROS_ERROR("%s: %s", executionNode[itmResult][itmErrorSymbol].asString().c_str(),                                 \
                executionNode[itmResult][itmErrorText].asString().c_str());                                            \
    } /* NOLINT */                                                                                                     \
  }   /* NOLINT */                                                                                                     \
  catch (...)                                                                                                          \
  {                                                                                                                    \
  } /* NOLINT */                                                                                                       \
  ROS_DEBUG("Current NxLib tree: %s", NxLibItem().asJson(true).c_str());

// The following macros are called at the beginning and end of each action
// handler that uses the NxLib. In case of an NxLib exception they
// automatically abort the action and return the corresponding error code and
// message.
// This assumes that all of our actions have the property error that represents
// an NxLibException.

#define START_NXLIB_ACTION(ACTION_NAME, ACTION_SERVER)                                                                 \
  ROS_DEBUG("Received a " #ACTION_NAME " request.");                                                                   \
  auto& server = ACTION_SERVER;                                                                                        \
  if (server->isPreemptRequested())                                                                                    \
  {                                                                                                                    \
    server->setPreempted();                                                                                            \
    return;                                                                                                            \
  } /* NOLINT */                                                                                                       \
  std::lock_guard<std::mutex> lock(nxLibMutex);                                                                        \
  try                                                                                                                  \
  {
#define FINISH_NXLIB_ACTION(ACTION_NAME)                                                                               \
  } /* NOLINT */                                                                                                       \
  catch (NxLibException & e)                                                                                           \
  {                                                                                                                    \
    ROS_ERROR("NxLibException %d (%s) for item %s", e.getErrorCode(), e.getErrorText().c_str(),                        \
              e.getItemPath().c_str());                                                                                \
    LOG_NXLIB_EXCEPTION(e)                                                                                             \
    ensenso_camera_msgs::ACTION_NAME##Result result;                                                                   \
    result.error.code = e.getErrorCode();                                                                              \
    result.error.message = e.getErrorText();                                                                           \
    server->setAborted(result);                                                                                        \
    return;                                                                                                            \
  } /* NOLINT */                                                                                                       \
  catch (tf2::TransformException & e)                                                                                  \
  {                                                                                                                    \
    ROS_ERROR("TF Exception: %s", e.what());                                                                           \
    ensenso_camera_msgs::ACTION_NAME##Result result;                                                                   \
    result.error.code = ERROR_CODE_TF;                                                                                 \
    result.error.message = e.what();                                                                                   \
    server->setAborted(result);                                                                                        \
    return;                                                                                                            \
  } /* NOLINT */                                                                                                       \
  catch (std::exception & e)                                                                                           \
  {                                                                                                                    \
    ROS_ERROR("Unknown Exception: %s", e.what());                                                                      \
    ensenso_camera_msgs::ACTION_NAME##Result result;                                                                   \
    result.error.code = ERROR_CODE_UNKNOWN_EXCEPTION;                                                                  \
    result.error.message = e.what();                                                                                   \
    server->setAborted(result);                                                                                        \
    return;                                                                                                            \
  }

#define PREEMPT_ACTION_IF_REQUESTED                                                                                    \
  if (server->isPreemptRequested())                                                                                    \
  {                                                                                                                    \
    server->setPreempted();                                                                                            \
    return;                                                                                                            \
  }

using AccessTreeServer = QueuedActionServer<ensenso_camera_msgs::AccessTreeAction>;
using ExecuteCommandServer = QueuedActionServer<ensenso_camera_msgs::ExecuteCommandAction>;
using GetParameterServer = QueuedActionServer<ensenso_camera_msgs::GetParameterAction>;
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);
};

class Camera
{
protected:
  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 = false;

  VersionInfo nxLibVersion;

  std::string serial;
  NxLibItem cameraNode;

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

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

  std::string cameraFrame;
  std::string linkFrame;
  std::string targetFrame;

  ros::NodeHandle nh;

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

  // tf buffer, that will store transformations for 10 seconds and dropping transformation afterwards.
  tf2_ros::Buffer tfBuffer;
  std::unique_ptr<tf2_ros::TransformListener> transformListener;
  std::unique_ptr<tf2_ros::TransformBroadcaster> transformBroadcaster;

  std::unique_ptr<AccessTreeServer> accessTreeServer;
  std::unique_ptr<ExecuteCommandServer> executeCommandServer;
  std::unique_ptr<GetParameterServer> getParameterServer;
  std::unique_ptr<SetParameterServer> setParameterServer;

  // saves the latest transforms for a specific frame
  mutable std::map<std::string, geometry_msgs::TransformStamped> transformationCache;

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

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

public:
  Camera(ros::NodeHandle const& n, std::string serial, std::string fileCameraPath, bool fixed, std::string cameraFrame,
         std::string targetFrame, std::string linkFrame);

  virtual bool open();
  void close();

  virtual void initTfPublishTimer();

  virtual void initStatusTimer();

  virtual 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);

  virtual void onSetParameter(ensenso_camera_msgs::SetParameterGoalConstPtr const& goal) = 0;

protected:
  void saveParameterSet(std::string name);

  bool cameraIsAvailable() const;

  bool cameraIsOpen() const;

  virtual void publishStatus(ros::TimerEvent const& event) const;

  void saveDefaultParameterSet();
  void loadParameterSet(std::string name);

  virtual ros::Time capture() const = 0;

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

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

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

  void updateTransformations(tf2::Transform const& targetFrameTransformation) const;

  void fillBasicCameraInfoFromNxLib(sensor_msgs::CameraInfoPtr const& info) const;
  virtual void updateCameraInfo() = 0;

  virtual ensenso_camera_msgs::ParameterPtr readParameter(std::string const& key) const;

  virtual void writeParameter(ensenso_camera_msgs::Parameter const& parameter);

  void publishCurrentLinks(ros::TimerEvent const& timerEvent = ros::TimerEvent());
  void publishCameraLink();

  geometry_msgs::TransformStamped stampedLinkToCamera();
  tf2::Transform getCameraToLinkTransform();
};