diff --git a/CMakeLists.txt b/CMakeLists.txt index 5553df8..b7ad9ea 100644 --- 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 index 013a8b2..25dd201 100644 --- 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 git@github.com: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 index 12cfda5..1f7eb70 100644 --- a/helper/computations.h +++ b/helper/computations.h @@ -4,20 +4,21 @@ #include #include #include -#include #include +#include /* 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 \ No newline at end of file diff --git a/helper/conversions.h b/helper/conversions.h index 4fdbdf2..e47f489 100644 --- a/helper/conversions.h +++ b/helper/conversions.h @@ -4,7 +4,7 @@ #include #include -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 \ No newline at end of file diff --git a/helper/src/computations.cpp b/helper/src/computations.cpp index d58a10c..c89eeb0 100644 --- 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) ); } \ No newline at end of file diff --git a/helper/src/conversions.cpp b/helper/src/conversions.cpp index e06ed02..6c92c31 100644 --- 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); diff --git a/launch/fourDronesNodes.launch b/launch/fourDronesNodes.launch new file mode 100644 index 0000000..78e01a5 --- /dev/null +++ b/launch/fourDronesNodes.launch @@ -0,0 +1,143 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/oneDrone.launch b/launch/oneDrone.launch new file mode 100644 index 0000000..30a0f60 --- /dev/null +++ b/launch/oneDrone.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/msg/dcontrol.msg b/msg/dcontrol.msg new file mode 100644 index 0000000..1b13700 --- /dev/null +++ b/msg/dcontrol.msg @@ -0,0 +1,3 @@ +# Commands: SHUTOFF, STOP, LIFT, LAND, CHECK, TRANSIT_ADD, TRANSIT_NEW +std_msgs/String command +geometry_msgs/Point target \ No newline at end of file diff --git a/msg/dresponse.msg b/msg/dresponse.msg new file mode 100644 index 0000000..6d515c8 --- /dev/null +++ b/msg/dresponse.msg @@ -0,0 +1,2 @@ +# Responses: States, ERROR, RECEIVED, REACHED +std_msgs/String response \ No newline at end of file diff --git a/package.xml b/package.xml index b247b9e..4270371 100644 --- a/package.xml +++ b/package.xml @@ -1,13 +1,13 @@ - simple_movements + drone_control 0.0.0 - The simple_movements package + The drone_control package - iseanbhanot + Isean @@ -37,36 +37,29 @@ - + message_generation - + message_runtime catkin - roscpp - rospy - std_msgs - - - roscpp - rospy - std_msgs + ros + roscpp + rospy - roscpp - rospy - std_msgs - geometry_msgs - mavros_msgs - ros + std_msgs + + mavros mav_trajectory_generation + diff --git a/src/src/multiDrone.cpp b/src/src/multiDrone.cpp new file mode 100644 index 0000000..9d71467 --- /dev/null +++ b/src/src/multiDrone.cpp @@ -0,0 +1,152 @@ +#include +#include "drone_control/dcontrol.h" +#include "drone_control/dresponse.h" + +drone_control::dresponse d1_response; + +void storeInfo(const drone_control::dresponse::ConstPtr& msg); + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "multi_control_node"); + ros::NodeHandle nh; + ros::Time last_request = ros::Time::now(); + ros::Rate rate(20.0); + + ros::Publisher drone1_cmd_pub = nh.advertise + ("drone0/cmds", 5); + ros::Publisher drone2_cmd_pub = nh.advertise + ("drone12/cmds", 5); + ros::Publisher drone3_cmd_pub = nh.advertise + ("drone22/cmds", 5); + ros::Publisher drone4_cmd_pub = nh.advertise + ("drone32/cmds", 5); + + drone_control::dcontrol msg; + bool lift = false; + bool transit = false; + bool transit2 = false; + bool transit3 = false; + bool transit4 = false; + bool transit5 = false; + bool transit6 = false; + bool transit7 = false; + // bool transit2 = false; + while(ros::ok() && !lift){ + if(ros::Time::now() - last_request >= ros::Duration(1.0)){ + ROS_INFO("LIFT info sent"); + msg.command.data = "LIFT"; + msg.target.z = 1; + drone1_cmd_pub.publish(msg); + drone2_cmd_pub.publish(msg); + drone3_cmd_pub.publish(msg); + drone4_cmd_pub.publish(msg); + last_request = ros::Time::now(); + lift = true; + } + ros::spinOnce(); + rate.sleep(); + } + while(ros::ok() && !transit7){ + if(!transit && ros::Time::now() - last_request >= ros::Duration(15.0)){ + ROS_INFO(" TRANSIT ADD"); + msg.command.data = "TRANSIT_ADD"; + msg.target.x = 1; + msg.target.y = 1; + msg.target.z = 2; + drone1_cmd_pub.publish(msg); + drone2_cmd_pub.publish(msg); + drone3_cmd_pub.publish(msg); + drone4_cmd_pub.publish(msg); + last_request = ros::Time::now(); + transit = true; + } + else if(!transit2 && transit && ros::Time::now() - last_request >= ros::Duration(10.0)){ + ROS_INFO(" TRANSIT ADD"); + msg.command.data = "TRANSIT_ADD"; + msg.target.x = -1; + msg.target.y = -2; + msg.target.z = 2; + drone1_cmd_pub.publish(msg); + drone2_cmd_pub.publish(msg); + drone3_cmd_pub.publish(msg); + drone4_cmd_pub.publish(msg); + last_request = ros::Time::now(); + transit2 = true; + } + else if(!transit3 && transit2 && ros::Time::now() - last_request >= ros::Duration(1.0)){ + ROS_INFO(" TRANSIT ADD"); + msg.command.data = "TRANSIT_ADD"; + msg.target.x = 0; + msg.target.y = 0; + msg.target.z = 1; + drone1_cmd_pub.publish(msg); + drone2_cmd_pub.publish(msg); + drone3_cmd_pub.publish(msg); + drone4_cmd_pub.publish(msg); + last_request = ros::Time::now(); + transit3 = true; + } + else if(!transit4 && transit3 && ros::Time::now() - last_request >= ros::Duration(15.0)){ + ROS_INFO(" TRANSIT NEW"); + msg.command.data = "TRANSIT_NEW"; + msg.target.x = -1; + msg.target.y = 2; + msg.target.z = 2; + drone1_cmd_pub.publish(msg); + drone2_cmd_pub.publish(msg); + drone3_cmd_pub.publish(msg); + drone4_cmd_pub.publish(msg); + last_request = ros::Time::now(); + transit4 = true; + } + else if(!transit5 && transit4 && ros::Time::now() - last_request >= ros::Duration(3.0)){ + ROS_INFO(" TRANSIT NEW"); + msg.command.data = "TRANSIT_NEW"; + msg.target.x = 1; + msg.target.y = -2; + msg.target.z = 2; + drone1_cmd_pub.publish(msg); + drone2_cmd_pub.publish(msg); + drone3_cmd_pub.publish(msg); + drone4_cmd_pub.publish(msg); + last_request = ros::Time::now(); + transit5 = true; + } + else if(!transit6 && transit5 && ros::Time::now() - last_request >= ros::Duration(3.0)){ + ROS_INFO(" TRANSIT ADD"); + msg.command.data = "TRANSIT_ADD"; + msg.target.x = 3; + msg.target.y = -2; + msg.target.z = 2; + drone1_cmd_pub.publish(msg); + drone2_cmd_pub.publish(msg); + drone3_cmd_pub.publish(msg); + drone4_cmd_pub.publish(msg); + last_request = ros::Time::now(); + transit6 = true; + } + else if(!transit7 && transit6 && ros::Time::now() - last_request >= ros::Duration(1.0)){ + ROS_INFO(" TRANSIT NEW"); + msg.command.data = "TRANSIT_NEW"; + msg.target.x = 0; + msg.target.y = 0; + msg.target.z = 2; + drone1_cmd_pub.publish(msg); + drone2_cmd_pub.publish(msg); + drone3_cmd_pub.publish(msg); + drone4_cmd_pub.publish(msg); + last_request = ros::Time::now(); + transit7 = true; + ROS_INFO("DONE"); + } + ros::spinOnce(); + rate.sleep(); + } + return 0; +} + + +void storeInfo(const drone_control::dresponse::ConstPtr& msg){ + d1_response = *msg; +} \ No newline at end of file diff --git a/src/offboard.cpp b/src/src/oneDrone.cpp similarity index 84% rename from src/offboard.cpp rename to src/src/oneDrone.cpp index 9f7250b..1b169c5 100644 --- a/src/offboard.cpp +++ b/src/src/oneDrone.cpp @@ -1,6 +1,6 @@ /* Custom files */ -#include "../helper/conversions.h" -#include "../helper/computations.h" +#include "../../helper/conversions.h" +#include "../../helper/computations.h" /* Libraries */ #include @@ -34,34 +34,34 @@ void updatePose(const geometry_msgs::PoseStamped::ConstPtr& inputPose); /* Updates curr_velocity values, linear and angular, with inputted pose */ void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose); -void printTrajInfo(const mav_trajectory_generation::Segment::Vector& allSegments); +// void printTrajInfo(const mav_trajectory_generation::Segment::Vector& allSegments); int main(int argc, char **argv){ - ros::init(argc, argv, "offb_node"); + ros::init(argc, argv, "onlyd_node"); setup(); ros::NodeHandle nh; PossiableState droneState = LIFTING_OFF; ros::Subscriber state_sub = nh.subscribe - ("mavros/state", 0, state_cb); + ("uav0/mavros/state", 0, state_cb); ros::Subscriber pos_sub = nh.subscribe - ("mavros/local_position/pose", 0, updatePose); + ("uav0/mavros/local_position/pose", 0, updatePose); ros::Subscriber vel_sub = nh.subscribe - ("mavros/local_position/velocity", 0, updateVel); + ("uav0/mavros/local_position/velocity", 0, updateVel); ros::ServiceClient arming_client = nh.serviceClient - ("mavros/cmd/arming"); + ("uav0/mavros/cmd/arming"); ros::ServiceClient set_mode_client = nh.serviceClient - ("mavros/set_mode"); + ("uav0/mavros/set_mode"); ros::ServiceClient end_flight_client = nh.serviceClient - ("mavros/cmd/command"); + ("uav0/mavros/cmd/command"); ros::Publisher local_pos_pub = nh.advertise - ("mavros/setpoint_position/local", 0); + ("uav0/mavros/setpoint_position/local", 0); ros::Publisher local_vel_pub = nh.advertise - ("mavros/setpoint_velocity/cmd_vel_unstamped", 0); + ("uav0/mavros/setpoint_velocity/cmd_vel_unstamped", 0); ros::Publisher mav_pub = nh.advertise - ("mavros/setpoint_raw/local", 20); + ("uav0/mavros/setpoint_raw/local", 20); // Optimzation Code @@ -297,19 +297,19 @@ void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose){ curr_velocity.angular.z = inputPose->twist.angular.z; } -void printTrajInfo(const mav_trajectory_generation::Segment::Vector& allSegments){ - mav_trajectory_generation::Trajectory trajectory; - trajectory.setSegments(allSegments);// dont use addSegments, uncessary calculations - - std::cout << "Trajectory Properties:\n" << std::endl; - std::cout << "Number of Dimensions : "< + +/* Data Types */ +#include +#include +#include +#include +#include +#include +#include "drone_control/dcontrol.h" +#include "drone_control/dresponse.h" +#include + +enum PossiableState{ + GROUND_IDLE, + LIFTING_OFF, + HOVERING, + IN_TRANSIT, + LANDING, + SHUTTING_DOWN // A drone is only momentarily in this state +}; + +struct TrajectoryGroupedInfo{ + mav_trajectory_generation::Segment segmentThis; + geometry_msgs::PoseStamped startPoint; + geometry_msgs::PoseStamped endPoint; + float trajectoryTime; + TrajectoryGroupedInfo(mav_trajectory_generation::Segment inputSeg, geometry_msgs::PoseStamped inputStart, geometry_msgs::PoseStamped inputEnd, float inputTime) : segmentThis(inputSeg), startPoint(inputStart), endPoint(inputEnd), trajectoryTime(inputTime) {}; +}; +//Must use commands to start,end. Add point + +mavros_msgs::State current_state; +geometry_msgs::PoseStamped curr_position; +geometry_msgs::Twist curr_velocity; +mavros_msgs::PositionTarget curr_target;// not endpoint, dynamic changed +drone_control::dcontrol curr_message; + +geometry_msgs::PoseStamped hover_position; +drone_control::dresponse last_response; +mavros_msgs::SetMode offb_set_mode; +mavros_msgs::CommandBool arm_cmd; + +std::string uavName; +std::string dPrefix; +PossiableState droneState; +ros::Publisher multi_info_pub; +ros::Publisher local_pos_pub; + +std::queue curr_trajectories; +float currTime; +float currTrajTime; +const float timeStepSize = .04; + +/* Assumes drone is on ground. Initializes curr_position & curr_velocity values to 0 */ +void setup(ros::NodeHandle& nodehandler,std::string droneNum); + +/* Used for initiating contact with drone and setting it to OFFBOARD mode */ +void state_cb(const mavros_msgs::State::ConstPtr& msg); + +/* Updates curr_position values, position and orientation, with inputted pose */ +void updatePose(const geometry_msgs::PoseStamped::ConstPtr& inputPose); + +/* Updates curr_velocity values, linear and angular, with inputted pose */ +void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose); + +// /* Prints crutial information about the inputted trajectory */ +// void printTrajInfo(mav_trajectory_generation::Trajectory trajectory); + +/* Stores msgs from multi control node in last*/ +void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg); + +mav_trajectory_generation::Segment generateTraj(int bx, int by, int bz, int ex, int ey, int ez); + +int main(int argc, char **argv) +{ + // initalizing node + std::cout << "Initalizing Drone " + static_cast(argv[1]) + " ..." << std::endl; + if(argc != 2){ + throw; + } + + ros::init(argc, argv, "drone" + static_cast(argv[1])); + ros::NodeHandle nh; + + setup(nh, static_cast(argv[1])); + ros::Time last_request = ros::Time::now(); // used for periodic messaging + + // all subs/services/pubs + ros::Subscriber state_sub = nh.subscribe + (dPrefix +"mavros/state", 0, state_cb); + ros::Subscriber pos_sub = nh.subscribe + (dPrefix +"mavros/local_position/pose", 0, updatePose); + ros::Subscriber vel_sub = nh.subscribe + (dPrefix +"mavros/local_position/velocity", 0, updateVel); + + ros::ServiceClient arming_client = nh.serviceClient + (dPrefix +"mavros/cmd/arming"); + ros::ServiceClient set_mode_client = nh.serviceClient + (dPrefix +"mavros/set_mode"); + ros::ServiceClient end_flight_client = nh.serviceClient + (dPrefix +"mavros/cmd/command"); + + ros::Publisher local_vel_pub = nh.advertise + (dPrefix +"mavros/setpoint_velocity/cmd_vel_unstamped", 0); + ros::Publisher mav_pub = nh.advertise + (dPrefix +"mavros/setpoint_raw/local", 20); + ros::Publisher curr_targ_pub = nh.advertise + (dPrefix +"curr_target", 5); + + // mutli control sub/pub + ros::Subscriber multi_cmd_sub = nh.subscribe + (uavName + "/cmds", 0, storeCommand); + + //the setpoint publishing rate MUST be faster than 2Hz + ros::Rate rate(20.0); + + // wait for FCU connection + while(ros::ok() && !current_state.connected){ + ros::spinOnce(); + rate.sleep(); + } + std::cout << "Drone " + static_cast(argv[1]) + " Initialized!\n"; + + // state based control + for(unsigned i = 0; i < 20; ++i){ + local_pos_pub.publish(hover_position); + } + while(ros::ok() && droneState != SHUTTING_DOWN){ + if(droneState == GROUND_IDLE){ + if(ros::Time::now() - last_request > ros::Duration(5.0)){ + ROS_INFO("Ground Idling..."); + last_request = ros::Time::now(); + } + }else if(droneState == LIFTING_OFF){ + if( current_state.mode != "OFFBOARD" && + (ros::Time::now() - last_request > ros::Duration(3))){ + if( set_mode_client.call(offb_set_mode) && + offb_set_mode.response.mode_sent){ + ROS_INFO("OFFBOARD ENABLED"); + } + last_request = ros::Time::now(); + } else { + if( !current_state.armed && + (ros::Time::now() - last_request > ros::Duration(3))){ + if( arming_client.call(arm_cmd) && + arm_cmd.response.success){ + ROS_INFO("ARMED ENABLED"); + } + last_request = ros::Time::now(); + } + } + if(ros::Time::now() - last_request > ros::Duration(3)){ + last_request = ros::Time::now(); + } + local_pos_pub.publish(hover_position); + if((reachedLocation(curr_position, hover_position, .1)) && (isStationary(curr_velocity, .05))){ + hover_position.pose = curr_position.pose; + curr_target.position = hover_position.pose.position; + droneState = HOVERING; + currTrajTime = 0; + last_response.response.data = "REACHED"; + multi_info_pub.publish(last_response); + } + }else if(droneState == HOVERING){ // update hover_position before transiting to HOVERING + if(ros::Time::now() - last_request > ros::Duration(5.0)){ + ROS_INFO("HOVERING..."); + last_request = ros::Time::now(); + } + local_pos_pub.publish(hover_position); + }else if(droneState == IN_TRANSIT){ + if(curr_trajectories.size() == 0){ // no traj loaded + hover_position = curr_position; + currTrajTime = 0; + currTime = 0; + curr_target.position = hover_position.pose.position; + droneState = HOVERING; + } + else{// at least one traj planned + TrajectoryGroupedInfo* first_trajectory; + if(currTrajTime == 0){// trajectory not loaded + first_trajectory = curr_trajectories.front(); + currTrajTime = first_trajectory->trajectoryTime; + currTime = 0; + curr_target = segmentToPoint(first_trajectory->segmentThis, currTime); + mav_pub.publish(curr_target); + } + else{// trajectory loaded + if(reachedLocation(curr_position, curr_target, .35)){// if near end of target + currTime += timeStepSize; + if(currTime > currTrajTime){ + currTime = currTrajTime; + } + curr_target = segmentToPoint(first_trajectory->segmentThis, currTime); + mav_pub.publish(curr_target); + if(currTime == currTrajTime){ + curr_trajectories.pop(); + delete first_trajectory; + currTrajTime = 0; + } + } + else{ + mav_pub.publish(curr_target); + } + } + } + + if(ros::Time::now() - last_request > ros::Duration(10.0)){ + ROS_INFO("In Transit..."); + last_request = ros::Time::now(); + } + }else if(droneState == LANDING){ + if(ros::Time::now() - last_request > ros::Duration(2.0)){ + ROS_INFO("Landing..."); + last_request = ros::Time::now(); + } + } + curr_targ_pub.publish(curr_target); + ros::spinOnce(); + rate.sleep(); + } + return 0; +} +void setup(ros::NodeHandle& nodehandler, std::string droneNum){ + uavName = "drone" + droneNum; + dPrefix = "uav" + droneNum + "/"; + + hover_position.pose.position.x = curr_position.pose.position.x; + hover_position.pose.position.y = curr_position.pose.position.y; + hover_position.pose.position.z = 2; + + curr_target.position.x = curr_position.pose.position.x; + curr_target.position.y = curr_position.pose.position.y; + curr_target.position.z = 2; + + droneState = GROUND_IDLE; + multi_info_pub = nodehandler.advertise + (uavName + "/info", 0); + last_response.response.data = "NULL"; + local_pos_pub = nodehandler.advertise + (dPrefix +"mavros/setpoint_position/local", 20); + + currTime = 0; + currTrajTime = 0; + + offb_set_mode.request.custom_mode = "OFFBOARD"; + arm_cmd.request.value = true; +} + +void state_cb(const mavros_msgs::State::ConstPtr& msg){ + current_state = *msg; +} + +void updatePose(const geometry_msgs::PoseStamped::ConstPtr& inputPose){ + curr_position = *inputPose; +} + +void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose){ + curr_velocity.linear.x = inputPose->twist.linear.x; + curr_velocity.linear.y = inputPose->twist.linear.y; + curr_velocity.linear.z = inputPose->twist.linear.z; + curr_velocity.angular.x = inputPose->twist.angular.x; + curr_velocity.angular.y = inputPose->twist.angular.y; + curr_velocity.angular.z = inputPose->twist.angular.z; +} + +mav_trajectory_generation::Segment generateTraj(int bx, int by, int bz, int ex, int ey, int ez){ + Eigen::Vector3d startPoint(bx,by,bz); + Eigen::Vector3d endPoint(ex,ey,ez); + + mav_trajectory_generation::Segment::Vector segments; + mav_trajectory_generation::Vertex::Vector vertices; + const int dimension = 3; + const int derivative_to_optimize = mav_trajectory_generation::derivative_order::SNAP; + mav_trajectory_generation::Vertex start(dimension), end(dimension); + + start.makeStartOrEnd(startPoint, derivative_to_optimize); + vertices.push_back(start); + + end.makeStartOrEnd(endPoint, derivative_to_optimize); + vertices.push_back(end); + + std::vector segment_times; + const double v_max = 2.0; + const double a_max = 2.0; + segment_times = estimateSegmentTimes(vertices, v_max, a_max); + + const int N = 10;// needs to be at least 10 for snap, 8 for jerk + mav_trajectory_generation::PolynomialOptimization opt(dimension); + opt.setupFromVertices(vertices, segment_times, derivative_to_optimize); + opt.solveLinear(); + + opt.getSegments(&segments);// fills "segments" variable with segments of the path + return mav_trajectory_generation::Segment(segments[0]); +} + +void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ + ros::Time last_request = ros::Time::now(); + last_response.response.data = "RECEIVED"; + curr_message = *inputMsg; + + //handles input command + std::string inputCmd = inputMsg->command.data; + if(inputCmd == "SHUTOFF"){ + droneState = SHUTTING_DOWN; + } + else if(inputCmd == "LIFT"){ + if(droneState == GROUND_IDLE){ + //if given a point, update default_liftoff_pos + hover_position.pose = curr_position.pose; + if(inputMsg->target.z > 0){ + hover_position.pose.position.z = 2; + } + curr_target.position = hover_position.pose.position; + droneState = LIFTING_OFF; + } + else{ + ROS_INFO("LIFT Error: Drone is off the ground."); + last_response.response.data = "ERROR"; + } + } + else if(inputCmd == "STOP"){ + if(droneState == GROUND_IDLE){ + ROS_INFO("STOP Error: Drone is already stationary."); + last_response.response.data = "ERROR"; + } + else; + hover_position.pose = curr_position.pose; + curr_target.position = hover_position.pose.position; + droneState = HOVERING; + } + else if(inputCmd == "LAND"){ + if(droneState == LANDING || droneState == GROUND_IDLE){ + ROS_INFO("LAND Error: Drone is already landing/landed."); + last_response.response.data = "ERROR"; + } + else; + droneState = LANDING; + } + else if(inputCmd == "TRANSIT_ADD" || inputCmd == "TRANSIT_NEW"){ + if(droneState == GROUND_IDLE){ + ROS_INFO("TRANSIT Error: Run LIFT command first."); + last_response.response.data = "ERROR"; + } + else if(droneState == LIFTING_OFF){ + ROS_INFO("TRANSIT Error: Wait for liftoff completion."); + last_response.response.data = "ERROR"; + } + else if(droneState == LANDING){ + ROS_INFO("TRANSIT Error: Wait for landing completion."); + last_response.response.data = "ERROR"; + } + else if(inputCmd == "TRANSIT_ADD"){ + Eigen::Vector3d starting; + if(curr_trajectories.size() == 0){ + starting = Eigen::Vector3d(curr_position.pose.position.x, curr_position.pose.position.y, curr_position.pose.position.z); + ROS_INFO("ZERO START"); + } else{ + TrajectoryGroupedInfo* last_traj = curr_trajectories.back(); + starting = Eigen::Vector3d(last_traj->endPoint.pose.position.x, last_traj->endPoint.pose.position.y, last_traj->endPoint.pose.position.z); + } + Eigen::Vector3d ending(inputMsg->target.x, inputMsg->target.y, inputMsg->target.z); + mav_trajectory_generation::Segment outputSegment(generateTraj(starting[0], starting[1], starting[2], ending[0], ending[1], ending[2])); + + geometry_msgs::PoseStamped startingPoint; + startingPoint.pose.position.x = starting[0]; + startingPoint.pose.position.y = starting[1]; + startingPoint.pose.position.z = starting[2]; + geometry_msgs::PoseStamped endingPoint; + endingPoint.pose.position.x = ending[0]; + endingPoint.pose.position.y = ending[1]; + endingPoint.pose.position.z = ending[2]; + + TrajectoryGroupedInfo* outputGroupedInfo = new TrajectoryGroupedInfo(outputSegment, startingPoint, endingPoint, static_cast(outputSegment.getTime())); + curr_trajectories.push(outputGroupedInfo); + + droneState = IN_TRANSIT; + } + else{ + // setup transit new + hover_position.pose.position.x = curr_position.pose.position.x; + hover_position.pose.position.y = curr_position.pose.position.y; + hover_position.pose.position.z = curr_position.pose.position.z; + //hover for a little bit, to stablize before new trajectory. + ros::Rate temprate(20.0); + while(!isFlat(curr_position,.05) && !isStationary(curr_velocity, .05, .05)){ + local_pos_pub.publish(hover_position); + ROS_INFO("STABLIZING"); + ros::spinOnce(); + temprate.sleep(); + } + while(curr_trajectories.size() != 0){ + TrajectoryGroupedInfo* first_traj = curr_trajectories.front(); + delete first_traj; + curr_trajectories.pop(); + } + currTrajTime = 0; + currTime = 0; + + Eigen::Vector3d starting(curr_position.pose.position.x, curr_position.pose.position.y, curr_position.pose.position.z); + Eigen::Vector3d ending(inputMsg->target.x, inputMsg->target.y, inputMsg->target.z); + mav_trajectory_generation::Segment outputSegment(generateTraj(starting[0], starting[1], starting[2], ending[0], ending[1], ending[2])); + + geometry_msgs::PoseStamped startingPoint; + startingPoint.pose.position.x = starting[0]; + startingPoint.pose.position.y = starting[1]; + startingPoint.pose.position.z = starting[2]; + geometry_msgs::PoseStamped endingPoint; + endingPoint.pose.position.x = ending[0]; + endingPoint.pose.position.y = ending[1]; + endingPoint.pose.position.z = ending[2]; + + TrajectoryGroupedInfo* outputGroupedInfo = new TrajectoryGroupedInfo(outputSegment, startingPoint, endingPoint, static_cast(outputSegment.getTime())); + curr_trajectories.push(outputGroupedInfo); + + droneState = IN_TRANSIT; + last_response.response.data = "RECIEVED"; + } + } + else{ + ROS_INFO("Error: Invalid Command."); + last_response.response.data = "ERROR"; + } + multi_info_pub.publish(last_response); +} + +// void printTrajInfo(mav_trajectory_generation::Trajectory trajectory){ +// // const mav_trajectory_generation::Segment::Vector& allSegments +// // mav_trajectory_generation::Trajectory trajectory; +// // trajectory.setSegments(allSegments);// dont use addSegments, uncessary calculations +// std::cout << "Trajectory Properties:\n" << std::endl; +// std::cout << "Number of Dimensions : "<