develop auto pilot system

uzumal · Nov 7, 2022 · 040e502 · 040e502
1 parent 0816111
commit 040e502
Show file tree

Hide file tree

Showing 5 changed files with 542 additions and 13 deletions.
diff --git a/Tello_ROS_ORBSLAM/ROS/tello_catkin_ws/src/flock/flock_driver/launch/orbslam2_auto.launch b/Tello_ROS_ORBSLAM/ROS/tello_catkin_ws/src/flock/flock_driver/launch/orbslam2_auto.launch
@@ -0,0 +1,104 @@
+<launch>
+
+  <node pkg="tf" type="static_transform_publisher" name="world" args="0 0 0 0 0 0 map world 100" /> 
+
+  <node name="orb_slam2_mono" pkg="orb_slam2_ros" type="orb_slam2_ros_mono" output="screen">
+
+
+      <remap from="/camera/image_raw" to="/tello/camera/image_raw" />
+
+      <param name="publish_pointcloud" type="bool" value="true" />
+      <param name="publish_pose" type="bool" value="true" />
+      <param name="localize_only" type="bool" value="false" />
+      <param name="reset_map" type="bool" value="false" />
+
+      <!-- static parameters -->
+      <!-- <param name="use_viewer" type="bool" value="true" /> -->
+      <param name="load_map" type="bool" value="false" />
+      <param name="map_file" type="string" value="map.bin" />
+      <param name="voc_file" type="string" value="$(find orb_slam2_ros)/orb_slam2/Vocabulary/ORBvoc.txt" />
+
+      <param name="pointcloud_frame_id" type="string" value="map" />
+      <param name="camera_frame_id" type="string" value="camera_link" />
+      <param name="min_num_kf_in_map" type="int" value="15" />
+
+       <!-- ORB parameters -->
+       <param name="/ORBextractor/nFeatures" type="int" value="2500" />
+       <param name="/ORBextractor/scaleFactor" type="double" value="1.2" />
+       <param name="/ORBextractor/nLevels" type="int" value="8" />
+       <param name="/ORBextractor/iniThFAST" type="int" value="20" />
+       <param name="/ORBextractor/minThFAST" type="int" value="7" />
+
+       <param name="camera_fps" type="int" value="30" />
+       <!-- Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) -->
+       <!-- <param name="camera_rgb_encoding" type="bool" value="true" /> -->
+
+       <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" />
+       <!-- 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_k3" type="double" value="0.0" />
+  </node>
+
+  <!-- <node name="image_muxer" pkg="flock_driver" type="image_muxer.py" output="screen" /> -->
+
+  <node name="cloud_map_saver" pkg="flock_driver" type="cloud_map_saver.py" output="screen">
+  <param name="OUT_FILE_PATH" type="string" value="/home/uzu/map_point.txt" />
+  <param name="CLOUD_TOPIC_NAME" type="string" value="/orb_slam2_mono/map_points" />
+  <param name="TRIGGER_TOPIC_NAME" type="string" value="/save_cloud_trigger" />
+  <param name="POSE_TOPIC_NAME" type="string" value="/orb_slam2_mono/pose" />
+  <param name="CAMERA_TOPIC_NAME" type="string" value="/orb_slam2_mono/debug_image" />
+
+
+</node>
+
+
+ <node name="flock_driver_node" pkg="flock_driver" type="flock_driver.py" output="screen">
+    <param name="network_interface" type="string" value="" />
+    <param name="ID" type="string" value="" />
+
+
+
+    <!-- <param name="network_interface" type="string" value="" /> -->
+    <!-- <param name="TELLO_IP" type="string" value="192.168.43.98" /> -->
+
+    <!-- <remap from="tello0/camera/image_raw" to="rectify/image_mono" /> -->
+    <!-- <remap from="/rectify/image_mono" to="tello0/camera/image_mono" /> -->
+    <!-- <remap from="tello0/camera/image_raw" to="/image" /> -->
+
+ </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" />
+  <param name="ID" type="string" value="" />
+</node>
+
+
+<node name="tello_ui" pkg="flock_driver" type="tello_ui.py" output="screen">
+  <param name="ID" type="string" value="" />
+  <param name="~POSE_TOPIC_NAME" type="string" value="/orb_slam2_mono/pose" />
+</node> 
+
+  <node name="tello_keyboard_node" pkg="flock_driver" type="tello_autodrive.py" output="screen">
+      <param name="ID" type="string" value="" />
+      <param name="CMD_FILE_PATH" type="string" value="/home/uzu/Log/cmdList.txt" />
+      <param name="CMD_TIME_FILE_PATH" type="string" value="/home/uzu/Log/cmdTimeList.txt" />
+      <!-- <remap from="/tello/cmd_vel" to="/tello0/cmd_vel" /> -->
+      <!-- <remap from="/tello/takeoff" to="/tello0/takeoff" /> -->
+      <!-- <remap from="/tello/land" to="/tello0/land" /> -->
+
+  </node>
+
+<node name="rviz" pkg="rviz" type="rviz" output="screen" />
+
+
+
+</launch>
diff --git a/..._ORBSLAM/ROS/tello_catkin_ws/src/flock/flock_driver/launch/orbslam2_with_cloud_map.launch b/..._ORBSLAM/ROS/tello_catkin_ws/src/flock/flock_driver/launch/orbslam2_with_cloud_map.launch
@@ -23,7 +23,7 @@
       <param name="min_num_kf_in_map" type="int" value="15" />
 
        <!-- ORB parameters -->
