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  // No serial specified. Try to use the first camera in the tree.
73  NxLibItem cameras = NxLibItem()[itmCameras][itmBySerialNo];
74  if (cameras.count() > 0)
75  {
76  serial = cameras[0].name();
77  }
78  else
79  {
80  NODELET_ERROR("Could not find any camera. Shutting down.");
81  nxLibFinalize();
82  exit(EXIT_FAILURE);
83  }
84  }
85 
86  nhLocal.param<std::string>("file_camera_path", fileCameraPath, "");
87 
88  // We can only check the type with file cameras, when we create file cameras, so for now we can only check whether the
89  // current physical camera is of the right type
90  if (fileCameraPath.empty())
91  {
92  std::string type = NxLibItem()[itmCameras][itmBySerialNo][serial][itmType].asString();
93  std::string const neededType = valMonocular;
94  if (type != neededType)
95  {
96  NODELET_ERROR("The camera to be opened is of the wrong type %s. It should be %s.", type.c_str(),
97  neededType.c_str());
98  nxLibFinalize();
99  exit(EXIT_FAILURE);
100  }
101  }
102 
103  nhLocal.param("fixed", cameraIsFixed, false);
104 
105  if (!nhLocal.getParam("camera_frame", cameraFrame))
106  {
107  cameraFrame = serial + "_optical_frame";
108  }
109 
110  nhLocal.getParam("target_frame", targetFrame);
111  nhLocal.getParam("link_frame", linkFrame);
112  if (!targetFrame.empty() && linkFrame.empty())
113  {
114  linkFrame = targetFrame;
115  }
116  else if (targetFrame.empty() && linkFrame.empty())
117  {
118  targetFrame = cameraFrame;
119  linkFrame = cameraFrame;
120  }
121 
122  camera = make_unique<MonoCamera>(nh, serial, fileCameraPath, cameraIsFixed, cameraFrame, targetFrame, linkFrame);
123  if (!camera->open())
124  {
125  NODELET_ERROR("Failed to open the camera. Shutting down.");
126  nxLibFinalize();
127  exit(EXIT_FAILURE);
128  }
129 
130  std::string settingsFile;
131  if (nhLocal.getParam("settings", settingsFile))
132  {
133  NODELET_DEBUG("Loading camera settings...");
134  if (!camera->loadSettings(settingsFile, true))
135  {
136  NODELET_ERROR("Failed to load the camera settings. Shutting down.");
137  nxLibFinalize();
138  exit(EXIT_FAILURE);
139  }
140  }
141 
142  camera->startServers();
143  camera->initTfPublishTimer();
144 }
145 
147 {
148  camera->close();
149  nxLibFinalize();
150 }
151 
152 } // namespace ensenso_camera
153 
#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
#define NODELET_DEBUG(...)


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