feat: stereo camera is now publishing pointcloud from the front vga only

Author: alireza@general

include/dji_osdk_ros/dji_vehicle_node.h

@@ -170,6 +170,10 @@
 #include <dji_osdk_ros/WaypointV2MissionEventPush.h>
 #include <dji_osdk_ros/WaypointV2MissionStatePush.h>
 
+#include <sensor_msgs/CameraInfo.h>
+
+#include <dji_osdk_ros/stereo_utility/m300_stereo_param_tool.hpp>
+
 #define C_EARTH (double)6378137.0
 #define C_PI (double)3.141592653589793
 #define DEG2RAD(DEG) ((DEG) * ((C_PI) / (180.0)))
@@ -197,6 +201,9 @@ namespace dji_osdk_ros
       bool initTopic();
       bool initDataSubscribeFromFC();
       bool cleanUpSubscribeFromFC();
+#ifdef ADVANCED_SENSING
+			void publishCameraInfo(const std_msgs::Header &header);
+#endif
     protected:
       /*! services */
       /*! for general */
@@ -332,13 +339,18 @@ namespace dji_osdk_ros
       ros::Publisher stereo_240p_front_depth_publisher_;
       ros::Publisher stereo_vga_front_left_publisher_;
       ros::Publisher stereo_vga_front_right_publisher_;
+      ros::Publisher left_camera_info_pub_;
+      ros::Publisher right_camera_info_pub_; 
+      ros::Timer info_timer_;
       #endif
 
       //waypointV2
       ros::Publisher waypointV2_mission_state_publisher_;
       ros::Publisher waypointV2_mission_event_publisher_;
 
     protected:
+    
+    	void initTimer();
       /*! for general */
       bool getDroneTypeCallback(dji_osdk_ros::GetDroneType::Request &request,
                                 dji_osdk_ros::GetDroneType::Response &response);
@@ -413,6 +425,10 @@ namespace dji_osdk_ros
       bool getM300StereoParamsCallback(dji_osdk_ros::GetM300StereoParams::Request& request,
                                        dji_osdk_ros::GetM300StereoParams::Response& response);
       void publishAdvancedSeningData();
+      
+      //void infoTimerCB(const ros::TimerEvent &even);
+      
+      
 #endif
       /*! for mission service callback*/
       // mission manager

include/dji_osdk_ros/stereo_utility/m300_stereo_param_tool.hpp

@@ -4,6 +4,7 @@
 #include "dji_vehicle.hpp"
 #include "dji_perception.hpp"
 #include "dji_osdk_ros/stereo_utility/config.hpp"
+#include <sensor_msgs/CameraInfo.h>
 
 using namespace M210_STEREO;
 using namespace DJI;
@@ -23,9 +24,13 @@ class M300StereoParamTool
     double rotaionLeftInRight[9];
     double translationLeftInRight[3];
   } DoubleCamParamType;
-
+	
+	
   /*!*/
   Perception::CamParamType getM300stereoParams(Perception::DirectionType direction);
+  
+  void getM300stereoCameraInfo(Perception::CamParamType param, sensor_msgs::CameraInfo &left,
+  sensor_msgs::CameraInfo &right);
 
   bool createStereoParamsYamlFile(std::string fileName,
                                   Perception::CamParamType param);

include/dji_osdk_ros_obsoleted/dji_sdk/dji_sdk_node.h

@@ -77,6 +77,7 @@
 #include <dji_osdk_ros/StereoDepthSubscription.h>
 #include <dji_osdk_ros/StereoVGASubscription.h>
 #include <dji_osdk_ros/SetupCameraStream.h>
+#include <sensor_msgs/CameraInfo.h>
 #endif
 
 //! SDK library
@@ -229,6 +230,7 @@ class DJISDKNode
                                      dji_osdk_ros::StereoVGASubscription::Response& response);
   bool setupCameraStreamCallback(dji_osdk_ros::SetupCameraStream::Request&  request,
                                  dji_osdk_ros::SetupCameraStream::Response& response);