-       <param name="/ORBextractor/nFeatures" type="int" value="2000" />
+       <param name="/ORBextractor/nFeatures" type="int" value="2500" />
        <param name="/ORBextractor/scaleFactor" type="double" value="1.2" />
        <param name="/ORBextractor/nLevels" type="int" value="8" />
        <param name="/ORBextractor/iniThFAST" type="int" value="20" />
@@ -50,7 +50,7 @@
   <!-- <node name="image_muxer" pkg="flock_driver" type="image_muxer.py" output="screen" /> -->
 
   <node name="cloud_map_saver" pkg="flock_driver" type="cloud_map_saver.py" output="screen">
-  <param name="OUT_FILE_PATH" type="string" value="/home/uzu/map_point.txt" />
+  <param name="OUT_FILE_PATH" type="string" value="/home/Log/map_point.txt" />
   <param name="CLOUD_TOPIC_NAME" type="string" value="/orb_slam2_mono/map_points" />
   <param name="TRIGGER_TOPIC_NAME" type="string" value="/save_cloud_trigger" />
   <param name="POSE_TOPIC_NAME" type="string" value="/orb_slam2_mono/pose" />
@@ -89,6 +89,9 @@
 
   <node name="tello_keyboard_node" pkg="flock_driver" type="tello_keyboard.py" output="screen">
       <param name="ID" type="string" value="" />
+      <param name="CMD_FILE_PATH" type="string" value="/home/uzu/Log/cmdList.txt" />
+      <param name="CMD_TIME_FILE_PATH" type="string" value="/home/uzu/Log/cmdTimeList.txt" />
+      <param name="SPENT_FILE_PATH" type="string" value="/home/uzu/Log/spentTimeList.txt" />
       <!-- <remap from="/tello/cmd_vel" to="/tello0/cmd_vel" /> -->
       <!-- <remap from="/tello/takeoff" to="/tello0/takeoff" /> -->
       <!-- <remap from="/tello/land" to="/tello0/land" /> -->

diff --git a/Tello_ROS_ORBSLAM/ROS/tello_catkin_ws/src/flock/flock_driver/src/cloud_map_saver.py b/Tello_ROS_ORBSLAM/ROS/tello_catkin_ws/src/flock/flock_driver/src/cloud_map_saver.py
@@ -460,11 +460,12 @@ def point_cloud_callback(self, point_cloud):
 
 
     def track_callback(self, track_data):
-	if track_data.transforms[0].transform.translation.x == 0:
-	    if track_data.transforms[0].transform.translation.y == 0:
-	        self.track_data_pub.publish("track losts")
-	else:
-	    self.track_data_pub.publish("track exists")
+	if track_data.transforms[0].child_frame_id == "base_link":
+	    if track_data.transforms[0].transform.translation.x == 0:
+	        if track_data.transforms[0].transform.translation.y == 0:
+	            self.track_data_pub.publish("track losts")
+	    else:
+	        self.track_data_pub.publish("track exists")
 
     def point_cloud_server1_callback(self, point_cloud):
         fields_str = str(point_cloud.fields)