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


ensenso_camera
Author(s): Ensenso
autogenerated on Sat Nov 9 2019 03:53:27