+  void publishCameraInfo(const std_msgs::Header &header);
 #endif
 
   //! data broadcast callback
@@ -343,6 +345,8 @@ class DJISDKNode
   ros::ServiceServer subscribe_stereo_depth_server;
   ros::ServiceServer subscribe_stereo_vga_server;
   ros::ServiceServer camera_stream_server;
+  ros::Publisher left_camera_info_pub_;
+ 	ros::Publisher right_camera_info_pub_; 
 #endif
 
   //! flight control subscribers

launch/dji_vehicle_node.launch

@@ -4,10 +4,11 @@
     <param name="acm_name" type="string" value="/dev/ttyACM0"/>
     <param name="serial_name" type="string" value="/dev/ttyUSB0"/>
     <param name="baud_rate" type="int" value="921600"/>
-    <param name="app_id" type="int" value="12345"/>
+    <param name="app_id" type="int" value= "1023987"/>
     <param name="app_version" type="int" value="1"/>
     <param name="align_time" type="bool" value="false"/>
-    <param name="enc_key" type="string" value="abc123"/>
+    <param name="enc_key" type="string" value="afe96210ea022d24e17808d30229be0cfa5d41d75bb37f4965f1502b6d3cc62b"/>
     <param name="use_broadcast" type="bool" value="false"/>
+    <param name="dxc" type="bool" value="false"/>
     </node>
 </launch>

launch/stereo_camera.launch

@@ -0,0 +1,8 @@
+<?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"/>
+</launch>

src/dji_osdk_ros/dji_vehicle_node.cpp

@@ -30,11 +30,16 @@
 #include <dji_osdk_ros/dji_vehicle_node.h>
 #include <dji_osdk_ros/vehicle_wrapper.h>
 
-#ifdef OPEN_CV_INSTALLED
+/*#ifdef OPEN_CV_INSTALLED
+#include <dji_osdk_ros/stereo_utility/m300_stereo_param_tool.hpp>
+#endif*/
 #include <dji_osdk_ros/stereo_utility/m300_stereo_param_tool.hpp>
+#include <vector>
+
+#ifdef ADVANCED_SENSING
+#include <sensor_msgs/CameraInfo.h>
 #endif
 
-#include <vector>
 //CODE
 using namespace dji_osdk_ros;
 #define M300_FRONT_STEREO_PARAM_YAML_NAME "m300_front_stereo_param.yaml"
@@ -83,6 +88,7 @@ VehicleNode::VehicleNode():telemetry_from_fc_(TelemetryType::USE_ROS_BROADCAST),
   initCameraModule();
   initService();
   initTopic();
+  initTimer();
 }
 
 VehicleNode::~VehicleNode()
@@ -271,6 +277,7 @@ void VehicleNode::initService()
   subscribe_stereo_240p_server_  = nh_.advertiseService("stereo_240p_subscription",   &VehicleNode::stereo240pSubscriptionCallback, this);
   subscribe_stereo_depth_server_ = nh_.advertiseService("stereo_depth_subscription",  &VehicleNode::stereoDepthSubscriptionCallback,this);
   subscribe_stereo_vga_server_   = nh_.advertiseService("stereo_vga_subscription",    &VehicleNode::stereoVGASubscriptionCallback,  this);
