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"
15 
16 #include "nxLib.h"
17 
20 #include <ros/ros.h>
21 #include <sensor_msgs/CameraInfo.h>
25 #include <geometry_msgs/TransformStamped.h>
27 #include <diagnostic_msgs/DiagnosticArray.h>
28 
29 #include <map>
30 #include <memory>
31 #include <mutex>
32 #include <string>
33 #include <vector>
34 #include <fstream>
35 
40 double const STATUS_INTERVAL = 3.0; // Seconds.
41 
46 double const POSE_TF_INTERVAL = 1; // Seconds.
47 
51 double const TRANSFORMATION_REQUEST_TIMEOUT = 10.; // Seconds.
52 
57 std::string const DEFAULT_PARAMETER_SET = "default";
58 
62 std::string const TARGET_FRAME_LINK = "Workspace";
63 
64 // The ROS node gives back error codes from the NxLib. Additionally, we use the
65 // following error codes to indicate errors from the ROS node itself.
67 int const ERROR_CODE_TF = 101;
68 
69 #define LOG_NXLIB_EXCEPTION(EXCEPTION) \
70  try \
71  { \
72  if (EXCEPTION.getErrorCode() == NxLibExecutionFailed) \
73  { \
74  NxLibItem executionNode(EXCEPTION.getItemPath()); \
75  ROS_ERROR("%s: %s", executionNode[itmResult][itmErrorSymbol].asString().c_str(), \
76  executionNode[itmResult][itmErrorText].asString().c_str()); \
77  } /* NOLINT */ \
78  } /* NOLINT */ \
79  catch (...) \
80  { \
81  } /* NOLINT */ \
82  ROS_DEBUG("Current NxLib tree: %s", NxLibItem().asJson(true).c_str());
83 
84 // The following macros are called at the beginning and end of each action
85 // handler that uses the NxLib. In case of an NxLib exception they
86 // automatically abort the action and return the corresponding error code and
87 // message.
88 // This assumes that all of our actions have the property error that represents
89 // an NxLibException.
90 
91 #define START_NXLIB_ACTION(ACTION_NAME, ACTION_SERVER) \
92  ROS_DEBUG("Received a " #ACTION_NAME " request."); \
93  auto& server = ACTION_SERVER; \
94  if (server->isPreemptRequested()) \
95  { \
96  server->setPreempted(); \
97  return; \
98  } /* NOLINT */ \
99  std::lock_guard<std::mutex> lock(nxLibMutex); \
100  try \
101  {
102 #define FINISH_NXLIB_ACTION(ACTION_NAME) \
103  } /* NOLINT */ \
104  catch (NxLibException & e) \
105  { \
106  ROS_ERROR("NxLibException %d (%s) for item %s", e.getErrorCode(), e.getErrorText().c_str(), \
107  e.getItemPath().c_str()); \
108  LOG_NXLIB_EXCEPTION(e) \
109  ensenso_camera_msgs::ACTION_NAME##Result result; \
110  result.error.code = e.getErrorCode(); \
111  result.error.message = e.getErrorText(); \
112  server->setAborted(result); \
113  return; \
114  } /* NOLINT */ \
115  catch (tf2::TransformException & e) \
116  { \
117  ROS_ERROR("TF Exception: %s", e.what()); \
118  ensenso_camera_msgs::ACTION_NAME##Result result; \
119  result.error.code = ERROR_CODE_TF; \
120  result.error.message = e.what(); \
121  server->setAborted(result); \
122  return; \
123  } /* NOLINT */ \
124  catch (std::exception & e) \
125  { \
126  ROS_ERROR("Unknown Exception: %s", e.what()); \
127  ensenso_camera_msgs::ACTION_NAME##Result result; \
128  result.error.code = ERROR_CODE_UNKNOWN_EXCEPTION; \
129  result.error.message = e.what(); \
130  server->setAborted(result); \
131  return; \
132  }
133 
134 #define PREEMPT_ACTION_IF_REQUESTED \
135  if (server->isPreemptRequested()) \
136  { \
137  server->setPreempted(); \
138  return; \
139  }
140 
145 
151 {
156  NxLibItem node;
157 
161  bool useROI = false;
162 
168 
174  bool autoProjector = true;
175 
176  ParameterSet(std::string const& name, NxLibItem const& defaultParameters);
177 };
178 
179 class Camera
180 {
181 protected:
183  std::string fileCameraPath;
184  // Whether the camera was created by this node. If that is the case, we will
185  // delete it again after it got closed.
186  bool createdFileCamera = false;
187 
189 
190  std::string serial;
191  NxLibItem cameraNode;
192 
193  // Controls parallel access to the NxLib.
194  mutable std::mutex nxLibMutex;
195 
196  // Whether the camera is fixed in the world or moves with a robot.
197  bool fixed;
198 
199  std::string cameraFrame;
200  std::string linkFrame;
201  std::string targetFrame;
202 
204 
208 
209  // tf buffer, that will store transformations for 10 seconds and dropping transformation afterwards.
211  std::unique_ptr<tf2_ros::TransformListener> transformListener;
212  std::unique_ptr<tf2_ros::TransformBroadcaster> transformBroadcaster;
213 
214  std::unique_ptr<AccessTreeServer> accessTreeServer;
215  std::unique_ptr<ExecuteCommandServer> executeCommandServer;
216  std::unique_ptr<GetParameterServer> getParameterServer;
217  std::unique_ptr<SetParameterServer> setParameterServer;
218 
219  // saves the latest transforms for a specific frame
220  mutable std::map<std::string, geometry_msgs::TransformStamped> transformationCache;
221 
222  // Contains a parameter tree that is used for creating new parameter sets.
223  NxLibItem defaultParameters;
224 
225  std::map<std::string, ParameterSet> parameterSets;
226  std::string currentParameterSet;
227 
228 public:
229  Camera(ros::NodeHandle const& n, std::string serial, std::string fileCameraPath, bool fixed, std::string cameraFrame,
230  std::string targetFrame, std::string linkFrame);
231 
232  virtual bool open();
233  void close();
234 
238  virtual void initTfPublishTimer();
239 
243  virtual void initStatusTimer();
244 
249  virtual void startServers() const;
250 
258  bool loadSettings(std::string const& jsonFile, bool saveAsDefaultParameters = false);
259 
263  void onAccessTree(ensenso_camera_msgs::AccessTreeGoalConstPtr const& goal);
264 
268  void onExecuteCommand(ensenso_camera_msgs::ExecuteCommandGoalConstPtr const& goal);
269 
273  void onGetParameter(ensenso_camera_msgs::GetParameterGoalConstPtr const& goal);
274 
278  virtual void onSetParameter(ensenso_camera_msgs::SetParameterGoalConstPtr const& goal) = 0;
279 
280 protected:
288  void saveParameterSet(std::string name);
289 
293  bool cameraIsAvailable() const;
294 
298  bool cameraIsOpen() const;
299 
304  virtual void publishStatus(ros::TimerEvent const& event) const;
305 
311  void saveDefaultParameterSet();
316  void loadParameterSet(std::string name);
317 
321  virtual ros::Time capture() const = 0;
322 
333  virtual geometry_msgs::TransformStamped estimatePatternPose(ros::Time imageTimestamp = ros::Time::now(),
334  std::string const& targetFrame = "",
335  bool latestPatternOnly = false) const;
336 
340  virtual std::vector<geometry_msgs::TransformStamped> estimatePatternPoses(ros::Time imageTimestamp = ros::Time::now(),
341  std::string const& targetFrame = "") const;
342 
352  void updateGlobalLink(ros::Time time = ros::Time::now(), std::string frame = "",
353  bool useCachedTransformation = false) const;
354 
360  void updateTransformations(tf2::Transform const& targetFrameTransformation) const;
361 
371  void fillBasicCameraInfoFromNxLib(sensor_msgs::CameraInfoPtr const& info) const;
376  virtual void updateCameraInfo() = 0;
377 
378  virtual ensenso_camera_msgs::ParameterPtr readParameter(std::string const& key) const;
379 
380  virtual void writeParameter(ensenso_camera_msgs::Parameter const& parameter);
381 
386  void publishCurrentLinks(ros::TimerEvent const& timerEvent = ros::TimerEvent());
387  void publishCameraLink();
388 
389  geometry_msgs::TransformStamped stampedLinkToCamera();
390  tf2::Transform getCameraToLinkTransform();
391 };
std::string const DEFAULT_PARAMETER_SET
Definition: camera.h:57
bool useROI
Definition: camera.h:161
std::map< std::string, ParameterSet > parameterSets
Definition: camera.h:225
NxLibItem node
Definition: camera.h:156
bool fixed
Definition: camera.h:197
std::string targetFrame
Definition: camera.h:201
ros::NodeHandle nh
Definition: camera.h:203
double const POSE_TF_INTERVAL
Definition: camera.h:46
std::string const TARGET_FRAME_LINK
Definition: camera.h:62
double const STATUS_INTERVAL
Definition: camera.h:40
bool autoProjector
Definition: camera.h:174
int const ERROR_CODE_UNKNOWN_EXCEPTION
Definition: camera.h:66
ros::Publisher statusPublisher
Definition: camera.h:205
ros::Timer cameraPosePublisher
Definition: camera.h:207
std::string cameraFrame
Definition: camera.h:199
NxLibItem defaultParameters
Definition: camera.h:223
std::mutex nxLibMutex
Definition: camera.h:194
double const TRANSFORMATION_REQUEST_TIMEOUT
Definition: camera.h:51
PointCloudROI roi
Definition: camera.h:167
std::unique_ptr< SetParameterServer > setParameterServer
Definition: camera.h:217
std::string serial
Definition: camera.h:190
std::string fileCameraPath
Definition: camera.h:183
std::unique_ptr< ExecuteCommandServer > executeCommandServer
Definition: camera.h:215
bool isFileCamera
Definition: camera.h:182
NxLibItem cameraNode
Definition: camera.h:191
VersionInfo nxLibVersion
Definition: camera.h:188
tf2_ros::Buffer tfBuffer
Definition: camera.h:210
std::string linkFrame
Definition: camera.h:200
ParameterSet(std::string const &name, NxLibItem const &defaultParameters)
Definition: camera.cpp:9
static Time now()
ros::Timer statusTimer
Definition: camera.h:206
std::unique_ptr< AccessTreeServer > accessTreeServer
Definition: camera.h:214
std::map< std::string, geometry_msgs::TransformStamped > transformationCache
Definition: camera.h:220
std::unique_ptr< GetParameterServer > getParameterServer
Definition: camera.h:216
int const ERROR_CODE_TF
Definition: camera.h:67
Definition: camera.h:179
std::string currentParameterSet
Definition: camera.h:226
std::unique_ptr< tf2_ros::TransformListener > transformListener
Definition: camera.h:211
std::unique_ptr< tf2_ros::TransformBroadcaster > transformBroadcaster
Definition: camera.h:212


ensenso_camera
Author(s): Ensenso
autogenerated on Thu Feb 6 2020 03:48:46