develop downsampling and publish rgb-compressedImage

Tello_ROS_ORBSLAM/ROS/tello_catkin_ws/src/flock/flock_driver/launch/orbslam2_with_cloud_map.launch

@@ -35,15 +35,15 @@
 
        <param name="load_calibration_from_cam" type="bool" value="false" />
        <!-- Camera calibration and distortion parameters (OpenCV) -->
-      <param name="camera_fx" type="double" value="924.873180" />
-      <param name="camera_fy" type="double" value="923.504522" />
-      <param name="camera_cx" type="double" value="486.997346" />
-      <param name="camera_cy" type="double" value="364.308527" />
+      <param name="camera_fx" type="double" value="923.142686" />
+      <param name="camera_fy" type="double" value="923.825922" />
+      <param name="camera_cx" type="double" value="460.935256" />
+      <param name="camera_cy" type="double" value="340.083201" />
        <!-- Camera calibration and distortion parameters (OpenCV) -->
-      <param name="camera_k1" type="double" value="-0.034749" />
-      <param name="camera_k2" type="double" value="0.071514" />
-      <param name="camera_p1" type="double" value="0.000363" />
-      <param name="camera_p2" type="double" value="0.003131" />
+      <param name="camera_k1" type="double" value="-0.012958" />
+      <param name="camera_k2" type="double" value="0.090227" />
+      <param name="camera_p1" type="double" value="0.000519" />
+      <param name="camera_p2" type="double" value="-0.000980" />
       <param name="camera_k3" type="double" value="0.0" />
   </node>
 
@@ -75,6 +75,12 @@
 
  </node>
 
+<node name="image_republish" pkg="image_transport" type="republish" args="raw in:=/tello/camera/image_rgb compressed out:=/image_exp"/>
+
+<node name="image_view" pkg="image_view" type="image_view" >
+  <remap from="image" to="/image_exp"/>
+</node>
+
 
 <node name="tello_slam_control" pkg="flock_driver" type="tello_slam_control.py" output="screen">
   <param name="~POSE_TOPIC_NAME" type="string" value="/orb_slam2_mono/pose" />

Tello_ROS_ORBSLAM/ROS/tello_catkin_ws/src/flock/flock_driver/src/flock_driver.py

@@ -78,6 +78,7 @@ def __init__(self):
         # ROS publishers
         self._flight_data_pub = rospy.Publisher(self.publish_prefix+'flight_data', FlightData, queue_size=10)
         self._image_pub = rospy.Publisher(self.publish_prefix+'camera/image_raw', Image, queue_size=10)
+        self._image_pub_rgb = rospy.Publisher(self.publish_prefix+'camera/image_rgb', Image, queue_size=10)
         self._wifi_strength_data_pub = rospy.Publisher(self.publish_prefix+'wifi_strength', Int32, queue_size=10)
         self._light_strength_data_pub = rospy.Publisher(self.publish_prefix+'light_strength', Int32, queue_size=10)
         self._camera_info_pub = rospy.Publisher(self.publish_prefix+'camera/camera_info', CameraInfo, queue_size=10)
@@ -133,6 +134,8 @@ def __init__(self):
         rospy.loginfo(self._drone)
         self.packet_data = ""
         self._drone.subscribe(self._drone.EVENT_VIDEO_FRAME, self.videoFrameHandler)
+	print("-------------------")
+	print(self._drone.EVENT_VIDEO_FRAME)
         rospy.loginfo(FlightData())
         # rospy.on_shutdown(self.cleanup)
 
@@ -364,6 +367,9 @@ def videoFrameHandler(self, event, sender, data):
                 # print("decoded frame")  
                 # Convert PyAV frame => PIL image => OpenCV Mat
                 # color_mat = cv2.cvtColor(numpy.array(frame.to_image()), cv2.COLOR_RGB2BGR)
+		img_rotate_90_clockwise = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE)
+                img_rgb_msg = self._cv_bridge.cv2_to_imgmsg(img_rotate_90_clockwise, 'rgb8')
+                self._image_pub_rgb.publish(img_rgb_msg)
                 color_mat = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
 
                 # Convert OpenCV Mat => ROS Image message and publish
@@ -378,7 +384,6 @@ def videoFrameHandler(self, event, sender, data):
             self.packet_data = ""
 
 
-
     def _h264_decode(self, packet_data):
         """
         decode raw h264 format data from Tello

Tello_ROS_ORBSLAM/ROS/tello_catkin_ws/src/flock/flock_driver/src/tello_keyboard.py

@@ -276,15 +276,16 @@ def collision_callback(msg):
 
     startFlag = False
     experimentFlag = True
+    colliCnt = 0
 
     try:
         while not rospy.is_shutdown():
-	    print(isStop)
 	    if isStop:
                 keyname = pygame.key.name(e.key)
 		if keyname in list_of_pressed_keys:
                     list_of_pressed_keys.remove(keyname)
 		twist = reset_command(keyname, twist)
+		colliCnt += 1;
 
             for keyname in list_of_pressed_keys:
                 print('raise_com: ' + keyname)
@@ -325,6 +326,8 @@ def collision_callback(msg):
 			    [file.write(str(element)+'\n') for element in list_of_times_keys]
 			print("---EXPERIMENT TIME---")			
 			print(time.time() - experimentTime)
+			print("---COLLISION COUNT---")			
+			print(colliCnt)
                         raise KeyboardInterrupt
 
                     if keyname == 'backspace':

Tello_ROS_ORBSLAM/ROS/tello_catkin_ws/src/orb_slam_2_ros/.gitignore

@@ -7,7 +7,7 @@ Thirdparty/DBoW2/lib/
 Thirdparty/g2o/build/
 Thirdparty/g2o/config.h
 Thirdparty/g2o/lib/
-Vocabulary/ORBvoc.txt
+orb_slam2/Vocabulary/ORBvoc.txt
 build/
 *~
 lib/