feat: changes for opencv4

alireza@general · alireza@general · commit e6145cfd03f5 · 2021-03-11T11:15:25.000-08:00
diff --git a/include/dji_osdk_ros_obsoleted/dji_sdk/dji_sdk_node.h b/include/dji_osdk_ros_obsoleted/dji_sdk/dji_sdk_node.h
@@ -78,6 +78,7 @@
 #include <dji_osdk_ros/StereoVGASubscription.h>
 #include <dji_osdk_ros/SetupCameraStream.h>
 #include <sensor_msgs/CameraInfo.h>
+#include <cv_bridge/cv_bridge.h>
 #endif
 
 //! SDK library
@@ -232,7 +233,7 @@ class DJISDKNode
                                  dji_osdk_ros::SetupCameraStream::Response& response);
   void publishCameraInfo(const std_msgs::Header &header);
   
-  sensor_msgs::CameraInfo getCameraInfo(int camera_select);
+  sensor_msgs::CameraInfo getCameraInfo(int camera_select, bool isLeftRequired);
 #endif
 
   //! data broadcast callback
@@ -298,6 +299,8 @@ class DJISDKNode
   static void publishMainCameraImage(CameraRGBImage img, void* userData);
 
   static void publishFPVCameraImage(CameraRGBImage img, void* userData);
+  
+  static void processRosImage(sensor_msgs::Image &img, int camera_select);
 #endif
 
 private:
diff --git a/launch/stereo_disparity.launch b/launch/stereo_disparity.launch
diff --git a/launch/stereo_pointcloud.launch b/launch/stereo_pointcloud.launch
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<launch>
+	<remap from="/left/image_raw" to="/dji_osdk_ros/stereo_vga/left/image_raw"/>
+	<remap from="/left/camera_info" to="/dji_osdk_ros/stereo_vga/left/camera_info"/>
+	<remap from="/right/image_raw" to="/dji_osdk_ros/stereo_vga/right/image_raw"/>
+	<remap from="/right/camera_info" to="/dji_osdk_ros/stereo_vga/right/camera_info"/>
+	
+	
+	
+	<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/>
+	<!--<include file="$(find stereo_image_proc)/launch/stereo_image_proc.launch">
+		<arg name="left" value="/dji_osdk_ros/stereo_vga/left"/>
+		<arg name="right" value="/dji_osdk_ros/stereo_vga/right"/>
+		<arg name="manager" value="manager"/>
+		<node pkg="nodelet" type="nodelet" name="manager"  args="manager"/>
+		-->
+	</include>
+</launch>
diff --git a/package.xml b/package.xml
@@ -60,7 +60,8 @@
   <build_depend>actionlib_msgs</build_depend>
   <build_depend>nmea_msgs</build_depend>
   <build_depend>message_filters</build_depend>
-
+	<build_depend>cv_bridge</build_depend>
+	
   <exec_depend>roscpp</exec_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>message_runtime</exec_depend>
@@ -71,10 +72,10 @@
   <exec_depend>actionlib</exec_depend>
   <exec_depend>actionlib_msgs</exec_depend>
   <exec_depend>message_filters</exec_depend>
-
+	<exec_depend>cv_bridge</exec_depend>
   
   <export>
     
 
   </export>
