camera.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include "ensenso_camera_msgs/AccessTreeAction.h"
4 #include "ensenso_camera_msgs/CalibrateHandEyeAction.h"
5 #include "ensenso_camera_msgs/CalibrateWorkspaceAction.h"
6 #include "ensenso_camera_msgs/ExecuteCommandAction.h"
7 #include "ensenso_camera_msgs/FitPrimitiveAction.h"
8 #include "ensenso_camera_msgs/GetParameterAction.h"
9 #include "ensenso_camera_msgs/SetParameterAction.h"
14 
15 #include "nxLib.h"
16 
19 #include <ros/ros.h>
20 #include <sensor_msgs/CameraInfo.h>
24 #include <geometry_msgs/TransformStamped.h>
26 #include <diagnostic_msgs/DiagnosticArray.h>
27 
28 #include <map>
29 #include <memory>
30 #include <mutex>
31 #include <string>
32 #include <vector>
33 #include <fstream>
34 
39 double const STATUS_INTERVAL = 3.0; // Seconds.
40 
45 double const POSE_TF_INTERVAL = 1; // Seconds.
46 
50 double const TRANSFORMATION_REQUEST_TIMEOUT = 10.; // Seconds.
51 
56 std::string const DEFAULT_PARAMETER_SET = "default";
57 
61 std::string const TARGET_FRAME_LINK = "Workspace";
62 
63 // The ROS node gives back error codes from the NxLib. Additionally, we use the
64 // following error codes to indicate errors from the ROS node itself.
66 int const ERROR_CODE_TF = 101;
67 
68 #define LOG_NXLIB_EXCEPTION(EXCEPTION) \
69  try \
70  { \
71  if (EXCEPTION.getErrorCode() == NxLibExecutionFailed) \
72  { \
73  NxLibItem executionNode(EXCEPTION.getItemPath()); \
74  ROS_ERROR("%s: %s", executionNode[itmResult][itmErrorSymbol].asString().c_str(), \
75  executionNode[itmResult][itmErrorText].asString().c_str()); \
76  } /* NOLINT */ \
77  } /* NOLINT */ \
78  catch (...) \
79  { \
80  } /* NOLINT */ \
81  ROS_DEBUG("Current NxLib tree: %s", NxLibItem().asJson(true).c_str());
82 
83 // The following macros are called at the beginning and end of each action
84 // handler that uses the NxLib. In case of an NxLib exception they
85 // automatically abort the action and return the corresponding error code and
86 // message.
87 // This assumes that all of our actions have the property error that represents
88 // an NxLibException.
89 
90 #define START_NXLIB_ACTION(ACTION_NAME, ACTION_SERVER) \
91  ROS_DEBUG("Received a " #ACTION_NAME " request."); \
92  auto& server = ACTION_SERVER; \
93  if (server->isPreemptRequested()) \
94  { \
95  server->setPreempted(); \
96  return; \
97  } /* NOLINT */ \
98  std::lock_guard<std::mutex> lock(nxLibMutex); \
99  try \
100  {
101 #define FINISH_NXLIB_ACTION(ACTION_NAME) \
102  } /* NOLINT */ \
103  catch (NxLibException & e) \
104  { \
105  ROS_ERROR("NxLibException %d (%s) for item %s", e.getErrorCode(), e.getErrorText().c_str(), \
106  e.getItemPath().c_str()); \
107  LOG_NXLIB_EXCEPTION(e) \
108  ensenso_camera_msgs::ACTION_NAME##Result result; \
109  result.error.code = e.getErrorCode(); \
110  result.error.message = e.getErrorText(); \
111  server->setAborted(result); \
112  return; \
113  } /* NOLINT */ \
114  catch (tf2::TransformException & e) \
115  { \
116  ROS_ERROR("TF Exception: %s", e.what()); \
117  ensenso_camera_msgs::ACTION_NAME##Result result; \
118  result.error.code = ERROR_CODE_TF; \
119  result.error.message = e.what(); \
120  server->setAborted(result); \
121  return; \
122  } /* NOLINT */ \
123  catch (std::exception & e) \
124  { \
125  ROS_ERROR("Unknown Exception: %s", e.what()); \
126  ensenso_camera_msgs::ACTION_NAME##Result result; \
127  result.error.code = ERROR_CODE_UNKNOWN_EXCEPTION; \
128  result.error.message = e.what(); \
129  server->setAborted(result); \
130  return; \
131  }
132 
133 #define PREEMPT_ACTION_IF_REQUESTED \
134  if (server->isPreemptRequested()) \
135  { \
136  server->setPreempted(); \
137  return; \
138  }
139 
144 
150 {
155  NxLibItem node;
156 
160  bool useROI = false;
161 
167 
173  bool autoProjector = true;
174 
175  ParameterSet(std::string const& name, NxLibItem const& defaultParameters);
176 };
177 
178 class Camera
179 {
180 protected:
182  std::string fileCameraPath;
183  // Whether the camera was created by this node. If that is the case, we will
184  // delete it again after it got closed.
185  bool createdFileCamera = false;
186 
187  std::string serial;
188  NxLibItem cameraNode;
189 
190  // Controls parallel access to the NxLib.
191  mutable std::mutex nxLibMutex;
192 
193  // Whether the camera is fixed in the world or moves with a robot.
194  bool fixed;
195 
196  std::string cameraFrame;
197  std::string linkFrame;
198  std::string targetFrame;
199 
201 
205 
206  // tf buffer, that will store transformations for 10 seconds and dropping transformation afterwards.
208  std::unique_ptr<tf2_ros::TransformListener> transformListener;
209  std::unique_ptr<tf2_ros::TransformBroadcaster> transformBroadcaster;
210 
211  std::unique_ptr<AccessTreeServer> accessTreeServer;
212  std::unique_ptr<ExecuteCommandServer> executeCommandServer;
213  std::unique_ptr<GetParameterServer> getParameterServer;
214  std::unique_ptr<SetParameterServer> setParameterServer;
215 
216  // saves the latest transforms for a specific frame
217  mutable std::map<std::string, geometry_msgs::TransformStamped> transformationCache;
218 
219  // Contains a parameter tree that is used for creating new parameter sets.
220  NxLibItem defaultParameters;
221 
222  std::map<std::string, ParameterSet> parameterSets;
223  std::string currentParameterSet;
224 
225 public:
226  Camera(ros::NodeHandle const& n, std::string serial, std::string fileCameraPath, bool fixed, std::string cameraFrame,
227  std::string targetFrame, std::string linkFrame);
228 
229  virtual bool open();
230  void close();
231 
235  virtual void initTfPublishTimer();
236 
240  virtual void initStatusTimer();
241 
246  virtual void startServers() const;
247 
255  bool loadSettings(std::string const& jsonFile, bool saveAsDefaultParameters = false);
256 
260  void onAccessTree(ensenso_camera_msgs::AccessTreeGoalConstPtr const& goal);
261 
265  void onExecuteCommand(ensenso_camera_msgs::ExecuteCommandGoalConstPtr const& goal);
266 
270  void onGetParameter(ensenso_camera_msgs::GetParameterGoalConstPtr const& goal);
271 
275  virtual void onSetParameter(ensenso_camera_msgs::SetParameterGoalConstPtr const& goal) = 0;
276 
277 protected:
285  void saveParameterSet(std::string name);
286 
290  bool cameraIsAvailable() const;
291 
295  bool cameraIsOpen() const;
296 
301  virtual void publishStatus(ros::TimerEvent const& event) const;
302 
308  void saveDefaultParameterSet();
313  void loadParameterSet(std::string name);
314 
318  virtual ros::Time capture() const = 0;
319 
330  virtual geometry_msgs::TransformStamped estimatePatternPose(ros::Time imageTimestamp = ros::Time::now(),
331  std::string const& targetFrame = "",
332  bool latestPatternOnly = false) const;
333 
337  virtual std::vector<geometry_msgs::TransformStamped> estimatePatternPoses(ros::Time imageTimestamp = ros::Time::now(),
338  std::string const& targetFrame = "") const;
339 
349  void updateGlobalLink(ros::Time time = ros::Time::now(), std::string frame = "",
350  bool useCachedTransformation = false) const;
351 
357  void updateTransformations(tf2::Transform const& targetFrameTransformation) const;
358 
368  void fillBasicCameraInfoFromNxLib(sensor_msgs::CameraInfoPtr const& info) const;
373  virtual void updateCameraInfo() = 0;
374 
375  virtual ensenso_camera_msgs::ParameterPtr readParameter(std::string const& key) const;
376 
377  virtual void writeParameter(ensenso_camera_msgs::Parameter const& parameter);
378 
383  void publishCurrentLinks(ros::TimerEvent const& timerEvent = ros::TimerEvent());
384  void publishCameraLink();
385 
386  geometry_msgs::TransformStamped stampedLinkToCamera();
387  tf2::Transform getCameraToLinkTransform();
388 };
std::string const DEFAULT_PARAMETER_SET
Definition: camera.h:56
bool useROI
Definition: camera.h:160
std::map< std::string, ParameterSet > parameterSets
Definition: camera.h:222
NxLibItem node
Definition: camera.h:155
bool fixed
Definition: camera.h:194
std::string targetFrame
Definition: camera.h:198
ros::NodeHandle nh
Definition: camera.h:200
double const POSE_TF_INTERVAL
Definition: camera.h:45
std::string const TARGET_FRAME_LINK
Definition: camera.h:61
double const STATUS_INTERVAL
Definition: camera.h:39
bool autoProjector
Definition: camera.h:173
int const ERROR_CODE_UNKNOWN_EXCEPTION
Definition: camera.h:65
ros::Publisher statusPublisher
Definition: camera.h:202
ros::Timer cameraPosePublisher
Definition: camera.h:204
std::string cameraFrame
Definition: camera.h:196
NxLibItem defaultParameters
Definition: camera.h:220
std::mutex nxLibMutex
Definition: camera.h:191
double const TRANSFORMATION_REQUEST_TIMEOUT
Definition: camera.h:50
PointCloudROI roi
Definition: camera.h:166
std::unique_ptr< SetParameterServer > setParameterServer
Definition: camera.h:214
std::string serial
Definition: camera.h:187
std::string fileCameraPath
Definition: camera.h:182
std::unique_ptr< ExecuteCommandServer > executeCommandServer
Definition: camera.h:212
bool isFileCamera
Definition: camera.h:181
NxLibItem cameraNode
Definition: camera.h:188
tf2_ros::Buffer tfBuffer
Definition: camera.h:207
std::string linkFrame
Definition: camera.h:197
ParameterSet(std::string const &name, NxLibItem const &defaultParameters)
Definition: camera.cpp:9
static Time now()
ros::Timer statusTimer
Definition: camera.h:203
std::unique_ptr< AccessTreeServer > accessTreeServer
Definition: camera.h:211
std::map< std::string, geometry_msgs::TransformStamped > transformationCache
Definition: camera.h:217
std::unique_ptr< GetParameterServer > getParameterServer
Definition: camera.h:213
int const ERROR_CODE_TF
Definition: camera.h:66
Definition: camera.h:178
std::string currentParameterSet
Definition: camera.h:223
std::unique_ptr< tf2_ros::TransformListener > transformListener
Definition: camera.h:208
std::unique_ptr< tf2_ros::TransformBroadcaster > transformBroadcaster
Definition: camera.h:209


ensenso_camera
Author(s): Ensenso
autogenerated on Sat Jul 27 2019 03:51:24