Added support for dynamic UVC control enumeration

source/ecam_v4l2/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   std_msgs
 )
+add_definitions(-std=c++11 -Wall -g)
 find_package(PkgConfig REQUIRED)
 pkg_check_modules (libudev REQUIRED libudev)
 
@@ -34,17 +35,23 @@ INCLUDE_DIRS include
 LIBRARIES ecam_v4l2
 CATKIN_DEPENDS std_msgs roscpp message_runtime
 )
-
+add_library(ecam_v4l2
+  src/camera.cpp
+  src/device.cpp
+  src/srv_servers.cpp
+  )
 ## Declare a C++ library
-add_executable(ecam_v4l2_node src/ecam_v4l2.cpp src/camera.cpp src/device.cpp src/services.cpp)
+add_executable(ecam_v4l2_node
+		src/ecam_v4l2.cpp
+ )
 target_link_libraries(${PROJECT_NAME}_node
+   ecam_v4l2
    ${catkin_LIBRARIES}
    ${libudev_LIBRARIES}
  )
-#add_library(ecam_v4l2
-#src/ecam_v4l2.cpp
-#src/camera.cpp)
+
+
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
 ## either from message generation or dynamic reconfigure
- add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+ add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

source/ecam_v4l2/include/ecam_v4l2/V4l2.h

@@ -1,7 +1,8 @@
 /*
  * The MIT License (MIT)
  *
- * Copyright (c) 2016 ThundeRatz
+ * ECAM_V4L2
+ * Copyright (c) 2020 e-consystems
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -23,16 +24,17 @@
  */
 #ifndef V4L2_H
 #define V4L2_H
-#define PIX_FORMAT                				1
-#define RESOLUTION                				2
-#define FPS                       				3
+#define PIX_FORMAT												1
+#define RESOLUTION												2
+#define FPS																3
 #define MAX_FRAME_SIZE_SUPPORT						16
-#define MAX_PIXELFORMAT_SUPPORT 					16
+#define MAX_PIXELFORMAT_SUPPORT						16
 #define MAX_FRMINVAL_SUPPORT							10
-#define DISABLE 													0
-#define ENABLE 														1
-#define FAILURE 													-1
-#define SUCCESS 													0
+#define DISABLE														0
+#define ENABLE														1
+#define SUCCESS														0
+#define FAILURE														-1
+#define DEV_UNPLUGGED											-2
 #include <string>
 #include <vector>
 #include <asm/types.h>
@@ -42,79 +44,105 @@
 #include <sys/ioctl.h>
 #include <linux/videodev2.h>
 #include "ecam_v4l2/image.h"
-
+#include <ros/topic_manager.h>
 namespace ecam_v4l2
 {
 	class Camera
 	{
 		private:
-			int cam_fd;
 			class Buffer
 			{
 			  public:
 				uint8_t *start;
 				size_t length;
 			};// end of class Buffer
 
-			std::vector<Buffer> buffers;
-			bool streamon; // flag to indicate camera is streaming
-			class Format
+			class V4L2_structure
 			{
 				public:
 					struct v4l2_frmsizeenum 	fsize[MAX_FRAME_SIZE_SUPPORT];
 					struct v4l2_fmtdesc		ffmt[MAX_PIXELFORMAT_SUPPORT];
 					struct v4l2_frmivalenum		frminterval;
 					struct v4l2_fract		desc_frm_inval[MAX_FRMINVAL_SUPPORT];
-					struct v4l2_format		fmt,stream_fmt;
+					struct v4l2_format		stream_fmt;
 					struct v4l2_streamparm 		cam_parm;
+					struct v4l2_requestbuffers reqbuf;
+					struct v4l2_capability cap;
+					struct v4l2_buffer buffer;
 			};// end of class Format
+			int cam_fd;
+			int subscribers_count;
 			int width,height;
 			int numerator,denominator;
-			std::string pixelformat;
+			bool streamon; // flag to indicate camera is streaming
 			bool init; //flag to indicate initialisation
+			std::string pixelformat;
+			std::vector<Buffer> buffers;
+			std::string camera_name;
+			std::string video_node;
 
+// private member functions
+			void enqueue(int index);
 // public of class Camera
 		public:
 
-			Format frmt; //obj of class Format
+			V4L2_structure v4l2; //obj of class Format
 // Member function of class Camera
 			Camera();
 
 			int open_device(const std::string &device);
 
-			void init_format();
+			void cam_init();
+
+			void enum_pixelformat(std::vector<std::string> *list,std::string *str);
 
-			int set_format(const std::string& fourcc,int width,int height);
+			void enum_resolution(std::string pixelformat,
+													 std::vector<std::string> *list,
+													 std::string *str);
 
-			void get_format(std::string *PixelFormat,int *Height,int *Width,int type);
+			void enum_framerate(std::string pixelformat,int width,int height,
+													std::vector<std::string> *list,std::string *str);
 
-			int set_framerate(unsigned numerator, unsigned denominator);
+			int set_format(const std::string& fourcc,int input_width,int input_height);
+
+			void get_format(std::string *PixelFormat,int *Height,int *Width,
+											int *Numerator,int *Denominator,int type);
+
+			int set_framerate(unsigned input_numerator, unsigned input_denominator);
 
 			int request_buffer();
 
 			int stream_on();
 
-			void enqueue(int index);
-
 			int capture(ecam_v4l2::image *image);
 
-			int empty_image(int buffer_index,ecam_v4l2::image *image);
-
 			int stream_off();
 
 			int clean_buffer();
 
+			void munmap_buffers();
+
 			void close_device();
 
 
 			bool onInit();
 
+			void set_init(bool value);
+
+			int sub_count();
+
 			bool isStreamOn();
 
 			int xioctl(unsigned cmd, void *arg);
 
 			void get_four_character_code(int32_t pix_fmt,std::string *pixelformat);
 
+			void set_camera_name(std::string name,std::string node_name);
+
+			std::string get_camera_name();
+
+			void get_node_name(std::string *dev_node_name);
+
 	};// end of class Camera
 
 }// ecam_v4l2

