avt_vimba_api.h
Go to the documentation of this file.
1 
33 #ifndef AVT_VIMBA_API_H
34 #define AVT_VIMBA_API_H
35 
37 
38 #include <ros/ros.h>
39 #include <ros/console.h>
40 #include <sensor_msgs/Image.h>
42 #include <sensor_msgs/fill_image.h>
43 
44 #include <string>
45 #include <map>
46 
50 
51 namespace avt_vimba_camera {
52 class AvtVimbaApi {
53  public:
54  AvtVimbaApi(void) : vs(VimbaSystem::GetInstance()) {}
55 
56  void start() {
57  VmbErrorType err = vs.Startup();
58  if (VmbErrorSuccess == err) {
59  ROS_INFO_STREAM("[Vimba System]: AVT Vimba System initialized successfully");
61  } else {
62  ROS_ERROR_STREAM("[Vimba System]: Could not start Vimba system: "
63  << errorCodeToMessage(err));
64  }
65  }
66 
73  std::string errorCodeToMessage(VmbErrorType error) {
74  std::map<VmbErrorType, std::string> error_msg;
75  error_msg[VmbErrorSuccess] = "Success.";
76  error_msg[VmbErrorApiNotStarted] = "API not started.";
77  error_msg[VmbErrorNotFound] = "Not found.";
78  error_msg[VmbErrorBadHandle] = "Invalid handle ";
79  error_msg[VmbErrorDeviceNotOpen] = "Device not open.";
80  error_msg[VmbErrorInvalidAccess] = "Invalid access.";
81  error_msg[VmbErrorBadParameter] = "Bad parameter.";
82  error_msg[VmbErrorStructSize] = "Wrong DLL version.";
83  error_msg[VmbErrorWrongType] = "Wrong type.";
84  error_msg[VmbErrorInvalidValue] = "Invalid value.";
85  error_msg[VmbErrorTimeout] = "Timeout.";
86  error_msg[VmbErrorOther] = "TL error.";
87  error_msg[VmbErrorInvalidCall] = "Invalid call.";
88  error_msg[VmbErrorNoTL] = "TL not loaded.";
89  error_msg[VmbErrorNotImplemented] = "Not implemented.";
90  error_msg[VmbErrorNotSupported] = "Not supported.";
91  error_msg[VmbErrorResources] = "Resource not available.";
92  error_msg[VmbErrorInternalFault] = "Unexpected fault in VmbApi or driver.";
93  error_msg[VmbErrorMoreData] = "More data returned than memory provided.";
94 
95  std::map<VmbErrorType, std::string>::const_iterator iter =
96  error_msg.find(error);
97  if ( error_msg.end() != iter ) {
98  return iter->second;
99  }
100  return "Unsupported error code passed.";
101  }
102 
103  bool frameToImage(const FramePtr vimba_frame_ptr, sensor_msgs::Image& image) {
104  VmbPixelFormatType pixel_format;
105  VmbUint32_t width, height, nSize;
106 
107  vimba_frame_ptr->GetWidth(width);
108  vimba_frame_ptr->GetHeight(height);
109  vimba_frame_ptr->GetPixelFormat(pixel_format);
110  vimba_frame_ptr->GetImageSize(nSize);
111 
112  VmbUint32_t step = nSize / height;
113 
114  // NOTE: YUV and ARGB formats not supported
115  std::string encoding;
116  if (pixel_format == VmbPixelFormatMono8 ) encoding = sensor_msgs::image_encodings::MONO8;
117  else if (pixel_format == VmbPixelFormatMono10 ) encoding = sensor_msgs::image_encodings::MONO16;
118  else if (pixel_format == VmbPixelFormatMono12 ) encoding = sensor_msgs::image_encodings::MONO16;
119  else if (pixel_format == VmbPixelFormatMono12Packed ) encoding = sensor_msgs::image_encodings::MONO16;
120  else if (pixel_format == VmbPixelFormatMono14 ) encoding = sensor_msgs::image_encodings::MONO16;
121  else if (pixel_format == VmbPixelFormatMono16 ) encoding = sensor_msgs::image_encodings::MONO16;
122  else if (pixel_format == VmbPixelFormatBayerGR8 ) encoding = sensor_msgs::image_encodings::BAYER_GRBG8;
123  else if (pixel_format == VmbPixelFormatBayerRG8 ) encoding = sensor_msgs::image_encodings::BAYER_RGGB8;
124  else if (pixel_format == VmbPixelFormatBayerGB8 ) encoding = sensor_msgs::image_encodings::BAYER_GBRG8;
125  else if (pixel_format == VmbPixelFormatBayerBG8 ) encoding = sensor_msgs::image_encodings::BAYER_BGGR8;
126  else if (pixel_format == VmbPixelFormatBayerGR10 ) encoding = sensor_msgs::image_encodings::TYPE_16SC1;
127  else if (pixel_format == VmbPixelFormatBayerRG10 ) encoding = sensor_msgs::image_encodings::TYPE_16SC1;
128  else if (pixel_format == VmbPixelFormatBayerGB10 ) encoding = sensor_msgs::image_encodings::TYPE_16SC1;
129  else if (pixel_format == VmbPixelFormatBayerBG10 ) encoding = sensor_msgs::image_encodings::TYPE_16SC1;
130  else if (pixel_format == VmbPixelFormatBayerGR12 ) encoding = sensor_msgs::image_encodings::TYPE_16SC1;
131  else if (pixel_format == VmbPixelFormatBayerRG12 ) encoding = sensor_msgs::image_encodings::TYPE_16SC1;
132  else if (pixel_format == VmbPixelFormatBayerGB12 ) encoding = sensor_msgs::image_encodings::TYPE_16SC1;
133  else if (pixel_format == VmbPixelFormatBayerBG12 ) encoding = sensor_msgs::image_encodings::TYPE_16SC1;
134  else if (pixel_format == VmbPixelFormatBayerGR12Packed) encoding = sensor_msgs::image_encodings::TYPE_32SC4;
135  else if (pixel_format == VmbPixelFormatBayerRG12Packed) encoding = sensor_msgs::image_encodings::TYPE_32SC4;
136  else if (pixel_format == VmbPixelFormatBayerGB12Packed) encoding = sensor_msgs::image_encodings::TYPE_32SC4;
137  else if (pixel_format == VmbPixelFormatBayerBG12Packed) encoding = sensor_msgs::image_encodings::TYPE_32SC4;
138  else if (pixel_format == VmbPixelFormatBayerGR16 ) encoding = sensor_msgs::image_encodings::TYPE_16SC1;
139  else if (pixel_format == VmbPixelFormatBayerRG16 ) encoding = sensor_msgs::image_encodings::TYPE_16SC1;
140  else if (pixel_format == VmbPixelFormatBayerGB16 ) encoding = sensor_msgs::image_encodings::TYPE_16SC1;
141  else if (pixel_format == VmbPixelFormatBayerBG16 ) encoding = sensor_msgs::image_encodings::TYPE_16SC1;
142  else if (pixel_format == VmbPixelFormatRgb8 ) encoding = sensor_msgs::image_encodings::RGB8;
143  else if (pixel_format == VmbPixelFormatBgr8 ) encoding = sensor_msgs::image_encodings::BGR8;
144  else if (pixel_format == VmbPixelFormatRgba8 ) encoding = sensor_msgs::image_encodings::RGBA8;
145  else if (pixel_format == VmbPixelFormatBgra8 ) encoding = sensor_msgs::image_encodings::BGRA8;
146  else if (pixel_format == VmbPixelFormatRgb12 ) encoding = sensor_msgs::image_encodings::TYPE_16UC3;
147  else if (pixel_format == VmbPixelFormatRgb16 ) encoding = sensor_msgs::image_encodings::TYPE_16UC3;
148  else
149  ROS_WARN("Received frame with unsupported pixel format %d", pixel_format);
150  if (encoding == "") return false;
151 
152  VmbUchar_t *buffer_ptr;
153  VmbErrorType err = vimba_frame_ptr->GetImage(buffer_ptr);
154  bool res = false;
155  if ( VmbErrorSuccess == err ) {
156  res = sensor_msgs::fillImage(image,
157  encoding,
158  height,
159  width,
160  step,
161  buffer_ptr);
162  } else {
164  << "]: Could not GetImage. "
165  << "\n Error: " << errorCodeToMessage(err));
166  }
167  return res;
168  }
169 
170  private:
172 
173  void listAvailableCameras(void) {
174  std::string name;
175  CameraPtrVector cameras;
176  if (VmbErrorSuccess == vs.Startup()) {
177  if (VmbErrorSuccess == vs.GetCameras(cameras)) {
178  for (CameraPtrVector::iterator iter = cameras.begin();
179  cameras.end() != iter;
180  ++iter) {
181  if (VmbErrorSuccess == (*iter)->GetName(name)) {
182  ROS_DEBUG_STREAM("[" << ros::this_node::getName() << "]: Found camera: ");
183  }
184  std::string strID; // The ID of the cam
185  std::string strName; // The name of the cam
186  std::string strModelname; // The model name of the cam
187  std::string strSerialNumber; // The serial number of the cam
188  std::string strInterfaceID; // The ID of the interface the cam is connected to
189  VmbErrorType err = (*iter)->GetID( strID );
190  if ( VmbErrorSuccess != err )
191  {
192  ROS_ERROR_STREAM("[Could not get camera ID. Error code: " << err << "]");
193  }
194  err = (*iter)->GetName( strName );
195  if ( VmbErrorSuccess != err )
196  {
197  ROS_ERROR_STREAM("[Could not get camera name. Error code: " << err << "]");
198  }
199 
200  err = (*iter)->GetModel( strModelname );
201  if ( VmbErrorSuccess != err )
202  {
203  ROS_ERROR_STREAM("[Could not get camera mode name. Error code: " << err << "]");
204  }
205 
206  err = (*iter)->GetSerialNumber( strSerialNumber );
207  if ( VmbErrorSuccess != err )
208  {
209  ROS_ERROR_STREAM("[Could not get camera serial number. Error code: " << err << "]");
210  }
211 
212  err = (*iter)->GetInterfaceID( strInterfaceID );
213  if ( VmbErrorSuccess != err )
214  {
215  ROS_ERROR_STREAM("[Could not get interface ID. Error code: " << err << "]");
216  }
217  ROS_INFO_STREAM("\t/// Camera Name: " << strName);
218  ROS_INFO_STREAM("\t/// Model Name: " << strModelname);
219  ROS_INFO_STREAM("\t/// Camera ID: " << strID);
220  ROS_INFO_STREAM("\t/// Serial Number: " << strSerialNumber);
221  ROS_INFO_STREAM("\t/// @ Interface ID: " << strInterfaceID);
222 
223  }
224  } else {
225  ROS_WARN("Could not get cameras from Vimba System");
226  }
227  } else {
228  ROS_WARN("Could not start Vimba System");
229  }
230  }
231 };
232 }
233 #endif
static bool fillImage(Image &image, const std::string &encoding_arg, uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, const void *data_arg)
const std::string BAYER_GRBG8
NetPointer< Frame, AVT::VmbAPINET::Frame > FramePtr
VmbPixelFormatType
IMEXPORT VmbErrorType Startup()
Definition: VimbaSystem.cpp:93
std::vector< CameraPtr > CameraPtrVector
Definition: Camera.h:45
ROSCPP_DECL const std::string & getName()
#define ROS_WARN(...)
bool frameToImage(const FramePtr vimba_frame_ptr, sensor_msgs::Image &image)
VmbErrorType
const std::string BAYER_GBRG8
unsigned char VmbUchar_t
VmbErrorType GetCameras(CameraPtrVector &cameras)
NetPointer< Camera, AVT::VmbAPINET::Camera > CameraPtr
const std::string MONO16
const std::string TYPE_16UC3
#define ROS_DEBUG_STREAM(args)
const std::string BAYER_BGGR8
unsigned int VmbUint32_t
#define ROS_INFO_STREAM(args)
const std::string BAYER_RGGB8
const std::string TYPE_32SC4
#define ROS_ERROR_STREAM(args)
std::string errorCodeToMessage(VmbErrorType error)
Definition: avt_vimba_api.h:73
const std::string TYPE_16SC1


avt_vimba_camera
Author(s): Miquel Massot , Allied Vision Technologies
autogenerated on Wed Jun 5 2019 22:22:40