-</package>
+</package>
diff --git a/src/dji_osdk_ros/dji_vehicle_node.cpp b/src/dji_osdk_ros/dji_vehicle_node.cpp
@@ -946,7 +946,7 @@ bool VehicleNode::getM300StereoParamsCallback(dji_osdk_ros::GetM300StereoParams:
   Vehicle* vehicle = ptr_wrapper_->getVehicle();
   M300StereoParamTool *tool = new M300StereoParamTool(vehicle);
   Perception::CamParamType stereoParam =
-      tool->getM300stereoParams(Perception::DirectionType::RECTIFY_LEFT);
+      tool->getM300stereoParams(Perception::DirectionType::RECTIFY_UP);
   if (tool->createStereoParamsYamlFile(M300_FRONT_STEREO_PARAM_YAML_NAME, stereoParam))
   {
     tool->setParamFileForM300(M300_FRONT_STEREO_PARAM_YAML_NAME);
diff --git a/src/dji_osdk_ros/modules/dji_vehicle_node_publisher.cpp b/src/dji_osdk_ros/modules/dji_vehicle_node_publisher.cpp
@@ -836,7 +836,7 @@ void VehicleNode::publishCameraInfo(const std_msgs::Header &header)
 
 		Vehicle* vehicle = ptr_wrapper_->getVehicle();
 		M300StereoParamTool *tool = new M300StereoParamTool(vehicle);
-		Perception::CamParamType stereoParam = tool->getM300stereoParams(Perception::DirectionType::RECTIFY_FRONT);
+		Perception::CamParamType stereoParam = tool->getM300stereoParams(Perception::DirectionType::RECTIFY_DOWN);
 	
 		tool->getM300stereoCameraInfo(stereoParam, left_camera_info, right_camera_info);
 		isFirstTime = false;
diff --git a/src/dji_osdk_ros/modules/stereo_utility/m300_stereo_param_tool.cpp b/src/dji_osdk_ros/modules/stereo_utility/m300_stereo_param_tool.cpp
@@ -51,7 +51,7 @@ void M300StereoParamTool::PerceptionCamParamCB(
   if ((pack.directionNum > 0) && (pack.directionNum <= IMAGE_MAX_DIRECTION_NUM))
     for (int i = 0; i < pack.directionNum; i++) {
       if ((userData)
-          && (pack.cameraParam[i].direction == Perception::RECTIFY_LEFT)) {
+          && (pack.cameraParam[i].direction == Perception::RECTIFY_UP)) {
         auto camParam = (Perception::CamParamType *) userData;
         *camParam = pack.cameraParam[i];
       }
diff --git a/src/dji_osdk_ros/modules/stereo_utility/point_cloud_viewer.cpp b/src/dji_osdk_ros/modules/stereo_utility/point_cloud_viewer.cpp
diff --git a/src/dji_osdk_ros/modules/stereo_utility/stereo_frame.cpp b/src/dji_osdk_ros/modules/stereo_utility/stereo_frame.cpp
diff --git a/src/dji_osdk_ros/samples/CMakeLists.txt b/src/dji_osdk_ros/samples/CMakeLists.txt
@@ -90,28 +90,28 @@ target_link_libraries(waypointV2_node
         )
 add_dependencies(waypointV2_node dji_osdk_ros_generate_messages_cpp)
 
-if (OPEN_CV_3_3_0_INSTALLED AND FOUND_OPENCV_VIZ)
+#if (OPEN_CV_3_3_0_INSTALLED AND FOUND_OPENCV_VIZ)
 
     message(STATUS "Found OpenCV ${OpenCV_VERSION}, Viz3d, and advanced sensing module, stereo vision depth perception node will be compiled")
     add_definitions(-DOPEN_CV_INSTALLED)
     add_executable(stereo_vision_depth_perception_node
                    stereo_vision_depth_perception_node.cpp)
     target_include_directories(${PROJECT_NAME} PRIVATE ${OpenCV_INCLUDE_DIRS})
 
-    if (OPEN_CV_3_3_0_INSTALLED)
+#    if (OPEN_CV_3_3_0_INSTALLED)
         target_link_libraries(stereo_vision_depth_perception_node
                 ${catkin_LIBRARIES}  
                 ${PROJECT_NAME}
                 ${OpenCV_LIBRARIES}
                 )
-    else()
+#    else()
     target_link_libraries(stereo_vision_depth_perception_node
             ${catkin_LIBRARIES}        
             ${PROJECT_NAME}
             )
-    endif()
+#    endif()
     add_dependencies(stereo_vision_depth_perception_node dji_osdk_ros_generate_messages_cpp)
-else()
-   message(STATUS "Did not find required libraries, stereo vision depth perception node will not be compiled.")
-endif ()
+#else()
+#   message(STATUS "Did not find required libraries, stereo vision depth perception node will not be compiled.")
+#endif ()
 
diff --git a/src/dji_osdk_ros/samples/stereo_vision_depth_perception_node.cpp b/src/dji_osdk_ros/samples/stereo_vision_depth_perception_node.cpp
@@ -84,7 +84,7 @@ main(int argc, char** argv)
     }
     else
     {
-      M210_STEREO::Config::setParamFile(M300_FRONT_STEREO_PARAM_YAML_NAME);
+      M210_STEREO::Config::setParamFile("/home/alireza/catkin_ws/src/Onboard-SDK-4.0/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/m300_front_stereo_param.yaml");
     }
   }
 
@@ -107,9 +107,9 @@ main(int argc, char** argv)
 
   //! Setup ros related stuff
   rect_img_left_publisher =
-    nh.advertise<sensor_msgs::Image>("/stereo_depth_perception/rectified_vga_front_left_image", 10);
+    nh.advertise<sensor_msgs::Image>("/dji_osdk_ros/stereo_vga_front/left/image_raw", 10);
   rect_img_right_publisher =
-    nh.advertise<sensor_msgs::Image>("/stereo_depth_perception/rectified_vga_front_right_image", 10);
+    nh.advertise<sensor_msgs::Image>("/dji_osdk_ros/stereo_vga_front/right/image_raw", 10);
   left_disparity_publisher =
     nh.advertise<sensor_msgs::Image>("/stereo_depth_perception/disparity_front_left_image", 10);
   point_cloud_publisher =
@@ -532,4 +532,4 @@ shutDownHandler(int s)
   }
 
   exit(1);
-}
+}
diff --git a/src/dji_osdk_ros_obsoleted/CMakeLists.txt b/src/dji_osdk_ros_obsoleted/CMakeLists.txt
@@ -17,6 +17,9 @@ set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
 #catkin_python_setup()
 
