Merge pull request #28 from IseanB/multiDroneControl

Working multi/single drone control
IseanB · Aug 24, 2022 · c040531 · c040531
2 parents e4ffcbb + ecdc4d4
commit c040531
Show file tree

Hide file tree

Showing 15 changed files with 946 additions and 108 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.0.2)
-project(simple_movements)
+project(drone_control)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
@@ -8,12 +8,13 @@ project(simple_movements)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  rospy
   std_msgs
   geometry_msgs
+  roscpp
+  rospy
   mavros_msgs
   mav_trajectory_generation
+  message_generation
 )
 
 ## System dependencies are found with CMake's conventions
@@ -50,11 +51,11 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+add_message_files(
+  FILES
+  dcontrol.msg
+  dresponse.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -71,10 +72,11 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  std_msgs 
+  geometry_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -113,6 +115,7 @@ rospy
 std_msgs 
 geometry_msgs 
 mav_trajectory_generation
+message_runtime
 #  DEPENDS system_lib
 )
 
@@ -123,7 +126,7 @@ mav_trajectory_generation
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(
-# include
+  # include
   ${catkin_INCLUDE_DIRS}
 )
 
@@ -141,10 +144,21 @@ include_directories(
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
 # add_executable(${PROJECT_NAME}_node src/simple_movements_node.cpp)
-add_executable(offboard_node 
+
+add_executable(multi_control
+  helper/src/conversions.cpp
+  helper/src/computations.cpp
+  src/src/multiDrone.cpp)
+
+add_executable(single_control 
   helper/src/conversions.cpp
   helper/src/computations.cpp
-  src/offboard.cpp)
+  src/src/singleDrone.cpp)
+
+# add_executable(singleDroneControl_node 
+#   helper/src/conversions.cpp
+#   helper/src/computations.cpp
+#   src/src/single_control.cpp)
 
   ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -154,13 +168,25 @@ add_executable(offboard_node
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(multi_control drone_control_generate_messages)
+add_dependencies(single_control drone_control_generate_messages)
 
 ## Specify libraries to link a library or executable target against
-target_link_libraries(offboard_node
+
+target_link_libraries(multi_control
+  ${catkin_LIBRARIES}
+)
+
+target_link_libraries(single_control
   ${catkin_LIBRARIES}
 )
 
+
+# target_link_libraries(singleDroneControl_node
+#   ${catkin_LIBRARIES}
+# )
+
+
 #############
 ## Install ##
 #############
@@ -208,9 +234,9 @@ target_link_libraries(offboard_node
 #############
 
 ## Add gtest based cpp test target and link libraries
-catkin_add_executable_with_gtest(simple_mov_test 
-  helper/src/computations.cpp
-  test/test_helperFunc.cpp)
+# catkin_add_executable_with_gtest(simple_mov_test 
+#   helper/src/computations.cpp
+#   test/test_helperFunc.cpp)
 # if(TARGET simple_mov_test)
 #   target_link_libraries(simple_mov_test ${PROJECT_NAME})
 # endif()

diff --git a/README.md b/README.md
@@ -1,6 +1,6 @@
 # UCR Drone Control
 
-This repository uses various control and robotics libraries to autonomously control drone(s). Our code is built with the [ROS (Melodic)](http://wiki.ros.org/melodic) and [PX4 Autopilot](https://github.com/PX4/PX4-Autopilot) frameworks to develop drone control software. The [MAV Trajectory Generation](https://github.com/ethz-asl/mav_trajectory_generation) library generates an optimized, minimum snap, path for our drone(s) to follow. Finally, [MAVLink (MAVROS)](http://wiki.ros.org/mavros) is our primary messaging protocol, interfacing with the drone(s).
+This repository combines a trajectory planning, communication protocol, and robotics libraries to autonomously control drone(s). Our code is built with the [ROS (Melodic)](http://wiki.ros.org/melodic) and [PX4 Autopilot](https://github.com/PX4/PX4-Autopilot) frameworks to develop drone control software. The [MAV Trajectory Generation](https://github.com/ethz-asl/mav_trajectory_generation) library generates an optimized, minimum snap, path for our drone(s) to follow. Finally, [MAVLink (MAVROS)](http://wiki.ros.org/mavros) is our primary messaging protocol, interfacing with the drone(s).
 
 ---
 
@@ -13,28 +13,35 @@ This repository uses various control and robotics libraries to autonomously cont
   Tip: Only use ```catkin build```, never use ```catkin_make```.
   1. Install VMWare and set up a disc image of Ubuntu 18.04 LTS.[^11][^12] 
      - The rest of these instructions take place in the Ubuntu environment.
-  2. Follow the [Ubuntu Development Enviorment](https://wiki.hanzheteng.com/quadrotor/px4#ubuntu-development-environment) and [Gazebo SITL Simulation](https://wiki.hanzheteng.com/quadrotor/px4#gazebo-sitl-simulation) setup. 
+     - Disable "Accelerate 3D graphics" setting under *Virtual Machine Settings* -> *Display*.
+  2. Install Git on VMWare and set up SSH keys.[^14]
+     - Use ```sudo apt install git``` to install Git.
+  3. Follow the [Ubuntu Development Enviorment](https://wiki.hanzheteng.com/quadrotor/px4#ubuntu-development-environment) and [Gazebo SITL Simulation](https://wiki.hanzheteng.com/quadrotor/px4#gazebo-sitl-simulation) setup. 
      - Install PX4 Related Dependencies. [^1]
-       - Delete PX4 folder after completing steps.
+       - Once *Firmware* folder cloned, use the latest stable version. i.e. Run ```git checkout tags/v1.13.0``` in *Firmware*.
+       - Delete *Firmware* folder after completing steps. 
      - Install ROS Melodic. [^2]
        - Full Desktop Installation
      - Install Simulation Common Dependencies. [^3]
        - Run all code, except line 12, line by line in the terminal.
+       - Running pyulog install command is not necessary.
      - Build Quadrotor Model & Setup Gazebo[^10]
+       - Before building quadcopter models, update Pillow[^6] and GStreamer[^7]
      - Install MAVROS, MAVLink, & Setup Workspace [^4]
        - Source Installation (Released/Stable)
-  3. Install [MAV Trajectory Generation](https://github.com/ethz-asl/mav_trajectory_generation) Package [^13]
+  4. Install [MAV Trajectory Generation](https://github.com/ethz-asl/mav_trajectory_generation) Package [^13]
      - When installing additional system dependencies, replace ```indigo``` with ```melodic```.
      - Replace ```catkin config --merge-devel``` with ```catkin config --link-devel```
 
-  4. Install QGroundContol [^5]
+  5. Install QGroundContol [^5]
+     - Install Linux/Ubuntu version.
 
-  5. Create a ROS package "simple_movements"
+  6. Create a ROS package "drone_control"
   ```
   cd ~/[Workspace Name]/src
-  mkdir simple_movements
-  cd simple_movements/
-  git clone --recursive https://github.com/IseanB/UCR-Drone-Control.git
+  git clone --recursive [email protected]:IseanB/UCR-Drone-Control.git
+  mv UCR-Drone-Control drone_control
+  cd drone_control
   cd ~/[Workspace Name]
   catkin build
   . ~/[Workspace Name]/devel/setup.bash
@@ -43,13 +50,13 @@ This repository uses various control and robotics libraries to autonomously cont
 ### Installation Bug Fixes:
 1. Pillow Build Error [^6]
 2. GStreamer Error [^7]
-3. VMWare REST Error [^8]
-4. Gazebo/Rendering Error [^9]
+3. Gazebo/Rendering Error [^9]
+4. Symforce Error (Look above, under *Install PX4 Related Dependencies* for fix.)
 
 ---
 ## Documentation
 
-The ***simple_movements*** package is structured into three main folders: the src, helper, and the test folder. Below are quick summaries of each folder's purpose.
+The ***drone_control*** package is structured into three main folders: the src, helper, and the test folder. Below are quick summaries of each folder's purpose.
 ### src
 The src folder stores all the control code used to create the offboard node. Currently, the file offboard.cpp optimizes the path followed, controls the state of the drone, and sends control signals to the drone. 
 
@@ -60,18 +67,31 @@ Each loop contains unique control software used during that state of the drone.
 The helper folder stores any mathematical computations or data conversions needed. This allows for testing and easy extensibility for future computations/conversions. Some of the functions written are the isStationary(...), isFlat(...), reachedLocation(...), and segmentToPoint(...).
 ### test
 The test folder contains tests for the helper functions in the helper folder. Some of the functions tested are mathematical computations, such as isStationary(...), while others are data conversions like in the segmentToPoint(...) function. GoogleTest is the test framework used.
+### launch
+The launch folder contains the setup of the Gazebo simulation and MAVROS nodes. The fourDrones and oneDrone.launch are example launch files used for testing the single/multi control structure.
+### msg
+The msg folder contains the custom messages that are communicated between the individual drone nodes and the multi control node.
 
 ---
 ## Usage
 
 ### Launching World in Gazebo
-```roslaunch px4 mavros_posix_sitl.launch```
+Tip: Before launch run ```echo "export SVGA_VGPU10=0" >> ~/.bashrc``` and ```source ~/.bashrc```, to prevent VMWare REST Error below. [^8] 
 
-### Running offboard_node
-```rosrun simple_movements offboard_node```
+### Setuping World, MAVROS nodes, and four drones
+```roslaunch drone_control fourDronesNodes.launch```
+
+### Setuping World, MAVROS nodes, and a single drones
+```roslaunch drone_control oneDrone.launch```
+
+### Running drone# node(single drone control)
+```rosrun drone_control single_control #(0-3)```
+
+### Running multi_control node(multi drone control)
+```rosrun drone_control single_control```
 
 ### Running Google Tests(Optional)
-```rosrun simple_movements simple_mov_test```
+```rosrun drone_control simple_mov_test```
 
 ---
 ## Tools
@@ -113,5 +133,5 @@ The test folder contains tests for the helper functions in the helper folder. So
   [^10]:https://wiki.hanzheteng.com/quadrotor/px4#gazebo-sitl-simulation (Gazebo)
   [^11]:https://customerconnect.vmware.com/en/downloads/info/slug/desktop_end_user_computing/vmware_workstation_pro/16_0 (VMWare)
   [^12]:https://releases.ubuntu.com/18.04/ (Ubuntu)
-
   [^13]:https://github.com/ethz-asl/mav_trajectory_generation#installation-instructions-ubuntu (MAV Trajcetory Generation)
+  [^14]:https://www.digitalocean.com/community/tutorials/how-to-configure-ssh-key-based-authentication-on-a-linux-server (SSH Keys)
diff --git a/helper/computations.h b/helper/computations.h
@@ -4,20 +4,21 @@
 #include <geometry_msgs/Pose.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Twist.h>
-#include <geometry_msgs/TwistStamped.h>
 #include <geometry_msgs/Point.h>
+#include <mavros_msgs/PositionTarget.h>
 
 
 /*  Functions   */
 
 /*Makes sures this_pos is not tilted more than maxTilt, degrees, in x or y axis*/
-bool isFlat(const geometry_msgs::Pose& this_pos, float maxTilt = 0);
+bool isFlat(const geometry_msgs::PoseStamped& this_pos, float maxTilt = 0);
 
 /* Finds magnitude of this_vel values, x and y and z, checks its less than or equal to maxSpeed(m/s)*/
 bool isStationary(const geometry_msgs::Twist& this_vel, float maxSpeed = 0, float maxTiltSpeed = 0);
 
 /*Finds distance from curr_pos to desired_pos and checks it's within a accuracyDistance*/
+bool reachedLocation(const geometry_msgs::PoseStamped& this_pos, const mavros_msgs::PositionTarget& desired_pos, float accuracyDistance);
+bool reachedLocation(const geometry_msgs::PoseStamped& this_pos, const geometry_msgs::PoseStamped& desired_pos, float accuracyDistance);
 bool reachedLocation(const geometry_msgs::Pose& this_pos, const geometry_msgs::PoseStamped& desired_pos, float accuracyDistance);
-bool reachedLocation(const geometry_msgs::Pose& this_pos, const geometry_msgs::Point& desired_pos, float accuracyDistance);
 
 #endif
diff --git a/helper/conversions.h b/helper/conversions.h
@@ -4,7 +4,7 @@
 #include <mav_trajectory_generation/polynomial_optimization_linear.h>
 #include <mavros_msgs/PositionTarget.h>
 
-mavros_msgs::PositionTarget segmentToPoint(const mav_trajectory_generation::Segment& s, double time);
+mavros_msgs::PositionTarget segmentToPoint(const mav_trajectory_generation::Segment& s, float time);
 
 
 #endif
diff --git a/helper/src/computations.cpp b/helper/src/computations.cpp
@@ -12,16 +12,16 @@ bool isStationary(const geometry_msgs::Twist& this_vel, float maxSpeed, float ma
          );
 }
 
-bool isFlat(const geometry_msgs::Pose& this_pos, float maxTilt){
-    return (abs(this_pos.orientation.x) <= abs(maxTilt) && abs(this_pos.orientation.y) <= abs(maxTilt));
+bool isFlat(const geometry_msgs::PoseStamped& this_pos, float maxTilt){
+    return (abs(this_pos.pose.orientation.x) <= abs(maxTilt) && abs(this_pos.pose.orientation.y) <= abs(maxTilt));
 }
 
-bool reachedLocation(const geometry_msgs::Pose& this_pos, const geometry_msgs::PoseStamped& desired_pos, float accuracyDistance){
+bool reachedLocation(const geometry_msgs::PoseStamped& this_pos, const geometry_msgs::PoseStamped& desired_pos, float accuracyDistance){
     return( 
         pow( 
-            pow(this_pos.position.x - desired_pos.pose.position.x, 2) + 
-            pow(this_pos.position.y - desired_pos.pose.position.y, 2) + 
-            pow(this_pos.position.z - desired_pos.pose.position.z, 2), .5) <= abs(accuracyDistance) );
+            pow(this_pos.pose.position.x - desired_pos.pose.position.x, 2) + 
+            pow(this_pos.pose.position.y - desired_pos.pose.position.y, 2) + 
+            pow(this_pos.pose.position.z - desired_pos.pose.position.z, 2), .5) <= abs(accuracyDistance) );
 }
 
 bool reachedLocation(const geometry_msgs::Pose& this_pos, const geometry_msgs::Point& desired_pos, float accuracyDistance){
@@ -30,4 +30,12 @@ bool reachedLocation(const geometry_msgs::Pose& this_pos, const geometry_msgs::P
             pow(this_pos.position.x - desired_pos.x, 2) + 
             pow(this_pos.position.y - desired_pos.y, 2) + 
             pow(this_pos.position.z - desired_pos.z, 2), .5) <= abs(accuracyDistance) );
+}
+
+bool reachedLocation(const geometry_msgs::PoseStamped& this_pos, const mavros_msgs::PositionTarget& desired_pos, float accuracyDistance){
+    return( 
+        pow( 
+            pow(this_pos.pose.position.x - desired_pos.position.x, 2) + 
+            pow(this_pos.pose.position.y - desired_pos.position.y, 2) + 
+            pow(this_pos.pose.position.z - desired_pos.position.z, 2), .5) <= abs(accuracyDistance) );
 }
diff --git a/helper/src/conversions.cpp b/helper/src/conversions.cpp
@@ -1,7 +1,8 @@
 #include "../conversions.h"
 
-mavros_msgs::PositionTarget segmentToPoint(const mav_trajectory_generation::Segment& s, double time){
+mavros_msgs::PositionTarget segmentToPoint(const mav_trajectory_generation::Segment& s, float time){
     mavros_msgs::PositionTarget result;
+    time = abs(time);
     Eigen::VectorXd pos = s.evaluate(time, 0);
     Eigen::VectorXd vel = s.evaluate(time, 1);
     Eigen::VectorXd acc = s.evaluate(time, 2);