feat: auto-unsubscribtion to the vga stream

Author: alireza@general

include/dji_osdk_ros_obsoleted/dji_sdk/dji_sdk_node.h

@@ -299,6 +299,11 @@ class DJISDKNode
 #endif
 
 private:
+
+#ifdef ADVANCED_SENSING
+	int latest_camera_ = {-1};
+#endif	
+	
   //! OSDK core
   Vehicle* vehicle;
   LinuxSetup* linuxEnvironment;

src/dji_osdk_ros_obsoleted/modules/dji_sdk_node.cpp

@@ -77,6 +77,15 @@ DJISDKNode::~DJISDKNode()
   {
     cleanUpSubscribeFromFC();
   }
+
+#ifdef ADVANCED_SENSING
+	if(latest_camera_ > 0)
+	{
+		vehicle->advancedSensing->unsubscribeVGAImages(latest_camera_);
+		ROS_INFO("Unsubscribed from the VGA Stream");
+	}
+#endif
+
   if (vehicle)
   {
     delete vehicle;

src/dji_osdk_ros_obsoleted/modules/dji_sdk_node_publisher.cpp

@@ -241,7 +241,7 @@ DJISDKNode::publish5HzData(Vehicle *vehicle, RecvContainer recvFrame,
 
   //TODO: publish gps detail data if needed
   Telemetry::TypeMap<Telemetry::TOPIC_BATTERY_INFO>::type battery_info=
-    vehicle->subscribe->getValue<Tele/points2metry::TOPIC_BATTERY_INFO>();
+    vehicle->subscribe->getValue<Telemetry::TOPIC_BATTERY_INFO>();
   sensor_msgs::BatteryState msg_battery_state;
   msg_battery_state.header.stamp = msg_time;
   msg_battery_state.capacity = battery_info.capacity;
@@ -279,7 +279,7 @@ DJISDKNode::publish5HzData(Vehicle *vehicle, RecvContainer recvFrame,
     sensor_msgs::NavSatFix rtk_position;
     rtk_position.header.stamp = msg_time;
     rtk_position.latitude = rtk_telemetry_position.latitude;
-    rtk_position.longitude = rtk_telem/points2etry_position.longitude;
+    rtk_position.longitude = rtk_telemetry_position.longitude;
     rtk_position.altitude = rtk_telemetry_position.HFSL;
     p->rtk_position_publisher.publish(rtk_position);
 
@@ -396,7 +396,6 @@ DJISDKNode::publish50HzData(Vehicle* vehicle, RecvContainer recvFrame,
   std_msgs::Float32 height;
   height.data = fused_height;
   p->height_publisher.publish(height);
-/points2
   Telemetry::TypeMap<Telemetry::TOPIC_STATUS_FLIGHT>::type fs =
     vehicle->subscribe->getValue<Telemetry::TOPIC_STATUS_FLIGHT>();
 
@@ -433,7 +432,7 @@ DJISDKNode::publish50HzData(Vehicle* vehicle, RecvContainer recvFrame,
 
   geometry_msgs::Vector3Stamped gimbal_angle_vec3;
 
-  gimbal_angle_vec3.header.stamp = ros::Time::now()/points2;
+  gimbal_angle_vec3.header.stamp = ros::Time::now();
   gimbal_angle_vec3.vector.x     = gimbal_angle.x;
   gimbal_angle_vec3.vector.y     = gimbal_angle.y;
   gimbal_angle_vec3.vector.z     = gimbal_angle.z;

src/dji_osdk_ros_obsoleted/modules/dji_sdk_node_services.cpp

@@ -394,6 +394,8 @@ DJISDKNode::stereoDepthSubscriptionCallback(dji_osdk_ros::StereoDepthSubscriptio
   return true;
 }
 
+
+
 bool
 DJISDKNode::stereoVGASubscriptionCallback(dji_osdk_ros::StereoVGASubscription::Request&  request,
                                           dji_osdk_ros::StereoVGASubscription::Response& response)
@@ -402,9 +404,10 @@ DJISDKNode::stereoVGASubscriptionCallback(dji_osdk_ros::StereoVGASubscription::R
 
   if (request.unsubscribe_vga == 1)
   {
-    vehicle->advancedSensing->unsubscribeVGAImages();
+    vehicle->advancedSensing->unsubscribeVGAImages(request.front_vga);
     response.result = true;
     ROS_INFO("unsubscribe stereo vga images");
+    latest_camera_ = -1;
     return true;
   }
 
@@ -415,17 +418,37 @@ DJISDKNode::stereoVGASubscriptionCallback(dji_osdk_ros::StereoVGASubscription::R
     response.result = false;
     return true;
   }
-
-  if (request.front_vga == 1)
-  {
+	
+	
+	/* In this version of the function, request.front_vga represent the side of the camera according to the followings 	
+	 *	1 ---------------------> front
+   *	2 ---------------------> left
+   *	3 ---------------------> right
+   *	4 ---------------------> rear
+   *	5 ---------------------> down
+   *	6 ---------------------> top 
+   */
+  if (request.front_vga > 0 && request.front_vga <= 6)
+  {
+  	//un-subscribe from the previous stream first
+  	if(request.front_vga != latest_camera_ && latest_camera_ > 0)
+  	{
+  		vehicle->advancedSensing->unsubscribeVGAImages(latest_camera_);
+		  ROS_INFO("unsubscribe stereo vga images");
+		  latest_camera_ = -1;
+		  ros::Duration(1).sleep();
+  	}
+  
     this->stereo_vga_subscription_success = false;
-    vehicle->advancedSensing->subscribeFrontStereoVGA(request.vga_freq, &publishVGAStereoImage, this);
+    vehicle->advancedSensing->subscribeFrontStereoVGA(request.vga_freq, request.front_vga, &publishVGAStereoImage, this);
     ros::Duration(1).sleep();
   }
 
   if (this->stereo_vga_subscription_success == true)
   {
     response.result = true;
+    //if the service call returns success, only then update the buffer
+    latest_camera_ = request.front_vga;
   }
   else
   {