+  get_m300_stereo_params_server_ = nh_.advertiseService("get_m300_stereo_params", &VehicleNode::getM300StereoParamsCallback, this);
 #ifdef OPEN_CV_INSTALLED
   /*! @brief
    *  get m300 stereo params server
@@ -322,6 +329,14 @@ void VehicleNode::initService()
   ROS_INFO_STREAM("Services startup!");
 }
 
+void VehicleNode::initTimer()
+{
+#ifdef ADVANCED_SENSING
+	//info_timer_ = nh_.createTimer(ros::Duration(0.05), &VehicleNode::infoTimerCB, this);
+#endif
+}
+
+
 bool VehicleNode::initTopic()
 {
   attitude_publisher_ = nh_.advertise<geometry_msgs::QuaternionStamped>("dji_osdk_ros/attitude", 10);
@@ -396,8 +411,11 @@ bool VehicleNode::initTopic()
   stereo_240p_down_front_publisher_ = nh_.advertise<sensor_msgs::Image>("dji_osdk_ros/stereo_240p_down_front_images", 10);
   stereo_240p_down_back_publisher_ = nh_.advertise<sensor_msgs::Image>("dji_osdk_ros/stereo_240p_down_back_images", 10);
   stereo_240p_front_depth_publisher_ = nh_.advertise<sensor_msgs::Image>("dji_osdk_ros/stereo_240p_front_depth_images", 10);
-  stereo_vga_front_left_publisher_ = nh_.advertise<sensor_msgs::Image>("dji_osdk_ros/stereo_vga_front_left_images", 10);
-  stereo_vga_front_right_publisher_ = nh_.advertise<sensor_msgs::Image>("dji_osdk_ros/stereo_vga_front_right_images", 10);
+  stereo_vga_front_left_publisher_ = nh_.advertise<sensor_msgs::Image>("dji_osdk_ros/stereo_vga_front/left/image_raw", 10);
+  stereo_vga_front_right_publisher_ = nh_.advertise<sensor_msgs::Image>("dji_osdk_ros/stereo_vga_front/right/image_raw", 10);
+  left_camera_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>("/dji_osdk_ros/stereo_vga_front/left/camera_info",10);
+  right_camera_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>("/dji_osdk_ros/stereo_vga_front/right/camera_info",10);
+  
   #endif
 
   waypointV2_mission_state_publisher_ = nh_.advertise<dji_osdk_ros::WaypointV2MissionStatePush>("dji_osdk_ros/waypointV2_mission_state", 10);
@@ -914,8 +932,6 @@ bool VehicleNode::stereoVGASubscriptionCallback(dji_osdk_ros::StereoVGASubscript
 
   return true;
 }
-
-#ifdef OPEN_CV_INSTALLED
 bool VehicleNode::getM300StereoParamsCallback(dji_osdk_ros::GetM300StereoParams::Request& request, 
                                               dji_osdk_ros::GetM300StereoParams::Response& response)
 {
@@ -943,6 +959,34 @@ bool VehicleNode::getM300StereoParamsCallback(dji_osdk_ros::GetM300StereoParams:
 
   return response.result;
 }
+#ifdef OPEN_CV_INSTALLED
+/*bool VehicleNode::getM300StereoParamsCallback(dji_osdk_ros::GetM300StereoParams::Request& request, 
+                                              dji_osdk_ros::GetM300StereoParams::Response& response)
+{
+  ROS_INFO("called getM300StereoParamsCallback");
+
+  if(ptr_wrapper_ == nullptr)
+  {
+    ROS_ERROR_STREAM("Vehicle modules is nullptr");
+    response.result = false;
+  }
+
+  Vehicle* vehicle = ptr_wrapper_->getVehicle();
+  M300StereoParamTool *tool = new M300StereoParamTool(vehicle);
+  Perception::CamParamType stereoParam =
+      tool->getM300stereoParams(Perception::DirectionType::RECTIFY_FRONT);
+  if (tool->createStereoParamsYamlFile(M300_FRONT_STEREO_PARAM_YAML_NAME, stereoParam))
+  {
+    tool->setParamFileForM300(M300_FRONT_STEREO_PARAM_YAML_NAME);
+    response.result = true;
+  }
+  else
+  {
+    response.result = false;
+  }
+
+  return response.result;
+}*/
 #endif
 #endif
 

src/dji_osdk_ros/modules/CMakeLists.txt

@@ -84,13 +84,15 @@ if(darknet_ros_FOUND)
     set(ENABLE_DARKNET_ROS 1 CACHE INTERNAL "ENABLE_DARKNET_ROS")
 endif()
 
