feat: individual camera info for each vga camera

Author: alireza@general

include/dji_osdk_ros_obsoleted/dji_sdk/dji_sdk_node.h

@@ -231,6 +231,8 @@ class DJISDKNode
   bool setupCameraStreamCallback(dji_osdk_ros::SetupCameraStream::Request&  request,
                                  dji_osdk_ros::SetupCameraStream::Response& response);
   void publishCameraInfo(const std_msgs::Header &header);
+  
+  sensor_msgs::CameraInfo getCameraInfo(int camera_select);
 #endif
 
   //! data broadcast callback

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_FRONT);
+      tool->getM300stereoParams(Perception::DirectionType::RECTIFY_LEFT);
   if (tool->createStereoParamsYamlFile(M300_FRONT_STEREO_PARAM_YAML_NAME, stereoParam))
   {
     tool->setParamFileForM300(M300_FRONT_STEREO_PARAM_YAML_NAME);

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_FRONT)) {
+          && (pack.cameraParam[i].direction == Perception::RECTIFY_LEFT)) {
         auto camParam = (Perception::CamParamType *) userData;
         *camParam = pack.cameraParam[i];
       }

src/dji_osdk_ros_obsoleted/modules/dji_sdk_node_publisher.cpp

@@ -792,6 +792,50 @@ void DJISDKNode::alignRosTimeWithFlightController(ros::Time now_time, uint32_t t
 }
 
 #ifdef ADVANCED_SENSING
+
+sensor_msgs::CameraInfo DJISDKNode::getCameraInfo(int camera_select)
+{
+	sensor_msgs::CameraInfo cam_info;
+	
+	cam_info.width = 640;
+	cam_info.height = 480;
+	
+	cam_info.distortion_model = "plumb_bob";
+	
+	cam_info.D = std::vector<double>(5,0.0);
+	
+	switch(camera_select)
+	{
+		//front camera
+		case 1:
+		{
+			cam_info.K = {407.1327819824219, 0, 321.8077392578125, 0.0, 407.1327819824219, 236.9904937744141, 0.0, 0.0, 1.0};
+			cam_info.R = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
+			cam_info.P = {407.1327819824219, 0, 321.8077392578125, -116.0410842895508, 0.0, 407.1327819824219, 236.9904937744141, 0.0, 0.0, 0.0, 1.0, 0.0};
+			break;
+		}
+		//left camera
+		case 2:
+		{
+			cam_info.K = {488.5599365234375, 0.0, 319.80328369140625, 0.0, 488.5599365234375, 239.97528076171875, 0.0, 0.0, 1.0};
+			cam_info.R = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
+			cam_info.P = {488.5599365234375, 0.0, 319.80328369140625, -99.43592071533203, 0.0, 488.5599365234375, 239.97528076171875, 0.0, 0.0, 0.0, 1.0, 0.0};
+			break;
+		}
+		//up camera
+		case 6:
+		{
+			cam_info.K = {405.4823913574219, 0.0, 322.5506896972656, 0.0, 405.4823913574219, 240.0494689941406, 0.0, 0.0, 1.0};
+			cam_info.R = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
+			cam_info.P = {405.4823913574219, 0.0, 322.5506896972656, 0.0, 0.0, 405.4823913574219, 240.0494689941406, 96.79345703125, 0.0, 0.0, 1.0, 0.0};
+			break;
+		}
+	}
+	
+	
+	return cam_info;
+}
+
 void DJISDKNode::publishCameraInfo(const std_msgs::Header &header)
 {	
 	static bool isFirstTime = true;