camera.h

Go to the documentation of this file.

#pragma once

#include "ensenso_camera/calibration_pattern.h"
#include "ensenso_camera/image_utilities.h"
#include "ensenso_camera/nxlib_version.h"
#include "ensenso_camera/point_cloud_utilities.h"
#include "ensenso_camera/queued_action_server.h"
#include "ensenso_camera/string_helper.h"
#include "ensenso_camera/virtual_object_handler.h"

#include "ensenso_camera/ros2/action_server.h"
#include "ensenso_camera/ros2/core.h"
#include "ensenso_camera/ros2/logging.h"
#include "ensenso_camera/ros2/namespace.h"
#include "ensenso_camera/ros2/node_handle.h"
#include "ensenso_camera/ros2/time.h"
#include "ensenso_camera/ros2/image_transport.h"

#include "ensenso_camera/ros2/diagnostic_msgs/diagnostic_array.h"
#include "ensenso_camera/ros2/ensenso_msgs/access_tree.h"
#include "ensenso_camera/ros2/ensenso_msgs/execute_command.h"
#include "ensenso_camera/ros2/ensenso_msgs/get_parameter.h"
#include "ensenso_camera/ros2/ensenso_msgs/parameter.h"
#include "ensenso_camera/ros2/ensenso_msgs/set_parameter.h"
#include "ensenso_camera/ros2/geometry_msgs/pose_stamped.h"
#include "ensenso_camera/ros2/geometry_msgs/transform_stamped.h"
#include "ensenso_camera/ros2/sensor_msgs/camera_info.h"

#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>

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

#include "nxLib.h"

double const STATUS_INTERVAL = 3.0;  // Seconds.

double const POSE_TF_INTERVAL = 1;  // Seconds.

double const TF_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());                                                                \
      ENSENSO_ERROR(nh, "%s: %s", executionNode[itmResult][itmErrorSymbol].asString().c_str(),                         \
                    executionNode[itmResult][itmErrorText].asString().c_str());                                        \
    } /* NOLINT */                                                                                                     \
  }   /* NOLINT */                                                                                                     \
  catch (...)                                                                                                          \
  {                                                                                                                    \
  } /* NOLINT */                                                                                                       \
  ENSENSO_DEBUG(nh, "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)                                                                 \
  ENSENSO_DEBUG(nh, "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)                                                                                           \
  {                                                                                                                    \
    ENSENSO_ERROR(nh, "NxLibException %d (%s) for item %s", e.getErrorCode(), e.getErrorText().c_str(),                \
                  e.getItemPath().c_str());                                                                            \
    LOG_NXLIB_EXCEPTION(e)                                                                                             \
    ensenso::action::ACTION_NAME##Result result;                                                                       \
    result.error.code = e.getErrorCode();                                                                              \
    result.error.message = e.getErrorText();                                                                           \
    server->setAborted(std::move(result));                                                                             \
    return;                                                                                                            \
  } /* NOLINT */                                                                                                       \
  catch (tf2::TransformException & e)                                                                                  \
  {                                                                                                                    \
    ENSENSO_ERROR(nh, "tf Exception: %s", e.what());                                                                   \
    ensenso::action::ACTION_NAME##Result result;                                                                       \
    result.error.code = ERROR_CODE_TF;                                                                                 \
    result.error.message = e.what();                                                                                   \
    server->setAborted(std::move(result));                                                                             \
    return;                                                                                                            \
  } /* NOLINT */                                                                                                       \
  catch (std::exception & e)                                                                                           \
  {                                                                                                                    \
    ENSENSO_ERROR(nh, "Unknown Exception: %s", e.what());                                                              \
    ensenso::action::ACTION_NAME##Result result;                                                                       \
    result.error.code = ERROR_CODE_UNKNOWN_EXCEPTION;                                                                  \
    result.error.message = e.what();                                                                                   \
    server->setAborted(std::move(result));                                                                             \
    return;                                                                                                            \
  }

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

struct ParameterSet
{
  NxLibItem node;

  bool useROI = false;

  PointCloudROI roi;

  bool autoProjector = true;

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

struct CameraParameters
{
  std::string serial;

  bool isFileCamera = false;

  std::string fileCameraPath;

  bool fixed = false;

  bool wait_for_camera = false;

  std::string cameraFrame;

  std::string linkFrame;

  std::string targetFrame;

  std::string robotFrame;

  std::string wristFrame;

  int captureTimeout = 0;

  std::unique_ptr<ensenso_camera::VirtualObjectHandler> virtualObjectHandler = nullptr;

  CameraParameters(ensenso::ros::NodeHandle& nh, std::string const& cameraType, std::string serial);
};

class Camera
{
protected:
  CameraParameters params;

  // Whether the camera is a file camera and was created by this node. If it was created by this node, it will be
  // deleted after it got closed.
  bool createdFileCamera = false;

  NxLibVersion nxLibVersion;

  NxLibItem cameraNode;

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

  ensenso::ros::NodeHandle nh;

  ensenso::ros::Publisher<diagnostic_msgs::msg::DiagnosticArray> statusPublisher;
  ensenso::ros::Timer statusTimer;
  ensenso::ros::Timer cameraPosePublisher;

  // tf buffer, that will store transformations for 10 seconds and dropping transformation afterwards.
  std::unique_ptr<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::msg::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(ensenso::ros::NodeHandle& nh, CameraParameters params);

  bool open();

  virtual void updateCameraTypeSpecifics()
  {
  }

  virtual void init() = 0;

  void close();

  virtual void startServers() const;

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

  void onAccessTree(ensenso::action::AccessTreeGoalConstPtr const& goal);

  void onExecuteCommand(ensenso::action::ExecuteCommandGoalConstPtr const& goal);

  void onGetParameter(ensenso::action::GetParameterGoalConstPtr const& goal);

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

protected:
  virtual void initTfPublishTimer();

  virtual void initStatusTimer();

  void saveParameterSet(std::string name);

  bool cameraIsAvailable() const;

  bool cameraIsOpen() const;

  bool hasLink() const;

  void publishStatus(TIMER_CALLBACK_DECLARATION_ARGS);

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

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

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

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

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

  std::string getNxLibTargetFrameName() const;

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

  void fillBasicCameraInfoFromNxLib(sensor_msgs::msg::CameraInfoPtr const& info) const;

  virtual void updateCameraInfo() = 0;

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

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

  void publishCurrentLinks(TIMER_CALLBACK_DECLARATION_ARGS);

  void publishCameraLink();

  geometry_msgs::msg::TransformStamped stampedLinkToCamera();

  tf2::Transform getCameraToLinkTransform();
};