-if (OPEN_CV_3_3_0_INSTALLED AND FOUND_OPENCV_VIZ)
-    FILE(GLOB SOURCES *.cc *.cpp *.c *.cxx stereo_utility/*cpp)
-    include_directories(../../../include/dji_osdk_ros/stereo_utility)
-    else()
-    FILE(GLOB SOURCES *.cc *.cpp *.c *.cxx)
-endif()
-
+#if (OPEN_CV_3_3_0_INSTALLED AND FOUND_OPENCV_VIZ)
+#    FILE(GLOB SOURCES *.cc *.cpp *.c *.cxx stereo_utility/*cpp)
+#    include_directories(../../../include/dji_osdk_ros/stereo_utility)
+#    else()
+#    FILE(GLOB SOURCES *.cc *.cpp *.c *.cxx)
+#endif()
+FILE(GLOB SOURCES *.cc *.cpp *.c *.cxx stereo_utility/*cpp)
+include_directories(../../../include/dji_osdk_ros/stereo_utility)
+    
 ADD_LIBRARY(${PROJECT_NAME} SHARED ${HEADERS} ${SOURCES})
 MESSAGE(STATUS "PROJECT_NAME: ${PROJECT_NAME}")
 MESSAGE(STATUS "DJIOSDK_LIB: ${DJIOSDK_LIBRARIES}")
@@ -114,4 +116,4 @@ target_include_directories(${PROJECT_NAME} PRIVATE ${OpenCV_INCLUDE_DIRS})
 
 if (OpenCV_FOUND)
 	target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS})
-endif ()
+endif ()

src/dji_osdk_ros/modules/dji_vehicle_node_publisher.cpp

@@ -815,8 +815,42 @@ void VehicleNode::publishVGAStereoImage(Vehicle*            vehicle,
   img.header.frame_id = "vga_right";
   memcpy((char*)(&img.data[0]), recvFrame.recvData.stereoVGAImgData->img_vec[1], 480*640);
   node_ptr->stereo_vga_front_right_publisher_.publish(img);
+  node_ptr->publishCameraInfo(img.header);
+  
 }
 
+#ifdef ADVANCED_SENSING	
+void VehicleNode::publishCameraInfo(const std_msgs::Header &header)
+{	
+	static bool isFirstTime = true;
+	
+	static sensor_msgs::CameraInfo left_camera_info;
+	static sensor_msgs::CameraInfo right_camera_info;
+			
+	if(isFirstTime)
+	{
+		left_camera_info.distortion_model = right_camera_info.distortion_model = "plumb_bob";
+		left_camera_info.width = right_camera_info.width = 640;
+		left_camera_info.height = right_camera_info.height = 480;
+			
+
+		Vehicle* vehicle = ptr_wrapper_->getVehicle();
+		M300StereoParamTool *tool = new M300StereoParamTool(vehicle);
+		Perception::CamParamType stereoParam = tool->getM300stereoParams(Perception::DirectionType::RECTIFY_FRONT);
+	
+		tool->getM300stereoCameraInfo(stereoParam, left_camera_info, right_camera_info);
+		isFirstTime = false;
+	}
+		
+	left_camera_info.header = right_camera_info.header = header;
+	left_camera_info.header.frame_id = "left_camera";
+	right_camera_info.header.frame_id = "right_camera";
+	left_camera_info_pub_.publish(left_camera_info);
+	right_camera_info_pub_.publish(right_camera_info);
+
+}
+#endif
+
 void VehicleNode::publishMainCameraImage(CameraRGBImage rgbImg, void* userData)
 {
   VehicleNode *node_ptr = (VehicleNode *)userData;

src/dji_osdk_ros/modules/stereo_utility/m300_stereo_param_tool.cpp

@@ -58,6 +58,71 @@ void M300StereoParamTool::PerceptionCamParamCB(
     }
 }
 
+void M300StereoParamTool::getM300stereoCameraInfo(Perception::CamParamType param, sensor_msgs::CameraInfo &left,
+  sensor_msgs::CameraInfo &right)
+{
+		DoubleCamParamType doubleCameraParams;
+  	doubleCameraParams.direction = param.direction;
+  
+  	/*! Convert float camera parameters matrix to be double matrix */
+		for (int i = 0; i < sizeof(param.leftIntrinsics); i++) 
+		{
+		  doubleCameraParams.leftIntrinsics[i] = (double)param.leftIntrinsics[i];
+		  doubleCameraParams.rightIntrinsics[i] = (double)param.rightIntrinsics[i];
+		  doubleCameraParams.rotaionLeftInRight[i] = (double)param.rotaionLeftInRight[i];
+		}
+  	  for (int i = 0; i < sizeof(param.translationLeftInRight); i++)
+    doubleCameraParams.translationLeftInRight[i] = (double)param.translationLeftInRight[i];
+
+		cv::Mat leftCameraIntrinsicMatrix(3, 3, CV_64F, doubleCameraParams.leftIntrinsics);
+		cv::Mat rightCameraIntrinsicMatrix(3, 3, CV_64F, doubleCameraParams.rightIntrinsics);
+		
+
+		//cv::Mat leftDistCoeffs = cv::Mat::zeros(5, 1, CV_64F);
+		//cv::Mat rightDistCoeffs = cv::Mat::zeros(5, 1, CV_64F);
+		//loading the distortion matrix
+		left.D = std::vector<double>(5,0.0);
+		right.D = std::vector<double>(5,0.0);
+		
+		cv::Mat leftRectificationMatrix = cv::Mat::eye(3, 3, CV_64F);
+		cv::Mat rightRectificationMatrix = cv::Mat::eye(3, 3, CV_64F);
+		cv::Mat translationLeftInRight(3, 1, CV_64F, doubleCameraParams.translationLeftInRight);
+		cv::Mat leftProjectionMatrix(leftCameraIntrinsicMatrix.rows, leftCameraIntrinsicMatrix.cols + translationLeftInRight.cols, CV_64F);
+		cv::hconcat(leftCameraIntrinsicMatrix, translationLeftInRight, leftProjectionMatrix);
+		cv::Mat rightProjectionMatrix(rightCameraIntrinsicMatrix.rows, rightCameraIntrinsicMatrix.cols + translationLeftInRight.cols, CV_64F);
+		cv::hconcat(rightCameraIntrinsicMatrix, translationLeftInRight, rightProjectionMatrix);
+		
+		//loading the rectification matrix
+		for(int r = 0; r < leftRectificationMatrix.rows; r++)
+		{
+			for(int c = 0; c < leftRectificationMatrix.cols; c++)
+			{
+				left.R[r*leftCameraIntrinsicMatrix.cols + c] = leftRectificationMatrix.at<double>(r,c);
+				right.R[r*rightRectificationMatrix.cols + c] = rightRectificationMatrix.at<double>(r,c);
+			}
+		}
+		
+		//loading the camera intrinsic matrix
+		for(int r = 0; r < leftCameraIntrinsicMatrix.rows; r++)
+		{
+			for(int c = 0; c < leftCameraIntrinsicMatrix.cols; c++)
+			{
+				left.K[r*leftCameraIntrinsicMatrix.cols + c] = leftCameraIntrinsicMatrix.at<double>(r,c);
+				right.K[r*rightCameraIntrinsicMatrix.cols + c] = rightCameraIntrinsicMatrix.at<double>(r,c);
+			}
+		}
+		
+		//loading the projection matrix
+		for(int r = 0; r < leftProjectionMatrix.rows; r++)
+		{
+			for(int c = 0; c < leftProjectionMatrix.cols; c++)
+			{
+				left.P[r*leftProjectionMatrix.cols + c] = leftProjectionMatrix.at<double>(r,c);
+				right.P[r*rightProjectionMatrix.cols + c] = rightProjectionMatrix.at<double>(r,c);
+			}
+		}
+}
+
 bool M300StereoParamTool::createStereoParamsYamlFile(std::string fileName,
                                                      Perception::CamParamType param) {
   DoubleCamParamType doubleCameraParams;