├── .gitignore ├── src ├── __pycache__ │ ├── idPD.cpython-38.pyc │ ├── depth2pcd.cpython-38.pyc │ ├── simple_tf.cpython-38.pyc │ ├── pointcloud_carli.cpython-38.pyc │ └── projectPointcloud.cpython-38.pyc ├── .vscode │ ├── settings.json │ ├── tasks.json │ ├── launch.json │ └── c_cpp_properties.json ├── cmd_vel2command.py ├── idPD.py ├── get_odom.py ├── trajectory_view.py ├── simple_tf.py ├── Tello_node.py ├── path_following.py ├── pd_topoint.py ├── depthmap_calibration.py ├── planner_ctrl.py └── depth2pcd.py ├── launch ├── rviz.launch ├── tello.launch ├── pad_ctrl.launch ├── slam.launch └── ego_tello.launch ├── LICENSE ├── config ├── tello.yaml ├── tello_advanced_param.xml └── default.rviz ├── package.xml ├── README.MD ├── CMakeLists.txt └── data └── my systeam en.drawio.svg /.gitignore: -------------------------------------------------------------------------------- 1 | /.idea/ 2 | /.vscode/ 3 | *.vc.* -------------------------------------------------------------------------------- /src/__pycache__/idPD.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/geturin/OAFD_Monocular/HEAD/src/__pycache__/idPD.cpython-38.pyc -------------------------------------------------------------------------------- /src/__pycache__/depth2pcd.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/geturin/OAFD_Monocular/HEAD/src/__pycache__/depth2pcd.cpython-38.pyc -------------------------------------------------------------------------------- /src/__pycache__/simple_tf.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/geturin/OAFD_Monocular/HEAD/src/__pycache__/simple_tf.cpython-38.pyc -------------------------------------------------------------------------------- /src/__pycache__/pointcloud_carli.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/geturin/OAFD_Monocular/HEAD/src/__pycache__/pointcloud_carli.cpython-38.pyc -------------------------------------------------------------------------------- /launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /src/__pycache__/projectPointcloud.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/geturin/OAFD_Monocular/HEAD/src/__pycache__/projectPointcloud.cpython-38.pyc -------------------------------------------------------------------------------- /launch/tello.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/home/kero/catkin_build/devel/lib/python3/dist-packages", 4 | "/home/kero/catkin_ws/devel/lib/python3/dist-packages", 5 | "/opt/ros/noetic/lib/python3/dist-packages" 6 | ], 7 | "python.analysis.extraPaths": [ 8 | "/home/kero/catkin_build/devel/lib/python3/dist-packages", 9 | "/home/kero/catkin_ws/devel/lib/python3/dist-packages", 10 | "/opt/ros/noetic/lib/python3/dist-packages" 11 | ] 12 | } -------------------------------------------------------------------------------- /src/cmd_vel2command.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rospy 3 | from std_msgs.msg import String 4 | from geometry_msgs.msg import Twist 5 | import numpy as np 6 | 7 | 8 | rospy.init_node("cmd_vel_to_command") 9 | ctrl =rospy.Publisher('/message',String,queue_size=1) 10 | msg = "rc 0 0 0 0" 11 | def callback(data): 12 | global ctrl,msg 13 | x = int(data.linear.x) 14 | y = int(-data.linear.y) 15 | yaw = int(-data.angular.z/np.pi*180.0) 16 | 17 | msg = 'rc {} {} {} {}'.format( 18 | y, 19 | x, 20 | 0, 21 | yaw 22 | ) 23 | ctrl.publish(msg) 24 | 25 | 26 | sub = rospy.Subscriber("/cmd_vel", Twist, callback, queue_size=1) 27 | rospy.spin() -------------------------------------------------------------------------------- /src/.vscode/tasks.json: -------------------------------------------------------------------------------- 1 | { 2 | "tasks": [ 3 | { 4 | "type": "cppbuild", 5 | "label": "C/C++: g++ 生成活动文件", 6 | "command": "/usr/bin/g++", 7 | "args": [ 8 | "-fdiagnostics-color=always", 9 | "-g", 10 | "${file}", 11 | "-o", 12 | "${fileDirname}/${fileBasenameNoExtension}" 13 | ], 14 | "options": { 15 | "cwd": "${fileDirname}" 16 | }, 17 | "problemMatcher": [ 18 | "$gcc" 19 | ], 20 | "group": { 21 | "kind": "build", 22 | "isDefault": true 23 | }, 24 | "detail": "调试器生成的任务。" 25 | } 26 | ], 27 | "version": "2.0.0" 28 | } -------------------------------------------------------------------------------- /launch/pad_ctrl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 geturin 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /src/.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // 使用 IntelliSense 了解相关属性。 3 | // 悬停以查看现有属性的描述。 4 | // 欲了解更多信息，请访问: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | { 8 | "name": "g++ - 生成和调试活动文件", 9 | "type": "cppdbg", 10 | "request": "launch", 11 | "program": "${fileDirname}/${fileBasenameNoExtension}", 12 | "args": [], 13 | "stopAtEntry": false, 14 | "cwd": "${fileDirname}", 15 | "environment": [], 16 | "externalConsole": false, 17 | "MIMode": "gdb", 18 | "setupCommands": [ 19 | { 20 | "description": "为 gdb 启用整齐打印", 21 | "text": "-enable-pretty-printing", 22 | "ignoreFailures": true 23 | }, 24 | { 25 | "description": "将反汇编风格设置为 Intel", 26 | "text": "-gdb-set disassembly-flavor intel", 27 | "ignoreFailures": true 28 | } 29 | ], 30 | "preLaunchTask": "C/C++: g++ 生成活动文件", 31 | "miDebuggerPath": "/usr/bin/gdb" 32 | } 33 | ] 34 | } -------------------------------------------------------------------------------- /src/idPD.py: -------------------------------------------------------------------------------- 1 | #Inexact differential PD control 2 | 3 | 4 | class PD(object): 5 | 6 | def __init__(self,P,D,scal): 7 | self.P = P 8 | self.D = D 9 | self.scal = scal 10 | self.lasterror = 0 11 | 12 | def ctrl(self,error): 13 | self.de = self.D*(error-self.lasterror) 14 | result = self.P*error + self.de 15 | result *= self.scal 16 | self.lasterror = error 17 | return result 18 | 19 | 20 | class idPD(object): 21 | 22 | def __init__(self,P,D,scal,alpha): 23 | self.P = P 24 | self.D = D 25 | self.scal = scal 26 | self.alpha = alpha 27 | self.lasterror = 0 28 | self.lastdev = 0 29 | 30 | def ctrl(self,error): 31 | self.dev = self.D*(1-self.alpha)*(error-self.lasterror)+self.alpha*self.lastdev 32 | 33 | result = self.P*error+self.dev 34 | result *= self.scal 35 | 36 | self.lasterror = error 37 | self.lastdev = self.dev 38 | 39 | return result 40 | 41 | 42 | 43 | if __name__ == "__main__": 44 | 45 | a = idPD(P=10,D=4,scal=1,alpha=0.15) 46 | b = PD(P=10, D=4, scal=1) 47 | while True: 48 | error = float(input()) 49 | print(a.ctrl(error),b.ctrl(error)) -------------------------------------------------------------------------------- /src/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db", 6 | "limitSymbolsToIncludedHeaders": false 7 | }, 8 | "includePath": [ 9 | "/opt/ros/noetic/include/**", 10 | "/home/kero/catkin_ws/devel/include/**", 11 | "/home/kero/catkin_build/devel/include/**", 12 | "/home/kero/catkin_ws/src/beginner_tutorials/include/**", 13 | "/home/kero/catkin_ws/src/VINS-Mono/camera_model/include/**", 14 | "/home/kero/catkin_ws/src/VINS-Fusion/camera_models/include/**", 15 | "/home/kero/catkin_ws/src/imu_tool/imu_complementary_filter/include/**", 16 | "/home/kero/catkin_ws/src/imu_tool/imu_filter_madgwick/include/**", 17 | "/home/kero/catkin_build/src/orb_slam3_ros_wrapper/include/**", 18 | "/home/kero/catkin_ws/src/robot_localization/include/**", 19 | "/home/kero/catkin_ws/src/serial/include/**", 20 | "/home/kero/catkin_ws/src/test_pkg/include/**", 21 | "/usr/include/**" 22 | ], 23 | "name": "ROS", 24 | "intelliSenseMode": "gcc-x64", 25 | "compilerPath": "/usr/bin/gcc", 26 | "cStandard": "gnu11", 27 | "cppStandard": "c++14" 28 | } 29 | ], 30 | "version": 4 31 | } -------------------------------------------------------------------------------- /src/get_odom.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3.8 2 | import rospy 3 | from geometry_msgs.msg import PoseStamped 4 | from nav_msgs.msg import Odometry 5 | import ros_numpy 6 | from simple_tf import * 7 | 8 | rospy.init_node('odometry_publisher') 9 | odom_pub = rospy.Publisher("/visual_slam/odom", Odometry, queue_size=1) 10 | pose_pub = rospy.Publisher("/fake/camera",PoseStamped,queue_size=1) 11 | 12 | odom = Odometry() 13 | odom.child_frame_id = "camera" 14 | 15 | fake_pose = PoseStamped() 16 | 17 | #get map2world transform 18 | map2world = simpele_TF("map","world") 19 | 20 | def callback(data): 21 | global odom_pub,odom 22 | 23 | 24 | #transform camera pose to drone pose 25 | tfdata = map2world.transform_pose(data) 26 | 27 | odom.header = data.header 28 | odom.pose.pose.position = data.pose.position 29 | odom.pose.pose.orientation = tfdata.pose.orientation 30 | 31 | fake_pose.header = data.header 32 | 33 | 34 | #缩放坐标轴 35 | # point = ros_numpy.geometry.point_to_numpy(odom.pose.pose.position) 36 | # point *=1 37 | # odom.pose.pose.position = ros_numpy.geometry.numpy_to_point(point) 38 | 39 | fake_pose.pose = odom.pose.pose 40 | 41 | odom_pub.publish(odom) 42 | pose_pub.publish(fake_pose) 43 | 44 | 45 | rospy.Subscriber("/orb_slam3_ros/camera",PoseStamped,callback) 46 | rospy.spin() -------------------------------------------------------------------------------- /src/trajectory_view.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rospy 3 | import numpy as np 4 | from std_msgs.msg import String,Float32 5 | import message_filters 6 | from geometry_msgs.msg import PoseStamped,Pose,Vector3 7 | from nav_msgs.msg import Path 8 | import tf2_ros 9 | import math 10 | from idPD import * 11 | from simple_tf import * 12 | from visualization_msgs.msg import Marker 13 | 14 | 15 | path = [] 16 | planner=0 17 | tello_waypoints = 0 18 | tello_path = [] 19 | 20 | def callback(data): 21 | global planner 22 | target = data.points[0] 23 | planner = data.points 24 | target_point =[target.x,target.y] 25 | path.append(target_point) 26 | 27 | def path_callback(data): 28 | global tello_waypoints 29 | tello_waypoints = data 30 | 31 | def poses2np(): 32 | for i in tello_waypoints.poses: 33 | point = i.pose.position 34 | target = [point.x,point.y] 35 | tello_path.append(target) 36 | 37 | def add_final_path(): 38 | for i in planner: 39 | target = i 40 | target_point =[target.x,target.y] 41 | path.append(target_point) 42 | 43 | 44 | 45 | rospy.init_node('trajectory_view',anonymous=True) 46 | sub = rospy.Subscriber("/ego_planner_node/optimal_list",Marker,callback) 47 | tello_point_sub = rospy.Subscriber("/orb_slam3_ros/trajectory",Path,path_callback) 48 | rospy.spin() 49 | poses2np() 50 | add_final_path() 51 | path_array = np.array(path) 52 | tello_array = np.array(tello_path) 53 | np.save("./data/path.npy",path_array) 54 | np.save("./data/tello_path.npy",tello_array) -------------------------------------------------------------------------------- /launch/slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /src/simple_tf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3.8 2 | import rospy 3 | import tf2_geometry_msgs 4 | import tf2_ros 5 | import tf2_sensor_msgs 6 | import ros_numpy 7 | import numpy as np 8 | 9 | class simpele_TF(object): 10 | 11 | def __init__(self,source_frame,target_frame): 12 | self.source_frame = source_frame 13 | self.target_frame = target_frame 14 | self.creat_listener() 15 | self.time = rospy.Time(0) 16 | self.wating_time = rospy.Duration(1) 17 | return 18 | 19 | 20 | 21 | def creat_listener(self): 22 | self.tf_buffer = tf2_ros.Buffer(rospy.Duration(2)) 23 | tf2_ros.TransformListener(self.tf_buffer) 24 | return 25 | 26 | def get_transformation(self): 27 | # get the tf at first available time 28 | try: 29 | self.transformation = self.tf_buffer.lookup_transform(self.target_frame, 30 | self.source_frame, self.time, self.wating_time) 31 | except (tf2_ros.LookupException, tf2_ros.ConnectivityException, 32 | tf2_ros.ExtrapolationException): 33 | rospy.logerr('Unable to find the transformation from %s to %s' 34 | % self.source_frame, self.target_frame) 35 | return 36 | 37 | def get_inverse_transformation(self): 38 | self.inverse_transformation = self.transformation 39 | self.inverse_transformation.header.frame_id = self.source_frame 40 | tf = ros_numpy.geometry.transform_to_numpy(self.inverse_transformation.transform) 41 | tf = np.matrix(tf).I 42 | self.inverse_transformation.transform = ros_numpy.geometry.numpy_to_transform(tf) 43 | 44 | 45 | 46 | def transform_pose(self,pose): 47 | self.get_transformation() 48 | self.tfpose = tf2_geometry_msgs.do_transform_pose(pose,self.transformation) 49 | return self.tfpose 50 | 51 | def transform_pcd(self,pcd): 52 | self.get_transformation() 53 | self.pcd = tf2_sensor_msgs.do_transform_cloud(pcd,self.transformation) 54 | return self.pcd 55 | 56 | def inverse_transform_pcd(self,pcd): 57 | self.get_inverse_transformation() 58 | self.pcd = tf2_sensor_msgs.do_transform_cloud(pcd,self.inverse_transformation) 59 | return self.pcd 60 | -------------------------------------------------------------------------------- /config/tello.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | Camera.type: "PinHole" 7 | 8 | # Camera calibration and distortion parameters (OpenCV) 9 | Camera.fx: 910.072 10 | Camera.fy: 914.094 11 | Camera.cx: 485.523 12 | Camera.cy: 336.718 13 | 14 | Camera.k1: -0.005941 15 | Camera.k2: 0.055161 16 | Camera.p1: -0.006094 17 | Camera.p2: 0.003192 18 | 19 | # Camera resolution 20 | Camera.width: 960 21 | Camera.height: 720 22 | 23 | # Camera frames per second 24 | Camera.fps: 20.0 25 | 26 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 27 | Camera.RGB: 0 28 | 29 | # Transformation from camera to body-frame (imu) 30 | Tbc: !!opencv-matrix 31 | rows: 4 32 | cols: 4 33 | dt: f 34 | data: [ 0.03166095, 0.16683322, -0.98547667, -0.00284426, 35 | 0.97792113, -0.20893691, -0.00395312, 0.02377212, 36 | -0.20656196, -0.9635933, -0.16976487, -0.00615379, 37 | 0.0, 0.0, 0.0, 1.0] 38 | 39 | # IMU noise 40 | IMU.NoiseGyro: 0.0200 41 | IMU.NoiseAcc: 0.0500 42 | IMU.GyroWalk: 4.000e-05 43 | IMU.AccWalk: 0.0010 44 | IMU.Frequency: 100 45 | 46 | #-------------------------------------------------------------------------------------------- 47 | # ORB Parameters 48 | #-------------------------------------------------------------------------------------------- 49 | 50 | # ORB Extractor: Number of features per image 51 | ORBextractor.nFeatures: 1000 # 1000 52 | 53 | # ORB Extractor: Scale factor between levels in the scale pyramid 54 | ORBextractor.scaleFactor: 1.2 55 | 56 | # ORB Extractor: Number of levels in the scale pyramid 57 | ORBextractor.nLevels: 8 58 | 59 | # ORB Extractor: Fast threshold 60 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 61 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 62 | # You can lower these values if your images have low contrast 63 | ORBextractor.iniThFAST: 20 64 | ORBextractor.minThFAST: 7 65 | 66 | #-------------------------------------------------------------------------------------------- 67 | # Viewer Parameters 68 | #-------------------------------------------------------------------------------------------- 69 | Viewer.KeyFrameSize: 0.05 70 | Viewer.KeyFrameLineWidth: 1 71 | Viewer.GraphLineWidth: 0.9 72 | Viewer.PointSize: 2 73 | Viewer.CameraSize: 0.08 74 | Viewer.CameraLineWidth: 3 75 | Viewer.ViewpointX: 0 76 | Viewer.ViewpointY: -0.7 77 | Viewer.ViewpointZ: -3.5 # -1.8 78 | Viewer.ViewpointF: 500 -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | oafd 4 | 0.0.0 5 | The oafd package 6 | 7 | 8 | 9 | 10 | kero 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | rospy 54 | rospy 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /src/Tello_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from djitellopy import tello 3 | import rospy 4 | from sensor_msgs.msg import Image 5 | from std_msgs.msg import String,Float32 6 | from cv_bridge import CvBridge 7 | from threading import Thread 8 | 9 | 10 | class tello_ros: 11 | def __init__(self) -> None: 12 | # Initialize drone 13 | self.drone = tello.Tello() 14 | try: 15 | self.drone.connect() 16 | except: 17 | print("can't connect tello") 18 | return 19 | 20 | 21 | # Initialize parameter 22 | self.cam_pub = rospy.Publisher('/camera/image_raw',Image,queue_size=1) 23 | self._camera_rate = rospy.Rate(rospy.get_param("~camera_rate", 20)) 24 | self._command_rate = rospy.Rate(rospy.get_param("~command_rate", 20)) 25 | self._tof_rate = rospy.Rate(rospy.get_param("~tof_rate", 4)) 26 | self.tof_pub = rospy.Publisher("/tof",Float32,queue_size=1) 27 | self.bridge = CvBridge() 28 | self.take_off = rospy.get_param("take_off", True) 29 | self.command = "rc 0 0 0 0" 30 | 31 | #cam_pub 32 | self.drone.streamon() 33 | self.drone.get_frame_read().frame #wait stream 34 | self._cam_thread = Thread(target=self.cam_loop) 35 | self._cam_thread.start() 36 | 37 | 38 | #take off 39 | if self.take_off: 40 | self.drone.send_command_with_return("takeoff") 41 | else: 42 | pass 43 | 44 | #tof pub 45 | self._tof_thread = Thread(target=self.tof_data) 46 | self._tof_thread.start() 47 | 48 | #send vel 49 | command_sub = rospy.Subscriber('/message',String,self.get_vel_command,queue_size=1) 50 | self.send_vel_command() 51 | 52 | 53 | 54 | 55 | def cam_loop(self): 56 | while not rospy.is_shutdown(): 57 | try: 58 | #get image frome video stream and publish 59 | img = self.drone.get_frame_read().frame 60 | pubimg = self.bridge.cv2_to_imgmsg(img,"bgr8") 61 | pubimg.header.stamp = rospy.Time.now() 62 | self.cam_pub.publish(pubimg) 63 | except: 64 | print("video stream failed") 65 | pass 66 | 67 | self._camera_rate.sleep() 68 | 69 | 70 | 71 | def tof_data(self): 72 | #get&pub tof 73 | while not rospy.is_shutdown(): 74 | tof = self.drone.get_distance_tof() 75 | self.tof_pub.publish(tof) 76 | self._tof_rate.sleep() 77 | 78 | 79 | def send_vel_command(self): 80 | while not rospy.is_shutdown(): 81 | self.drone.send_command_without_return(self.command) 82 | self._command_rate.sleep() 83 | 84 | def get_vel_command(self,data): 85 | self.command = data.data 86 | 87 | def shutdown(self): 88 | self.drone.streamoff() 89 | self.drone.send_command_without_return("land") 90 | 91 | if __name__ == "__main__": 92 | rospy.init_node("tello_driver") 93 | node = tello_ros() 94 | rospy.spin() 95 | node.shutdown() -------------------------------------------------------------------------------- /src/path_following.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3.8 2 | import rospy 3 | import numpy as np 4 | from std_msgs.msg import String,Float32 5 | import message_filters 6 | from geometry_msgs.msg import PoseStamped,Pose 7 | import tf2_ros 8 | import math 9 | from nav_msgs.msg import Path 10 | from idPD import * 11 | from simple_tf import * 12 | 13 | 14 | 15 | 16 | #初始化ros 17 | rospy.init_node('tello_contrl',anonymous=True) 18 | 19 | ctrl =rospy.Publisher('/message',String,queue_size=1) 20 | 21 | 22 | #初始化tf2 23 | tf_buffer = tf2_ros.Buffer(rospy.Duration(10)) 24 | tf2_ros.TransformListener(tf_buffer) 25 | 26 | 27 | rate = rospy.Rate(20) 28 | 29 | alpha = 0 30 | # world frame origin 31 | origin = PoseStamped() 32 | origin.pose.position.x = 0 33 | origin.pose.position.y = 0 34 | origin.pose.position.z = 0 35 | 36 | #path 37 | star = 0 38 | pointnumber = 0 39 | 40 | def callback(data): 41 | global map_pose,wait_point,pointnumber 42 | if pointnumber == 0: 43 | pointnumber = len(data.poses) 44 | else: 45 | pass 46 | 47 | map_pose=data.poses[star] 48 | wait_point=1 49 | 50 | 51 | 52 | 53 | def getPoint(): 54 | tf_linster.get_transformation() 55 | tfpose = tf_linster.transform_pose(map_pose) 56 | point = tfpose.pose.position 57 | qutar = tfpose.pose.orientation 58 | 59 | x = point.x 60 | y = point.y 61 | z = point.z 62 | yaw = math.atan2(2*(qutar.w*qutar.z+qutar.x*qutar.y),1-2*(qutar.z*qutar.z+qutar.y*qutar.y)) 63 | yaw = yaw*180/math.pi 64 | return x,y,z,yaw,tfpose.pose 65 | 66 | def calibration(data): 67 | global alpha 68 | alpha = abs(data.data/tello_origin.pose.position.z) 69 | 70 | 71 | 72 | 73 | tf_linster = simpele_TF(source_frame="map",target_frame="tello") 74 | #calibration slam world frame and real world frame 75 | rospy.sleep(3.) 76 | 77 | # tf_linster.get_transformation() 78 | tello_origin = tf_linster.transform_pose(origin) 79 | 80 | while alpha == 0: 81 | tof = rospy.Subscriber("/tof",Float32,calibration) 82 | 83 | 84 | 85 | 86 | 87 | #idPDcontrll set 88 | x=idPD(P=0.83, D=1.65, scal=0.1*alpha, alpha=0.12) 89 | y=idPD(P=0.83, D=1.65, scal=0.1*alpha, alpha=0.12) 90 | z=idPD(P=0.81, D=1.78, scal=0.1*alpha, alpha=0.12) 91 | yaw=idPD(P=0.08, D=0.6, scal=0.05*alpha, alpha=0.1) 92 | 93 | 94 | wait_point=0 95 | 96 | #set subscriber 97 | rviz = message_filters.Subscriber("/orb_slam3_ros/trajectory", Path) 98 | rviz.registerCallback(callback) 99 | 100 | while not rospy.is_shutdown(): 101 | 102 | if wait_point ==1: 103 | error_x,error_y,error_z,error_yaw,error_pose=getPoint() 104 | 105 | sp_x =x.ctrl(error_x) 106 | sp_y = y.ctrl(error_y) 107 | sp_z = z.ctrl(error_z) 108 | sp_yaw = yaw.ctrl(error_yaw) 109 | 110 | #set speed max and min 111 | sp_x = round(np.clip(sp_x,-30,30)) 112 | sp_y = round(np.clip(sp_y, -30, 30)) 113 | sp_z = round(np.clip(sp_z, -30, 30)) 114 | sp_yaw = round(np.clip(sp_yaw, -20, 20)) 115 | 116 | 117 | msg = 'rc {} {} {} {}'.format( 118 | -1*sp_y, 119 | sp_x, 120 | sp_z, 121 | -1*sp_yaw 122 | ) 123 | 124 | if abs(sp_y)+abs(sp_x)+abs(sp_z) <= 22: 125 | star = star +1 126 | else: 127 | pass 128 | # if star >= pointnumber-5: 129 | # msg = "land" 130 | # rate = rospy.Rate(1) 131 | if star >= pointnumber-7: 132 | star = 0 133 | else: 134 | pass 135 | 136 | ctrl.publish(msg) 137 | 138 | else : 139 | pass 140 | 141 | 142 | rate.sleep() -------------------------------------------------------------------------------- /src/pd_topoint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3.8 2 | import rospy 3 | import numpy as np 4 | from std_msgs.msg import String,Float32 5 | import message_filters 6 | from geometry_msgs.msg import PoseWithCovarianceStamped,PoseStamped,Pose 7 | import tf2_geometry_msgs 8 | import tf2_ros 9 | import math 10 | from idPD import * 11 | from simple_tf import * 12 | 13 | 14 | 15 | 16 | #初始化ros 17 | rospy.init_node('tello_contrl',anonymous=True) 18 | 19 | ctrl =rospy.Publisher('/message',String,queue_size=1) 20 | error_vector_pub=rospy.Publisher("/error_vector",Pose,queue_size=1) 21 | 22 | 23 | #初始化tf2 24 | tf_buffer = tf2_ros.Buffer(rospy.Duration(10)) 25 | tf2_ros.TransformListener(tf_buffer) 26 | 27 | 28 | rate = rospy.Rate(20) 29 | 30 | alpha = 0 31 | wait_point = 0 32 | map_pose = PoseWithCovarianceStamped() 33 | # world frame origin 34 | origin = PoseStamped() 35 | origin.pose.position.x = 0 36 | origin.pose.position.y = 0 37 | origin.pose.position.z = 0 38 | 39 | def callback(data): 40 | global map_pose,wait_point 41 | map_pose=data 42 | wait_point=1 43 | 44 | 45 | 46 | 47 | def getPoint(): 48 | tf_linster.get_transformation() 49 | tfpose = tf_linster.transform_pose(map_pose.pose) 50 | point = tfpose.pose.position 51 | qutar = tfpose.pose.orientation 52 | 53 | x = point.x 54 | y = point.y 55 | z = point.z 56 | yaw = math.atan2(2*(qutar.w*qutar.z+qutar.x*qutar.y),1-2*(qutar.z*qutar.z+qutar.y*qutar.y)) 57 | yaw = yaw*180/math.pi 58 | return x,y,z,yaw,tfpose.pose 59 | 60 | def calibration(data): 61 | global alpha 62 | alpha = abs(data.data/tello_origin.pose.position.z) 63 | 64 | 65 | 66 | 67 | tf_linster = simpele_TF(source_frame="map",target_frame="tello") 68 | #calibration slam world frame and real world frame 69 | rospy.sleep(3.) 70 | 71 | tf_linster.get_transformation() 72 | tello_origin = tf_linster.transform_pose(origin) 73 | 74 | while alpha == 0: 75 | tof = rospy.Subscriber("/tof",Float32,calibration) 76 | 77 | 78 | 79 | 80 | #PDcontrll set 81 | # x=PD(P=0.9, D=1.2, scal=0.15*alpha) 82 | # y=PD(P=0.9, D=1.2, scal=0.15*alpha) 83 | # z=PD(P=0.9, D=1.2, scal=0.15*alpha) 84 | # yaw=PD(P=0.08, D=0.02, scal=0.05*alpha) 85 | 86 | #idPDcontrll set 87 | x=idPD(P=0.9, D=2.3, scal=0.15*alpha, alpha=0.1) 88 | y=idPD(P=0.9, D=2.3, scal=0.15*alpha, alpha=0.1) 89 | z=idPD(P=0.9, D=2.3, scal=0.15*alpha, alpha=0.1) 90 | yaw=idPD(P=0.08, D=2.3, scal=0.05*alpha, alpha=0.1) 91 | 92 | 93 | 94 | 95 | 96 | 97 | #set subscriber 98 | rviz = message_filters.Subscriber("/initialpose", PoseWithCovarianceStamped) 99 | rviz.registerCallback(callback) 100 | while not rospy.is_shutdown(): 101 | 102 | if wait_point ==1: 103 | error_x,error_y,error_z,error_yaw,error_pose=getPoint() 104 | 105 | #误差速度话题 106 | 107 | error_vector_pub.publish(error_pose) 108 | 109 | 110 | sp_x =x.ctrl(error_x) 111 | sp_y = y.ctrl(error_y) 112 | sp_z = z.ctrl(error_z) 113 | sp_yaw = yaw.ctrl(error_yaw) 114 | 115 | #set speed max and min 116 | sp_x = round(np.clip(sp_x,-30,30)) 117 | sp_y = round(np.clip(sp_y, -30, 30)) 118 | sp_z = round(np.clip(sp_z, -30, 30)) 119 | sp_yaw = round(np.clip(sp_yaw, -20, 20)) 120 | 121 | 122 | msg = 'rc {} {} {} {}'.format( 123 | -1*sp_y, 124 | sp_x, 125 | 0, 126 | -1*sp_yaw 127 | ) 128 | ctrl.publish(msg) 129 | else : 130 | msg = 'rc {} {} {} {}'.format( 131 | 0, 132 | 0, 133 | 0, 134 | 0 135 | ) 136 | ctrl.publish(msg) 137 | 138 | 139 | rate.sleep() 140 | 141 | 142 | 143 | 144 | -------------------------------------------------------------------------------- /src/depthmap_calibration.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3.8 2 | import rospy 3 | from sensor_msgs.msg import PointCloud2,Image,PointField 4 | import sensor_msgs.point_cloud2 as pcd2 5 | from std_msgs.msg import Header 6 | import numpy as np 7 | from depth2pcd import * 8 | import message_filters 9 | from simple_tf import * 10 | from cv_bridge import CvBridge 11 | import ros_numpy 12 | 13 | 14 | 15 | rospy.init_node('pcd_pub', anonymous=True) 16 | 17 | depth_transform = depth_to_pcd(5) 18 | depth_calibra = Clibration() 19 | 20 | start = 0 21 | bridge = CvBridge() 22 | 23 | 24 | 25 | # RGB8 26 | fields = [ PointField("x",0,PointField.FLOAT32,1), 27 | PointField("y",4,PointField.FLOAT32,1), 28 | PointField("z",8,PointField.FLOAT32,1), 29 | PointField("rgb",12,PointField.FLOAT32,1)] 30 | pcd_publish = rospy.Publisher("test_pcd",PointCloud2,queue_size=1) 31 | 32 | #get map2world transform 33 | map2world = simpele_TF("map","world") 34 | 35 | scale = 20 36 | 37 | def callback(image,depth,pcd): 38 | global depth_transform , depth_calibra , start , bridge , fields , pcd_publish,tflistener ,scale 39 | 40 | if start==0: 41 | try: 42 | tflistener = simpele_TF("map","camera") 43 | start = 1 44 | except: 45 | print("cant get tranform from map to camera ,wait transform") 46 | return 47 | else: 48 | #get depth map and rgb image 49 | rgb = bridge.imgmsg_to_cv2(image,"rgb8") 50 | #test scale 51 | depth = bridge.imgmsg_to_cv2(depth,"32FC1") 52 | depth = 1/depth 53 | 54 | # depth = depth[:,:,0] 55 | 56 | #orb_pcd map->camera 57 | tflistener.time = image.header.stamp 58 | try: 59 | pcd=tflistener.transform_pcd(pcd) 60 | except: 61 | print("tf lost") 62 | return 63 | 64 | #最小二乘法计算orb点云和ai深度的比例并缩放ai深度图 65 | pcd = ros_numpy.numpify(pcd)#pointcloud2 msg -> numpy arry 66 | 67 | # if scale == 0: 68 | # scale = depth_calibra.depth_calibration(depth,pcd) 69 | # else: 70 | # pass 71 | # filter = np.percentile(depth,66) 72 | # depth[depth>filter]=0 73 | # depth = 10*scale*depth 74 | 75 | scale = depth_calibra.depth_calibration(depth,pcd) 76 | depth = scale * depth 77 | 78 | # #with out calibration test 79 | # depth[depth>0.55]=0 80 | # depth = 20*depth 81 | 82 | 83 | # to rgb pointlcoud 84 | pcd = depth_transform.get_rgbpcd(depth,rgb) 85 | header = Header() 86 | header.frame_id = "camera" 87 | msg = pcd2.create_cloud(header=header,fields=fields,points=pcd) 88 | 89 | # to pointcloud 90 | # pcd = depth_transform.get_pcd(depth) 91 | 92 | # #pcl filter test 93 | # p = pcl.PointCloud(np.array(pcd, dtype=np.float32)) 94 | # fil = p.make_statistical_outlier_filter() 95 | # fil.set_mean_k(40) 96 | # fil.set_std_dev_mul_thresh(1.5) 97 | # pcd = fil.filter() 98 | # pcd=pcd.to_array() 99 | 100 | # header = Header() 101 | # header.frame_id = "camera" 102 | # msg =pcd2.create_cloud_xyz32(header,pcd) 103 | 104 | #取得map->camera的逆矩阵 105 | msg = tflistener.inverse_transform_pcd(msg) 106 | msg = map2world.transform_pcd(msg) 107 | msg.header.frame_id = "world" 108 | pcd_publish.publish(msg) 109 | return 110 | 111 | camera_sub = message_filters.Subscriber('/camera/image_raw', Image) 112 | depth_sub = message_filters.Subscriber('/camera/depth', Image) 113 | pcd_sub = message_filters.Subscriber('/orb_slam3_ros/map_points', PointCloud2) 114 | 115 | ts = message_filters.TimeSynchronizer([camera_sub, depth_sub,pcd_sub], 3) 116 | ts.registerCallback(callback) 117 | rospy.spin() -------------------------------------------------------------------------------- /launch/ego_tello.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 99 | 100 | 101 | 102 | -------------------------------------------------------------------------------- /src/planner_ctrl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3.8 2 | import rospy 3 | import numpy as np 4 | from std_msgs.msg import String,Float32 5 | import message_filters 6 | from geometry_msgs.msg import PoseStamped,Pose,Vector3 7 | import tf2_ros 8 | import math 9 | from idPD import * 10 | from simple_tf import * 11 | from visualization_msgs.msg import Marker 12 | 13 | 14 | 15 | 16 | #初始化ros 17 | rospy.init_node('tello_contrl',anonymous=True) 18 | 19 | ctrl =rospy.Publisher('/message',String,queue_size=1) 20 | 21 | #PD结果publish 22 | pdresult = rospy.Publisher('/PDctrl_result',Vector3,queue_size=1) 23 | dresult = rospy.Publisher('/de_result',Vector3,queue_size=1) 24 | 25 | pd_vector = Vector3() 26 | d_vector = Vector3() 27 | 28 | 29 | #初始化tf2 30 | tf_buffer = tf2_ros.Buffer(rospy.Duration(10)) 31 | tf2_ros.TransformListener(tf_buffer) 32 | 33 | 34 | rate = rospy.Rate(20) 35 | 36 | alpha = 0 37 | # world frame origin 38 | origin = PoseStamped() 39 | origin.pose.position.x = 0 40 | origin.pose.position.y = 0 41 | origin.pose.position.z = 0 42 | 43 | #path 44 | star = 0 45 | pointnumber = 0 46 | map_pose = PoseStamped() 47 | goal_pose = PoseStamped() 48 | 49 | def callback(data): 50 | global map_pose,wait_point,pointnumber,star,waypoint_limt 51 | 52 | 53 | if pointnumber!=len(data.points): 54 | pointnumber = len(data.points) 55 | if pointnumber <= 11: 56 | # waypoint_limt <= 9 57 | star = pointnumber - 1 58 | else: 59 | # waypoint_limt = 2 60 | star = 0 61 | 62 | # star = star-3 63 | # star = np.clip(star,0,waypoint_limt) 64 | 65 | 66 | map_pose.pose.position=data.points[star] 67 | goal_pose.pose.position = data.points[pointnumber-1] 68 | wait_point=1 69 | 70 | 71 | 72 | 73 | def getPoint(): 74 | tf_linster.time = rospy.Time.now() 75 | tf_linster.get_transformation() 76 | tfpose = tf_linster.transform_pose(map_pose) 77 | goal = tf_linster.transform_pose(goal_pose) 78 | point = tfpose.pose.position 79 | qutar = tfpose.pose.orientation 80 | 81 | x = point.x 82 | y = point.y 83 | z = point.z 84 | # yaw = math.atan2(2*(qutar.w*qutar.z+qutar.x*qutar.y),1-2*(qutar.z*qutar.z+qutar.y*qutar.y)) 85 | # yaw = yaw*180/math.pi 86 | yaw = goal.pose.position.y 87 | return x,y,z,yaw,tfpose.pose 88 | 89 | def calibration(data): 90 | global alpha 91 | alpha = abs(data.data/tello_origin.pose.position.z) 92 | 93 | 94 | 95 | 96 | tf_linster = simpele_TF(source_frame="world",target_frame="tello") 97 | #calibration slam world frame and real world frame 98 | rospy.sleep(3.) 99 | 100 | # tf_linster.get_transformation() 101 | tello_origin = tf_linster.transform_pose(origin) 102 | 103 | while alpha == 0: 104 | tof = rospy.Subscriber("/tof",Float32,calibration) 105 | 106 | 107 | 108 | s = 0.35 109 | #idPDcontrll set 110 | x=PD(P=0.92, D=1.74, scal=s*alpha) 111 | y=PD(P=0.92, D=1.74, scal=s*alpha) 112 | z=PD(P=0.92, D=1.74, scal=s*alpha) 113 | 114 | yaw=PD(P=0.3, D=1.2, scal=s*alpha) 115 | 116 | 117 | wait_point=0 118 | waypoint_limt =2 119 | 120 | #set subscriber 121 | bspline = message_filters.Subscriber("/ego_planner_node/optimal_list", Marker) 122 | bspline.registerCallback(callback) 123 | 124 | while not rospy.is_shutdown(): 125 | 126 | if wait_point ==1: 127 | error_x,error_y,error_z,error_yaw,error_pose=getPoint() 128 | 129 | sp_x =x.ctrl(error_x) 130 | sp_y = y.ctrl(error_y) 131 | sp_z = z.ctrl(error_z) 132 | 133 | sp_yaw = yaw.ctrl(error_yaw) 134 | 135 | #set speed max and min 136 | sp_x = round(np.clip(sp_x,-30,30)) 137 | sp_y = round(np.clip(sp_y, -30, 30)) 138 | sp_z = round(np.clip(sp_z, -30, 30)) 139 | 140 | sp_yaw = round(np.clip(sp_yaw, -15, 15)) 141 | 142 | #pub pd控制器结算结果 143 | pd_vector.x , pd_vector.y , pd_vector.z = sp_x , sp_y , sp_z 144 | d_vector.x , d_vector.y , d_vector.z = x.de , y.de , z.de 145 | 146 | pdresult.publish(pd_vector) 147 | dresult.publish(d_vector) 148 | 149 | #pub tello控制指令 150 | msg = 'rc {} {} {} {}'.format( 151 | -1*sp_y, 152 | sp_x, 153 | sp_z, 154 | -1*sp_yaw 155 | ) 156 | 157 | ctrl.publish(msg) 158 | else : 159 | pass 160 | 161 | 162 | rate.sleep() 163 | -------------------------------------------------------------------------------- /README.MD: -------------------------------------------------------------------------------- 1 | # OAFD_MONOCULAR 2 | this work is my Master thesis research ：[**3D mapping by a monocular camera with application to realtime obstacle avoidance control of quadrotor**](https://1drv.ms/p/s!Ajd6qirY1nEUiTj_6OC2qKYMyUgU?e=t9BPCA) 3 | 4 | use monocular depth estimation and monocular visual SLAM to construct an autonomous flight system for a monocular-only quadrotor. 5 | ![](./data/my%20systeam%20en.drawio.svg) 6 | 7 | https://user-images.githubusercontent.com/87827677/209362745-71a07fed-f7f8-49f1-a064-7b98f55e34b8.mp4 8 | 9 | # [more experiment video](https://youtube.com/playlist?list=PLQYTjwXEt_RxlJJVSr9-HWyi-2qyCrkhP) 10 | 11 | # Prerequisites 12 | Tested on Ubuntu 20.04 & 18.04 13 | 14 | ## [ORB_SLAM3](https://github.com/UZ-SLAMLab/ORB_SLAM3) 15 | Use the ORB_SLAM3(**v0.4beta**)as VSLAM(monocular) 16 | - Install the [prerequisites](https://github.com/UZ-SLAMLab/ORB_SLAM3#2-prerequisites). 17 | - Clone ORB-SLAM3: 18 | ``` 19 | cd ~ 20 | git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git ORB_SLAM3 21 | ``` 22 | - Make changes to the source code if necessary to build successfully. For Ubuntu 20.04, you will need to change CMakeList from C++11 to C++14. I have incorporated the changes in [this fork]( 23 | https://github.com/thien94/ORB_SLAM3). 24 | - Build: 25 | ``` 26 | cd ORB_SLAM3 27 | 28 | git reset --hard a80b4677009e673b9939a7e91e6ea7bcb5090294 29 | 30 | chmod +x build.sh 31 | ./build.sh 32 | ``` 33 | - Make sure that **`libORB_SLAM3.so`** is created in the *ORB_SLAM3/lib* folder. If not, check the issue list from the [original repo](https://github.com/UZ-SLAMLab/ORB_SLAM3/issues) and retry. 34 | 35 | ### [orb_slam3_ros_wrapper](https://github.com/geturin/orb_slam3_ros_wrapper) 36 | 37 | - Clone the package. Note that it should be a `catkin build` workspace. 38 | ``` 39 | cd ~/catkin_build/src/ 40 | git clone https://github.com/geturin/orb_slam3_ros_wrapper.git 41 | ``` 42 | 43 | - Open `CMakeLists.txt` and change the directory that leads to ORB-SLAM3 library at the beginning of the file (default is home folder `~/`) 44 | ``` 45 | cd ~/catkin_ws/src/orb_slam3_ros_wrapper/ 46 | nano CMakeLists.txt 47 | 48 | # Change this to your installation of ORB-SLAM3. Default is ~/ 49 | set(ORB_SLAM3_DIR 50 | $ENV{HOME}/ORB_SLAM3 51 | ) 52 | ``` 53 | 54 | - Build the package normally. 55 | ``` 56 | cd ~/catkin_build/ 57 | catkin build 58 | ``` 59 | 60 | - Next, copy the `ORBvoc.txt` file from `ORB-SLAM3/Vocabulary/` folder to the `config` folder in this package. Alternatively, you can change the `voc_file` param in the launch file to point to the right location. 61 | 62 | - (Optional) Install `hector-trajectory-server` to visualize the trajectory. 63 | ``` 64 | sudo apt install ros-[DISTRO]-hector-trajectory-server 65 | ``` 66 | 67 | ## [MiDAS](https://github.com/geturin/MiDaS/tree/master/ros) 68 | use the MiDAS as monocular depth estimation model 69 | * create symlink for OpenCV: 70 | 71 | ```bash 72 | sudo ln -s /usr/include/opencv4 /usr/include/opencv 73 | ``` 74 | 75 | * download and install MiDaS: 76 | 77 | ```bash 78 | source ~/.bashrc 79 | cd ~/ 80 | mkdir catkin_ws 81 | cd catkin_ws 82 | git clone https://github.com/geturin/MiDaS 83 | mkdir src 84 | cp -r MiDaS/ros/* src 85 | 86 | chmod +x src/additions/*.sh 87 | chmod +x src/*.sh 88 | chmod +x src/midas_cpp/scripts/*.py 89 | cp src/additions/do_catkin_make.sh ./do_catkin_make.sh 90 | ./do_catkin_make.sh 91 | ./src/additions/downloads.sh 92 | ``` 93 | ## [EGO_PLANNER](https://github.com/ZJU-FAST-Lab/ego-planner) 94 | use the EGO_PLANNER as local planner 95 | 96 | ``` 97 | sudo apt-get install libarmadillo-dev 98 | git clone https://github.com/ZJU-FAST-Lab/ego-planner.git 99 | cd ego-planner 100 | catkin_make 101 | source devel/setup.bash 102 | roslaunch ego_planner simple_run.launch 103 | ``` 104 | # Installation 105 | ``` 106 | sudo apt-get install ros-$release-ros-numpy 107 | sudo apt-get install ros-$release-tf2-sensor-msgs 108 | cd catkin_ws/src 109 | git clone https://github.com/geturin/OAFD_Monocular.git 110 | cd ../ 111 | catkin_make 112 | ``` 113 | # How to use 114 | 115 | - Connect to tello's wifi 116 | 117 | - run ORB_SLAM3 & Midas & ego planner 118 | ``` 119 | roslaunch oafd slam.launch 120 | roslaunch midas_cpp midas_cpp.launch model_name:="model-small-traced.pt" input_topic:="/camera/image_raw" output_topic:="/camera/depth" out_orig_size:="true" 121 | roslaunch ego_tello.launch 122 | ``` 123 | - when ORB_SLAM3 get ready 124 | ``` 125 | roslaunch oafd tello.launch 126 | ``` 127 | - When tello finish taking off 128 | 129 | ``` 130 | rosrun oafd planner_ctrl.py 131 | ``` 132 | 133 | -------------------------------------------------------------------------------- /src/depth2pcd.py: -------------------------------------------------------------------------------- 1 | #用numpy将连续的深度图转为点云以及和AI点云和稀疏点云的标定 2 | import numpy as np 3 | import cv2 4 | 5 | #点云投影 6 | class depth_to_pcd(object): 7 | 8 | def __init__(self, resize_scale): 9 | self.resize_scale = resize_scale 10 | self.resize_camera = np.array((910.072 / self.resize_scale, 0, 485.523 / self.resize_scale, 0, 11 | 0, 914.094 / self.resize_scale, 336.718 / self.resize_scale, 0, 12 | 0, 0, 1, 0), dtype=np.float32).reshape(3, 4) 13 | 14 | self.pixel = np.array([0,0,1]).reshape(3,-1) 15 | self.resize_camera = np.matrix(self.resize_camera).I 16 | 17 | self.pcd_list = np.zeros((720//self.resize_scale,960//self.resize_scale,3)) 18 | self.get_depth_vector_v2() 19 | 20 | self.pcd_list = self.pcd_list.reshape(-1,1,3) 21 | self.pcd_list[:,:,0],self.pcd_list[:,:,1],self.pcd_list[:,:,2]=self.pcd_list[:,:,2],-self.pcd_list[:,:,0],-self.pcd_list[:,:,1] 22 | self.pcd_list = self.pcd_list.reshape(720//self.resize_scale,960//self.resize_scale,3) 23 | return 24 | 25 | # def get_depth_vector(self): 26 | # fake_depth =np.zeros((720//self.resize_scale,960//self.resize_scale)) 27 | # it = np.nditer(fake_depth, flags=['multi_index']) 28 | # with it: 29 | # while not it.finished: 30 | # self.pixel[0] = it.multi_index[1] 31 | # self.pixel[1] = it.multi_index[0] 32 | # point = np.dot(self.resize_camera, self.pixel) 33 | # self.pcd_list[it.multi_index] = point[0:3].T[0] 34 | # it.iternext() 35 | 36 | #new 37 | def get_depth_vector_v2(self): 38 | u = 960//self.resize_scale 39 | v = 720//self.resize_scale 40 | u_vector = np.ones((1,u,3)) 41 | v_vector = np.ones((u,1,3)) 42 | 43 | for i in range(u): 44 | self.pixel[0] = self.pixel[1] = i 45 | point = np.dot(self.resize_camera, self.pixel) 46 | u_vector[0,i,0] = point[0] 47 | v_vector[i,0,1] = point[1] 48 | 49 | v_vector = v_vector[:v,:,:] 50 | v_vector = np.tile(v_vector,(u,1)) 51 | u_vector = np.tile(u_vector ,(v,1,1)) 52 | self.pcd_list = v_vector * u_vector 53 | 54 | def get_pcd(self, depth): 55 | self.depth = cv2.resize(depth, (int(960 / self.resize_scale), int(720 / self.resize_scale)),interpolation=cv2.INTER_NEAREST) 56 | vector_array = self.pcd_list.copy() 57 | vector_array *= self.depth.reshape((int(720 / self.resize_scale), int(960 / self.resize_scale),1)) 58 | 59 | 60 | pointcloud = vector_array.reshape(-1,3) 61 | 62 | #去除所有的0，0，0点 63 | pointcloud = pointcloud[pointcloud.sum(axis=1)!=0,:] 64 | 65 | 66 | return pointcloud 67 | 68 | def get_rgbpcd(self,depth,rgb): 69 | self.depth = cv2.resize(depth, (int(960 / self.resize_scale), int(720 / self.resize_scale)),interpolation=cv2.INTER_NEAREST) 70 | vector_array = self.pcd_list.copy() 71 | vector_array *= self.depth.reshape((int(720 / self.resize_scale), int(960 / self.resize_scale),1)) 72 | 73 | 74 | pointcloud = vector_array.reshape(-1,3) 75 | 76 | 77 | rgb = cv2.resize(rgb, (int(960 / self.resize_scale), int(720 / self.resize_scale)),interpolation=cv2.INTER_NEAREST) 78 | rgb_list = rgb.reshape(-1,3) 79 | 80 | #去除所有的0，0，0点 81 | rgb_list = rgb_list[pointcloud.sum(axis=1)!=0,:] 82 | pointcloud = pointcloud[pointcloud.sum(axis=1)!=0,:] 83 | 84 | 85 | r,g,b = rgb_list[:,0].astype(np.uint32), rgb_list[:,1].astype(np.uint32), rgb_list[:,2].astype(np.uint32) 86 | rgb = np.array((r << 16) | (g << 8 ) | (b << 0),dtype=np.uint32) 87 | color = rgb.reshape(-1,1) 88 | color.dtype = np.float32 89 | 90 | rgbpointcloud = np.hstack((pointcloud,color)).astype(np.float32) 91 | 92 | return rgbpointcloud 93 | 94 | #标定 95 | 96 | class Clibration(object): 97 | 98 | def __init__(self) -> None: 99 | self.rotation=np.array([0,0,0],dtype=np.float32).reshape(3,1) 100 | self.translation=np.array([0,0,0],dtype=np.float32).reshape(1,3) 101 | self.distortion=np.array([[-0.005941,0.055161,-0.006094,0.003192]]) 102 | self.camera=np.array((910.072,0,485.523, 103 | 0,914.094,336.718, 104 | 0,0,1),dtype=np.float32).reshape(3,3) 105 | pass 106 | 107 | def orb_pcd_reprojet(self,orb_pcd): 108 | camera_pcd=np.zeros((orb_pcd.shape[0],3)) 109 | camera_pcd[:,0]=-1*orb_pcd["y"] 110 | camera_pcd[:,1]=-1*orb_pcd["z"] 111 | camera_pcd[:,2]=orb_pcd["x"] 112 | 113 | reTransform=cv2.projectPoints(camera_pcd,self.rotation,self.translation,self.camera,self.distortion) 114 | reTransform = reTransform[0][:,0].astype(int) 115 | 116 | pixel = reTransform 117 | filter = np.where((pixel[:,0]<960)&(pixel[:,1]<720)&(pixel[:,0]>=0)&(pixel[:,1]>=0)) 118 | pixel = pixel[filter] 119 | depth = camera_pcd[:,2].reshape(-1,1)[filter] 120 | 121 | self.depth_image=np.zeros((720,960)) 122 | self.depth_image[pixel[:,1],pixel[:,0]] = depth[:,0] 123 | 124 | return self.depth_image 125 | 126 | def depth_calibration(self,ai_depth,orb_pcd): 127 | orb_depth = self.orb_pcd_reprojet(orb_pcd) 128 | #消除前30%远的点 129 | filter = np.percentile(ai_depth,70) 130 | ai_depth[ai_depth>filter]=0 131 | 132 | #去除无穷大值 133 | ai_depth[np.isinf(ai_depth)] = 0 134 | #选出ai深度图以及orb稀疏深度图都有深度值的地方 135 | filter = np.where((ai_depth!=0)&(orb_depth!=0)) 136 | ai_list = ai_depth[filter] 137 | orb_list = orb_depth[filter] 138 | 139 | ai_list = ai_list.reshape(-1,1) 140 | orb_list = orb_list.reshape(-1,1) 141 | 142 | #最小二乘计算尺度 143 | scale=np.linalg.lstsq(ai_list,orb_list)[0] 144 | 145 | return scale 146 | 147 | 148 | -------------------------------------------------------------------------------- /config/tello_advanced_param.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(oafd) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 34 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 35 | ## but can be declared for certainty nonetheless: 36 | ## * add a exec_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | # generate_messages( 70 | # DEPENDENCIES 71 | # std_msgs # Or other packages containing msgs 72 | # ) 73 | 74 | ################################################ 75 | ## Declare ROS dynamic reconfigure parameters ## 76 | ################################################ 77 | 78 | ## To declare and build dynamic reconfigure parameters within this 79 | ## package, follow these steps: 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 82 | ## * In this file (CMakeLists.txt): 83 | ## * add "dynamic_reconfigure" to 84 | ## find_package(catkin REQUIRED COMPONENTS ...) 85 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 86 | ## and list every .cfg file to be processed 87 | 88 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 89 | # generate_dynamic_reconfigure_options( 90 | # cfg/DynReconf1.cfg 91 | # cfg/DynReconf2.cfg 92 | # ) 93 | 94 | ################################### 95 | ## catkin specific configuration ## 96 | ################################### 97 | ## The catkin_package macro generates cmake config files for your package 98 | ## Declare things to be passed to dependent projects 99 | ## INCLUDE_DIRS: uncomment this if your package contains header files 100 | ## LIBRARIES: libraries you create in this project that dependent projects also need 101 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 102 | ## DEPENDS: system dependencies of this project that dependent projects also need 103 | catkin_package( 104 | # INCLUDE_DIRS include 105 | # LIBRARIES kitti 106 | # CATKIN_DEPENDS rospy 107 | # DEPENDS system_lib 108 | ) 109 | 110 | ########### 111 | ## Build ## 112 | ########### 113 | 114 | ## Specify additional locations of header files 115 | ## Your package locations should be listed before other locations 116 | include_directories( 117 | # include 118 | ${catkin_INCLUDE_DIRS} 119 | ) 120 | 121 | ## Declare a C++ library 122 | # add_library(${PROJECT_NAME} 123 | # src/${PROJECT_NAME}/kitti.cpp 124 | # ) 125 | 126 | ## Add cmake target dependencies of the library 127 | ## as an example, code may need to be generated before libraries 128 | ## either from message generation or dynamic reconfigure 129 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 130 | 131 | ## Declare a C++ executable 132 | ## With catkin_make all packages are built within a single CMake context 133 | ## The recommended prefix ensures that target names across packages don't collide 134 | # add_executable(${PROJECT_NAME}_node src/kitti_node.cpp) 135 | 136 | ## Rename C++ executable without prefix 137 | ## The above recommended prefix causes long target names, the following renames the 138 | ## target back to the shorter version for ease of user use 139 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 140 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 141 | 142 | ## Add cmake target dependencies of the executable 143 | ## same as for the library above 144 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 145 | 146 | ## Specify libraries to link a library or executable target against 147 | # target_link_libraries(${PROJECT_NAME}_node 148 | # ${catkin_LIBRARIES} 149 | # ) 150 | 151 | ############# 152 | ## Install ## 153 | ############# 154 | 155 | # all install targets should use catkin DESTINATION variables 156 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 157 | 158 | ## Mark executable scripts (Python etc.) for installation 159 | ## in contrast to setup.py, you can choose the destination 160 | # catkin_install_python(PROGRAMS 161 | # scripts/my_python_script 162 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 163 | # ) 164 | 165 | ## Mark executables for installation 166 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 167 | # install(TARGETS ${PROJECT_NAME}_node 168 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 169 | # ) 170 | 171 | ## Mark libraries for installation 172 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 173 | # install(TARGETS ${PROJECT_NAME} 174 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 175 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 177 | # ) 178 | 179 | ## Mark cpp header files for installation 180 | # install(DIRECTORY include/${PROJECT_NAME}/ 181 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 182 | # FILES_MATCHING PATTERN "*.h" 183 | # PATTERN ".svn" EXCLUDE 184 | # ) 185 | 186 | ## Mark other files for installation (e.g. launch and bag files, etc.) 187 | # install(FILES 188 | # # myfile1 189 | # # myfile2 190 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 191 | # ) 192 | 193 | ############# 194 | ## Testing ## 195 | ############# 196 | 197 | ## Add gtest based cpp test target and link libraries 198 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_kitti.cpp) 199 | # if(TARGET ${PROJECT_NAME}-test) 200 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 201 | # endif() 202 | 203 | ## Add folders to be run by python nosetests 204 | # catkin_add_nosetests(test) 205 | -------------------------------------------------------------------------------- /config/default.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Axes1 9 | - /Mapping1/simulation_map1/Autocompute Value Bounds1 10 | - /Mapping1/map inflate1 11 | - /TF1/Frames1 12 | Splitter Ratio: 0.5 13 | Tree Height: 329 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | - /ThirdPersonFollower1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: map inflate 34 | - Class: rviz/Views 35 | Expanded: 36 | - /Current View1 37 | Name: Views 38 | Splitter Ratio: 0.5 39 | - Class: rviz/Tool Properties 40 | Expanded: ~ 41 | Name: Tool Properties 42 | Splitter Ratio: 0.5 43 | Preferences: 44 | PromptSaveOnExit: true 45 | Toolbars: 46 | toolButtonStyle: 2 47 | Visualization Manager: 48 | Class: "" 49 | Displays: 50 | - Class: rviz/Axes 51 | Enabled: true 52 | Length: 1 53 | Name: Axes 54 | Radius: 0.10000000149011612 55 | Reference Frame: 56 | Value: true 57 | - Alpha: 0.5 58 | Cell Size: 1 59 | Class: rviz/Grid 60 | Color: 160; 160; 164 61 | Enabled: true 62 | Line Style: 63 | Line Width: 0.029999999329447746 64 | Value: Lines 65 | Name: Grid 66 | Normal Cell Count: 0 67 | Offset: 68 | X: 0 69 | Y: 0 70 | Z: 0 71 | Plane: XY 72 | Plane Cell Count: 40 73 | Reference Frame: 74 | Value: true 75 | - Class: rviz/Group 76 | Displays: 77 | - Class: rviz/Marker 78 | Enabled: true 79 | Marker Topic: /ego_planner_node/goal_point 80 | Name: goal_point 81 | Namespaces: 82 | {} 83 | Queue Size: 100 84 | Value: true 85 | - Class: rviz/Marker 86 | Enabled: false 87 | Marker Topic: /ego_planner_node/global_list 88 | Name: global_path 89 | Namespaces: 90 | {} 91 | Queue Size: 100 92 | Value: false 93 | - Class: rviz/Marker 94 | Enabled: true 95 | Marker Topic: /ego_planner_node/optimal_list 96 | Name: optimal_traj 97 | Namespaces: 98 | {} 99 | Queue Size: 100 100 | Value: true 101 | - Class: rviz/Marker 102 | Enabled: false 103 | Marker Topic: /ego_planner_node/a_star_list 104 | Name: AStar 105 | Namespaces: 106 | {} 107 | Queue Size: 100 108 | Value: false 109 | - Class: rviz/Marker 110 | Enabled: false 111 | Marker Topic: /ego_planner_node/init_list 112 | Name: InitTraj 113 | Namespaces: 114 | {} 115 | Queue Size: 100 116 | Value: false 117 | - Alpha: 1 118 | Buffer Length: 1 119 | Class: rviz/Path 120 | Color: 25; 255; 0 121 | Enabled: true 122 | Head Diameter: 0.30000001192092896 123 | Head Length: 0.20000000298023224 124 | Length: 0.30000001192092896 125 | Line Style: Lines 126 | Line Width: 0.029999999329447746 127 | Name: drone_path 128 | Offset: 129 | X: 0 130 | Y: 0 131 | Z: 0 132 | Pose Color: 255; 85; 255 133 | Pose Style: Axes 134 | Radius: 0.029999999329447746 135 | Shaft Diameter: 0.10000000149011612 136 | Shaft Length: 0.10000000149011612 137 | Topic: /odom_visualization/path 138 | Unreliable: false 139 | Value: true 140 | Enabled: true 141 | Name: Planning 142 | - Class: rviz/Group 143 | Displays: 144 | - Alpha: 0.20000000298023224 145 | Autocompute Intensity Bounds: true 146 | Autocompute Value Bounds: 147 | Max Value: 2 148 | Min Value: -1 149 | Value: false 150 | Axis: Z 151 | Channel Name: intensity 152 | Class: rviz/PointCloud2 153 | Color: 85; 170; 255 154 | Color Transformer: FlatColor 155 | Decay Time: 0 156 | Enabled: true 157 | Invert Rainbow: false 158 | Max Color: 255; 255; 255 159 | Min Color: 0; 0; 0 160 | Name: simulation_map 161 | Position Transformer: XYZ 162 | Queue Size: 10 163 | Selectable: true 164 | Size (Pixels): 3 165 | Size (m): 0.10000000149011612 166 | Style: Boxes 167 | Topic: /map_generator/global_cloud 168 | Unreliable: false 169 | Use Fixed Frame: true 170 | Use rainbow: true 171 | Value: true 172 | - Alpha: 1 173 | Autocompute Intensity Bounds: true 174 | Autocompute Value Bounds: 175 | Max Value: 0.9399999976158142 176 | Min Value: 0.03999999910593033 177 | Value: true 178 | Axis: Z 179 | Channel Name: intensity 180 | Class: rviz/PointCloud2 181 | Color: 255; 255; 255 182 | Color Transformer: AxisColor 183 | Decay Time: 0 184 | Enabled: true 185 | Invert Rainbow: false 186 | Max Color: 255; 255; 255 187 | Min Color: 0; 0; 0 188 | Name: map inflate 189 | Position Transformer: XYZ 190 | Queue Size: 10 191 | Selectable: true 192 | Size (Pixels): 3 193 | Size (m): 0.10000000149011612 194 | Style: Flat Squares 195 | Topic: /grid_map/occupancy_inflate 196 | Unreliable: false 197 | Use Fixed Frame: true 198 | Use rainbow: true 199 | Value: true 200 | - Alpha: 1 201 | Autocompute Intensity Bounds: true 202 | Autocompute Value Bounds: 203 | Max Value: 2.7592508792877197 204 | Min Value: -0.9228500127792358 205 | Value: false 206 | Axis: Z 207 | Channel Name: intensity 208 | Class: rviz/PointCloud2 209 | Color: 187; 187; 187 210 | Color Transformer: AxisColor 211 | Decay Time: 0 212 | Enabled: false 213 | Invert Rainbow: false 214 | Max Color: 255; 255; 255 215 | Min Color: 0; 0; 0 216 | Name: real_map 217 | Position Transformer: XYZ 218 | Queue Size: 10 219 | Selectable: true 220 | Size (Pixels): 3 221 | Size (m): 0.10000000149011612 222 | Style: Boxes 223 | Topic: /grid_map/occupancy 224 | Unreliable: false 225 | Use Fixed Frame: true 226 | Use rainbow: true 227 | Value: false 228 | Enabled: true 229 | Name: Mapping 230 | - Class: rviz/Group 231 | Displays: 232 | - Class: rviz/Marker 233 | Enabled: true 234 | Marker Topic: /odom_visualization/robot 235 | Name: robot 236 | Namespaces: 237 | {} 238 | Queue Size: 100 239 | Value: true 240 | Enabled: false 241 | Name: Simulation 242 | - Class: rviz/Image 243 | Enabled: false 244 | Image Topic: /pcl_render_node/depth 245 | Max Value: 1 246 | Median window: 5 247 | Min Value: 0 248 | Name: depth 249 | Normalize Range: true 250 | Queue Size: 2 251 | Transport Hint: raw 252 | Unreliable: false 253 | Value: false 254 | - Alpha: 1 255 | Autocompute Intensity Bounds: true 256 | Autocompute Value Bounds: 257 | Max Value: 10 258 | Min Value: -10 259 | Value: true 260 | Axis: Z 261 | Channel Name: intensity 262 | Class: rviz/PointCloud2 263 | Color: 255; 60; 21 264 | Color Transformer: RGB8 265 | Decay Time: 0 266 | Enabled: true 267 | Invert Rainbow: false 268 | Max Color: 255; 86; 20 269 | Min Color: 232; 44; 19 270 | Name: PointCloud2 271 | Position Transformer: XYZ 272 | Queue Size: 10 273 | Selectable: true 274 | Size (Pixels): 3 275 | Size (m): 0.05000000074505806 276 | Style: Flat Squares 277 | Topic: /test_pcd 278 | Unreliable: false 279 | Use Fixed Frame: true 280 | Use rainbow: true 281 | Value: true 282 | - Class: rviz/Image 283 | Enabled: true 284 | Image Topic: /camera/image_raw 285 | Max Value: 1 286 | Median window: 5 287 | Min Value: 0 288 | Name: Image 289 | Normalize Range: true 290 | Queue Size: 2 291 | Transport Hint: raw 292 | Unreliable: false 293 | Value: true 294 | - Class: rviz/Image 295 | Enabled: true 296 | Image Topic: /camera/depth 297 | Max Value: 1 298 | Median window: 5 299 | Min Value: 0 300 | Name: Image 301 | Normalize Range: true 302 | Queue Size: 2 303 | Transport Hint: raw 304 | Unreliable: false 305 | Value: true 306 | - Class: rviz/TF 307 | Enabled: true 308 | Frame Timeout: 15 309 | Frames: 310 | All Enabled: false 311 | camera: 312 | Value: false 313 | map: 314 | Value: false 315 | tello: 316 | Value: true 317 | world: 318 | Value: false 319 | Marker Scale: 3 320 | Name: TF 321 | Show Arrows: true 322 | Show Axes: true 323 | Show Names: true 324 | Tree: 325 | world: 326 | map: 327 | camera: 328 | tello: 329 | {} 330 | Update Interval: 0 331 | Value: true 332 | - Alpha: 1 333 | Buffer Length: 1 334 | Class: rviz/Path 335 | Color: 25; 255; 0 336 | Enabled: true 337 | Head Diameter: 0.30000001192092896 338 | Head Length: 0.20000000298023224 339 | Length: 0.30000001192092896 340 | Line Style: Billboards 341 | Line Width: 0.029999999329447746 342 | Name: Path 343 | Offset: 344 | X: 0 345 | Y: 0 346 | Z: 0 347 | Pose Color: 255; 85; 255 348 | Pose Style: None 349 | Radius: 0.029999999329447746 350 | Shaft Diameter: 0.10000000149011612 351 | Shaft Length: 0.10000000149011612 352 | Topic: /orb_slam3_ros/trajectory 353 | Unreliable: false 354 | Value: true 355 | - Alpha: 1 356 | Autocompute Intensity Bounds: true 357 | Autocompute Value Bounds: 358 | Max Value: 0 359 | Min Value: -1.1070904731750488 360 | Value: true 361 | Axis: Z 362 | Channel Name: intensity 363 | Class: rviz/PointCloud2 364 | Color: 255; 0; 0 365 | Color Transformer: FlatColor 366 | Decay Time: 0 367 | Enabled: true 368 | Invert Rainbow: true 369 | Max Color: 255; 255; 2 370 | Min Color: 0; 0; 0 371 | Name: PointCloud2 372 | Position Transformer: XYZ 373 | Queue Size: 10 374 | Selectable: true 375 | Size (Pixels): 3 376 | Size (m): 0.10000000149011612 377 | Style: Flat Squares 378 | Topic: /orb_slam3_ros/map_points 379 | Unreliable: false 380 | Use Fixed Frame: true 381 | Use rainbow: true 382 | Value: true 383 | Enabled: true 384 | Global Options: 385 | Background Color: 255; 253; 224 386 | Default Light: true 387 | Fixed Frame: world 388 | Frame Rate: 30 389 | Name: root 390 | Tools: 391 | - Class: rviz/Interact 392 | Hide Inactive Objects: true 393 | - Class: rviz/Select 394 | - Class: rviz/FocusCamera 395 | - Class: rviz/Measure 396 | - Class: rviz/SetInitialPose 397 | Theta std deviation: 0.2617993950843811 398 | Topic: /initialpose 399 | X std deviation: 0.5 400 | Y std deviation: 0.5 401 | - Class: rviz/SetGoal 402 | Topic: /move_base_simple/goal 403 | - Class: rviz/PublishPoint 404 | Single click: true 405 | Topic: /clicked_point 406 | Value: true 407 | Views: 408 | Current: 409 | Class: rviz/ThirdPersonFollower 410 | Distance: 8.692840576171875 411 | Enable Stereo Rendering: 412 | Stereo Eye Separation: 0.05999999865889549 413 | Stereo Focal Distance: 1 414 | Swap Stereo Eyes: false 415 | Value: false 416 | Focal Point: 417 | X: 0.9303780198097229 418 | Y: -0.6129530668258667 419 | Z: 0.2402573525905609 420 | Focal Shape Fixed Size: true 421 | Focal Shape Size: 0.05000000074505806 422 | Invert Z Axis: false 423 | Name: Current View 424 | Near Clip Distance: 0.009999999776482582 425 | Pitch: 0.5533459186553955 426 | Target Frame: tello 427 | Value: ThirdPersonFollower (rviz) 428 | Yaw: 3.113880157470703 429 | Saved: 430 | - Class: rviz/ThirdPersonFollower 431 | Distance: 17.48538589477539 432 | Enable Stereo Rendering: 433 | Stereo Eye Separation: 0.05999999865889549 434 | Stereo Focal Distance: 1 435 | Swap Stereo Eyes: false 436 | Value: false 437 | Focal Point: 438 | X: -16.308002471923828 439 | Y: 0.4492051601409912 440 | Z: 8.589673598180525e-6 441 | Focal Shape Fixed Size: true 442 | Focal Shape Size: 0.05000000074505806 443 | Invert Z Axis: false 444 | Name: ThirdPersonFollower 445 | Near Clip Distance: 0.009999999776482582 446 | Pitch: 1.0347968339920044 447 | Target Frame: 448 | Value: ThirdPersonFollower (rviz) 449 | Yaw: 3.150407314300537 450 | Window Geometry: 451 | Displays: 452 | collapsed: false 453 | Height: 789 454 | Hide Left Dock: false 455 | Hide Right Dock: false 456 | Image: 457 | collapsed: false 458 | QMainWindow State: 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 459 | Selection: 460 | collapsed: false 461 | Time: 462 | collapsed: false 463 | Tool Properties: 464 | collapsed: false 465 | Views: 466 | collapsed: false 467 | Width: 1530 468 | X: 8 469 | Y: 32 470 | depth: 471 | collapsed: false 472 | -------------------------------------------------------------------------------- /data/my systeam en.drawio.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | --------------------------------------------------------------------------------