nodelet_mono.cpp
Go to the documentation of this file.
2 
4 #include "nxLib.h"
5 
7 #include <string>
8 
9 namespace ensenso_camera
10 {
12 {
15 
16  NODELET_DEBUG("Initializing the NxLib...");
17  try
18  {
19  nxLibInitialize(true);
20  }
21  catch (NxLibException& e)
22  {
23  NODELET_ERROR("Error while initializing the NxLib. Shutting down.");
24  exit(EXIT_FAILURE);
25  }
26 
27  int threads;
28  if (nhLocal.getParam("threads", threads))
29  {
30  NxLibItem()[itmParameters][itmThreads] = threads;
31  }
32 
33  std::string serial, fileCameraPath, cameraFrame, targetFrame, linkFrame;
34  bool cameraIsFixed;
35 
36  if (!nhLocal.getParam("serial", serial))
37  {
38  // Try to get the serial as an integer, because rosparam sometimes
39  // automatically converts the serials to numbers.
40  int intSerial;
41  if (nhLocal.getParam("serial", intSerial))
42  {
43  serial = std::to_string(intSerial);
44  }
45 
46  // For long serial numbers that exceed the range of an integer with 32-bits, we can only determine the serial number
47  // if it is passed as a numeric string. For this you have to append a "!" to the number so that it can be
48  // interpreted as a string.
49 
50  NxLibItem cameraNode = NxLibItem()[itmCameras][itmBySerialNo][serial];
51  if (!cameraNode.exists())
52  {
53  NODELET_WARN("The serial of the camera has been too long and was interpreted as an 32-bit integer and exceeds "
54  "its length. Please append a \"!\" to the number. E.g. \'_serial:=1234567\' to "
55  "\'_serial:=1234567!\', so it can be interpreted as a numerical string. If you are using a launch "
56  "file, just define the parameter's type as string, e.g.: type=\"string\".");
57  NODELET_ERROR("Could not find any camera. Shutting down.");
58  nxLibFinalize();
59  exit(EXIT_FAILURE);
60  }
61  }
62 
63  // Check if string contains the "!" and delete it.
64  std::size_t found = serial.find('!');
65  if (found != std::string::npos)
66  {
67  serial.erase(found);
68  }
69 
70  if (serial.empty())
71  {
72  NxLibItem cameras = NxLibItem()[itmCameras][itmBySerialNo];
73  // Try to find the first monocular camera.
74  bool foundAppropriateCamera = false;
75  for (int i = 0; i < cameras.count(); i++)
76  {
77  if (cameras[i][itmType].exists() && cameras[i][itmType].asString() == valMonocular)
78  {
79  foundAppropriateCamera = true;
80  serial = cameras[i].name();
81  break;
82  }
83  }
84  if (!foundAppropriateCamera)
85  {
86  NODELET_ERROR("Could not find any camera. Shutting down.");
87  nxLibFinalize();
88  exit(EXIT_FAILURE);
89  }
90  }
91 
92  nhLocal.param<std::string>("file_camera_path", fileCameraPath, "");
93 
94  // We can only check the type with file cameras, when we create file cameras, so for now we can only check whether the
95  // current physical camera is of the right type
96  if (fileCameraPath.empty())
97  {
98  std::string type = NxLibItem()[itmCameras][itmBySerialNo][serial][itmType].asString();
99  std::string const neededType = valMonocular;
100  if (type != neededType)
101  {
102  NODELET_ERROR("The camera to be opened is of the wrong type %s. It should be %s.", type.c_str(),
103  neededType.c_str());
104  nxLibFinalize();
105  exit(EXIT_FAILURE);
106  }
107  }
108 
109  nhLocal.param("fixed", cameraIsFixed, false);
110 
111  if (!nhLocal.getParam("camera_frame", cameraFrame))
112  {
113  cameraFrame = serial + "_optical_frame";
114  }
115 
116  nhLocal.getParam("target_frame", targetFrame);
117  nhLocal.getParam("link_frame", linkFrame);
118  if (!targetFrame.empty() && linkFrame.empty())
119  {
120  linkFrame = targetFrame;
121  }
122  else if (targetFrame.empty() && linkFrame.empty())
123  {
124  targetFrame = cameraFrame;
125  linkFrame = cameraFrame;
126  }
127 
128  camera = ::make_unique<MonoCamera>(nh, serial, fileCameraPath, cameraIsFixed, cameraFrame, targetFrame, linkFrame);
129  if (!camera->open())
130  {
131  NODELET_ERROR("Failed to open the camera. Shutting down.");
132  nxLibFinalize();
133  exit(EXIT_FAILURE);
134  }
135 
136  std::string settingsFile;
137  if (nhLocal.getParam("settings", settingsFile))
138  {
139  NODELET_DEBUG("Loading camera settings...");
140  if (!camera->loadSettings(settingsFile, true))
141  {
142  NODELET_ERROR("Failed to load the camera settings. Shutting down.");
143  nxLibFinalize();
144  exit(EXIT_FAILURE);
145  }
146  }
147 
148  camera->startServers();
149  camera->initTfPublishTimer();
150 }
151 
153 {
154  camera->close();
155  nxLibFinalize();
156 }
157 
158 } // namespace ensenso_camera
159 
#define NODELET_ERROR(...)
#define NODELET_WARN(...)
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
ros::NodeHandle & getPrivateNodeHandle() const
std::unique_ptr< MonoCamera > camera
Definition: nodelet_mono.h:14
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::NodeHandle & getNodeHandle() const
bool getParam(const std::string &key, std::string &s) const
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
#define NODELET_DEBUG(...)


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