+find_package(OpenCV REQUIRED)
+find_package(catkin REQUIRED COMPONENTS cv_bridge roscpp rospy std_msgs)
+
 ################################################
 ## Declare ROS messages, services and actions ##
 ################################################
@@ -63,12 +66,14 @@ set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-# include_directories(include)
+# include_directories(include
+
 include_directories(
         ../../include/dji_osdk_ros_obsoleted
   ../../include
 #  ${DJIOSDK_INCLUDE_DIRS}
   ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
 )
 
 ## Declare a cpp library
@@ -98,14 +103,21 @@ add_executable(dji_sdk_node
 add_dependencies(dji_sdk_node
   dji_osdk_ros_generate_messages_cpp)
 
-## Specify libraries to link a library or executable target against
-target_link_libraries(dji_sdk_node
+set(link_Libraries 
   ${catkin_LIBRARIES}
     djiosdk-core.a
     dji-linker.a
     advanced-sensing.a
     ${LIBUSB_1_LIBRARIES}
     ${FFMPEG_LIBRARIES}
+    ${OpenCV_LIBRARIES}
+)
+
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(dji_sdk_node
+	${link_Libraries}
+
 #        waypointv2-interface.a
 #        waypointv2-core.a
 #        DJIProtobuf.a
diff --git a/src/dji_osdk_ros_obsoleted/modules/dji_sdk_node_publisher.cpp b/src/dji_osdk_ros_obsoleted/modules/dji_sdk_node_publisher.cpp

Original file line number	Diff line number	Diff line change
`@@ -946,7 +946,7 @@ bool VehicleNode::getM300StereoParamsCallback(dji_osdk_ros::GetM300StereoParams:`
`946`	`946`	`Vehicle* vehicle = ptr_wrapper_->getVehicle();`
`947`	`947`	`M300StereoParamTool *tool = new M300StereoParamTool(vehicle);`
`948`	`948`	`Perception::CamParamType stereoParam =`
`949`		`- tool->getM300stereoParams(Perception::DirectionType::RECTIFY_LEFT);`
	`949`	`+ tool->getM300stereoParams(Perception::DirectionType::RECTIFY_UP);`
`950`	`950`	`if (tool->createStereoParamsYamlFile(M300_FRONT_STEREO_PARAM_YAML_NAME, stereoParam))`
`951`	`951`	`{`
`952`	`952`	`tool->setParamFileForM300(M300_FRONT_STEREO_PARAM_YAML_NAME);`
Original file line number	Diff line number	Diff line change
`@@ -51,7 +51,7 @@ void M300StereoParamTool::PerceptionCamParamCB(`
`51`	`51`	`if ((pack.directionNum > 0) && (pack.directionNum <= IMAGE_MAX_DIRECTION_NUM))`
`52`	`52`	`for (int i = 0; i < pack.directionNum; i++) {`
`53`	`53`	`if ((userData)`
`54`		`- && (pack.cameraParam[i].direction == Perception::RECTIFY_LEFT)) {`
	`54`	`+ && (pack.cameraParam[i].direction == Perception::RECTIFY_UP)) {`
`55`	`55`	`auto camParam = (Perception::CamParamType *) userData;`
`56`	`56`	`*camParam = pack.cameraParam[i];`
`57`	`57`	`}`