nodelet.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, robotFrame, wristFrame;
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 
47  if (serial.empty())
48  {
49  NxLibItem cameras = NxLibItem()[itmCameras][itmBySerialNo];
50  // Try to find the first stereo camera.
51  bool foundAppropriateCamera = false;
52  for (int i = 0; i < cameras.count(); i++)
53  {
54  if (cameras[i][itmType].exists() && cameras[i][itmType].asString() == valStereo)
55  {
56  foundAppropriateCamera = true;
57  serial = cameras[i].name();
58  break;
59  }
60  }
61  if (!foundAppropriateCamera)
62  {
63  NODELET_ERROR("Could not find any camera. Shutting down.");
64  nxLibFinalize();
65  exit(EXIT_FAILURE);
66  }
67  }
68 
69  nhLocal.param<std::string>("file_camera_path", fileCameraPath, "");
70 
71  // We can only check the type with file cameras, when we create file cameras, so for now we can only check whether the
72  // current physical camera is of the right type
73  if (fileCameraPath.empty())
74  {
75  std::string type = NxLibItem()[itmCameras][itmBySerialNo][serial][itmType].asString();
76  std::string const neededType = valStereo;
77  if (type != neededType)
78  {
79  NODELET_ERROR("The camera to be opened is of the wrong type %s. It should be %s.", type.c_str(),
80  neededType.c_str());
81  nxLibFinalize();
82  exit(EXIT_FAILURE);
83  }
84  }
85 
86  nhLocal.param("fixed", cameraIsFixed, false);
87 
88  if (!nhLocal.getParam("camera_frame", cameraFrame))
89  {
90  cameraFrame = serial + "_optical_frame";
91  }
92 
93  nhLocal.param<std::string>("robot_frame", robotFrame, "");
94  nhLocal.param<std::string>("wrist_frame", wristFrame, "");
95 
96  if (cameraIsFixed && robotFrame.empty())
97  {
98  robotFrame = cameraFrame;
99  }
100  if (!cameraIsFixed && wristFrame.empty())
101  {
102  wristFrame = cameraFrame;
103  }
104 
105  nhLocal.getParam("target_frame", targetFrame);
106  nhLocal.getParam("link_frame", linkFrame);
107 
108  if (!targetFrame.empty() && linkFrame.empty())
109  {
110  linkFrame = targetFrame;
111  }
112  else if (targetFrame.empty() && linkFrame.empty())
113  {
114  targetFrame = cameraFrame;
115  linkFrame = cameraFrame;
116  }
117 
118  int captureTimeout;
119  nhLocal.param("capture_timeout", captureTimeout, 0);
120 
121  camera = ::make_unique<StereoCamera>(nh, serial, fileCameraPath, cameraIsFixed, cameraFrame, targetFrame, robotFrame,
122  wristFrame, linkFrame, captureTimeout);
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  camera->initStatusTimer();
145 }
146 
148 {
149  camera->close();
150  nxLibFinalize();
151 }
152 
153 } // namespace ensenso_camera
154 
#define NODELET_ERROR(...)
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
void onInit() override
Definition: nodelet.cpp:11
std::unique_ptr< StereoCamera > camera
Definition: nodelet.h:14
ros::NodeHandle & getPrivateNodeHandle() const
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