├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md └── drive_drone ├── CMakeLists.txt ├── launch ├── p1_a_drone_fly_gazebo_rviz.launch.py └── p1_b_tb3_drone_gazebo.launch.py ├── package.xml └── src ├── p1_a_drive_node.cpp └── p1_b_tb3_follower.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "sjtu_drone"] 2 | path = sjtu_drone 3 | url = https://github.com/noshluk2/sjtu_drone 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Creative Commons Legal Code 2 | 3 | CC0 1.0 Universal 4 | 5 | CREATIVE COMMONS CORPORATION IS NOT A LAW FIRM AND DOES NOT PROVIDE 6 | LEGAL SERVICES. DISTRIBUTION OF THIS DOCUMENT DOES NOT CREATE AN 7 | ATTORNEY-CLIENT RELATIONSHIP. CREATIVE COMMONS PROVIDES THIS 8 | INFORMATION ON AN "AS-IS" BASIS. CREATIVE COMMONS MAKES NO WARRANTIES 9 | REGARDING THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS 10 | PROVIDED HEREUNDER, AND DISCLAIMS LIABILITY FOR DAMAGES RESULTING FROM 11 | THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS PROVIDED 12 | HEREUNDER. 13 | 14 | Statement of Purpose 15 | 16 | The laws of most jurisdictions throughout the world automatically confer 17 | exclusive Copyright and Related Rights (defined below) upon the creator 18 | and subsequent owner(s) (each and all, an "owner") of an original work of 19 | authorship and/or a database (each, a "Work"). 20 | 21 | Certain owners wish to permanently relinquish those rights to a Work for 22 | the purpose of contributing to a commons of creative, cultural and 23 | scientific works ("Commons") that the public can reliably and without fear 24 | of later claims of infringement build upon, modify, incorporate in other 25 | works, reuse and redistribute as freely as possible in any form whatsoever 26 | and for any purposes, including without limitation commercial purposes. 27 | These owners may contribute to the Commons to promote the ideal of a free 28 | culture and the further production of creative, cultural and scientific 29 | works, or to gain reputation or greater distribution for their Work in 30 | part through the use and efforts of others. 31 | 32 | For these and/or other purposes and motivations, and without any 33 | expectation of additional consideration or compensation, the person 34 | associating CC0 with a Work (the "Affirmer"), to the extent that he or she 35 | is an owner of Copyright and Related Rights in the Work, voluntarily 36 | elects to apply CC0 to the Work and publicly distribute the Work under its 37 | terms, with knowledge of his or her Copyright and Related Rights in the 38 | Work and the meaning and intended legal effect of CC0 on those rights. 39 | 40 | 1. Copyright and Related Rights. A Work made available under CC0 may be 41 | protected by copyright and related or neighboring rights ("Copyright and 42 | Related Rights"). Copyright and Related Rights include, but are not 43 | limited to, the following: 44 | 45 | i. the right to reproduce, adapt, distribute, perform, display, 46 | communicate, and translate a Work; 47 | ii. moral rights retained by the original author(s) and/or performer(s); 48 | iii. publicity and privacy rights pertaining to a person's image or 49 | likeness depicted in a Work; 50 | iv. rights protecting against unfair competition in regards to a Work, 51 | subject to the limitations in paragraph 4(a), below; 52 | v. rights protecting the extraction, dissemination, use and reuse of data 53 | in a Work; 54 | vi. database rights (such as those arising under Directive 96/9/EC of the 55 | European Parliament and of the Council of 11 March 1996 on the legal 56 | protection of databases, and under any national implementation 57 | thereof, including any amended or successor version of such 58 | directive); and 59 | vii. other similar, equivalent or corresponding rights throughout the 60 | world based on applicable law or treaty, and any national 61 | implementations thereof. 62 | 63 | 2. Waiver. To the greatest extent permitted by, but not in contravention 64 | of, applicable law, Affirmer hereby overtly, fully, permanently, 65 | irrevocably and unconditionally waives, abandons, and surrenders all of 66 | Affirmer's Copyright and Related Rights and associated claims and causes 67 | of action, whether now known or unknown (including existing as well as 68 | future claims and causes of action), in the Work (i) in all territories 69 | worldwide, (ii) for the maximum duration provided by applicable law or 70 | treaty (including future time extensions), (iii) in any current or future 71 | medium and for any number of copies, and (iv) for any purpose whatsoever, 72 | including without limitation commercial, advertising or promotional 73 | purposes (the "Waiver"). Affirmer makes the Waiver for the benefit of each 74 | member of the public at large and to the detriment of Affirmer's heirs and 75 | successors, fully intending that such Waiver shall not be subject to 76 | revocation, rescission, cancellation, termination, or any other legal or 77 | equitable action to disrupt the quiet enjoyment of the Work by the public 78 | as contemplated by Affirmer's express Statement of Purpose. 79 | 80 | 3. Public License Fallback. Should any part of the Waiver for any reason 81 | be judged legally invalid or ineffective under applicable law, then the 82 | Waiver shall be preserved to the maximum extent permitted taking into 83 | account Affirmer's express Statement of Purpose. In addition, to the 84 | extent the Waiver is so judged Affirmer hereby grants to each affected 85 | person a royalty-free, non transferable, non sublicensable, non exclusive, 86 | irrevocable and unconditional license to exercise Affirmer's Copyright and 87 | Related Rights in the Work (i) in all territories worldwide, (ii) for the 88 | maximum duration provided by applicable law or treaty (including future 89 | time extensions), (iii) in any current or future medium and for any number 90 | of copies, and (iv) for any purpose whatsoever, including without 91 | limitation commercial, advertising or promotional purposes (the 92 | "License"). The License shall be deemed effective as of the date CC0 was 93 | applied by Affirmer to the Work. Should any part of the License for any 94 | reason be judged legally invalid or ineffective under applicable law, such 95 | partial invalidity or ineffectiveness shall not invalidate the remainder 96 | of the License, and in such case Affirmer hereby affirms that he or she 97 | will not (i) exercise any of his or her remaining Copyright and Related 98 | Rights in the Work or (ii) assert any associated claims and causes of 99 | action with respect to the Work, in either case contrary to Affirmer's 100 | express Statement of Purpose. 101 | 102 | 4. Limitations and Disclaimers. 103 | 104 | a. No trademark or patent rights held by Affirmer are waived, abandoned, 105 | surrendered, licensed or otherwise affected by this document. 106 | b. Affirmer offers the Work as-is and makes no representations or 107 | warranties of any kind concerning the Work, express, implied, 108 | statutory or otherwise, including without limitation warranties of 109 | title, merchantability, fitness for a particular purpose, non 110 | infringement, or the absence of latent or other defects, accuracy, or 111 | the present or absence of errors, whether or not discoverable, all to 112 | the greatest extent permissible under applicable law. 113 | c. Affirmer disclaims responsibility for clearing rights of other persons 114 | that may apply to the Work or any use thereof, including without 115 | limitation any person's Copyright and Related Rights in the Work. 116 | Further, Affirmer disclaims responsibility for obtaining any necessary 117 | consents, permissions or other rights required for any use of the 118 | Work. 119 | d. Affirmer understands and acknowledges that Creative Commons is not a 120 | party to this document and has no duty or obligation with respect to 121 | this CC0 or use of the Work. 122 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Drones with ROS2 2 | ## Introduction 3 | This repository contains the code for the drones with ROS2 project. The project is divided in two parts: the first one is the simulation of a drone in Gazebo and the second one is the control of a real drone with ROS2. The simulation is done with the [rotors_simulator] 4 | 5 | 6 | ## Simulation 7 | The simulation is done with the [rotors_simulator] package. The package is a collection of Gazebo plugins and ROS2 modules for high fidelity simulation of multirotor vehicles. The package is a fork of the [rotors_simulator] package for ROS1. The simulation is done with the [rotors_gazebo_plugins] package. The package contains the plugins for the simulation of the drone. The package is a fork of the [rotors_gazebo_plugins] package for ROS1. The simulation is done with the [rotors_control] package. The package contains the controllers for the simulation of the drone. The package is a fork of the [rotors_control] package for ROS1. The simulation is done with the [rotors_description] package. The package contains the description of the drone. The package is a fork of the [rotors_description] package for ROS1. The simulation is done with the [rotors_comm] package. The package contains the messages for the simulation of the drone. The package is a fork of the [rotors_comm] package for ROS1. 8 | 9 | 10 | 11 | ## Running Projects 12 | - Project [#1](https://github.com/Robotisim/drones_ROS2/issues/1): Follow Ground Robot -> [Wiki_link](https://github.com/Robotisim/drones_ROS2/wiki/Project-%231:-Follow-Ground-Robot) 13 | -------------------------------------------------------------------------------- /drive_drone/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(drive_drone) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(geometry_msgs REQUIRED) 12 | find_package(sensor_msgs REQUIRED) 13 | find_package(cv_bridge REQUIRED) 14 | find_package(OpenCV REQUIRED) 15 | 16 | add_executable(p1_a_drive_node src/p1_a_drive_node.cpp) 17 | ament_target_dependencies(p1_a_drive_node rclcpp geometry_msgs) 18 | 19 | 20 | add_executable(p1_b_tb3_follower src/p1_b_tb3_follower.cpp) 21 | ament_target_dependencies(p1_b_tb3_follower rclcpp sensor_msgs cv_bridge) 22 | target_link_libraries(p1_b_tb3_follower ${OpenCV_LIBS}) 23 | 24 | 25 | 26 | 27 | install(TARGETS p1_a_drive_node p1_b_tb3_follower 28 | 29 | DESTINATION lib/${PROJECT_NAME}) 30 | 31 | install(DIRECTORY launch 32 | DESTINATION share/${PROJECT_NAME}/ 33 | ) 34 | 35 | 36 | 37 | ament_package() 38 | -------------------------------------------------------------------------------- /drive_drone/launch/p1_a_drone_fly_gazebo_rviz.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | 5 | from ament_index_python.packages import get_package_share_directory 6 | from launch import LaunchDescription 7 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription , ExecuteProcess 8 | from launch.substitutions import LaunchConfiguration 9 | from launch_ros.actions import Node 10 | from launch.launch_description_sources import PythonLaunchDescriptionSource 11 | 12 | import xacro 13 | 14 | def generate_launch_description(): 15 | 16 | drone_pkg = get_package_share_directory('sjtu_drone_bringup') 17 | 18 | 19 | 20 | 21 | 22 | drone_bringup= IncludeLaunchDescription( 23 | PythonLaunchDescriptionSource( 24 | os.path.join(drone_pkg, 'launch', 'sjtu_drone_bringup.launch.py') 25 | ), 26 | 27 | ) 28 | 29 | takeoff_cmd= ExecuteProcess( 30 | cmd=['ros2', 'topic', 'pub', '/drone/takeoff', 'std_msgs/msg/Empty', '{}','--once'], 31 | shell=True, 32 | output="screen" 33 | ) 34 | 35 | drive_drone= Node( 36 | package="drive_drone", 37 | executable="p1_a_drive_node", 38 | output="screen" 39 | ) 40 | 41 | nodes_to_run = [ 42 | drone_bringup, 43 | takeoff_cmd, 44 | # drive_drone 45 | 46 | 47 | ] 48 | 49 | return LaunchDescription(nodes_to_run) -------------------------------------------------------------------------------- /drive_drone/launch/p1_b_tb3_drone_gazebo.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | 5 | from ament_index_python.packages import get_package_share_directory 6 | from launch import LaunchDescription 7 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription , ExecuteProcess 8 | from launch.substitutions import LaunchConfiguration 9 | from launch_ros.actions import Node 10 | from launch.launch_description_sources import PythonLaunchDescriptionSource 11 | 12 | import xacro 13 | 14 | def generate_launch_description(): 15 | 16 | drone_pkg = get_package_share_directory('sjtu_drone_bringup') 17 | tb3_pkg = get_package_share_directory('turtlebot3_gazebo') 18 | 19 | 20 | 21 | 22 | drone_bringup= IncludeLaunchDescription( 23 | PythonLaunchDescriptionSource( 24 | os.path.join(drone_pkg, 'launch', 'sjtu_drone_bringup.launch.py') 25 | ), 26 | 27 | ) 28 | 29 | tb3_bringup= IncludeLaunchDescription( 30 | PythonLaunchDescriptionSource( 31 | os.path.join(tb3_pkg, 'launch', 'spawn_turtlebot3.launch.py') 32 | ), 33 | 34 | ) 35 | 36 | takeoff_cmd= ExecuteProcess( 37 | cmd=['ros2', 'topic', 'pub', '/drone/takeoff', 'std_msgs/msg/Empty', '{}','--once'], 38 | shell=True, 39 | output="screen" 40 | ) 41 | 42 | drive_drone= Node( 43 | package="drive_drone", 44 | executable="p1_drive_node", 45 | output="screen" 46 | ) 47 | 48 | nodes_to_run = [ 49 | drone_bringup, 50 | takeoff_cmd, 51 | # drive_drone, 52 | tb3_bringup 53 | 54 | 55 | ] 56 | 57 | return LaunchDescription(nodes_to_run) -------------------------------------------------------------------------------- /drive_drone/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | drive_drone 5 | 0.0.0 6 | TODO: Package description 7 | luqman 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /drive_drone/src/p1_a_drive_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "rclcpp/rclcpp.hpp" 3 | #include "geometry_msgs/msg/twist.hpp" 4 | 5 | using namespace std::chrono_literals; 6 | 7 | class DroneController : public rclcpp::Node 8 | { 9 | public: 10 | DroneController() 11 | : Node("drone_controller") 12 | { 13 | publisher_ = this->create_publisher("/drone/cmd_vel", 10); 14 | 15 | auto timer_callback = 16 | [this]() -> void { 17 | auto message = geometry_msgs::msg::Twist(); 18 | 19 | // Forward 20 | message.linear.x = 1.0; 21 | publisher_->publish(message); 22 | rclcpp::sleep_for(1s); 23 | 24 | // Left 25 | message.linear.x = 0.0; 26 | message.angular.z = 1.0; 27 | publisher_->publish(message); 28 | rclcpp::sleep_for(1s); 29 | 30 | // Right 31 | message.angular.z = -1.0; 32 | publisher_->publish(message); 33 | rclcpp::sleep_for(1s); 34 | 35 | // Back 36 | message.angular.z = 0.0; 37 | message.linear.x = -1.0; 38 | publisher_->publish(message); 39 | rclcpp::sleep_for(1s); 40 | 41 | // Stop 42 | message.linear.x = 0.0; 43 | publisher_->publish(message); 44 | }; 45 | timer_ = this->create_wall_timer(5000ms, timer_callback); 46 | } 47 | 48 | private: 49 | rclcpp::TimerBase::SharedPtr timer_; 50 | rclcpp::Publisher::SharedPtr publisher_; 51 | }; 52 | 53 | int main(int argc, char * argv[]) 54 | { 55 | rclcpp::init(argc, argv); 56 | rclcpp::spin(std::make_shared()); 57 | rclcpp::shutdown(); 58 | return 0; 59 | } 60 | -------------------------------------------------------------------------------- /drive_drone/src/p1_b_tb3_follower.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | #include "sensor_msgs/msg/image.hpp" 3 | #include "geometry_msgs/msg/twist.hpp" 4 | #include "cv_bridge/cv_bridge.h" 5 | #include "opencv2/opencv.hpp" 6 | 7 | class camera_subscriber : public rclcpp::Node 8 | { 9 | public: 10 | camera_subscriber() 11 | : Node("camera_subscriber_node") 12 | { 13 | subscription_ = this->create_subscription( 14 | "/drone/bottom/image_raw", 10, std::bind(&camera_subscriber::camera_callback, this, std::placeholders::_1)); 15 | 16 | publisher_ = this->create_publisher("/drone/cmd_vel", 10); 17 | auto velocity_msg = geometry_msgs::msg::Twist(); 18 | RCLCPP_INFO(this->get_logger(), "------ Node Started -----"); 19 | } 20 | 21 | 22 | 23 | private: 24 | int cX,cY,mid_x,mid_y,error_x,error_y=0; 25 | geometry_msgs::msg::Twist velocity_msg = geometry_msgs::msg::Twist(); 26 | int largest_contour_index = 0; 27 | double largest_area = 0.0; 28 | 29 | void camera_callback(const sensor_msgs::msg::Image::SharedPtr camera_msg) 30 | { 31 | cv_bridge::CvImagePtr cv_ptr; 32 | cv_ptr = cv_bridge::toCvCopy(camera_msg,"bgr8"); 33 | cv::Mat gray_image,binary; 34 | // Flipping 35 | cv::flip(cv_ptr->image,cv_ptr->image,0); 36 | cv::cvtColor(cv_ptr->image, gray_image, cv::COLOR_BGR2GRAY); 37 | 38 | // ## Binary Image 39 | cv::threshold(gray_image,binary,200,255,cv::THRESH_BINARY); 40 | 41 | // ## Find Contour 42 | std::vector > contour; 43 | cv::findContours(binary,contour,cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); 44 | RCLCPP_INFO(this->get_logger(), "contours.size() = %d", contour.size()); 45 | 46 | // ## Draw Contour 47 | cv::drawContours(cv_ptr->image,contour,-1,cv::Scalar(0,255,0),2); 48 | 49 | // ## Draw Center of Contours with Moment 50 | for (int i = 0; i < contour.size(); i++) { 51 | double area = cv::contourArea(contour[i]); // Calculate the area of the contour 52 | if (area > largest_area) { 53 | largest_area = area; 54 | largest_contour_index = i; 55 | } 56 | } 57 | if(contour.size() ){ // made change here 58 | cv::Moments M=cv::moments(contour[largest_contour_index]); 59 | cX= int(M.m10 / M.m00); 60 | cY= int(M.m01 / M.m00); 61 | cv::circle(cv_ptr->image,cv::Point(cX,cY),5,cv::Scalar(0,0,255),-1); 62 | } 63 | // ## Error Calculation 64 | mid_x = gray_image.cols/2; 65 | mid_y = gray_image.rows/2; 66 | cv::circle(cv_ptr->image,cv::Point(mid_x,mid_y),5,cv::Scalar(255,0,0),-1); 67 | error_x = mid_x - cX; 68 | error_y = mid_y - cY; 69 | 70 | RCLCPP_INFO(this->get_logger(), "error_x = %d error_y = %d",error_x,error_y); 71 | 72 | // ## Error Reaction 73 | 74 | // linear Motion 75 | velocity_msg.linear.x= -error_y * 0.001; 76 | // Angular motion 77 | velocity_msg.angular.z = error_x *0.005; 78 | 79 | // ## Sendign Velocity 80 | publisher_->publish(velocity_msg); 81 | 82 | 83 | cv::imshow("Image",cv_ptr->image); 84 | cv::waitKey(1); 85 | } 86 | 87 | rclcpp::Publisher::SharedPtr publisher_; 88 | rclcpp::Subscription::SharedPtr subscription_; 89 | rclcpp::TimerBase::SharedPtr timer_; 90 | }; 91 | 92 | 93 | int main(int argc, char ** argv) 94 | { 95 | 96 | 97 | rclcpp::init(argc, argv); 98 | rclcpp::spin(std::make_shared()); 99 | rclcpp::shutdown(); 100 | return 0; 101 | } --------------------------------------------------------------------------------