├── .gazebo └── pictures │ └── land.jpg ├── Pictures └── simulation.png ├── catkin_ws └── src │ ├── .rosinstall.bak │ ├── opencvtest_py │ ├── src │ │ ├── calib_images │ │ │ ├── left01.jpg │ │ │ ├── left02.jpg │ │ │ ├── left03.jpg │ │ │ ├── left04.jpg │ │ │ ├── left05.jpg │ │ │ └── test.yaml │ │ ├── test.py~ │ │ ├── test.py │ │ └── aruco_tracker.py │ ├── launch │ │ └── test.launch │ ├── package.xml │ └── CMakeLists.txt │ ├── .rosinstall │ ├── keyboard │ ├── package.xml~ │ ├── package.xml │ ├── src │ │ ├── keyboard_node.cpp │ │ └── keyboard_node.cpp~ │ ├── CMakeLists.txt │ └── CMakeLists.txt~ │ ├── control │ ├── package.xml │ ├── package.xml~ │ ├── src │ │ ├── control.py~ │ │ ├── new_land.py │ │ ├── control.py │ │ ├── land.py │ │ ├── new_land_kf.py │ │ └── kf.py │ ├── CMakeLists.txt │ └── CMakeLists.txt~ │ └── offboard │ ├── package.xml │ ├── src │ ├── offboard_node.cpp~ │ └── offboard_node.cpp │ ├── CMakeLists.txt │ └── CMakeLists.txt~ ├── README.md └── source /.gazebo/pictures/land.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Oliver-ss/DroneLanding/HEAD/.gazebo/pictures/land.jpg -------------------------------------------------------------------------------- /Pictures/simulation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Oliver-ss/DroneLanding/HEAD/Pictures/simulation.png -------------------------------------------------------------------------------- /catkin_ws/src/.rosinstall.bak: -------------------------------------------------------------------------------- 1 | # THIS IS AN AUTOGENERATED FILE, LAST GENERATED USING wstool ON 2019-04-07 2 | -------------------------------------------------------------------------------- /catkin_ws/src/opencvtest_py/src/calib_images/left01.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Oliver-ss/DroneLanding/HEAD/catkin_ws/src/opencvtest_py/src/calib_images/left01.jpg -------------------------------------------------------------------------------- /catkin_ws/src/opencvtest_py/src/calib_images/left02.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Oliver-ss/DroneLanding/HEAD/catkin_ws/src/opencvtest_py/src/calib_images/left02.jpg -------------------------------------------------------------------------------- /catkin_ws/src/opencvtest_py/src/calib_images/left03.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Oliver-ss/DroneLanding/HEAD/catkin_ws/src/opencvtest_py/src/calib_images/left03.jpg -------------------------------------------------------------------------------- /catkin_ws/src/opencvtest_py/src/calib_images/left04.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Oliver-ss/DroneLanding/HEAD/catkin_ws/src/opencvtest_py/src/calib_images/left04.jpg -------------------------------------------------------------------------------- /catkin_ws/src/opencvtest_py/src/calib_images/left05.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Oliver-ss/DroneLanding/HEAD/catkin_ws/src/opencvtest_py/src/calib_images/left05.jpg -------------------------------------------------------------------------------- /catkin_ws/src/.rosinstall: -------------------------------------------------------------------------------- 1 | # THIS IS AN AUTOGENERATED FILE, LAST GENERATED USING wstool ON 2019-04-07 2 | - git: 3 | local-name: mavlink 4 | uri: https://github.com/mavlink/mavlink-gbp-release.git 5 | version: release/kinetic/mavlink/2019.4.4-0 6 | - git: 7 | local-name: mavros 8 | uri: https://github.com/mavlink/mavros.git 9 | version: 0.29.2 10 | -------------------------------------------------------------------------------- /catkin_ws/src/opencvtest_py/src/calib_images/test.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | camera_matrix: !!opencv-matrix 4 | rows: 3 5 | cols: 3 6 | dt: d 7 | data: [ 5.3434144579284975e+02, 0., 3.3915527836173959e+02, 0., 8 | 5.3468425881789324e+02, 2.3384359492532246e+02, 0., 0., 1. ] 9 | dist_coeff: !!opencv-matrix 10 | rows: 1 11 | cols: 5 12 | dt: d 13 | data: [ -2.8832098285875657e-01, 5.4107968489116441e-02, 14 | 1.7350162244695508e-03, -2.6133389531953340e-04, 15 | 2.0411046472667685e-01 ] 16 | -------------------------------------------------------------------------------- /catkin_ws/src/opencvtest_py/launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Drone Vision Landing Project on PX4 SITL Simulation 2 | ![image](https://github.com/Oliver-ss/DroneLanding/blob/master/Pictures/simulation.png) 3 | 4 | ## Our project is to make the drone autonomously land on a launch pad by ROS. 5 | 6 | ## The task could be mostly split into two parts: Vision and Control. 7 | 8 | The first part is vision system which would detect the pad on the ground and compute the relative distance. This is based on opencv aruco marker algorithm. (/catkin_ws/src/opencvtest_py/src/test.py) The input of the system is the raw image of the camera and the output is the relative distance between drone and the launch pad in camera reference frame. 9 | 10 | The second part is the PID control system(catkin_ws/control/src/control.py). When the vision system publishes the relative distance , the control system would subscribe it and use it as the input. A 3 dimension velocity loop PID control is built as we would control the x, y, z linear velocity to approach the mark. 11 | 12 | ## Have fun about our project!!! 13 | 14 | 15 | # Quick Start 16 | ## Open the PX4 simulation environment 17 | cd ~/src/Firmware 18 | 19 | source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default 20 | export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd) 21 | export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo 22 | 23 | roslaunch px4 drone.launch 24 | 25 | ## Run the offboard program so that the drone could enter offboard mode and be armed 26 | rosrun offboard offboard_node 27 | 28 | ## Run the keyboard program so that we could use keyboard to make the drone fly 29 | rosrun keyboard keyboard_ndoe 30 | 31 | ## Run the vision system 32 | rosrun opencvtest_py test.py 33 | 34 | ## Run the control system(Only when the mark is within the view of drone camera) 35 | rosrun control control.py 36 | 37 | ## Then you could use keyboad to move the drone and let it automously landing on the mark by control program! 38 | ![image](https://github.com/Oliver-ss/DroneLanding/blob/master/.gazebo/pictures/land.jpg) 39 | -------------------------------------------------------------------------------- /catkin_ws/src/keyboard/package.xml~: -------------------------------------------------------------------------------- 1 | 2 | 3 | keyboard 4 | 0.0.0 5 | The keyboard package 6 | 7 | 8 | 9 | 10 | yuan 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 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /catkin_ws/src/keyboard/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | keyboard 4 | 0.0.0 5 | The keyboard package 6 | 7 | 8 | 9 | 10 | yuan 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 | roscpp 36 | std_msgs 37 | geometry_msgs 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | catkin 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /source: -------------------------------------------------------------------------------- 1 | 2 | # deb cdrom:[Ubuntu 16.04.5 LTS _Xenial Xerus_ - Release amd64 (20180731)]/ xenial main restricted 3 | 4 | # See http://help.ubuntu.com/community/UpgradeNotes for how to upgrade to 5 | # newer versions of the distribution. 6 | deb http://us.archive.ubuntu.com/ubuntu/ xenial main restricted 7 | # deb-src http://us.archive.ubuntu.com/ubuntu/ xenial main restricted 8 | 9 | ## Major bug fix updates produced after the final release of the 10 | ## distribution. 11 | deb http://us.archive.ubuntu.com/ubuntu/ xenial-updates main restricted 12 | # deb-src http://us.archive.ubuntu.com/ubuntu/ xenial-updates main restricted 13 | 14 | ## N.B. software from this repository is ENTIRELY UNSUPPORTED by the Ubuntu 15 | ## team. Also, please note that software in universe WILL NOT receive any 16 | ## review or updates from the Ubuntu security team. 17 | deb http://us.archive.ubuntu.com/ubuntu/ xenial universe 18 | # deb-src http://us.archive.ubuntu.com/ubuntu/ xenial universe 19 | deb http://us.archive.ubuntu.com/ubuntu/ xenial-updates universe 20 | # deb-src http://us.archive.ubuntu.com/ubuntu/ xenial-updates universe 21 | 22 | ## N.B. software from this repository is ENTIRELY UNSUPPORTED by the Ubuntu 23 | ## team, and may not be under a free licence. Please satisfy yourself as to 24 | ## your rights to use the software. Also, please note that software in 25 | ## multiverse WILL NOT receive any review or updates from the Ubuntu 26 | ## security team. 27 | deb http://us.archive.ubuntu.com/ubuntu/ xenial multiverse 28 | # deb-src http://us.archive.ubuntu.com/ubuntu/ xenial multiverse 29 | deb http://us.archive.ubuntu.com/ubuntu/ xenial-updates multiverse 30 | # deb-src http://us.archive.ubuntu.com/ubuntu/ xenial-updates multiverse 31 | 32 | ## N.B. software from this repository may not have been tested as 33 | ## extensively as that contained in the main release, although it includes 34 | ## newer versions of some applications which may provide useful features. 35 | ## Also, please note that software in backports WILL NOT receive any review 36 | ## or updates from the Ubuntu security team. 37 | deb http://us.archive.ubuntu.com/ubuntu/ xenial-backports main restricted universe multiverse 38 | # deb-src http://us.archive.ubuntu.com/ubuntu/ xenial-backports main restricted universe multiverse 39 | 40 | ## Uncomment the following two lines to add software from Canonical's 41 | ## 'partner' repository. 42 | ## This software is not part of Ubuntu, but is offered by Canonical and the 43 | ## respective vendors as a service to Ubuntu users. 44 | # deb http://archive.canonical.com/ubuntu xenial partner 45 | # deb-src http://archive.canonical.com/ubuntu xenial partner 46 | 47 | deb http://security.ubuntu.com/ubuntu xenial-security main restricted 48 | # deb-src http://security.ubuntu.com/ubuntu xenial-security main restricted 49 | deb http://security.ubuntu.com/ubuntu xenial-security universe 50 | # deb-src http://security.ubuntu.com/ubuntu xenial-security universe 51 | deb http://security.ubuntu.com/ubuntu xenial-security multiverse 52 | # deb-src http://security.ubuntu.com/ubuntu xenial-security multiverse 53 | -------------------------------------------------------------------------------- /catkin_ws/src/control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | control 4 | 0.0.0 5 | The control package 6 | 7 | 8 | 9 | 10 | yuan 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 | geometry_msgs 53 | rospy 54 | tf 55 | geometry_msgs 56 | rospy 57 | tf 58 | geometry_msgs 59 | rospy 60 | tf 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /catkin_ws/src/offboard/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | offboard 4 | 0.0.0 5 | The offboard package 6 | 7 | 8 | 9 | 10 | yuan 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 | geometry_msgs 53 | mavros 54 | roscpp 55 | geometry_msgs 56 | mavros 57 | roscpp 58 | geometry_msgs 59 | mavros 60 | roscpp 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /catkin_ws/src/offboard/src/offboard_node.cpp~: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | mavros_msgs::State current_state; 9 | void state_cb(const mavros_msgs::State::ConstPtr& msg){ 10 | current_state = *msg; 11 | } 12 | 13 | void receive_callback(const geometry_msgs::Twist::ConstPtr& msg){ 14 | geometry_msgs::Twist vel = *msg; 15 | } 16 | int main(int argc, char **argv) 17 | { 18 | ros::init(argc, argv, "offb_node"); 19 | ros::NodeHandle nh; 20 | 21 | ros::Subscriber state_sub = nh.subscribe 22 | ("mavros/state", 10, state_cb); 23 | ros::Subscriber cmd_receive = nh.subscribe 24 | ("cmd_vel/", 10, receive_callback); 25 | ros::Publisher local_vel_pub = nh.advertise 26 | ("mavros/setpoint_velocity/cmd_vel_unstamped", 10); 27 | ros::ServiceClient arming_client = nh.serviceClient 28 | ("mavros/cmd/arming"); 29 | ros::ServiceClient set_mode_client = nh.serviceClient 30 | ("mavros/set_mode"); 31 | 32 | //the setpoint publishing rate MUST be faster than 2Hz 33 | ros::Rate rate(20.0); 34 | 35 | // wait for FCU connection 36 | while(ros::ok() && !current_state.connected){ 37 | ros::spinOnce(); 38 | rate.sleep(); 39 | } 40 | 41 | //geometry_msgs::Twist vel; 42 | // vel.linear.x = 0; 43 | //pose.pose.position.y = 0; 44 | //pose.pose.position.z = 0; 45 | 46 | //send a few setpoints before starting 47 | // for(int i = 100; ros::ok() && i > 0; --i){ 48 | // local_pos_pub.publish(pose); 49 | // ros::spinOnce(); 50 | // rate.sleep(); 51 | // } 52 | 53 | mavros_msgs::SetMode offb_set_mode; 54 | offb_set_mode.request.custom_mode = "OFFBOARD"; 55 | 56 | mavros_msgs::CommandBool arm_cmd; 57 | arm_cmd.request.value = true; 58 | 59 | ros::Time last_request = ros::Time::now(); 60 | 61 | //while(ros::ok() &&(current_state.mode != "OFFBOARD") ||(!current_state.arme) ){ 62 | while(ros::ok()){ 63 | if( current_state.mode != "OFFBOARD" && 64 | (ros::Time::now() - last_request > ros::Duration(5.0))){ 65 | if( set_mode_client.call(offb_set_mode) && 66 | offb_set_mode.response.mode_sent){ 67 | ROS_INFO("Offboard enabled"); 68 | } 69 | last_request = ros::Time::now(); 70 | } else { 71 | if( !current_state.armed && 72 | (ros::Time::now() - last_request > ros::Duration(5.0))){ 73 | if( arming_client.call(arm_cmd) && 74 | arm_cmd.response.success){ 75 | ROS_INFO("Vehicle armed"); 76 | } 77 | last_request = ros::Time::now(); 78 | } 79 | } 80 | 81 | local_vel_pub.publish(vel); 82 | 83 | ros::spinOnce(); 84 | rate.sleep(); 85 | } 86 | //ros::spin(); 87 | 88 | return 0; 89 | } 90 | -------------------------------------------------------------------------------- /catkin_ws/src/offboard/src/offboard_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | mavros_msgs::State current_state; 9 | geometry_msgs::Twist vel; 10 | void state_cb(const mavros_msgs::State::ConstPtr& msg){ 11 | current_state = *msg; 12 | } 13 | 14 | void receive_callback(const geometry_msgs::Twist::ConstPtr& msg){ 15 | vel = *msg; 16 | } 17 | int main(int argc, char **argv) 18 | { 19 | ros::init(argc, argv, "offb_node"); 20 | ros::NodeHandle nh; 21 | 22 | ros::Subscriber state_sub = nh.subscribe 23 | ("mavros/state", 10, state_cb); 24 | ros::Subscriber cmd_receive = nh.subscribe 25 | ("cmd_vel/", 10, receive_callback); 26 | ros::Publisher local_vel_pub = nh.advertise 27 | ("mavros/setpoint_velocity/cmd_vel_unstamped", 10); 28 | ros::ServiceClient arming_client = nh.serviceClient 29 | ("mavros/cmd/arming"); 30 | ros::ServiceClient set_mode_client = nh.serviceClient 31 | ("mavros/set_mode"); 32 | 33 | //the setpoint publishing rate MUST be faster than 2Hz 34 | ros::Rate rate(20.0); 35 | 36 | // wait for FCU connection 37 | while(ros::ok() && !current_state.connected){ 38 | ros::spinOnce(); 39 | rate.sleep(); 40 | } 41 | 42 | //geometry_msgs::Twist vel; 43 | // vel.linear.x = 0; 44 | //pose.pose.position.y = 0; 45 | //pose.pose.position.z = 0; 46 | 47 | //send a few setpoints before starting 48 | // for(int i = 100; ros::ok() && i > 0; --i){ 49 | // local_pos_pub.publish(pose); 50 | // ros::spinOnce(); 51 | // rate.sleep(); 52 | // } 53 | 54 | mavros_msgs::SetMode offb_set_mode; 55 | offb_set_mode.request.custom_mode = "OFFBOARD"; 56 | 57 | mavros_msgs::CommandBool arm_cmd; 58 | arm_cmd.request.value = true; 59 | 60 | ros::Time last_request = ros::Time::now(); 61 | 62 | //while(ros::ok() &&(current_state.mode != "OFFBOARD") ||(!current_state.arme) ){ 63 | while(ros::ok()){ 64 | if( current_state.mode != "OFFBOARD" && 65 | (ros::Time::now() - last_request > ros::Duration(5.0))){ 66 | if( set_mode_client.call(offb_set_mode) && 67 | offb_set_mode.response.mode_sent){ 68 | ROS_INFO("Offboard enabled"); 69 | } 70 | last_request = ros::Time::now(); 71 | } else { 72 | if( !current_state.armed && 73 | (ros::Time::now() - last_request > ros::Duration(5.0))){ 74 | if( arming_client.call(arm_cmd) && 75 | arm_cmd.response.success){ 76 | ROS_INFO("Vehicle armed"); 77 | } 78 | last_request = ros::Time::now(); 79 | } 80 | } 81 | 82 | local_vel_pub.publish(vel); 83 | 84 | ros::spinOnce(); 85 | rate.sleep(); 86 | } 87 | //ros::spin(); 88 | 89 | return 0; 90 | } 91 | -------------------------------------------------------------------------------- /catkin_ws/src/control/package.xml~: -------------------------------------------------------------------------------- 1 | 2 | 3 | control 4 | 0.0.0 5 | The control package 6 | 7 | 8 | 9 | 10 | yuan 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 | geometry_msgs 53 | rospy 54 | tf 55 | math 56 | geometry_msgs 57 | rospy 58 | tf 59 | math 60 | geometry_msgs 61 | rospy 62 | tf 63 | math 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /catkin_ws/src/opencvtest_py/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | opencvtest_py 4 | 0.0.0 5 | The opencvtest_py package 6 | 7 | 8 | 9 | 10 | yuan 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 | cv_bridge 53 | opencv2 54 | rospy 55 | sensor_msgs 56 | std_msgs 57 | cv_bridge 58 | opencv2 59 | rospy 60 | sensor_msgs 61 | std_msgs 62 | cv_bridge 63 | opencv2 64 | rospy 65 | sensor_msgs 66 | std_msgs 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /catkin_ws/src/opencvtest_py/src/test.py~: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from __future__ import print_function 3 | import time 4 | import roslib 5 | #roslib.load_manifest('my_package') 6 | import sys 7 | import rospy 8 | import cv2 9 | import glob 10 | import numpy as np 11 | import cv2.aruco as aruco 12 | from std_msgs.msg import String 13 | from sensor_msgs.msg import Image 14 | from std_msgs.msg import Float32MultiArray 15 | from cv_bridge import CvBridge, CvBridgeError 16 | 17 | class image_converter: 18 | 19 | def __init__(self): 20 | self.image_pub = rospy.Publisher("/cv_image",Image) 21 | self.distance_pub = rospy.Publisher("/relative_distance",Float32MultiArray) 22 | 23 | self.bridge = CvBridge() 24 | self.image_sub = rospy.Subscriber("/iris/image_raw",Image,self.callback) 25 | 26 | def callback(self,data): 27 | try: 28 | # tranform ROS image message into opencv image 29 | cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") 30 | except CvBridgeError as e: 31 | print(e) 32 | 33 | global ret, mtx, dist, rvecs, tvecs 34 | 35 | # aruco basic setting 36 | aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) 37 | parameters = aruco.DetectorParameters_create() 38 | 39 | # convert the image 40 | cv_image_gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) 41 | 42 | # detect maker configuraiton 43 | corners, ids, rejectedImgPoints = aruco.detectMarkers(cv_image_gray, aruco_dict, parameters=parameters) 44 | 45 | if np.all(ids != None): 46 | # pose estimation 47 | # 0.19: markerLength, mtx: cameraMatrix, dist: distortion coefficients 48 | # rvec: 49 | rvec, tvec,_ = aruco.estimatePoseSingleMarkers(corners[0], 0.19, mtx, dist) 50 | 51 | # read corners informatio 52 | topleftX = corners[0][0][0][0] 53 | topleftY = corners[0][0][0][1] 54 | 55 | toprightX = corners[0][0][1][0] 56 | toprightY = corners[0][0][1][1] 57 | 58 | bottomleftX = corners[0][0][2][0] 59 | bottomlextY = corners[0][0][2][1] 60 | 61 | bottomrightX = corners[0][0][3][0] 62 | bottomrightY = corners[0][0][3][1] 63 | 64 | # print corners information 65 | #print("topleft corner x {}".format(topleftX)) 66 | #print("topleft corner y {}".format(topleftY)) 67 | 68 | #print("topright corner x {}".format(toprightX)) 69 | #print("topright corner y {}".format(toprightY)) 70 | 71 | # print("bottomleft corner x {}".format(bottomleftX)) 72 | # print("bottomleft corner y {}".format(bottomlextY)) 73 | 74 | # print("bottomright corner x {}".format(bottomrightX)) 75 | # print("bottomright corner y {}".format(bottomrightY)) 76 | 77 | # get pose information 78 | cor_x = tvec[0][0][0] 79 | cor_y = tvec[0][0][1] 80 | cor_z = tvec[0][0][2] 81 | print("x=",cor_x) 82 | print("y=",cor_y) 83 | print("z=",cor_z) 84 | # time.sleep(2);#delete later 85 | midpointX = (topleftX + bottomrightX)/2 86 | midpointY = (topleftY + bottomrightY)/2 87 | 88 | # draw axis and detected marker 89 | aruco.drawAxis(cv_image, mtx, dist, rvec[0], tvec[0], 0.1) 90 | aruco.drawDetectedMarkers(cv_image, corners) 91 | 92 | # draw ID text on top of image 93 | font = cv2.FONT_HERSHEY_SIMPLEX 94 | cv2.putText(cv_image, "Id: " + str(ids), (0,64), font, 1, (0,255,0),2,cv2.LINE_AA) 95 | 96 | # incorporate pose information together and print on image 97 | dis=Float32MultiArray() 98 | #dis.layout=(3,1) 99 | dis.data=(cor_x, cor_y, cor_z) 100 | 101 | cv2.imshow("Image window", cv_image) 102 | cv2.waitKey(3) 103 | 104 | # Node Publish - pose information 105 | self.distance_pub.publish(dis) 106 | 107 | # Node Publish - cv_image 108 | try: 109 | self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) 110 | 111 | except CvBridgeError as e: 112 | print(e) 113 | 114 | def main(args): 115 | global ret, mtx, dist, rvecs, tvecs 116 | 117 | # cameraMatrix and distortion coefficients 118 | mtx = np.array([[552.1877667, 0. , 289.37921553], [ 0. , 550.98791255, 228.87373308], [0. , 0. , 1.]]) 119 | dist = np.array([[ 1.51623055e-03, 9.03278089e-02, 6.51492926e-03, -9.21777965e-05, -4.29890497e-01]]) 120 | 121 | ic = image_converter() 122 | rospy.init_node('image_converter', anonymous=True) 123 | 124 | try: 125 | rospy.spin() 126 | except KeyboardInterrupt: 127 | print("Shutting down") 128 | cv2.destroyAllWindows() 129 | 130 | if __name__ == '__main__': 131 | main(sys.argv) 132 | -------------------------------------------------------------------------------- /catkin_ws/src/control/src/control.py~: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import tf 4 | import geometry_msgs.msg 5 | from geometry_msgs.msg import Twist 6 | from geometry_msgs.msg import Pose 7 | from geometry_msgs.msg import TransformStamped 8 | from math import pow, atan2, sqrt, pi, degrees 9 | from std_msgs.msg import Float32MultiArray 10 | from tf.transformations import euler_from_quaternion 11 | from tf.transformations import quaternion_from_euler 12 | 13 | def euclidean_distance(xd, yd, zd): 14 | return sqrt(pow((xd), 2) + pow((yd), 2) + pow((zd), 2)) 15 | 16 | 17 | class State: 18 | def __init__(self, x = 0, y = 0, z = 0): 19 | self.x = x 20 | self.y = y 21 | self.z = z 22 | 23 | class PID: 24 | def __init__(self,kp=1,kd=0,ki=0,dt=0.01): 25 | 26 | #GAINS 27 | self.kp = kp 28 | self.kd = kd 29 | self.ki = ki 30 | 31 | #TIME STEP 32 | self.dt = dt 33 | 34 | #Default ERROR INITIALIZATION 35 | self.err_previous = 0.001 36 | self.err_acc = 0 37 | 38 | def compute(self,err): 39 | 40 | #compute dervivative 41 | err_deriv = (err - self.err_previous)/self.dt 42 | 43 | #update integration 44 | self.err_acc = self.err_acc + self.dt * (err + self.err_previous)/2 45 | 46 | #compute pid equation 47 | pid = self.kp*err + self.kd*err_deriv + self.ki*self.err_acc 48 | 49 | #update error 50 | self.err_previous = err 51 | 52 | return pid 53 | 54 | class Controller: 55 | ''' 56 | main class of the ros node controlling the robot. 57 | ''' 58 | 59 | def __init__(self): 60 | 61 | #initialization of the ros node and relevant pub/sub 62 | rospy.init_node("PID_node") 63 | self.velocity_publisher=rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped",Twist,queue_size=1) 64 | self.bebop_subscriber=rospy.Subscriber("/relative_distance", Float32MultiArray ,self.call_back) 65 | 66 | #robot current state 67 | self.state = State() 68 | 69 | #controller frequency in Hz 70 | self.hz=50.0 71 | self.rate = rospy.Rate(self.hz) 72 | self.dt=(1.0/self.hz) 73 | 74 | #define pids 75 | self.pid_rho = PID(kp=0.2,dt=self.dt) 76 | 77 | # transformation 78 | def call_back(self, msg): 79 | #print(msg.data) 80 | self.state.x = msg.data[1] 81 | self.state.y = msg.data[0] 82 | self.state.z = -msg.data[2] 83 | 84 | def move_to_goal(self): 85 | 86 | #variable initialization 87 | vel_msg = Twist() 88 | tolerance_position = 0.01 89 | 90 | rho = euclidean_distance(self.state.x, self.state.y, self.state.z) 91 | while rho >= tolerance_position or rho==0: 92 | rospy.loginfo("Distance from goal:"+str(rho)) 93 | 94 | rho = euclidean_distance(self.state.x, self.state.y, self.state.z) 95 | err_x = self.state.x 96 | err_y = self.state.y 97 | err_z = self.state.z 98 | 99 | #Compute PID 100 | vx = self.pid_rho.compute(err_x) 101 | vy = 0 102 | #vy = self.pid_rho.compute(err_y) 103 | vz = 0 104 | #vz = self.pid_rho.compute(err_z) * 0.25 105 | 106 | #fill message 107 | vel_msg.linear.x = vx 108 | vel_msg.linear.y = vy 109 | vel_msg.linear.z = vz 110 | vel_msg.angular.x = 0.0 111 | vel_msg.angular.y = 0.0 112 | vel_msg.angular.z = 0.0 113 | 114 | #debugging 115 | print("vx: {}".format(vel_msg.linear.x)) 116 | print("x: {}".format(self.state.x)) 117 | print("vy: {}".format(vel_msg.linear.y)) 118 | print("y: {}".format(self.state.y)) 119 | print("vz: {}".format(vel_msg.linear.z)) 120 | print("z: {}".format(self.state.z)) 121 | print("_________________") 122 | 123 | #publish 124 | self.velocity_publisher.publish(vel_msg) 125 | self.rate.sleep() 126 | 127 | #stop the robot 128 | vel_msg.linear.x=0.0 129 | vel_msg.linear.y=0.0 130 | vel_msg.linear.z=0.0 131 | self.velocity_publisher.publish(vel_msg) 132 | rospy.loginfo("I'm here(relative info): "+ str(self.state.x) + " , " + str(self.state.y) +" , " + str(self.state.z)) 133 | print("___") 134 | 135 | return 136 | 137 | 138 | if __name__ == '__main__': 139 | try: 140 | x = Controller() 141 | 142 | #MOVE TO THE GOALS 143 | x.move_to_goal() 144 | 145 | #spin 146 | rospy.spin() 147 | 148 | except rospy.ROSInterruptException: 149 | pass 150 | -------------------------------------------------------------------------------- /catkin_ws/src/opencvtest_py/src/test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from __future__ import print_function 3 | import time 4 | import roslib 5 | #roslib.load_manifest('my_package') 6 | import sys 7 | import rospy 8 | import cv2 9 | import glob 10 | import numpy as np 11 | import cv2.aruco as aruco 12 | from std_msgs.msg import String 13 | from sensor_msgs.msg import Image 14 | from std_msgs.msg import Float32MultiArray 15 | from cv_bridge import CvBridge, CvBridgeError 16 | 17 | class image_converter: 18 | 19 | def __init__(self): 20 | self.image_pub = rospy.Publisher("/cv_image",Image) 21 | self.distance_pub = rospy.Publisher("/relative_distance",Float32MultiArray,queue_size=1) 22 | 23 | self.bridge = CvBridge() 24 | self.image_sub = rospy.Subscriber("/iris/image_raw",Image,self.callback) 25 | self.lostnumber=0 26 | 27 | def callback(self,data): 28 | try: 29 | # tranform ROS image message into opencv image 30 | cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") 31 | except CvBridgeError as e: 32 | print(e) 33 | 34 | global ret, mtx, dist, rvecs, tvecs 35 | 36 | # aruco basic setting 37 | aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50) 38 | parameters = aruco.DetectorParameters_create() 39 | 40 | # convert the image 41 | cv_image_gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) 42 | 43 | # detect maker configuraiton 44 | corners, ids, rejectedImgPoints = aruco.detectMarkers(cv_image_gray, aruco_dict, parameters=parameters) 45 | if np.all(ids != None): 46 | self.lostnumber=0 47 | id=ids[0][0] 48 | lock_number=0 49 | for i in range(ids.size): 50 | if ids[i][0]>id: 51 | id=ids[i][0] 52 | lock_number=i 53 | markersize=0 54 | if id==1: 55 | markersize=0.139 56 | elif id==2: 57 | markersize=0.071 58 | elif id==3: 59 | markersize=0.0325 60 | elif id==4: 61 | markersize=0.016 62 | 63 | # pose estimation 64 | # 0.19: markerLength, mtx: cameraMatrix, dist: distortion coefficients 65 | # rvec: 66 | rvec, tvec,_ = aruco.estimatePoseSingleMarkers(corners[lock_number], markersize, mtx, dist) 67 | 68 | 69 | # read corners informatio 70 | topleftX = corners[0][0][0][0] 71 | topleftY = corners[0][0][0][1] 72 | 73 | toprightX = corners[0][0][1][0] 74 | toprightY = corners[0][0][1][1] 75 | 76 | bottomleftX = corners[0][0][2][0] 77 | bottomlextY = corners[0][0][2][1] 78 | 79 | bottomrightX = corners[0][0][3][0] 80 | bottomrightY = corners[0][0][3][1] 81 | 82 | # get pose information 83 | cor_x = tvec[0][0][0] 84 | cor_y = tvec[0][0][1] 85 | cor_z = tvec[0][0][2] 86 | print("x=",cor_x) 87 | print("y=",cor_y) 88 | print("z=",cor_z) 89 | 90 | midpointX = (topleftX + bottomrightX)/2 91 | midpointY = (topleftY + bottomrightY)/2 92 | 93 | # draw axis and detected marker 94 | aruco.drawAxis(cv_image, mtx, dist, rvec[0], tvec[0], 0.1) 95 | aruco.drawDetectedMarkers(cv_image, corners) 96 | 97 | # draw ID text on top of image 98 | font = cv2.FONT_HERSHEY_SIMPLEX 99 | cv2.putText(cv_image, "X: {}".format(cor_x), (0,364), font, 1, (0,255,0),2,cv2.LINE_AA) 100 | cv2.putText(cv_image, "Y: {}".format(cor_y), (0,400), font, 1, (0,255,0),2,cv2.LINE_AA) 101 | cv2.putText(cv_image, "Z: {}".format(cor_z), (0,436), font, 1, (0,255,0),2,cv2.LINE_AA) 102 | 103 | # incorporate pose information together and print on image 104 | dis=Float32MultiArray() 105 | #dis.layout=(3,1) 106 | dis.data=(cor_x, cor_y, cor_z) 107 | 108 | cv2.imshow("Image window", cv_image) 109 | cv2.waitKey(3) 110 | 111 | # Node Publish - pose information 112 | self.distance_pub.publish(dis) 113 | 114 | # Node Publish - cv_image 115 | try: 116 | self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) 117 | except CvBridgeError as e: 118 | print(e) 119 | 120 | else: 121 | self.lostnumber+=1 122 | if self.lostnumber>100: 123 | dis = Float32MultiArray() 124 | dis.data = (float('nan'), float('nan'), float('nan')) 125 | self.distance_pub.publish(dis) 126 | 127 | 128 | def main(args): 129 | global ret, mtx, dist, rvecs, tvecs 130 | 131 | # cameraMatrix and distortion coefficients 132 | mtx = np.array([[552.1877667, 0. , 289.37921553], [ 0. , 550.98791255, 228.87373308], [0. , 0. , 1.]]) 133 | dist = np.array([[ 1.51623055e-03, 9.03278089e-02, 6.51492926e-03, -9.21777965e-05, -4.29890497e-01]]) 134 | 135 | ic = image_converter() 136 | rospy.init_node('image_converter', anonymous=True) 137 | 138 | try: 139 | rospy.spin() 140 | except KeyboardInterrupt: 141 | print("Shutting down") 142 | cv2.destroyAllWindows() 143 | 144 | if __name__ == '__main__': 145 | main(sys.argv) 146 | -------------------------------------------------------------------------------- /catkin_ws/src/opencvtest_py/src/aruco_tracker.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | import cv2.aruco as aruco 4 | import glob 5 | import socket 6 | 7 | #ser = serial.Serial('/dev/ttyUSB0') 8 | 9 | print("I'm working...") 10 | 11 | #TCP_IP = '169.254.137.76' 12 | TCP_IP = '127.0.0.1' 13 | TCP_PORT = 5005 14 | BUFFER_SIZE = 1024 15 | 16 | s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 17 | s.connect((TCP_IP, TCP_PORT)) 18 | 19 | print("I'm connected...") 20 | 21 | 22 | 23 | cap = cv2.VideoCapture(0) 24 | 25 | # termination criteria 26 | criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) 27 | 28 | # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) 29 | objp = np.zeros((6*7,3), np.float32) 30 | objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2) 31 | 32 | # Arrays to store object points and image points from all the images. 33 | objpoints = [] # 3d point in real world space 34 | imgpoints = [] # 2d points in image plane. 35 | 36 | images = glob.glob('calib_images/*.jpg') 37 | 38 | 39 | for fname in images: 40 | img = cv2.imread(fname) 41 | gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) 42 | 43 | # Find the chess board corners 44 | ret, corners = cv2.findChessboardCorners(gray, (7,6),None) 45 | #print(corners) 46 | #print(objpoints) 47 | 48 | # If found, add object points, image points (after refining them) 49 | if ret == True: 50 | objpoints.append(objp) 51 | 52 | corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria) 53 | imgpoints.append(corners2) 54 | 55 | # Draw and display the corners 56 | img = cv2.drawChessboardCorners(img, (7,6), corners2,ret) 57 | 58 | 59 | ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None) 60 | 61 | 62 | while (True): 63 | ret, frame = cap.read() 64 | # operations on the frame come here 65 | gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) 66 | aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) 67 | parameters = aruco.DetectorParameters_create() 68 | 69 | #lists of ids and the corners beloning to each id 70 | corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters) 71 | 72 | 73 | # print(ids) 74 | 75 | font = cv2.FONT_HERSHEY_SIMPLEX #font for displaying text (below) 76 | 77 | if np.all(ids != None): 78 | rvec, tvec,_ = aruco.estimatePoseSingleMarkers(corners[0], 0.05, mtx, dist) #Estimate pose of each marker and return the values rvet and tvec---different from camera coefficients 79 | #(rvec-tvec).any() # get rid of that nasty numpy value array error 80 | 81 | topleftX = corners[0][0][0][0] 82 | topleftY = corners[0][0][0][1] 83 | 84 | toprightX = corners[0][0][1][0] 85 | toprightY = corners[0][0][1][1] 86 | 87 | bottomleftX = corners[0][0][2][0] 88 | bottomlextY = corners[0][0][2][1] 89 | 90 | bottomrightX = corners[0][0][3][0] 91 | bottomrightY = corners[0][0][3][1] 92 | 93 | distance = tvec[0][0][2] 94 | 95 | 96 | print("topleft corner x {}".format(topleftX)) 97 | print("topleft corner y {}".format(topleftY)) 98 | 99 | print("topright corner x {}".format(toprightX)) 100 | print("topright corner y {}".format(toprightY)) 101 | 102 | print("bottomleft corner x {}".format(bottomleftX)) 103 | print("bottomleft corner y {}".format(bottomlextY)) 104 | 105 | print("bottomright corner x {}".format(bottomrightX)) 106 | print("bottomright corner y {}".format(bottomrightY)) 107 | 108 | print("distance {}".format(distance)) 109 | 110 | ''' 111 | s.send(b"/tly", {}.format(topleftX)) 112 | s.send(b"/tlx", "{}".format(topleftY)) 113 | 114 | s.send(b"/trx", '{}'.format(toprightX)) 115 | s.send(b"/try", '{}'.format(toprightY)) 116 | 117 | s.send(b"/blx", '{}'.format(bottomleftX)) 118 | s.send(b"/bly", '{}'.format(bottomlextY)) 119 | 120 | s.send(b"/brx", '{}'.format(bottomrightX)) 121 | s.send(b"/bry", '{}'.format(bottomrightY)) 122 | ''' 123 | 124 | midpointX = (topleftX + bottomrightX)/2 125 | midpointY = (topleftY + bottomrightY)/2 126 | 127 | 128 | 129 | print("midpoint X: {}, Y: {}".format(midpointX, midpointY)) 130 | s.send("{}/{}/{}".format(midpointX, midpointY, distance).encode()) 131 | 132 | 133 | aruco.drawAxis(frame, mtx, dist, rvec[0], tvec[0], 0.1) #Draw Axis 134 | aruco.drawDetectedMarkers(frame, corners) #Draw A square around the markers 135 | 136 | 137 | ###### DRAW ID ##### 138 | cv2.putText(frame, "Id: " + str(ids), (0,64), font, 1, (0,255,0),2,cv2.LINE_AA) 139 | else: 140 | print("midpoint X: n, Y: n") 141 | s.send(b"/n/n/n") 142 | 143 | 144 | 145 | # Display the resulting frame 146 | cv2.imshow('frame',frame) 147 | if cv2.waitKey(1) & 0xFF == ord('q'): 148 | break 149 | 150 | # When everything done, release the capture 151 | cap.release() 152 | cv2.destroyAllWindows() -------------------------------------------------------------------------------- /catkin_ws/src/keyboard/src/keyboard_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #define KEYCODE_UP 0x69 // I, Z axis up 13 | #define KEYCODE_LEFT 0x6A // J, Z rotate clockwise 14 | #define KEYCODE_RIGHT 0x6C // L, Z rotate anticlockwise 15 | #define KEYCODE_DOWN 0x6B // K, Z axis down 16 | 17 | #define KEYCODE_A_CAP 0x61 // Y axis up 18 | #define KEYCODE_D_CAP 0x64 // Y axis down 19 | #define KEYCODE_S_CAP 0x73 // X axis up 20 | #define KEYCODE_W_CAP 0x77 // X axis down 21 | 22 | class KeyboardTeleopNode 23 | { 24 | private: 25 | double walk_vel_; 26 | // double run_vel_; 27 | double yaw_rate_; 28 | //double yaw_rate_run_; 29 | 30 | geometry_msgs::Twist cmdvel_; 31 | ros::NodeHandle n_; 32 | ros::Publisher pub_; 33 | 34 | public: 35 | KeyboardTeleopNode() 36 | { 37 | pub_ = n_.advertise("cmd_vel", 1); 38 | 39 | ros::NodeHandle n_private("~"); 40 | n_private.param("walk_vel", walk_vel_, 1.0); 41 | n_private.param("yaw_rate", yaw_rate_, 1.0); 42 | } 43 | 44 | ~KeyboardTeleopNode() { } 45 | void keyboardLoop(); 46 | 47 | void stopRobot() 48 | { 49 | cmdvel_.linear.x = 0.0; 50 | cmdvel_.linear.y = 0.0; 51 | cmdvel_.linear.z = 0.0; 52 | cmdvel_.angular.z = 0.0; 53 | pub_.publish(cmdvel_); 54 | } 55 | }; 56 | 57 | KeyboardTeleopNode* tbk; 58 | int kfd = 0; 59 | struct termios cooked, raw; 60 | bool done; 61 | 62 | int main(int argc, char** argv) 63 | { 64 | ros::init(argc,argv,"tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler); 65 | KeyboardTeleopNode tbk; 66 | 67 | boost::thread t = boost::thread(boost::bind(&KeyboardTeleopNode::keyboardLoop, &tbk)); 68 | 69 | ros::spin(); 70 | 71 | t.interrupt(); 72 | t.join(); 73 | tbk.stopRobot(); 74 | tcsetattr(kfd, TCSANOW, &cooked); 75 | 76 | return(0); 77 | } 78 | 79 | void KeyboardTeleopNode::keyboardLoop() 80 | { 81 | char c; 82 | double max_tv = walk_vel_; 83 | double max_rv = yaw_rate_; 84 | bool dirty = false; 85 | int x_speed = 0; 86 | int y_speed = 0; 87 | int z_speed = 0; 88 | int z_turn = 0; 89 | 90 | 91 | // get the console in raw mode 92 | tcgetattr(kfd, &cooked); 93 | memcpy(&raw, &cooked, sizeof(struct termios)); 94 | raw.c_lflag &=~ (ICANON | ECHO); 95 | raw.c_cc[VEOL] = 1; 96 | raw.c_cc[VEOF] = 2; 97 | tcsetattr(kfd, TCSANOW, &raw); 98 | 99 | puts("Reading from keyboard"); 100 | puts("Use WASD keys to control the robot"); 101 | // puts("Press Shift to move faster"); 102 | 103 | struct pollfd ufd; 104 | ufd.fd = kfd; 105 | ufd.events = POLLIN; 106 | 107 | for(;;) 108 | { 109 | boost::this_thread::interruption_point(); 110 | 111 | // get the next event from the keyboard 112 | int num; 113 | 114 | if ((num = poll(&ufd, 1, 250)) < 0) 115 | { 116 | perror("poll():"); 117 | return; 118 | } 119 | else if(num > 0) 120 | { 121 | if(read(kfd, &c, 1) < 0) 122 | { 123 | perror("read():"); 124 | return; 125 | } 126 | } 127 | else 128 | { 129 | if (dirty == true) 130 | { 131 | stopRobot(); 132 | dirty = false; 133 | } 134 | 135 | continue; 136 | } 137 | 138 | switch(c) 139 | { 140 | case KEYCODE_UP: 141 | //max_tv = walk_vel_; 142 | x_speed = 0; 143 | y_speed = 0; 144 | z_speed = 1; 145 | z_turn = 0; 146 | dirty = true; 147 | break; 148 | case KEYCODE_DOWN: 149 | // max_tv = walk_vel_; 150 | x_speed = 0; 151 | y_speed = 0; 152 | z_speed = -1; 153 | z_turn = 0; 154 | dirty = true; 155 | break; 156 | case KEYCODE_LEFT: 157 | // max_rv = yaw_rate_; 158 | x_speed = 0; 159 | y_speed = 0; 160 | z_speed = 0; 161 | z_turn = 1; 162 | dirty = true; 163 | break; 164 | case KEYCODE_RIGHT: 165 | // max_rv = yaw_rate_; 166 | x_speed = 0; 167 | y_speed = 0; 168 | z_speed = 0; 169 | z_turn = -1; 170 | dirty = true; 171 | break; 172 | 173 | case KEYCODE_W_CAP: 174 | // max_tv = run_vel_; 175 | x_speed = 1; 176 | y_speed = 0; 177 | z_speed = 0; 178 | z_turn = 0; 179 | dirty = true; 180 | break; 181 | case KEYCODE_S_CAP: 182 | // max_tv = run_vel_; 183 | x_speed = -1; 184 | y_speed = 0; 185 | z_speed = 0; 186 | z_turn = 0; 187 | dirty = true; 188 | break; 189 | case KEYCODE_A_CAP: 190 | //max_rv = yaw_rate_run_; 191 | x_speed = 0; 192 | y_speed = 1; 193 | z_speed = 0; 194 | z_turn = 0; 195 | dirty = true; 196 | break; 197 | case KEYCODE_D_CAP: 198 | // max_rv = yaw_rate_run_; 199 | x_speed = 0; 200 | y_speed = -1; 201 | z_speed = 0; 202 | z_turn = 0; 203 | dirty = true; 204 | break; 205 | 206 | default: 207 | //max_tv = walk_vel_; 208 | //max_rv = yaw_rate_; 209 | x_speed = 0; 210 | y_speed = 0; 211 | z_speed = 0; 212 | z_turn = 0; 213 | dirty = false; 214 | } 215 | 216 | cmdvel_.linear.x = x_speed * max_tv; 217 | cmdvel_.linear.y = y_speed * max_tv; 218 | cmdvel_.linear.z = z_speed * max_tv; 219 | cmdvel_.angular.z = z_turn * max_rv; 220 | 221 | pub_.publish(cmdvel_); 222 | } 223 | } 224 | -------------------------------------------------------------------------------- /catkin_ws/src/keyboard/src/keyboard_node.cpp~: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #define KEYCODE_UP 0x85 // 5, Z axis up 13 | #define KEYCODE_LEFT 0x81 // 1, Z rotate clockwise 14 | #define KEYCODE_RIGHT 0x83 // 3, Z rotate anticlockwise 15 | #define KEYCODE_DOWN 0x82 // 2, Z axis down 16 | 17 | #define KEYCODE_A_CAP 0x61 // Y axis up 18 | #define KEYCODE_D_CAP 0x64 // Y axis down 19 | #define KEYCODE_S_CAP 0x73 // X axis up 20 | #define KEYCODE_W_CAP 0x77 // X axis down 21 | 22 | class KeyboardTeleopNode 23 | { 24 | private: 25 | double walk_vel_; 26 | double run_vel_; 27 | double yaw_rate_; 28 | double yaw_rate_run_; 29 | 30 | geometry_msgs::Twist cmdvel_; 31 | ros::NodeHandle n_; 32 | ros::Publisher pub_; 33 | 34 | public: 35 | KeyboardTeleopNode() 36 | { 37 | pub_ = n_.advertise("cmd_vel", 1); 38 | 39 | ros::NodeHandle n_private("~"); 40 | n_private.param("walk_vel", walk_vel_, 0.5); 41 | n_private.param("yaw_rate", yaw_rate_, 1.0); 42 | } 43 | 44 | ~KeyboardTeleopNode() { } 45 | void keyboardLoop(); 46 | 47 | void stopRobot() 48 | { 49 | cmdvel_.linear.x = 0.0; 50 | cmdvel_.linear.y = 0.0; 51 | cmdvel_.linear.z = 0.0; 52 | cmdvel_.angular.z = 0.0; 53 | pub_.publish(cmdvel_); 54 | } 55 | }; 56 | 57 | KeyboardTeleopNode* tbk; 58 | int kfd = 0; 59 | struct termios cooked, raw; 60 | bool done; 61 | 62 | int main(int argc, char** argv) 63 | { 64 | ros::init(argc,argv,"tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler); 65 | KeyboardTeleopNode tbk; 66 | 67 | boost::thread t = boost::thread(boost::bind(&KeyboardTeleopNode::keyboardLoop, &tbk)); 68 | 69 | ros::spin(); 70 | 71 | t.interrupt(); 72 | t.join(); 73 | tbk.stopRobot(); 74 | tcsetattr(kfd, TCSANOW, &cooked); 75 | 76 | return(0); 77 | } 78 | 79 | void KeyboardTeleopNode::keyboardLoop() 80 | { 81 | char c; 82 | double max_tv = walk_vel_; 83 | double max_rv = yaw_rate_; 84 | bool dirty = false; 85 | int x_speed = 0; 86 | int y_speed = 0; 87 | int z_speed = 0; 88 | int z_turn = 0; 89 | 90 | 91 | // get the console in raw mode 92 | tcgetattr(kfd, &cooked); 93 | memcpy(&raw, &cooked, sizeof(struct termios)); 94 | raw.c_lflag &=~ (ICANON | ECHO); 95 | raw.c_cc[VEOL] = 1; 96 | raw.c_cc[VEOF] = 2; 97 | tcsetattr(kfd, TCSANOW, &raw); 98 | 99 | puts("Reading from keyboard"); 100 | puts("Use WASD keys to control the robot"); 101 | // puts("Press Shift to move faster"); 102 | 103 | struct pollfd ufd; 104 | ufd.fd = kfd; 105 | ufd.events = POLLIN; 106 | 107 | for(;;) 108 | { 109 | boost::this_thread::interruption_point(); 110 | 111 | // get the next event from the keyboard 112 | int num; 113 | 114 | if ((num = poll(&ufd, 1, 250)) < 0) 115 | { 116 | perror("poll():"); 117 | return; 118 | } 119 | else if(num > 0) 120 | { 121 | if(read(kfd, &c, 1) < 0) 122 | { 123 | perror("read():"); 124 | return; 125 | } 126 | } 127 | else 128 | { 129 | if (dirty == true) 130 | { 131 | stopRobot(); 132 | dirty = false; 133 | } 134 | 135 | continue; 136 | } 137 | 138 | switch(c) 139 | { 140 | case KEYCODE_UP: 141 | //max_tv = walk_vel_; 142 | x_speed = 0; 143 | y_speed = 0; 144 | z_speed = 1; 145 | z_turn = 0; 146 | dirty = true; 147 | break; 148 | case KEYCODE_DOWN: 149 | // max_tv = walk_vel_; 150 | x_speed = 0; 151 | y_speed = 0; 152 | z_speed = -1; 153 | z_turn = 0; 154 | dirty = true; 155 | break; 156 | case KEYCODE_LEFT: 157 | // max_rv = yaw_rate_; 158 | x_speed = 0; 159 | y_speed = 0; 160 | z_speed = 0; 161 | z_turn = 1; 162 | dirty = true; 163 | break; 164 | case KEYCODE_RIGHT: 165 | // max_rv = yaw_rate_; 166 | x_speed = 0; 167 | y_speed = 0; 168 | z_speed = 0; 169 | z_turn = -1; 170 | dirty = true; 171 | break; 172 | 173 | case KEYCODE_W_CAP: 174 | // max_tv = run_vel_; 175 | x_speed = 1; 176 | y_speed = 0; 177 | z_speed = 0; 178 | z_turn = 0; 179 | dirty = true; 180 | break; 181 | case KEYCODE_S_CAP: 182 | // max_tv = run_vel_; 183 | x_speed = -1; 184 | y_speed = 0; 185 | z_speed = 0; 186 | z_turn = 0; 187 | dirty = true; 188 | break; 189 | case KEYCODE_A_CAP: 190 | //max_rv = yaw_rate_run_; 191 | x_speed = 0; 192 | y_speed = 1; 193 | z_speed = 0; 194 | z_turn = 0; 195 | dirty = true; 196 | break; 197 | case KEYCODE_D_CAP: 198 | // max_rv = yaw_rate_run_; 199 | x_speed = 0; 200 | y_speed = -1; 201 | z_speed = 0; 202 | z_turn = 0; 203 | dirty = true; 204 | break; 205 | 206 | default: 207 | //max_tv = walk_vel_; 208 | //max_rv = yaw_rate_; 209 | x_speed = 0; 210 | y_speed = 0; 211 | z_speed = 0; 212 | z_turn = 0; 213 | dirty = false; 214 | } 215 | 216 | cmdvel_.linear.x = x_speed * max_tv; 217 | cmdvel_.linear.y = y_speed * max_tv; 218 | cmdvel_.linear.z = z_speed * max_tv; 219 | cmdvel_.angular.z = z_turn * max_rv; 220 | 221 | pub_.publish(cmdvel_); 222 | } 223 | } 224 | -------------------------------------------------------------------------------- /catkin_ws/src/control/src/new_land.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import tf 4 | import math 5 | import geometry_msgs.msg 6 | from geometry_msgs.msg import Twist 7 | from geometry_msgs.msg import Pose, PoseStamped 8 | from geometry_msgs.msg import TransformStamped 9 | from math import pow, atan2, sqrt, pi, degrees 10 | from std_msgs.msg import Float32MultiArray 11 | from tf.transformations import euler_from_quaternion 12 | from tf.transformations import quaternion_from_euler 13 | from mavros_msgs.srv import SetMode 14 | from mavros_msgs.srv import CommandBool 15 | from mavros_msgs.srv import CommandTOL 16 | 17 | def euclidean_distance(xd, yd, zd): 18 | return sqrt(pow((xd), 2) + pow((yd), 2) + pow((zd), 2)) 19 | 20 | 21 | class State: 22 | def __init__(self, x = 0, y = 0, z = 0): 23 | self.x = x 24 | self.y = y 25 | self.z = z 26 | 27 | class PID: 28 | def __init__(self,kp=1,kd=0,ki=0,dt=0.01): 29 | 30 | #GAINS 31 | self.kp = kp 32 | self.kd = kd 33 | self.ki = ki 34 | 35 | #TIME STEP 36 | self.dt = dt 37 | 38 | #Default ERROR INITIALIZATION 39 | self.err_previous = 0.001 40 | self.err_acc = 0 41 | 42 | def compute(self,err): 43 | 44 | #compute dervivative 45 | err_deriv = (err - self.err_previous)/self.dt 46 | 47 | #update integration 48 | self.err_acc = self.err_acc + self.dt * (err + self.err_previous)/2 49 | 50 | #compute pid equation 51 | pid = self.kp*err + self.kd*err_deriv + self.ki*self.err_acc 52 | 53 | #update error 54 | self.err_previous = err 55 | 56 | return pid 57 | 58 | class Controller: 59 | ''' 60 | main class of the ros node controlling the robot. 61 | ''' 62 | 63 | def __init__(self): 64 | 65 | #initialization of the ros node and relevant pub/sub 66 | rospy.init_node("PID_node") 67 | self.velocity_publisher=rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped",Twist,queue_size=1) 68 | self.position_publisher=rospy.Publisher("/mavros/setpoint_position/local",PoseStamped,queue_size=1) 69 | self.bebop_subscriber=rospy.Subscriber("/relative_distance", Float32MultiArray ,self.call_back) 70 | self.position_subscriber=rospy.Subscriber("/mavros/local_position/pose", PoseStamped ,self.pos_call_back) 71 | 72 | #robot current state 73 | self.state = State() 74 | 75 | self.local_position = PoseStamped() 76 | 77 | #controller frequency in Hz 78 | self.hz=50.0 79 | self.rate = rospy.Rate(self.hz) 80 | self.dt=(1.0/self.hz) 81 | 82 | #define pids 83 | self.pid_rho = PID(kp=0.3, ki=0.02, dt=self.dt) 84 | self.pid_rho_height = PID(kp=0.1, ki=0.01, dt=self.dt) 85 | 86 | # transformation 87 | def call_back(self, msg): 88 | #print(msg.data) 89 | self.state.x = -msg.data[1] 90 | self.state.y = -msg.data[0] 91 | self.state.z = -msg.data[2] 92 | 93 | def pos_call_back(self, msg): 94 | self.local_position.pose.position.x = msg.pose.position.x 95 | self.local_position.pose.position.y = msg.pose.position.y 96 | self.local_position.pose.position.z = msg.pose.position.z 97 | 98 | 99 | def takeoff(self): 100 | print("Fly to z = 2") 101 | pos_msg = PoseStamped() 102 | pos_msg.pose.position.x = 0 103 | pos_msg.pose.position.y = 0 104 | pos_msg.pose.position.z = 2 105 | for i in range(200): 106 | self.position_publisher.publish(pos_msg) 107 | self.rate.sleep() 108 | 109 | def move_to_goal(self): 110 | #variable initialization 111 | vel_msg = Twist() 112 | pose_msg = PoseStamped() 113 | tolerance_position = 0.01 114 | 115 | rho = euclidean_distance(self.state.x, self.state.y, 0.0) 116 | # print("rho: {}".format(rho)) 117 | 118 | while (rho >= tolerance_position or rho == 0) and not math.isnan(self.state.x): 119 | rospy.loginfo("Distance from goal:"+str(rho)) 120 | rho = euclidean_distance(self.state.x, self.state.y, 0.0) 121 | 122 | vx = self.pid_rho.compute(self.state.x) 123 | vy = self.pid_rho.compute(self.state.y) 124 | 125 | if (rho / abs(self.state.z)) >= 0.1: 126 | vz = 0.0 127 | else: 128 | vz = -0.25 129 | 130 | #fill message 131 | vel_msg.linear.x = vx 132 | vel_msg.linear.y = vy 133 | vel_msg.linear.z = vz 134 | vel_msg.angular.x = 0.0 135 | vel_msg.angular.y = 0.0 136 | vel_msg.angular.z = 0.0 137 | 138 | #debugging 139 | print("vx: {:6f}, x distance: {:6f}".format(vel_msg.linear.x, self.state.x)) 140 | print("vy: {:6f}, y distance: {:6f}".format(vel_msg.linear.y, self.state.y)) 141 | print("vz: {:6f}, z distance: {:6f}".format(vel_msg.linear.z, self.state.z)) 142 | print("_________________") 143 | 144 | #publish 145 | self.velocity_publisher.publish(vel_msg) 146 | self.rate.sleep() 147 | 148 | # stop the robot 149 | vel_msg.linear.x=0.0 150 | vel_msg.linear.y=0.0 151 | vel_msg.linear.z=0.0 152 | self.velocity_publisher.publish(vel_msg) 153 | 154 | # Land 155 | print "\n Landing" 156 | rospy.wait_for_service('/mavros/cmd/land') 157 | try: 158 | land_cl = rospy.ServiceProxy('/mavros/cmd/land', CommandTOL) 159 | response = land_cl(altitude = abs(self.state.z), latitude=0, longitude=0, min_pitch=0, yaw=0) 160 | rospy.loginfo(response) 161 | except rospy.ServiceException as e: 162 | print("Landing failed: %s" %e) 163 | 164 | # Disarm 165 | print "\n Disarming" 166 | rospy.wait_for_service('/mavros/cmd/arming') 167 | try: 168 | arming_cl = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) 169 | response = arming_cl(value = False) 170 | rospy.loginfo(response) 171 | except rospy.ServiceException as e: 172 | print("Disarming failed: %s" %e) 173 | 174 | #if not math.isnan(self.state.x): 175 | 176 | 177 | #if self.local_position.pose.position.z > 0: 178 | # print("landing begin") 179 | # vel_msg.linear.x=0.0 180 | # vel_msg.linear.y=0.0 181 | # vel_msg.linear.z=-2.0 182 | # for i in range(1000): 183 | # self.velocity_publisher.publish(vel_msg) 184 | # self.rate.sleep() 185 | 186 | 187 | #pos_msg = PoseStamped() 188 | #pos_msg.pose.position.x = self.local_position.pose.position.x 189 | #pos_msg.pose.position.y = self.local_position.pose.position.y 190 | #pos_msg.pose.position.z = 0 191 | #for i in range(200): 192 | # self.position_publisher.publish(pos_msg) 193 | # self.rate.sleep() 194 | 195 | rospy.loginfo("I'm here(relative info): "+ str(self.state.x) + " , " + str(self.state.y) +" , " + str(self.state.z)) 196 | print("___") 197 | 198 | return 199 | 200 | 201 | if __name__ == '__main__': 202 | try: 203 | x = Controller() 204 | 205 | # TAKE OFF 206 | x.takeoff() 207 | 208 | #MOVE TO THE GOALS 209 | x.move_to_goal() 210 | 211 | #spin 212 | rospy.spin() 213 | 214 | except rospy.ROSInterruptException: 215 | pass 216 | -------------------------------------------------------------------------------- /catkin_ws/src/keyboard/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(keyboard) 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 | geometry_msgs 12 | roscpp 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs # Or other packages containing msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES keyboard 108 | # CATKIN_DEPENDS other_catkin_pkg 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/keyboard.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(keyboard ${keyboard_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | add_executable(keyboard_node src/keyboard_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(keyboard_node ${keyboard_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | target_link_libraries(keyboard_node 150 | ${catkin_LIBRARIES} 151 | ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # install(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables and/or libraries for installation 168 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 169 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark cpp header files for installation 175 | # install(DIRECTORY include/${PROJECT_NAME}/ 176 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 177 | # FILES_MATCHING PATTERN "*.h" 178 | # PATTERN ".svn" EXCLUDE 179 | # ) 180 | 181 | ## Mark other files for installation (e.g. launch and bag files, etc.) 182 | # install(FILES 183 | # # myfile1 184 | # # myfile2 185 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 186 | # ) 187 | 188 | ############# 189 | ## Testing ## 190 | ############# 191 | 192 | ## Add gtest based cpp test target and link libraries 193 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_keyboard.cpp) 194 | # if(TARGET ${PROJECT_NAME}-test) 195 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 196 | # endif() 197 | 198 | ## Add folders to be run by python nosetests 199 | # catkin_add_nosetests(test) 200 | -------------------------------------------------------------------------------- /catkin_ws/src/keyboard/CMakeLists.txt~: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(keyboard) 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 | geometry_msgs 12 | roscpp 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs # Or other packages containing msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES keyboard 108 | # CATKIN_DEPENDS other_catkin_pkg 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/keyboard.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | add_dependencies(keyboard ${keyboard_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | add_executable(keyboard_node src/keyboard_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | add_dependencies(keyboard_node ${keyboard_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | target_link_libraries(keyboard_node 150 | ${catkin_LIBRARIES} 151 | ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # install(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables and/or libraries for installation 168 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 169 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark cpp header files for installation 175 | # install(DIRECTORY include/${PROJECT_NAME}/ 176 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 177 | # FILES_MATCHING PATTERN "*.h" 178 | # PATTERN ".svn" EXCLUDE 179 | # ) 180 | 181 | ## Mark other files for installation (e.g. launch and bag files, etc.) 182 | # install(FILES 183 | # # myfile1 184 | # # myfile2 185 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 186 | # ) 187 | 188 | ############# 189 | ## Testing ## 190 | ############# 191 | 192 | ## Add gtest based cpp test target and link libraries 193 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_keyboard.cpp) 194 | # if(TARGET ${PROJECT_NAME}-test) 195 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 196 | # endif() 197 | 198 | ## Add folders to be run by python nosetests 199 | # catkin_add_nosetests(test) 200 | -------------------------------------------------------------------------------- /catkin_ws/src/offboard/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(offboard) 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 | geometry_msgs 12 | mavros 13 | roscpp 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # geometry_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES offboard 108 | # CATKIN_DEPENDS geometry_msgs mavros roscpp 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/offboard.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | add_executable(offboard_node src/offboard_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | target_link_libraries(offboard_node 150 | ${catkin_LIBRARIES} 151 | ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # install(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables and/or libraries for installation 168 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 169 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark cpp header files for installation 175 | # install(DIRECTORY include/${PROJECT_NAME}/ 176 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 177 | # FILES_MATCHING PATTERN "*.h" 178 | # PATTERN ".svn" EXCLUDE 179 | # ) 180 | 181 | ## Mark other files for installation (e.g. launch and bag files, etc.) 182 | # install(FILES 183 | # # myfile1 184 | # # myfile2 185 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 186 | # ) 187 | 188 | ############# 189 | ## Testing ## 190 | ############# 191 | 192 | ## Add gtest based cpp test target and link libraries 193 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_offboard.cpp) 194 | # if(TARGET ${PROJECT_NAME}-test) 195 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 196 | # endif() 197 | 198 | ## Add folders to be run by python nosetests 199 | # catkin_add_nosetests(test) 200 | -------------------------------------------------------------------------------- /catkin_ws/src/control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(control) 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 | sensor_msgs 12 | rospy 13 | tf 14 | std_msgs 15 | ) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | # find_package(Boost REQUIRED COMPONENTS system) 19 | 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | # catkin_python_setup() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | 30 | ## To declare and build messages, services or actions from within this 31 | ## package, follow these steps: 32 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 33 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 34 | ## * In the file package.xml: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 38 | ## but can be declared for certainty nonetheless: 39 | ## * add a exec_depend tag for "message_runtime" 40 | ## * In this file (CMakeLists.txt): 41 | ## * add "message_generation" and every package in MSG_DEP_SET to 42 | ## find_package(catkin REQUIRED COMPONENTS ...) 43 | ## * add "message_runtime" and every package in MSG_DEP_SET to 44 | ## catkin_package(CATKIN_DEPENDS ...) 45 | ## * uncomment the add_*_files sections below as needed 46 | ## and list every .msg/.srv/.action file to be processed 47 | ## * uncomment the generate_messages entry below 48 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 49 | 50 | ## Generate messages in the 'msg' folder 51 | # add_message_files( 52 | # FILES 53 | # Message1.msg 54 | # Message2.msg 55 | # ) 56 | 57 | ## Generate services in the 'srv' folder 58 | # add_service_files( 59 | # FILES 60 | # Service1.srv 61 | # Service2.srv 62 | # ) 63 | 64 | ## Generate actions in the 'action' folder 65 | # add_action_files( 66 | # FILES 67 | # Action1.action 68 | # Action2.action 69 | # ) 70 | 71 | ## Generate added messages and services with any dependencies listed here 72 | # generate_messages( 73 | # DEPENDENCIES 74 | # geometry_msgs 75 | # ) 76 | 77 | ################################################ 78 | ## Declare ROS dynamic reconfigure parameters ## 79 | ################################################ 80 | 81 | ## To declare and build dynamic reconfigure parameters within this 82 | ## package, follow these steps: 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 85 | ## * In this file (CMakeLists.txt): 86 | ## * add "dynamic_reconfigure" to 87 | ## find_package(catkin REQUIRED COMPONENTS ...) 88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 89 | ## and list every .cfg file to be processed 90 | 91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 92 | # generate_dynamic_reconfigure_options( 93 | # cfg/DynReconf1.cfg 94 | # cfg/DynReconf2.cfg 95 | # ) 96 | 97 | ################################### 98 | ## catkin specific configuration ## 99 | ################################### 100 | ## The catkin_package macro generates cmake config files for your package 101 | ## Declare things to be passed to dependent projects 102 | ## INCLUDE_DIRS: uncomment this if your package contains header files 103 | ## LIBRARIES: libraries you create in this project that dependent projects also need 104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 105 | ## DEPENDS: system dependencies of this project that dependent projects also need 106 | catkin_package( 107 | # INCLUDE_DIRS include 108 | # LIBRARIES control 109 | # CATKIN_DEPENDS geometry_msgs rospy tf 110 | # DEPENDS system_lib 111 | ) 112 | 113 | ########### 114 | ## Build ## 115 | ########### 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | include_directories( 120 | # include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | ## Declare a C++ library 125 | # add_library(${PROJECT_NAME} 126 | # src/${PROJECT_NAME}/control.cpp 127 | # ) 128 | 129 | ## Add cmake target dependencies of the library 130 | ## as an example, code may need to be generated before libraries 131 | ## either from message generation or dynamic reconfigure 132 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 133 | 134 | ## Declare a C++ executable 135 | ## With catkin_make all packages are built within a single CMake context 136 | ## The recommended prefix ensures that target names across packages don't collide 137 | # add_executable(${PROJECT_NAME}_node src/control_node.cpp) 138 | 139 | ## Rename C++ executable without prefix 140 | ## The above recommended prefix causes long target names, the following renames the 141 | ## target back to the shorter version for ease of user use 142 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 143 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 144 | 145 | ## Add cmake target dependencies of the executable 146 | ## same as for the library above 147 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | # target_link_libraries(${PROJECT_NAME}_node 151 | # ${catkin_LIBRARIES} 152 | # ) 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # install(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables and/or libraries for installation 169 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 170 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark cpp header files for installation 176 | # install(DIRECTORY include/${PROJECT_NAME}/ 177 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 178 | # FILES_MATCHING PATTERN "*.h" 179 | # PATTERN ".svn" EXCLUDE 180 | # ) 181 | 182 | ## Mark other files for installation (e.g. launch and bag files, etc.) 183 | # install(FILES 184 | # # myfile1 185 | # # myfile2 186 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 187 | # ) 188 | 189 | ############# 190 | ## Testing ## 191 | ############# 192 | 193 | ## Add gtest based cpp test target and link libraries 194 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_control.cpp) 195 | # if(TARGET ${PROJECT_NAME}-test) 196 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 197 | # endif() 198 | 199 | ## Add folders to be run by python nosetests 200 | # catkin_add_nosetests(test) 201 | -------------------------------------------------------------------------------- /catkin_ws/src/control/CMakeLists.txt~: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(control) 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 | sensor_msgs 12 | rospy 13 | tf 14 | std_msgs 15 | ) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | # find_package(Boost REQUIRED COMPONENTS system) 19 | 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | # catkin_python_setup() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | 30 | ## To declare and build messages, services or actions from within this 31 | ## package, follow these steps: 32 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 33 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 34 | ## * In the file package.xml: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 38 | ## but can be declared for certainty nonetheless: 39 | ## * add a exec_depend tag for "message_runtime" 40 | ## * In this file (CMakeLists.txt): 41 | ## * add "message_generation" and every package in MSG_DEP_SET to 42 | ## find_package(catkin REQUIRED COMPONENTS ...) 43 | ## * add "message_runtime" and every package in MSG_DEP_SET to 44 | ## catkin_package(CATKIN_DEPENDS ...) 45 | ## * uncomment the add_*_files sections below as needed 46 | ## and list every .msg/.srv/.action file to be processed 47 | ## * uncomment the generate_messages entry below 48 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 49 | 50 | ## Generate messages in the 'msg' folder 51 | # add_message_files( 52 | # FILES 53 | # Message1.msg 54 | # Message2.msg 55 | # ) 56 | 57 | ## Generate services in the 'srv' folder 58 | # add_service_files( 59 | # FILES 60 | # Service1.srv 61 | # Service2.srv 62 | # ) 63 | 64 | ## Generate actions in the 'action' folder 65 | # add_action_files( 66 | # FILES 67 | # Action1.action 68 | # Action2.action 69 | # ) 70 | 71 | ## Generate added messages and services with any dependencies listed here 72 | # generate_messages( 73 | # DEPENDENCIES 74 | # geometry_msgs 75 | # ) 76 | 77 | ################################################ 78 | ## Declare ROS dynamic reconfigure parameters ## 79 | ################################################ 80 | 81 | ## To declare and build dynamic reconfigure parameters within this 82 | ## package, follow these steps: 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 85 | ## * In this file (CMakeLists.txt): 86 | ## * add "dynamic_reconfigure" to 87 | ## find_package(catkin REQUIRED COMPONENTS ...) 88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 89 | ## and list every .cfg file to be processed 90 | 91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 92 | # generate_dynamic_reconfigure_options( 93 | # cfg/DynReconf1.cfg 94 | # cfg/DynReconf2.cfg 95 | # ) 96 | 97 | ################################### 98 | ## catkin specific configuration ## 99 | ################################### 100 | ## The catkin_package macro generates cmake config files for your package 101 | ## Declare things to be passed to dependent projects 102 | ## INCLUDE_DIRS: uncomment this if your package contains header files 103 | ## LIBRARIES: libraries you create in this project that dependent projects also need 104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 105 | ## DEPENDS: system dependencies of this project that dependent projects also need 106 | catkin_package( 107 | # INCLUDE_DIRS include 108 | # LIBRARIES control 109 | # CATKIN_DEPENDS geometry_msgs rospy tf 110 | # DEPENDS system_lib 111 | ) 112 | 113 | ########### 114 | ## Build ## 115 | ########### 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | include_directories( 120 | # include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | ## Declare a C++ library 125 | # add_library(${PROJECT_NAME} 126 | # src/${PROJECT_NAME}/control.cpp 127 | # ) 128 | 129 | ## Add cmake target dependencies of the library 130 | ## as an example, code may need to be generated before libraries 131 | ## either from message generation or dynamic reconfigure 132 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 133 | 134 | ## Declare a C++ executable 135 | ## With catkin_make all packages are built within a single CMake context 136 | ## The recommended prefix ensures that target names across packages don't collide 137 | # add_executable(${PROJECT_NAME}_node src/control_node.cpp) 138 | 139 | ## Rename C++ executable without prefix 140 | ## The above recommended prefix causes long target names, the following renames the 141 | ## target back to the shorter version for ease of user use 142 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 143 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 144 | 145 | ## Add cmake target dependencies of the executable 146 | ## same as for the library above 147 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | # target_link_libraries(${PROJECT_NAME}_node 151 | # ${catkin_LIBRARIES} 152 | # ) 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # install(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables and/or libraries for installation 169 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 170 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark cpp header files for installation 176 | # install(DIRECTORY include/${PROJECT_NAME}/ 177 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 178 | # FILES_MATCHING PATTERN "*.h" 179 | # PATTERN ".svn" EXCLUDE 180 | # ) 181 | 182 | ## Mark other files for installation (e.g. launch and bag files, etc.) 183 | # install(FILES 184 | # # myfile1 185 | # # myfile2 186 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 187 | # ) 188 | 189 | ############# 190 | ## Testing ## 191 | ############# 192 | 193 | ## Add gtest based cpp test target and link libraries 194 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_control.cpp) 195 | # if(TARGET ${PROJECT_NAME}-test) 196 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 197 | # endif() 198 | 199 | ## Add folders to be run by python nosetests 200 | # catkin_add_nosetests(test) 201 | -------------------------------------------------------------------------------- /catkin_ws/src/offboard/CMakeLists.txt~: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(offboard) 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 | geometry_msgs 12 | mavros 13 | roscpp 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # geometry_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES offboard 108 | # CATKIN_DEPENDS geometry_msgs mavros roscpp 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/offboard.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/offboard_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # install(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables and/or libraries for installation 168 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 169 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark cpp header files for installation 175 | # install(DIRECTORY include/${PROJECT_NAME}/ 176 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 177 | # FILES_MATCHING PATTERN "*.h" 178 | # PATTERN ".svn" EXCLUDE 179 | # ) 180 | 181 | ## Mark other files for installation (e.g. launch and bag files, etc.) 182 | # install(FILES 183 | # # myfile1 184 | # # myfile2 185 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 186 | # ) 187 | 188 | ############# 189 | ## Testing ## 190 | ############# 191 | 192 | ## Add gtest based cpp test target and link libraries 193 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_offboard.cpp) 194 | # if(TARGET ${PROJECT_NAME}-test) 195 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 196 | # endif() 197 | 198 | ## Add folders to be run by python nosetests 199 | # catkin_add_nosetests(test) 200 | -------------------------------------------------------------------------------- /catkin_ws/src/opencvtest_py/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(opencvtest_py) 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 | cv_bridge 12 | rospy 13 | sensor_msgs 14 | std_msgs 15 | ) 16 | find_package(opencv2) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a exec_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | # add_message_files( 53 | # FILES 54 | # Message1.msg 55 | # Message2.msg 56 | # ) 57 | 58 | ## Generate services in the 'srv' folder 59 | # add_service_files( 60 | # FILES 61 | # Service1.srv 62 | # Service2.srv 63 | # ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | # generate_messages( 74 | # DEPENDENCIES 75 | # sensor_msgs# std_msgs 76 | # ) 77 | 78 | ################################################ 79 | ## Declare ROS dynamic reconfigure parameters ## 80 | ################################################ 81 | 82 | ## To declare and build dynamic reconfigure parameters within this 83 | ## package, follow these steps: 84 | ## * In the file package.xml: 85 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "dynamic_reconfigure" to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 90 | ## and list every .cfg file to be processed 91 | 92 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 93 | # generate_dynamic_reconfigure_options( 94 | # cfg/DynReconf1.cfg 95 | # cfg/DynReconf2.cfg 96 | # ) 97 | 98 | ################################### 99 | ## catkin specific configuration ## 100 | ################################### 101 | ## The catkin_package macro generates cmake config files for your package 102 | ## Declare things to be passed to dependent projects 103 | ## INCLUDE_DIRS: uncomment this if your package contains header files 104 | ## LIBRARIES: libraries you create in this project that dependent projects also need 105 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 106 | ## DEPENDS: system dependencies of this project that dependent projects also need 107 | catkin_package( 108 | # INCLUDE_DIRS include 109 | # LIBRARIES opencvtest_py 110 | # CATKIN_DEPENDS cv_bridge opencv2 rospy sensor_msgs std_msgs 111 | # DEPENDS system_lib 112 | ) 113 | 114 | ########### 115 | ## Build ## 116 | ########### 117 | 118 | ## Specify additional locations of header files 119 | ## Your package locations should be listed before other locations 120 | include_directories( 121 | # include 122 | ${catkin_INCLUDE_DIRS} 123 | ) 124 | 125 | ## Declare a C++ library 126 | # add_library(${PROJECT_NAME} 127 | # src/${PROJECT_NAME}/opencvtest_py.cpp 128 | # ) 129 | 130 | ## Add cmake target dependencies of the library 131 | ## as an example, code may need to be generated before libraries 132 | ## either from message generation or dynamic reconfigure 133 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 134 | 135 | ## Declare a C++ executable 136 | ## With catkin_make all packages are built within a single CMake context 137 | ## The recommended prefix ensures that target names across packages don't collide 138 | # add_executable(${PROJECT_NAME}_node src/opencvtest_py_node.cpp) 139 | 140 | ## Rename C++ executable without prefix 141 | ## The above recommended prefix causes long target names, the following renames the 142 | ## target back to the shorter version for ease of user use 143 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 144 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 145 | 146 | ## Add cmake target dependencies of the executable 147 | ## same as for the library above 148 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 149 | 150 | ## Specify libraries to link a library or executable target against 151 | # target_link_libraries(${PROJECT_NAME}_node 152 | # ${catkin_LIBRARIES} 153 | # ) 154 | 155 | ############# 156 | ## Install ## 157 | ############# 158 | 159 | # all install targets should use catkin DESTINATION variables 160 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 161 | 162 | ## Mark executable scripts (Python etc.) for installation 163 | ## in contrast to setup.py, you can choose the destination 164 | # install(PROGRAMS 165 | # scripts/my_python_script 166 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark executables and/or libraries for installation 170 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 171 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark cpp header files for installation 177 | # install(DIRECTORY include/${PROJECT_NAME}/ 178 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 179 | # FILES_MATCHING PATTERN "*.h" 180 | # PATTERN ".svn" EXCLUDE 181 | # ) 182 | 183 | ## Mark other files for installation (e.g. launch and bag files, etc.) 184 | # install(FILES 185 | # # myfile1 186 | # # myfile2 187 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 188 | # ) 189 | 190 | ############# 191 | ## Testing ## 192 | ############# 193 | 194 | ## Add gtest based cpp test target and link libraries 195 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_opencvtest_py.cpp) 196 | # if(TARGET ${PROJECT_NAME}-test) 197 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 198 | # endif() 199 | 200 | ## Add folders to be run by python nosetests 201 | # catkin_add_nosetests(test) 202 | -------------------------------------------------------------------------------- /catkin_ws/src/control/src/control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import tf 4 | import math 5 | import geometry_msgs.msg 6 | from geometry_msgs.msg import Twist 7 | from geometry_msgs.msg import Pose, PoseStamped 8 | from geometry_msgs.msg import TransformStamped 9 | from math import pow, atan2, sqrt, pi, degrees 10 | from std_msgs.msg import Float32MultiArray 11 | from tf.transformations import euler_from_quaternion 12 | from tf.transformations import quaternion_from_euler 13 | from mavros_msgs.srv import SetMode 14 | from mavros_msgs.srv import CommandBool 15 | from mavros_msgs.srv import CommandTOL 16 | 17 | def euclidean_distance(xd, yd, zd): 18 | return sqrt(pow((xd), 2) + pow((yd), 2) + pow((zd), 2)) 19 | 20 | 21 | class State: 22 | def __init__(self, x = 0, y = 0, z = 0): 23 | self.x = x 24 | self.y = y 25 | self.z = z 26 | 27 | class PID: 28 | def __init__(self,kp=1,kd=0,ki=0,dt=0.01): 29 | 30 | #GAINS 31 | self.kp = kp 32 | self.kd = kd 33 | self.ki = ki 34 | 35 | #TIME STEP 36 | self.dt = dt 37 | 38 | #Default ERROR INITIALIZATION 39 | self.err_previous = 0.001 40 | self.err_acc = 0 41 | 42 | def compute(self,err): 43 | 44 | #compute dervivative 45 | err_deriv = (err - self.err_previous)/self.dt 46 | 47 | #update integration 48 | self.err_acc = self.err_acc + self.dt * (err + self.err_previous)/2 49 | 50 | #compute pid equation 51 | pid = self.kp*err + self.kd*err_deriv + self.ki*self.err_acc 52 | 53 | #update error 54 | self.err_previous = err 55 | 56 | return pid 57 | 58 | class Controller: 59 | ''' 60 | main class of the ros node controlling the robot. 61 | ''' 62 | 63 | def __init__(self): 64 | 65 | #initialization of the ros node and relevant pub/sub 66 | rospy.init_node("PID_node") 67 | self.velocity_publisher=rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped",Twist,queue_size=1) 68 | self.position_publisher=rospy.Publisher("/mavros/setpoint_position/local",PoseStamped,queue_size=1) 69 | self.bebop_subscriber=rospy.Subscriber("/relative_distance", Float32MultiArray ,self.call_back) 70 | self.position_subscriber=rospy.Subscriber("/mavros/local_position/pose", PoseStamped ,self.pos_call_back) 71 | 72 | #robot current state 73 | self.state = State() 74 | 75 | self.local_position = PoseStamped() 76 | 77 | #controller frequency in Hz 78 | self.hz=50.0 79 | self.rate = rospy.Rate(self.hz) 80 | self.dt=(1.0/self.hz) 81 | 82 | #define pids 83 | self.pid_rho = PID(kp=0.2 ,dt=self.dt) 84 | 85 | # transformation 86 | def call_back(self, msg): 87 | #print(msg.data) 88 | self.state.x = -msg.data[1] 89 | self.state.y = -msg.data[0] 90 | self.state.z = -msg.data[2] 91 | 92 | def pos_call_back(self, msg): 93 | self.local_position.pose.position.x = msg.pose.position.x 94 | self.local_position.pose.position.y = msg.pose.position.y 95 | self.local_position.pose.position.z = msg.pose.position.z 96 | 97 | 98 | def takeoff(self): 99 | print("Fly to z = 2") 100 | pos_msg = PoseStamped() 101 | pos_msg.pose.position.x = 0 102 | pos_msg.pose.position.y = 0 103 | pos_msg.pose.position.z = 2 104 | for i in range(200): 105 | self.position_publisher.publish(pos_msg) 106 | self.rate.sleep() 107 | 108 | def move_to_goal(self): 109 | 110 | #variable initialization 111 | vel_msg = Twist() 112 | tolerance_position = 0.01 113 | 114 | rho = euclidean_distance(self.state.x, self.state.y, self.state.z) 115 | while (rho >= tolerance_position or rho==0) and self.state.z < -1: 116 | rospy.loginfo("Distance from goal:"+str(rho)) 117 | # if math.isnan(self.state.x): 118 | # for i in range(100): 119 | # self.rate.sleep() 120 | # if ~math.isnan(self.state.x): 121 | # print("find not nan") 122 | # break 123 | # if math.isnan(self.state.x): 124 | # break 125 | rho = euclidean_distance(self.state.x, self.state.y, self.state.z) 126 | err_x = self.state.x 127 | err_y = self.state.y 128 | err_z = self.state.z 129 | 130 | #Compute PID 131 | vx = self.pid_rho.compute(err_x) 132 | vy = self.pid_rho.compute(err_y) 133 | vz = self.pid_rho.compute(err_z) * 0.25 134 | 135 | #fill message 136 | vel_msg.linear.x = vx 137 | vel_msg.linear.y = vy 138 | vel_msg.linear.z = vz 139 | vel_msg.angular.x = 0.0 140 | vel_msg.angular.y = 0.0 141 | vel_msg.angular.z = 0.0 142 | 143 | #debugging 144 | print("vx: {}".format(vel_msg.linear.x)) 145 | print("x: {}".format(self.state.x)) 146 | print("vy: {}".format(vel_msg.linear.y)) 147 | print("y: {}".format(self.state.y)) 148 | print("vz: {}".format(vel_msg.linear.z)) 149 | print("z: {}".format(self.state.z)) 150 | print("_________________") 151 | 152 | #publish 153 | self.velocity_publisher.publish(vel_msg) 154 | self.rate.sleep() 155 | 156 | if self.local_position.pose.position.z > 0: 157 | print("landing begin") 158 | vel_msg.linear.x=0.0 159 | vel_msg.linear.y=0.0 160 | vel_msg.linear.z=-2.0 161 | for i in range(1000): 162 | self.velocity_publisher.publish(vel_msg) 163 | self.rate.sleep() 164 | 165 | # stop the robot 166 | vel_msg.linear.x=0.0 167 | vel_msg.linear.y=0.0 168 | vel_msg.linear.z=0.0 169 | self.velocity_publisher.publish(vel_msg) 170 | 171 | # Land 172 | print "\n Landing" 173 | rospy.wait_for_service('/mavros/cmd/land') 174 | try: 175 | land_cl = rospy.ServiceProxy('/mavros/cmd/land', CommandTOL) 176 | response = land_cl(altitude=, latitude=0, longitude=0, min_pitch=0, yaw=0) 177 | rospy.loginfo(response) 178 | except rospy.ServiceException as e: 179 | print("Landing failed: %s" %e) 180 | 181 | # Disarm 182 | print "\nDisarming" 183 | rospy.wait_for_service('/mavros/cmd/arming') 184 | try: 185 | arming_cl = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) 186 | response = arming_cl(value = False) 187 | rospy.loginfo(response) 188 | except rospy.ServiceException as e: 189 | print("Disarming failed: %s" %e) 190 | 191 | 192 | #pos_msg = PoseStamped() 193 | #pos_msg.pose.position.x = self.local_position.pose.position.x 194 | #pos_msg.pose.position.y = self.local_position.pose.position.y 195 | #pos_msg.pose.position.z = 0 196 | #for i in range(200): 197 | # self.position_publisher.publish(pos_msg) 198 | # self.rate.sleep() 199 | 200 | rospy.loginfo("I'm here(relative info): "+ str(self.state.x) + " , " + str(self.state.y) +" , " + str(self.state.z)) 201 | print("___") 202 | 203 | return 204 | 205 | 206 | if __name__ == '__main__': 207 | try: 208 | x = Controller() 209 | 210 | # TAKE OFF 211 | x.takeoff() 212 | 213 | #MOVE TO THE GOALS 214 | x.move_to_goal() 215 | 216 | #spin 217 | rospy.spin() 218 | 219 | except rospy.ROSInterruptException: 220 | pass 221 | -------------------------------------------------------------------------------- /catkin_ws/src/control/src/land.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import tf 4 | import math 5 | import geometry_msgs.msg 6 | from geometry_msgs.msg import Twist 7 | from geometry_msgs.msg import Pose, PoseStamped 8 | from geometry_msgs.msg import TransformStamped 9 | from math import pow, atan2, sqrt, pi, degrees 10 | from std_msgs.msg import Float32MultiArray 11 | from tf.transformations import euler_from_quaternion 12 | from tf.transformations import quaternion_from_euler 13 | from mavros_msgs.srv import SetMode 14 | from mavros_msgs.srv import CommandBool 15 | from mavros_msgs.srv import CommandTOL 16 | 17 | def euclidean_distance(xd, yd, zd): 18 | return sqrt(pow((xd), 2) + pow((yd), 2) + pow((zd), 2)) 19 | 20 | 21 | class State: 22 | def __init__(self, x = 0, y = 0, z = 0): 23 | self.x = x 24 | self.y = y 25 | self.z = z 26 | 27 | class PID: 28 | def __init__(self,kp=1,kd=0,ki=0,dt=0.01): 29 | 30 | #GAINS 31 | self.kp = kp 32 | self.kd = kd 33 | self.ki = ki 34 | 35 | #TIME STEP 36 | self.dt = dt 37 | 38 | #Default ERROR INITIALIZATION 39 | self.err_previous = 0.001 40 | self.err_acc = 0 41 | 42 | def compute(self,err): 43 | 44 | #compute dervivative 45 | err_deriv = (err - self.err_previous)/self.dt 46 | 47 | #update integration 48 | self.err_acc = self.err_acc + self.dt * (err + self.err_previous)/2 49 | 50 | #compute pid equation 51 | pid = self.kp*err + self.kd*err_deriv + self.ki*self.err_acc 52 | 53 | #update error 54 | self.err_previous = err 55 | 56 | return pid 57 | 58 | class Controller: 59 | ''' 60 | main class of the ros node controlling the robot. 61 | ''' 62 | 63 | def __init__(self): 64 | 65 | #initialization of the ros node and relevant pub/sub 66 | rospy.init_node("PID_node") 67 | self.velocity_publisher=rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped",Twist,queue_size=1) 68 | self.position_publisher=rospy.Publisher("/mavros/setpoint_position/local",PoseStamped,queue_size=1) 69 | self.bebop_subscriber=rospy.Subscriber("/relative_distance", Float32MultiArray ,self.call_back) 70 | self.position_subscriber=rospy.Subscriber("/mavros/local_position/pose", PoseStamped ,self.pos_call_back) 71 | 72 | #robot current state 73 | self.state = State() 74 | 75 | self.local_position = PoseStamped() 76 | 77 | #controller frequency in Hz 78 | self.hz=50.0 79 | self.rate = rospy.Rate(self.hz) 80 | self.dt=(1.0/self.hz) 81 | 82 | #define pids 83 | self.pid_rho_2d = PID(kp=0.3, ki=0.01, dt=self.dt) 84 | self.pid_rho_height = PID(kp=0.1, ki=0.01, dt=self.dt) 85 | 86 | # transformation 87 | def call_back(self, msg): 88 | #print(msg.data) 89 | self.state.x = -msg.data[1] 90 | self.state.y = -msg.data[0] 91 | self.state.z = -msg.data[2] 92 | 93 | def pos_call_back(self, msg): 94 | self.local_position.pose.position.x = msg.pose.position.x 95 | self.local_position.pose.position.y = msg.pose.position.y 96 | self.local_position.pose.position.z = msg.pose.position.z 97 | 98 | 99 | def takeoff(self): 100 | print("Fly to z = 2") 101 | pos_msg = PoseStamped() 102 | pos_msg.pose.position.x = 0 103 | pos_msg.pose.position.y = 0 104 | pos_msg.pose.position.z = 2 105 | for i in range(200): 106 | self.position_publisher.publish(pos_msg) 107 | self.rate.sleep() 108 | 109 | def move_in_2d(self, tolerance_2d): 110 | vel_msg = Twist() 111 | 112 | # move in 2d 113 | rho = euclidean_distance(self.state.x, self.state.y, 0.0) 114 | while rho >= tolerance_2d or rho==0 and not math.isnan(self.state.x): 115 | rospy.loginfo("2d Distance from goal:"+str(rho)) 116 | rho = euclidean_distance(self.state.x, self.state.y, 0.0) 117 | 118 | err_x = self.state.x 119 | err_y = self.state.y 120 | 121 | #Compute PID 122 | vx = self.pid_rho_2d.compute(err_x) 123 | vy = self.pid_rho_2d.compute(err_y) 124 | 125 | #fill message 126 | vel_msg.linear.x = vx 127 | vel_msg.linear.y = vy 128 | vel_msg.linear.z = 0.0 129 | vel_msg.angular.x = 0.0 130 | vel_msg.angular.y = 0.0 131 | vel_msg.angular.z = 0.0 132 | 133 | #debugging 134 | print("vx: {:6f}, x distance: {:6f}".format(vel_msg.linear.x, self.state.x)) 135 | print("vy: {:6f}, y distance: {:6f}".format(vel_msg.linear.y, self.state.y)) 136 | print("vz: {:6f}, z distance: {:6f}".format(vel_msg.linear.z, self.state.z)) 137 | print("_________________") 138 | 139 | #publish 140 | self.velocity_publisher.publish(vel_msg) 141 | self.rate.sleep() 142 | 143 | def move_in_height(self, tolerance_height): 144 | vel_msg = Twist() 145 | height = self.state.z 146 | 147 | while abs(height) >= tolerance_height and not math.isnan(self.state.x): 148 | rospy.loginfo("height from goal:"+str(height)) 149 | height = self.state.z 150 | 151 | vz = self.pid_rho_height.compute(height) * 0.2 152 | 153 | #fill message 154 | vel_msg.linear.x = 0.0 155 | vel_msg.linear.y = 0.0 156 | vel_msg.linear.z = vz 157 | vel_msg.angular.x = 0.0 158 | vel_msg.angular.y = 0.0 159 | vel_msg.angular.z = 0.0 160 | 161 | #debugging 162 | print("vx: {:6f}, x distance: {:6f}".format(vel_msg.linear.x, self.state.x)) 163 | print("vy: {:6f}, y distance: {:6f}".format(vel_msg.linear.y, self.state.y)) 164 | print("vz: {:6f}, z distance: {:6f}".format(vel_msg.linear.z, self.state.z)) 165 | print("_________________") 166 | 167 | #publish 168 | self.velocity_publisher.publish(vel_msg) 169 | self.rate.sleep() 170 | 171 | 172 | def move_to_goal(self): 173 | vel_msg = Twist() 174 | tolerance_2d = [0.6, 0.3] 175 | tolerance_height = [1.5, 0.5] 176 | 177 | self.move_in_2d(tolerance_2d[0]) 178 | print("_________________start descending - 1_________________") 179 | self.move_in_height(tolerance_height[0]) 180 | 181 | self.move_in_2d(tolerance_2d[1]) 182 | print("_________________start descending - 2_________________") 183 | self.move_in_height(tolerance_height[1]) 184 | 185 | # stop the robot 186 | vel_msg.linear.x=0.0 187 | vel_msg.linear.y=0.0 188 | vel_msg.linear.z=0.0 189 | self.velocity_publisher.publish(vel_msg) 190 | 191 | # Land 192 | print "\n Landing" 193 | rospy.wait_for_service('/mavros/cmd/land') 194 | try: 195 | land_cl = rospy.ServiceProxy('/mavros/cmd/land', CommandTOL) 196 | response = land_cl(altitude = abs(self.state.z), latitude=0, longitude=0, min_pitch=0, yaw=0) 197 | rospy.loginfo(response) 198 | except rospy.ServiceException as e: 199 | print("Landing failed: %s" %e) 200 | 201 | # Disarm 202 | print "\n Disarming" 203 | rospy.wait_for_service('/mavros/cmd/arming') 204 | try: 205 | arming_cl = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) 206 | response = arming_cl(value = False) 207 | rospy.loginfo(response) 208 | except rospy.ServiceException as e: 209 | print("Disarming failed: %s" %e) 210 | 211 | #if not math.isnan(self.state.x): 212 | 213 | 214 | #if self.local_position.pose.position.z > 0: 215 | # print("landing begin") 216 | # vel_msg.linear.x=0.0 217 | # vel_msg.linear.y=0.0 218 | # vel_msg.linear.z=-2.0 219 | # for i in range(1000): 220 | # self.velocity_publisher.publish(vel_msg) 221 | # self.rate.sleep() 222 | 223 | 224 | #pos_msg = PoseStamped() 225 | #pos_msg.pose.position.x = self.local_position.pose.position.x 226 | #pos_msg.pose.position.y = self.local_position.pose.position.y 227 | #pos_msg.pose.position.z = 0 228 | #for i in range(200): 229 | # self.position_publisher.publish(pos_msg) 230 | # self.rate.sleep() 231 | 232 | rospy.loginfo("I'm here(relative info): "+ str(self.state.x) + " , " + str(self.state.y) +" , " + str(self.state.z)) 233 | print("___") 234 | 235 | return 236 | 237 | 238 | if __name__ == '__main__': 239 | try: 240 | x = Controller() 241 | 242 | # TAKE OFF 243 | x.takeoff() 244 | 245 | #MOVE TO THE GOALS 246 | x.move_to_goal() 247 | 248 | #spin 249 | rospy.spin() 250 | 251 | except rospy.ROSInterruptException: 252 | pass 253 | -------------------------------------------------------------------------------- /catkin_ws/src/control/src/new_land_kf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import tf 4 | import math 5 | import geometry_msgs.msg 6 | from geometry_msgs.msg import Twist 7 | from geometry_msgs.msg import Pose, PoseStamped 8 | from geometry_msgs.msg import TransformStamped 9 | from math import pow, atan2, sqrt, pi, degrees 10 | from std_msgs.msg import Float32MultiArray 11 | from tf.transformations import euler_from_quaternion 12 | from tf.transformations import quaternion_from_euler 13 | from mavros_msgs.srv import SetMode 14 | from mavros_msgs.srv import CommandBool 15 | from mavros_msgs.srv import CommandTOL 16 | import time 17 | 18 | def euclidean_distance(xd, yd, zd): 19 | return sqrt(pow((xd), 2) + pow((yd), 2) + pow((zd), 2)) 20 | 21 | 22 | class State: 23 | def __init__(self, x = 0, y = 0, z = 0): 24 | self.x = x 25 | self.y = y 26 | self.z = z 27 | 28 | class PID: 29 | def __init__(self,kp=1,kd=0,ki=0,dt=0.01): 30 | 31 | #GAINS 32 | self.kp = kp 33 | self.kd = kd 34 | self.ki = ki 35 | 36 | #TIME STEP 37 | self.dt = dt 38 | 39 | #Default ERROR INITIALIZATION 40 | self.err_previous = 0.001 41 | self.err_acc = 0 42 | 43 | def compute(self,err): 44 | 45 | #compute dervivative 46 | err_deriv = (err - self.err_previous)/self.dt 47 | 48 | #update integration 49 | self.err_acc = self.err_acc + self.dt * (err + self.err_previous)/2 50 | 51 | #compute pid equation 52 | pid = self.kp*err + self.kd*err_deriv + self.ki*self.err_acc 53 | 54 | #update error 55 | self.err_previous = err 56 | 57 | return pid 58 | 59 | class Controller: 60 | ''' 61 | main class of the ros node controlling the robot. 62 | ''' 63 | 64 | def __init__(self): 65 | 66 | #initialization of the ros node and relevant pub/sub 67 | rospy.init_node("PID_node") 68 | self.velocity_publisher=rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped",Twist,queue_size=1) 69 | self.position_publisher=rospy.Publisher("/mavros/setpoint_position/local",PoseStamped,queue_size=1) 70 | self.bebop_subscriber=rospy.Subscriber("/relative_distance", Float32MultiArray ,self.call_back) 71 | self.position_subscriber=rospy.Subscriber("/mavros/local_position/pose", PoseStamped ,self.pos_call_back) 72 | 73 | #robot current state 74 | self.state = State() 75 | self.local_position = PoseStamped() 76 | self.velocity = State() 77 | 78 | #controller frequency in Hz 79 | self.hz=50.0 80 | self.rate = rospy.Rate(self.hz) 81 | self.dt=(1.0/self.hz) 82 | 83 | #define pids 84 | self.pid_rho_xy = PID(kp=0.3, ki=0.02, dt=self.dt) 85 | 86 | #kf parameters 87 | self.A = np.eye(2, dtype = float) 88 | self.B = -self.dt * np.eye(2, dtype = float) 89 | self.C = np.eye(2, dtype = float) 90 | self.Q = 0.05 * np.eye(2, dtype = float) 91 | self.R = 0.05 * np.eye(2, dtype = float) 92 | self.P = np.eye(2, dtype = float) 93 | 94 | def kf_predict(self, X, U): 95 | # predict state and covariance 96 | pred_X = np.dot(self.A, X) + np.dot(self.B, u) 97 | self.P = np.dot(np.dot(self.A, self.P), self.A.T) + self.Q 98 | return pred_X 99 | 100 | def kf_update(self, pred_X, Y): 101 | # update state 102 | S = self.R + np.dot(self.C, np.dot(self.P, self.C.T)) 103 | K = np.dot(np.dot(self.P, self.C.T), np.linalg.inv(S)) 104 | corr_X = pred_X + np.dot(K, Y - np.dot(self.C, X)) 105 | 106 | # update state and covariance 107 | I = np.eye(3) 108 | self.P = np.dot(I - np.dot(K, self.C), self.P) 109 | 110 | return corr_X 111 | 112 | # transform from camera frame to drone frame 113 | def call_back(self, msg): 114 | 115 | # prediction 116 | pred_X = None 117 | if self.velocity.x is not 0.0 and not math.isnan(self.state.x): 118 | U = np.array([self.velocity.x, self.velocity.y]).reshape(2, 1) 119 | X = np.array([self.state.x, self.state.y]).reshape(2, 1) 120 | pred_X = self.kf_predict(X, U) 121 | 122 | # sensor(camera) measurement 123 | self.state.x = -msg.data[1] 124 | self.state.y = -msg.data[0] 125 | self.state.z = -msg.data[2] 126 | 127 | # state estimation(only xy) 128 | if pred_X is not None: 129 | Y = np.array([self.state.x, self.state.y]).reshape(2, 1) 130 | corr_X = self.kf_update(pred_x, Y) 131 | self.state.x = corr_X[0] 132 | self.state.y = corr_X[1] 133 | 134 | 135 | def pos_call_back(self, msg): 136 | self.local_position.pose.position.x = msg.pose.position.x 137 | self.local_position.pose.position.y = msg.pose.position.y 138 | self.local_position.pose.position.z = msg.pose.position.z 139 | 140 | 141 | def takeoff(self): 142 | # Arm 143 | #print "\nArming" 144 | #rospy.wait_for_service('/mavros/cmd/arming') 145 | #try: 146 | # arming_cl = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) 147 | # response = arming_cl(value = True) 148 | # rospy.loginfo(response) 149 | # except rospy.ServiceException as e: 150 | # print("Arming failed: %s" %e) 151 | 152 | # # Takeoff 153 | # print "\nTaking off" 154 | # rospy.wait_for_service('/mavros/cmd/takeoff') 155 | # try: 156 | # takeoff_cl = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL) 157 | # response = takeoff_cl(altitude=10, latitude=0, longitude=0, min_pitch=0, yaw=0) 158 | # rospy.loginfo(response) 159 | # except rospy.ServiceException as e: 160 | # print("Takeoff failed: %s" %e) 161 | # print "\n Hovering..." 162 | # time.sleep(3) 163 | 164 | print "\n Takeoff..." 165 | pos_msg = PoseStamped() 166 | pos_msg.pose.position.x = 0 167 | pos_msg.pose.position.y = 0 168 | pos_msg.pose.position.z = 2 169 | print("Fly to z = {}".format(pos_msg.pose.position.z)) 170 | for i in range(200): 171 | self.position_publisher.publish(pos_msg) 172 | self.rate.sleep() 173 | 174 | def move_to_goal(self): 175 | #variable initialization 176 | vel_msg = Twist() 177 | pose_msg = PoseStamped() 178 | tolerance_distance_xy = 0.01 179 | 180 | rho_xy = euclidean_distance(self.state.x, self.state.y, 0.0) 181 | # print("rho: {}".format(rho)) 182 | 183 | while (rho_xy >= tolerance_distance_xy or rho_xy == 0) and not math.isnan(self.state.x): 184 | rospy.loginfo("xy Distance from goal:"+str(rho_xy)) 185 | rho_xy = euclidean_distance(self.state.x, self.state.y, 0.0) 186 | 187 | # pid control 188 | vx = self.pid_rho_xy.compute(self.state.x) 189 | vy = self.pid_rho_xy.compute(self.state.y) 190 | 191 | if (rho_xy / abs(self.state.z)) >= 0.1: 192 | vz = 0.0 193 | else: 194 | vz = -0.25 195 | 196 | #fill message 197 | vel_msg.linear.x = vx 198 | vel_msg.linear.y = vy 199 | vel_msg.linear.z = vz 200 | vel_msg.angular.x = 0.0 201 | vel_msg.angular.y = 0.0 202 | vel_msg.angular.z = 0.0 203 | 204 | #debugging 205 | print("vx: {:6f}, x distance: {:6f}".format(vel_msg.linear.x, self.state.x)) 206 | print("vy: {:6f}, y distance: {:6f}".format(vel_msg.linear.y, self.state.y)) 207 | print("vz: {:6f}, z distance: {:6f}".format(vel_msg.linear.z, self.state.z)) 208 | print("_________________") 209 | 210 | #publish 211 | self.velocity_publisher.publish(vel_msg) 212 | self.rate.sleep() 213 | 214 | # stop the robot 215 | vel_msg.linear.x = 0.0 216 | vel_msg.linear.y = 0.0 217 | vel_msg.linear.z = 0.0 218 | self.velocity_publisher.publish(vel_msg) 219 | 220 | # Land 221 | print "\n Landing" 222 | rospy.wait_for_service('/mavros/cmd/land') 223 | try: 224 | land_cl = rospy.ServiceProxy('/mavros/cmd/land', CommandTOL) 225 | response = land_cl(altitude = abs(self.state.z), latitude=0, longitude=0, min_pitch=0, yaw=0) 226 | rospy.loginfo(response) 227 | except rospy.ServiceException as e: 228 | print("Landing failed: %s" %e) 229 | 230 | # Disarm 231 | print "\n Disarming" 232 | rospy.wait_for_service('/mavros/cmd/arming') 233 | try: 234 | arming_cl = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) 235 | response = arming_cl(value = False) 236 | rospy.loginfo(response) 237 | except rospy.ServiceException as e: 238 | print("Disarming failed: %s" %e) 239 | 240 | rospy.loginfo("I'm here(relative info): "+ str(self.state.x) + " , " + str(self.state.y) +" , " + str(self.state.z)) 241 | print("___") 242 | 243 | return 244 | 245 | 246 | if __name__ == '__main__': 247 | try: 248 | drone = Controller() 249 | 250 | # TAKE OFF 251 | drone.takeoff() 252 | 253 | # MOVE TO THE GOALS 254 | drone.move_to_goal() 255 | 256 | # SPIN 257 | rospy.spin() 258 | 259 | except rospy.ROSInterruptException: 260 | pass 261 | -------------------------------------------------------------------------------- /catkin_ws/src/control/src/kf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import tf 4 | import math 5 | import geometry_msgs.msg 6 | from geometry_msgs.msg import Twist 7 | from geometry_msgs.msg import Pose, PoseStamped 8 | from geometry_msgs.msg import TransformStamped 9 | from math import pow, atan2, sqrt, pi, degrees 10 | from std_msgs.msg import Float32MultiArray 11 | from tf.transformations import euler_from_quaternion 12 | from tf.transformations import quaternion_from_euler 13 | from mavros_msgs.srv import SetMode 14 | from mavros_msgs.srv import CommandBool 15 | from mavros_msgs.srv import CommandTOL 16 | import time 17 | import numpy as np 18 | 19 | def euclidean_distance(xd, yd, zd): 20 | return sqrt(pow((xd), 2) + pow((yd), 2) + pow((zd), 2)) 21 | 22 | 23 | class State: 24 | def __init__(self, x = 0, y = 0, z = 0): 25 | self.x = x 26 | self.y = y 27 | self.z = z 28 | 29 | class PID: 30 | def __init__(self,kp=1,kd=0,ki=0,dt=0.01): 31 | 32 | #GAINS 33 | self.kp = kp 34 | self.kd = kd 35 | self.ki = ki 36 | 37 | #TIME STEP 38 | self.dt = dt 39 | 40 | #Default ERROR INITIALIZATION 41 | self.err_previous = 0.001 42 | self.err_acc = 0 43 | 44 | def compute(self,err): 45 | 46 | #compute dervivative 47 | err_deriv = (err - self.err_previous)/self.dt 48 | 49 | #update integration 50 | self.err_acc = self.err_acc + self.dt * (err + self.err_previous)/2 51 | 52 | #compute pid equation 53 | pid = self.kp*err + self.kd*err_deriv + self.ki*self.err_acc 54 | 55 | #update error 56 | self.err_previous = err 57 | 58 | return pid 59 | 60 | class Controller: 61 | ''' 62 | main class of the ros node controlling the robot. 63 | ''' 64 | 65 | def __init__(self): 66 | 67 | #initialization of the ros node and relevant pub/sub 68 | rospy.init_node("PID_node") 69 | self.velocity_publisher=rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped",Twist,queue_size=1) 70 | self.position_publisher=rospy.Publisher("/mavros/setpoint_position/local",PoseStamped,queue_size=1) 71 | self.bebop_subscriber=rospy.Subscriber("/relative_distance", Float32MultiArray ,self.call_back) 72 | self.position_subscriber=rospy.Subscriber("/mavros/local_position/pose", PoseStamped ,self.pos_call_back) 73 | 74 | #robot current state 75 | self.state = State() 76 | self.local_position = PoseStamped() 77 | self.velocity = State() 78 | 79 | #controller frequency in Hz 80 | self.hz=50.0 81 | self.rate = rospy.Rate(self.hz) 82 | self.dt=(1.0/self.hz) 83 | 84 | #define pids 85 | self.pid_rho_xy = PID(kp=0.3, ki=0.02, dt=self.dt) 86 | 87 | #kf parameters 88 | self.A = np.eye(2, dtype = float) 89 | self.B = -self.dt * np.eye(2, dtype = float) 90 | self.C = np.eye(2, dtype = float) 91 | self.Q = 0.05 * np.eye(2, dtype = float) 92 | self.R = 0.05 * np.eye(2, dtype = float) 93 | self.P = np.eye(2, dtype = float) 94 | 95 | def kf_predict(self, X, U): 96 | # predict state and covariance 97 | pred_X = np.dot(self.A, X) + np.dot(self.B, U) 98 | self.P = np.dot(np.dot(self.A, self.P), self.A.T) + self.Q 99 | return pred_X 100 | 101 | def kf_update(self, pred_X, Y): 102 | # update state 103 | S = self.R + np.dot(self.C, np.dot(self.P, self.C.T)) 104 | K = np.dot(np.dot(self.P, self.C.T), np.linalg.inv(S)) 105 | corr_X = pred_X + np.dot(K, Y - np.dot(self.C, pred_X)) 106 | 107 | # update state and covariance 108 | I = np.eye(2) 109 | self.P = np.dot(I - np.dot(K, self.C), self.P) 110 | 111 | return corr_X 112 | 113 | # transform from camera frame to drone frame 114 | def call_back(self, msg): 115 | 116 | # prediction 117 | pred_X = None 118 | if self.velocity.x is not 0.0 and not math.isnan(self.state.x): 119 | U = np.array([self.velocity.x, self.velocity.y]).reshape((2, 1)) 120 | X = np.array([self.state.x, self.state.y]).reshape((2, 1)) 121 | pred_X = self.kf_predict(X, U) 122 | 123 | # sensor(camera) measurement 124 | self.state.x = -msg.data[1] 125 | self.state.y = -msg.data[0] 126 | self.state.z = -msg.data[2] 127 | 128 | # state estimation(only xy) 129 | if pred_X is not None: 130 | Y = np.array([self.state.x, self.state.y]).reshape((2, 1)) 131 | corr_X = self.kf_update(pred_X, Y) 132 | self.state.x = corr_X[0][0] 133 | self.state.y = corr_X[1][0] 134 | 135 | 136 | def pos_call_back(self, msg): 137 | self.local_position.pose.position.x = msg.pose.position.x 138 | self.local_position.pose.position.y = msg.pose.position.y 139 | self.local_position.pose.position.z = msg.pose.position.z 140 | 141 | 142 | def takeoff(self): 143 | # Arm 144 | #print "\nArming" 145 | #rospy.wait_for_service('/mavros/cmd/arming') 146 | #try: 147 | # arming_cl = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) 148 | # response = arming_cl(value = True) 149 | # rospy.loginfo(response) 150 | # except rospy.ServiceException as e: 151 | # print("Arming failed: %s" %e) 152 | 153 | # # Takeoff 154 | # print "\nTaking off" 155 | # rospy.wait_for_service('/mavros/cmd/takeoff') 156 | # try: 157 | # takeoff_cl = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL) 158 | # response = takeoff_cl(altitude=10, latitude=0, longitude=0, min_pitch=0, yaw=0) 159 | # rospy.loginfo(response) 160 | # except rospy.ServiceException as e: 161 | # print("Takeoff failed: %s" %e) 162 | # print "\n Hovering..." 163 | # time.sleep(3) 164 | 165 | print "\n Takeoff..." 166 | pos_msg = PoseStamped() 167 | pos_msg.pose.position.x = 0 168 | pos_msg.pose.position.y = 0 169 | pos_msg.pose.position.z = 2 170 | print("Fly to z = {}".format(pos_msg.pose.position.z)) 171 | for i in range(200): 172 | self.position_publisher.publish(pos_msg) 173 | self.rate.sleep() 174 | 175 | def move_to_goal(self): 176 | #variable initialization 177 | vel_msg = Twist() 178 | pose_msg = PoseStamped() 179 | tolerance_distance_xy = 0.01 180 | 181 | rho_xy = euclidean_distance(self.state.x, self.state.y, 0.0) 182 | # print("rho: {}".format(rho)) 183 | 184 | while (rho_xy >= tolerance_distance_xy or rho_xy == 0) and not math.isnan(self.state.x): 185 | rospy.loginfo("xy Distance from goal:"+str(rho_xy)) 186 | rho_xy = euclidean_distance(self.state.x, self.state.y, 0.0) 187 | 188 | # pid control 189 | vx = self.pid_rho_xy.compute(self.state.x) 190 | vy = self.pid_rho_xy.compute(self.state.y) 191 | 192 | if (rho_xy / abs(self.state.z)) >= 0.1: 193 | vz = 0.0 194 | else: 195 | vz = -0.25 196 | 197 | #fill message 198 | vel_msg.linear.x = vx 199 | vel_msg.linear.y = vy 200 | vel_msg.linear.z = vz 201 | vel_msg.angular.x = 0.0 202 | vel_msg.angular.y = 0.0 203 | vel_msg.angular.z = 0.0 204 | 205 | #debugging 206 | print("vx: {:6f}, x distance: {:6f}".format(vel_msg.linear.x, self.state.x)) 207 | print("vy: {:6f}, y distance: {:6f}".format(vel_msg.linear.y, self.state.y)) 208 | print("vz: {:6f}, z distance: {:6f}".format(vel_msg.linear.z, self.state.z)) 209 | print("_________________") 210 | 211 | #publish 212 | self.velocity_publisher.publish(vel_msg) 213 | self.rate.sleep() 214 | 215 | # stop the robot 216 | vel_msg.linear.x = 0.0 217 | vel_msg.linear.y = 0.0 218 | vel_msg.linear.z = 0.0 219 | self.velocity_publisher.publish(vel_msg) 220 | 221 | # Land 222 | print "\n Landing" 223 | rospy.wait_for_service('/mavros/cmd/land') 224 | try: 225 | land_cl = rospy.ServiceProxy('/mavros/cmd/land', CommandTOL) 226 | response = land_cl(altitude = abs(self.state.z), latitude=0, longitude=0, min_pitch=0, yaw=0) 227 | rospy.loginfo(response) 228 | except rospy.ServiceException as e: 229 | print("Landing failed: %s" %e) 230 | 231 | # Disarm 232 | print "\n Disarming" 233 | rospy.wait_for_service('/mavros/cmd/arming') 234 | try: 235 | arming_cl = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) 236 | response = arming_cl(value = False) 237 | rospy.loginfo(response) 238 | except rospy.ServiceException as e: 239 | print("Disarming failed: %s" %e) 240 | 241 | rospy.loginfo("I'm here(relative info): "+ str(self.state.x) + " , " + str(self.state.y) +" , " + str(self.state.z)) 242 | print("___") 243 | 244 | return 245 | 246 | 247 | if __name__ == '__main__': 248 | try: 249 | drone = Controller() 250 | 251 | # TAKE OFF 252 | drone.takeoff() 253 | 254 | # MOVE TO THE GOALS 255 | drone.move_to_goal() 256 | 257 | # SPIN 258 | rospy.spin() 259 | 260 | except rospy.ROSInterruptException: 261 | pass 262 | --------------------------------------------------------------------------------