source/ecam_v4l2/include/ecam_v4l2/device.h

@@ -1,3 +1,27 @@
+/*
+ * The MIT License (MIT)
+ *
+ * ECAM_V4L2
+ * Copyright (c) 2020 e-consystems
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
 #ifndef DEVICE_H
 #define DEVICE_H
 #include <libudev.h>
@@ -22,16 +46,20 @@ namespace ecam_v4l2
   class Devices
   {
   private:
-      std::vector<std::string>device_node_name;
-      std::vector<std::string>camera_name;
-      bool get_product_name(std::string &dev_node);
-      int check_for_valid_videonode(std::string &dev_node);
-      int check_camera_type(std::string &dev_node,std::string *productName,int *type);
+    std::vector<std::string>device_node_name;
+    std::vector<std::string>camera_name;
+    bool check_for_valid_videonode(std::string &dev_node);
+    int check_camera_type(std::string &dev_node,
+                          std::string *productName,int *type);
+    bool get_product_name(std::string &dev_node);
+    bool isEnumerated(std::string node_name);
+    void prepare_topic(std::string *productName);
   public:
-      int list_devices();
-      void get_camera_count(int *camera_count);
-      void get_device_node_name(int index,std::string *dev_node_name);
-      void get_camera_name(int index,std::string *cam_name);
+    int list_devices();
+    int get_camera_count();
+    bool remove_camera(std::string cam_name);
+    bool get_camera(int index,std::string *cam_name,std::string *dev_node_name);
+    std::vector<std::string> get_list();
   };
 } // namespace ecam_v4l2
 

source/ecam_v4l2/include/ecam_v4l2/srv_servers.h

@@ -0,0 +1,103 @@
+/*
+ * The MIT License (MIT)
+ *
+ * ECAM_V4L2
+ * Copyright (c) 2020 e-consystems
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+#ifndef SERVICES_SERVERS_H
+#define SERVICES_SERVERS_H
+#include "ecam_v4l2/camera.h"
+#include "ecam_v4l2/device.h"
+#include "ecam_v4l2/query_control.h"
+#include "ecam_v4l2/set_control.h"
+#include "ecam_v4l2/set_format.h"
+#include "ecam_v4l2/enum_format.h"
+#include "ecam_v4l2/V4l2.h"
+#include <sys/mman.h>
+#include <string>
+#include <vector>
+#include <algorithm>
+#define CTRL_TYPE                     7
+#define MENU_TYPE                     8
+#define GET_CTRL                      9
+#define GET_FORMAT                    10
+#define V4L2_CID_CUSTOM_CONTROLS      V4L2_CID_CAMERA_CLASS_BASE+50
+#define V4L2_CID_ROI_WINDOW_SIZE      V4L2_CID_CAMERA_CLASS_BASE+36
+#define V4L2_CID_ROI_EXPOSURE         V4L2_CID_CAMERA_CLASS_BASE+38
+
+namespace ecam_v4l2
+{
+   class Services
+   {
+     private:
+       Devices &dev; // container to hold obj of class Devices
+       std::vector<Camera>& cam; // container to hold obj of class Camera
+       struct v4l2_queryctrl queryctrl;
+       struct v4l2_querymenu querymenu;
+       std::vector<std::string> stream_list;// list of cameras which are streaming
+
+       bool compareList(std::vector<std::string> prev_list,
+                        std::vector<std::string> cur_list);
+
+  	 public:
+       // Declaration of publishers for all cameras connected.
+       std::vector<ros::Publisher> publisher;
+
+       Services(Devices &Devices_obj ,std::vector<Camera>& Camera_obj);
+
+       bool onEnumDevice(ecam_v4l2::camera::Request &req,
+                         ecam_v4l2::camera::Response &res);
+
+       bool onCameraChange(ecam_v4l2::camera::Request &req,
+                           ecam_v4l2::camera::Response &res);
+
+       bool isControlAvailable(ecam_v4l2::query_control::Request &req,
+                               ecam_v4l2::query_control::Response &res);
+
+       bool onSetControl(ecam_v4l2::set_control::Request &req,
+                         ecam_v4l2::set_control::Response &res);
+
+       bool onEnumFormat(ecam_v4l2::enum_format::Request &req,
+                         ecam_v4l2::enum_format::Response &res);
+
+       bool onFormatChange(ecam_v4l2::set_format::Request &req,
+                           ecam_v4l2::set_format::Response &res);
+
+       bool get_current_device(std::string name,int *index);
+
+       bool isNotStreaming(std::string name);
+
+       bool check_valid_control(int controlid);
+
+       void stop_publisher(ros::Publisher pub,int index);
+
+       void add_camera_streaming(std::string name);
+
+       void remove_camera_streaming(std::string name);
+
+       void get_stream_list(std::vector<std::string> *list);
+
+       bool get_camera_index(std::string cam_name,int *current_device);
+
+    };// end of class Services
+
+}// cam_v4l2__pub
+#endif // SERVICES_H