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 waitForCamera = 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();
};