From 3164685e4635bce9c90fe8076dfa205f060e977d Mon Sep 17 00:00:00 2001 From: IseanB Date: Mon, 8 Aug 2022 12:49:40 -0700 Subject: [PATCH 01/33] Added launch folder --- launch/oneDrone.launch | 54 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) create mode 100644 launch/oneDrone.launch 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 From 97edadfe135f64b417b16dcef108c2a2707bfc75 Mon Sep 17 00:00:00 2001 From: IseanB Date: Mon, 8 Aug 2022 12:50:07 -0700 Subject: [PATCH 02/33] Cleaned up .xml and deleted comments --- package.xml | 19 ++++++------------- test/test_helperFunc.cpp | 13 +------------ 2 files changed, 7 insertions(+), 25 deletions(-) diff --git a/package.xml b/package.xml index b247b9e..feb94ad 100644 --- a/package.xml +++ b/package.xml @@ -50,23 +50,16 @@ catkin - roscpp - rospy - std_msgs - - - roscpp - rospy - std_msgs + ros + roscpp + rospy - roscpp - rospy - std_msgs - geometry_msgs mavros_msgs - ros + std_msgs + mav_trajectory_generation + diff --git a/test/test_helperFunc.cpp b/test/test_helperFunc.cpp index 874f01c..1410709 100644 --- a/test/test_helperFunc.cpp +++ b/test/test_helperFunc.cpp @@ -445,15 +445,4 @@ TEST(reachedLocation_PoseStamped, negValues){ int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} - -/* -Makes sures this_pos is not tilted more than maxTilt, degrees, in any axis -bool isFlat(const geometry_msgs::Pose this_pos, float maxTilt); - -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); - -Finds distance from curr_pos to desired_pos and checks it's within a accuracyDistance -bool reachedLocation(const geometry_msgs::Pose this_pos, const geometry_msgs::PoseStamped desired_pos, float accuracyDistance); -*/ \ No newline at end of file +} \ No newline at end of file From fed2d6c7be8734a90e1b054d102b859bce59edfc Mon Sep 17 00:00:00 2001 From: IseanB Date: Mon, 8 Aug 2022 14:22:23 -0700 Subject: [PATCH 03/33] Multi-Drone launch file added --- launch/fourDrones.launch | 144 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 144 insertions(+) create mode 100644 launch/fourDrones.launch diff --git a/launch/fourDrones.launch b/launch/fourDrones.launch new file mode 100644 index 0000000..a3e1708 --- /dev/null +++ b/launch/fourDrones.launch @@ -0,0 +1,144 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From fc2b46cc5d711b05e0381e96afd21bc7872b16f5 Mon Sep 17 00:00:00 2001 From: IseanB Date: Mon, 8 Aug 2022 14:27:50 -0700 Subject: [PATCH 04/33] Organized dependences --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index feb94ad..be03b19 100644 --- a/package.xml +++ b/package.xml @@ -55,9 +55,9 @@ rospy geometry_msgs - mavros_msgs std_msgs + mavros mav_trajectory_generation From 5bbcc3442c78b9edbced7fb61845cd039d24f4dd Mon Sep 17 00:00:00 2001 From: IseanB Date: Mon, 8 Aug 2022 14:28:55 -0700 Subject: [PATCH 05/33] Comments update --- launch/fourDrones.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/fourDrones.launch b/launch/fourDrones.launch index a3e1708..2076976 100644 --- a/launch/fourDrones.launch +++ b/launch/fourDrones.launch @@ -1,7 +1,7 @@ - + From dd82d97dfb04783aca4ca5b0ccb456a12b62c8a2 Mon Sep 17 00:00:00 2001 From: IseanB Date: Wed, 10 Aug 2022 14:09:29 -0700 Subject: [PATCH 06/33] Renamed files for organization --- CMakeLists.txt | 19 +++++++++++++++---- package.xml | 6 +++--- src/{offboard.cpp => src/oneDrone.cpp} | 6 +++--- 3 files changed, 21 insertions(+), 10 deletions(-) rename src/{offboard.cpp => src/oneDrone.cpp} (98%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5553df8..d7a3b77 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) @@ -141,10 +141,16 @@ 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(oneDrone_node helper/src/conversions.cpp helper/src/computations.cpp - src/offboard.cpp) + helper/src/single_control.cpp + src/src/oneDrone.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 @@ -157,10 +163,15 @@ add_executable(offboard_node # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against -target_link_libraries(offboard_node +target_link_libraries(oneDrone_node ${catkin_LIBRARIES} ) +# target_link_libraries(singleDroneControl_node +# ${catkin_LIBRARIES} +# ) + + ############# ## Install ## ############# diff --git a/package.xml b/package.xml index be03b19..fa29895 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 diff --git a/src/offboard.cpp b/src/src/oneDrone.cpp similarity index 98% rename from src/offboard.cpp rename to src/src/oneDrone.cpp index 9f7250b..b73c01d 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 @@ -37,7 +37,7 @@ void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose); 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, "only_drone_node"); setup(); ros::NodeHandle nh; PossiableState droneState = LIFTING_OFF; From 77ea375df96390c8d59a5733ea226259c6950f8d Mon Sep 17 00:00:00 2001 From: IseanB Date: Fri, 12 Aug 2022 21:55:06 -0700 Subject: [PATCH 07/33] Added interfacing code for single drone --- helper/single_control.h | 76 +++++++++++++++++++++++++++++++++++ helper/src/single_control.cpp | 10 +++++ 2 files changed, 86 insertions(+) create mode 100644 helper/single_control.h create mode 100644 helper/src/single_control.cpp diff --git a/helper/single_control.h b/helper/single_control.h new file mode 100644 index 0000000..a8c1f6f --- /dev/null +++ b/helper/single_control.h @@ -0,0 +1,76 @@ +#ifndef SINGLE_CONTROL_H +#define SINGLE_CONTROL_H + +/* Custom files */ +#include "conversions.h" +#include "computations.h" + +/* Libraries */ +#include + +/* Data Types */ +#include +#include +#include +#include + +enum PossiableState{ + IDLE, + LIFTING_OFF, + IN_TRANSIT, + WAITING, + LANDING, + LANDED +}; + +enum PointType{ + RELATIVE, + GLOBAL +}; + +class SingleDroneControl{ + public: + SingleDroneControl(float max_velocity, float max_acceleration, int droneID = -1); + + /*Long Control*/ + // void addTrajectory(/*vector of points*/);/**/ + // void skipCurrTrajectory(); //halts trajectory and goes immediate to start of next trajectory + // void clearTrajectories(); + // void nextTraj(); //Executes next trajectory, if not near starting point of next traj it will go to that start. + + + /*Quick Control*/ + // void goToPoint(/*One Point, RELATIVE OR NOT*/); + // void goToPoints(/*Vector of Points, RELATIVE OR NOT*/); + // void haltTraj(); //Halts current trajectory immediately, and doesn't go to the next trajectory. + + /*Takes control and executes a command*/ + bool liftOff(); + bool landNow(); + void shutOff(); + + + //Modifiers + void changeMaxVelocity(float input_acc); + void changeMaxAcceleration(float input_acc); + + //Accessors + geometry_msgs::Twist currPosition() const; + geometry_msgs::Pose currVelocity() const; + int numberOfTrajs() const; + + private: + float default_max_velocity; + float default_max_acceleration; + mav_trajectory_generation::Trajectory* current_trajectory; + // queue trajectory_queue; + //No nodes, just adversite a single message + + geometry_msgs::Pose* curr_position; + geometry_msgs::Twist* curr_velocity; + PossiableState curr_state; + + mav_trajectory_generation::Trajectory* calculateTrajectory(float max_vel = -1, float max_accel = -1); +}; + +#endif \ No newline at end of file diff --git a/helper/src/single_control.cpp b/helper/src/single_control.cpp new file mode 100644 index 0000000..0482d53 --- /dev/null +++ b/helper/src/single_control.cpp @@ -0,0 +1,10 @@ +#include "../single_control.h" + +SingleDroneControl::SingleDroneControl(float max_velocity, float max_acceleration, int droneID) : + default_max_velocity(max_velocity), + default_max_acceleration(max_acceleration), + current_trajectory(nullptr), + curr_position(nullptr), + curr_state(IDLE){ + if(droneID == -1){} +} \ No newline at end of file From 9b34a077bf25a2e33160fa314b8f387919e92b63 Mon Sep 17 00:00:00 2001 From: IseanB Date: Fri, 12 Aug 2022 21:55:38 -0700 Subject: [PATCH 08/33] Fixed MAVROS node issue --- CMakeLists.txt | 2 +- launch/fourDrones.launch | 46 +++++++--------------------------------- src/src/oneDrone.cpp | 20 ++++++++--------- 3 files changed, 19 insertions(+), 49 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d7a3b77..e624565 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -144,7 +144,7 @@ include_directories( add_executable(oneDrone_node helper/src/conversions.cpp helper/src/computations.cpp - helper/src/single_control.cpp + # helper/src/single_control.cpp src/src/oneDrone.cpp) # add_executable(singleDroneControl_node diff --git a/launch/fourDrones.launch b/launch/fourDrones.launch index 2076976..c699d72 100644 --- a/launch/fourDrones.launch +++ b/launch/fourDrones.launch @@ -1,7 +1,7 @@ - + @@ -26,8 +26,8 @@ - - + + @@ -55,8 +55,8 @@ - - + + @@ -84,8 +84,8 @@ - - + + @@ -106,39 +106,9 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file +Set the malink_udp_port to 14560+id) --> diff --git a/src/src/oneDrone.cpp b/src/src/oneDrone.cpp index b73c01d..5d98f9b 100644 --- a/src/src/oneDrone.cpp +++ b/src/src/oneDrone.cpp @@ -37,31 +37,31 @@ void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose); void printTrajInfo(const mav_trajectory_generation::Segment::Vector& allSegments); int main(int argc, char **argv){ - ros::init(argc, argv, "only_drone_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 From 4d4c43eea9eca2cf71b663c2d14e21f4bfc83a3a Mon Sep 17 00:00:00 2001 From: Isean Bhanot Date: Sat, 13 Aug 2022 11:16:21 -0700 Subject: [PATCH 09/33] Clarified Readme instructions Created a new environment and went through all of the steps, making sure they are correct and clarified how to avoid erros. --- README.md | 33 +++++++++++++++++++++------------ 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index 013a8b2..68b707c 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,8 +50,8 @@ 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 @@ -65,6 +72,8 @@ The test folder contains tests for the helper functions in the helper folder. So ## Usage ### Launching World in Gazebo +Tip: Before launch run ```echo "export SVGA_VGPU10=0" >> ~/.bashrc``` and ```source ~/.bashrc```, to prevent VMWare REST Error below. [^8] + ```roslaunch px4 mavros_posix_sitl.launch``` ### Running offboard_node @@ -113,5 +122,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) From fb6ea457d773450ab09a878c0beb7b174f9ce1a8 Mon Sep 17 00:00:00 2001 From: IseanB Date: Wed, 17 Aug 2022 17:22:00 -0700 Subject: [PATCH 10/33] Changed structure from encapsulation to new nodes --- helper/single_control.h | 76 ----------------------------- helper/src/single_control.cpp | 10 ---- src/src/singleDrone.cpp | 91 +++++++++++++++++++++++++++++++++++ 3 files changed, 91 insertions(+), 86 deletions(-) delete mode 100644 helper/single_control.h delete mode 100644 helper/src/single_control.cpp create mode 100644 src/src/singleDrone.cpp diff --git a/helper/single_control.h b/helper/single_control.h deleted file mode 100644 index a8c1f6f..0000000 --- a/helper/single_control.h +++ /dev/null @@ -1,76 +0,0 @@ -#ifndef SINGLE_CONTROL_H -#define SINGLE_CONTROL_H - -/* Custom files */ -#include "conversions.h" -#include "computations.h" - -/* Libraries */ -#include - -/* Data Types */ -#include -#include -#include -#include - -enum PossiableState{ - IDLE, - LIFTING_OFF, - IN_TRANSIT, - WAITING, - LANDING, - LANDED -}; - -enum PointType{ - RELATIVE, - GLOBAL -}; - -class SingleDroneControl{ - public: - SingleDroneControl(float max_velocity, float max_acceleration, int droneID = -1); - - /*Long Control*/ - // void addTrajectory(/*vector of points*/);/**/ - // void skipCurrTrajectory(); //halts trajectory and goes immediate to start of next trajectory - // void clearTrajectories(); - // void nextTraj(); //Executes next trajectory, if not near starting point of next traj it will go to that start. - - - /*Quick Control*/ - // void goToPoint(/*One Point, RELATIVE OR NOT*/); - // void goToPoints(/*Vector of Points, RELATIVE OR NOT*/); - // void haltTraj(); //Halts current trajectory immediately, and doesn't go to the next trajectory. - - /*Takes control and executes a command*/ - bool liftOff(); - bool landNow(); - void shutOff(); - - - //Modifiers - void changeMaxVelocity(float input_acc); - void changeMaxAcceleration(float input_acc); - - //Accessors - geometry_msgs::Twist currPosition() const; - geometry_msgs::Pose currVelocity() const; - int numberOfTrajs() const; - - private: - float default_max_velocity; - float default_max_acceleration; - mav_trajectory_generation::Trajectory* current_trajectory; - // queue trajectory_queue; - //No nodes, just adversite a single message - - geometry_msgs::Pose* curr_position; - geometry_msgs::Twist* curr_velocity; - PossiableState curr_state; - - mav_trajectory_generation::Trajectory* calculateTrajectory(float max_vel = -1, float max_accel = -1); -}; - -#endif \ No newline at end of file diff --git a/helper/src/single_control.cpp b/helper/src/single_control.cpp deleted file mode 100644 index 0482d53..0000000 --- a/helper/src/single_control.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "../single_control.h" - -SingleDroneControl::SingleDroneControl(float max_velocity, float max_acceleration, int droneID) : - default_max_velocity(max_velocity), - default_max_acceleration(max_acceleration), - current_trajectory(nullptr), - curr_position(nullptr), - curr_state(IDLE){ - if(droneID == -1){} -} \ No newline at end of file diff --git a/src/src/singleDrone.cpp b/src/src/singleDrone.cpp new file mode 100644 index 0000000..cbd8619 --- /dev/null +++ b/src/src/singleDrone.cpp @@ -0,0 +1,91 @@ +#include +#include +#include +#include +#include + +mavros_msgs::State current_state; +void state_cb(const mavros_msgs::State::ConstPtr& msg){ + current_state = *msg; +} + +int main(int argc, char **argv) +{ + // initalizing node + if(argc != 2){ + ROS_INFO("Error: Drone number required! Format: \"rosrun \" "); + throw; + } + std::string dPrefix = "uav" + static_cast(argv[1]) + "/"; + ros::init(argc, argv, "drone" + static_cast(argv[1])); + ros::NodeHandle nh; + + // all topics + ros::Publisher local_pos_pub = nh.advertise + (dPrefix + "mavros/setpoint_position/local", 10); + ros::ServiceClient arming_client = nh.serviceClient + (dPrefix + "mavros/cmd/arming"); + ros::ServiceClient set_mode_client = nh.serviceClient + (dPrefix + "mavros/set_mode"); + + //the setpoint publishing rate MUST be faster than 2Hz + ros::Rate rate(20.0); + + // wait for FCU connection + ros::Subscriber state_sub = nh.subscribe + (dPrefix + "mavros/state", 10, state_cb); + + while(ros::ok() && !current_state.connected){ + ros::spinOnce(); + rate.sleep(); + } + + + + geometry_msgs::PoseStamped pose; + pose.pose.position.x = 0; + pose.pose.position.y = 0; + pose.pose.position.z = 2; + + //send a few setpoints before starting + for(int i = 100; ros::ok() && i > 0; --i){ + local_pos_pub.publish(pose); + ros::spinOnce(); + rate.sleep(); + } + + mavros_msgs::SetMode offb_set_mode; + offb_set_mode.request.custom_mode = "OFFBOARD"; + + mavros_msgs::CommandBool arm_cmd; + arm_cmd.request.value = true; + + ros::Time last_request = ros::Time::now(); + + while(ros::ok()){ + if( current_state.mode != "OFFBOARD" && + (ros::Time::now() - last_request > ros::Duration(5.0))){ + 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(5.0))){ + if( arming_client.call(arm_cmd) && + arm_cmd.response.success){ + ROS_INFO("Vehicle armed"); + } + last_request = ros::Time::now(); + } + } + + local_pos_pub.publish(pose); + + ros::spinOnce(); + rate.sleep(); + } + + return 0; +} \ No newline at end of file From 27b14ad7aacad6bc7f617d1c2cfb1992c7e2680c Mon Sep 17 00:00:00 2001 From: IseanB Date: Wed, 17 Aug 2022 17:22:36 -0700 Subject: [PATCH 11/33] Clarified naming --- ...urDrones.launch => fourDronesNodes.launch} | 41 ++++++++++++++++--- 1 file changed, 35 insertions(+), 6 deletions(-) rename launch/{fourDrones.launch => fourDronesNodes.launch} (75%) diff --git a/launch/fourDrones.launch b/launch/fourDronesNodes.launch similarity index 75% rename from launch/fourDrones.launch rename to launch/fourDronesNodes.launch index c699d72..78e01a5 100644 --- a/launch/fourDrones.launch +++ b/launch/fourDronesNodes.launch @@ -26,8 +26,8 @@ - - + + @@ -55,8 +55,8 @@ - - + + @@ -84,8 +84,8 @@ - - + + @@ -106,6 +106,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + message_runtime From 119b7fc2877e8d567477280c1ac8e00eb66481a7 Mon Sep 17 00:00:00 2001 From: IseanB Date: Fri, 19 Aug 2022 12:22:30 -0700 Subject: [PATCH 19/33] Basic working version with new custom message --- src/src/multiDrone.cpp | 21 +++++------------- src/src/singleDrone.cpp | 49 +++++++++++++++++++++-------------------- 2 files changed, 31 insertions(+), 39 deletions(-) diff --git a/src/src/multiDrone.cpp b/src/src/multiDrone.cpp index 11fafed..856d70a 100644 --- a/src/src/multiDrone.cpp +++ b/src/src/multiDrone.cpp @@ -1,5 +1,5 @@ #include -#include +#include "drone_control/dcontrol.h" int main(int argc, char **argv) { @@ -8,17 +8,14 @@ int main(int argc, char **argv) ros::Time last_request = ros::Time::now(); ros::Rate rate(20.0); - ros::Publisher drone_cmd_pub = nh.advertise - ("drone_cmd", 0); - - ros::Publisher drone_pos_pub = nh.advertise - ("drone1/target", 0); + ros::Publisher drone_cmd_pub = nh.advertise + ("drone1_cmds", 0); - std_msgs::String msg; + drone_control::dcontrol msg; while(ros::ok()){ if(ros::Time::now() - last_request > ros::Duration(10.0)){ ROS_INFO("info sent"); - msg.data = "LIFT"; + msg.command.data = "LIFT"; drone_cmd_pub.publish(msg); last_request = ros::Time::now(); } @@ -27,10 +24,4 @@ int main(int argc, char **argv) } return 0; -} - -/* -LIFT -STOP -LAND -*/ \ No newline at end of file +} \ No newline at end of file diff --git a/src/src/singleDrone.cpp b/src/src/singleDrone.cpp index 4ce6343..72824d9 100644 --- a/src/src/singleDrone.cpp +++ b/src/src/singleDrone.cpp @@ -11,6 +11,7 @@ #include #include #include +#include "drone_control/dcontrol.h" enum PossiableState{ GROUND_IDLE, @@ -25,6 +26,7 @@ mavros_msgs::State current_state; geometry_msgs::Pose curr_position; geometry_msgs::Twist curr_velocity; PossiableState droneState = GROUND_IDLE; +drone_control::dcontrol curr_message; /* Assumes drone is on ground. Initializes curr_position & curr_velocity values to 0 */ void setup(); @@ -41,8 +43,8 @@ void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose); /* Prints crutial information about the inputted trajectory */ // void printTrajInfo(const mav_trajectory_generation::Segment::Vector& allSegments); -/* Stores msgs from multi control node IN */ -void storeCommand(const std_msgs::String::ConstPtr& inputMsg); +/* Stores msgs from multi control node in last*/ +void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg); int main(int argc, char **argv) { @@ -81,11 +83,9 @@ int main(int argc, char **argv) ros::Publisher mav_pub = nh.advertise (dPrefix +"mavros/setpoint_raw/local", 20); - // subscribes to mutli_control - ros::Subscriber command_sub = nh.subscribe - ("drones_cmd", 0, storeCommand); - ros::Subscriber target_sub = nh.subscribe - ("drone" + static_cast(argv[1]) +"/target", 0, store); + // mutli control subscribers + ros::Subscriber multi_sub = nh.subscribe + ("drone1_cmds", 0, storeCommand); //the setpoint publishing rate MUST be faster than 2Hz ros::Rate rate(20.0); @@ -172,36 +172,37 @@ void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose){ curr_velocity.angular.z = inputPose->twist.angular.z; } -void storeCommand(const std_msgs::String::ConstPtr& inputMsg){ - if(inputMsg->data == "LIFT"){ - if(droneState == GROUND_IDLE); +void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ + std::string inputCmd = inputMsg->command.data; + if(inputCmd == "E_STOP"){ + droneState = SHUTTING_DOWN; + } + else if(inputCmd == "LIFT"){ + if(droneState == GROUND_IDLE){ droneState = LIFTING_OFF; - else; + } + else{ ROS_INFO("LIFT Error: Drone is off the ground."); + } } - else if(inputMsg->data == "STOP"){ - if(droneState == GROUND_IDLE || droneState == HOVERING); + else if(inputCmd == "STOP"){ + if(droneState == GROUND_IDLE){ ROS_INFO("STOP Error: Drone is already stationary."); + } else; droneState = HOVERING; } - else if(inputMsg->data == "LAND"){ - if(droneState == LANDING || droneState == GROUND_IDLE); + else if(inputCmd == "LAND"){ + if(droneState == LANDING || droneState == GROUND_IDLE){ ROS_INFO("LAND Error: Drone is already landing/landed."); + } else; droneState = LANDING; } - else; + else{ ROS_INFO("Error: Invalid Command."); + } } -// enum PossiableState{ -// GROUND_IDLE, -// LIFTING_OFF, -// IN_TRANSIT, -// HOVERING, -// LANDING, -// SHUTTING_DOWN -// }; // void printTrajInfo(const mav_trajectory_generation::Segment::Vector& allSegments){ // mav_trajectory_generation::Trajectory trajectory; From 4b6275fcfb00a1a8d16e7e8c7abf0da81616648d Mon Sep 17 00:00:00 2001 From: IseanB Date: Fri, 19 Aug 2022 21:13:20 -0700 Subject: [PATCH 20/33] Added new msg --- msg/dresponse.msg | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 msg/dresponse.msg diff --git a/msg/dresponse.msg b/msg/dresponse.msg new file mode 100644 index 0000000..8aef1e6 --- /dev/null +++ b/msg/dresponse.msg @@ -0,0 +1,2 @@ +# Responses: States, ERROR, RECIVED, REACHED +std_msgs/String response \ No newline at end of file From b86844a33e8239d4593ca6ff83d8478369e666a9 Mon Sep 17 00:00:00 2001 From: IseanB Date: Fri, 19 Aug 2022 21:14:50 -0700 Subject: [PATCH 21/33] Stopped compiling oneDrone, changed helper function --- CMakeLists.txt | 14 ++++---------- helper/computations.h | 1 + helper/src/computations.cpp | 8 ++++---- 3 files changed, 9 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5e8bab2..932793f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,6 +54,7 @@ find_package(catkin REQUIRED COMPONENTS add_message_files( FILES dcontrol.msg + dresponse.msg ) ## Generate services in the 'srv' folder @@ -142,10 +143,6 @@ 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(oneDrone_node - helper/src/conversions.cpp - helper/src/computations.cpp - src/src/oneDrone.cpp) add_executable(multi_control helper/src/conversions.cpp @@ -173,9 +170,6 @@ add_executable(single_control add_dependencies(multi_control ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against -target_link_libraries(oneDrone_node - ${catkin_LIBRARIES} -) target_link_libraries(multi_control ${catkin_LIBRARIES} @@ -238,9 +232,9 @@ target_link_libraries(single_control ############# ## 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/helper/computations.h b/helper/computations.h index 12cfda5..b88e2c7 100644 --- a/helper/computations.h +++ b/helper/computations.h @@ -17,6 +17,7 @@ bool isFlat(const geometry_msgs::Pose& this_pos, float maxTilt = 0); 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 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); diff --git a/helper/src/computations.cpp b/helper/src/computations.cpp index d58a10c..50f4166 100644 --- a/helper/src/computations.cpp +++ b/helper/src/computations.cpp @@ -16,12 +16,12 @@ 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 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){ From 794194f1248cb0fa02d1661424dd4fcd7c5f6ac5 Mon Sep 17 00:00:00 2001 From: IseanB Date: Fri, 19 Aug 2022 21:15:27 -0700 Subject: [PATCH 22/33] Finished Lift off State --- src/src/multiDrone.cpp | 3 +- src/src/singleDrone.cpp | 106 ++++++++++++++++++++++++++++++++-------- 2 files changed, 88 insertions(+), 21 deletions(-) diff --git a/src/src/multiDrone.cpp b/src/src/multiDrone.cpp index 856d70a..9cc2307 100644 --- a/src/src/multiDrone.cpp +++ b/src/src/multiDrone.cpp @@ -12,10 +12,11 @@ int main(int argc, char **argv) ("drone1_cmds", 0); drone_control::dcontrol msg; + msg.command.data = "LIFT"; + msg.target.z = 1; while(ros::ok()){ if(ros::Time::now() - last_request > ros::Duration(10.0)){ ROS_INFO("info sent"); - msg.command.data = "LIFT"; drone_cmd_pub.publish(msg); last_request = ros::Time::now(); } diff --git a/src/src/singleDrone.cpp b/src/src/singleDrone.cpp index 72824d9..469367c 100644 --- a/src/src/singleDrone.cpp +++ b/src/src/singleDrone.cpp @@ -16,18 +16,27 @@ enum PossiableState{ GROUND_IDLE, LIFTING_OFF, - IN_TRANSIT, HOVERING, + IN_TRANSIT, LANDING, SHUTTING_DOWN // A drone is only momentarily in this state }; +//Must use commands to start,end. Add point + mavros_msgs::State current_state; -geometry_msgs::Pose curr_position; +geometry_msgs::PoseStamped curr_position; geometry_msgs::Twist curr_velocity; -PossiableState droneState = GROUND_IDLE; +geometry_msgs::PoseStamped curr_target; drone_control::dcontrol curr_message; +geometry_msgs::PoseStamped default_liftoff_pos; +mavros_msgs::SetMode offb_set_mode; +mavros_msgs::CommandBool arm_cmd; + +PossiableState droneState; + + /* Assumes drone is on ground. Initializes curr_position & curr_velocity values to 0 */ void setup(); @@ -109,16 +118,48 @@ int main(int argc, char **argv) ROS_INFO("Lifting Off..."); last_request = ros::Time::now(); } - }else if(droneState == IN_TRANSIT){ - if(ros::Time::now() - last_request > ros::Duration(30.0)){ - ROS_INFO("In Transit..."); + 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(1.5))){ + if( arming_client.call(arm_cmd) && + arm_cmd.response.success){ + ROS_INFO("Vehicle armed"); + } + last_request = ros::Time::now(); + } + } + + if(curr_target.pose.position.x == 0 && curr_target.pose.position.y == 0 && curr_target.pose.position.z == 0){ + local_pos_pub.publish(default_liftoff_pos); + if((reachedLocation(curr_position, default_liftoff_pos, .1)) && (isStationary(curr_velocity, .05))){ + droneState = HOVERING; + } + } + else{ + local_pos_pub.publish(curr_target); + if((reachedLocation(curr_position, curr_target, .1)) && (isStationary(curr_velocity, .05))){ + droneState = HOVERING; + } } + }else if(droneState == HOVERING){ if(ros::Time::now() - last_request > ros::Duration(5.0)){ ROS_INFO("Hovering..."); last_request = ros::Time::now(); } + + }else if(droneState == IN_TRANSIT){ + if(ros::Time::now() - last_request > ros::Duration(30.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..."); @@ -133,13 +174,13 @@ int main(int argc, char **argv) return 0; } void setup(){ - curr_position.position.x = 0; - curr_position.position.y = 0; - curr_position.position.z = 0; - curr_position.orientation.x = 0; - curr_position.orientation.y = 0; - curr_position.orientation.z = 0; - curr_position.orientation.w = 0; + curr_position.pose.position.x = 0; + curr_position.pose.position.y = 0; + curr_position.pose.position.z = 0; + curr_position.pose.orientation.x = 0; + curr_position.pose.orientation.y = 0; + curr_position.pose.orientation.z = 0; + curr_position.pose.orientation.w = 0; curr_velocity.linear.x = 0; curr_velocity.linear.y = 0; @@ -147,6 +188,19 @@ void setup(){ curr_velocity.angular.x = 0; curr_velocity.angular.y = 0; curr_velocity.angular.z = 0; + + curr_target.pose.position.x = 0; + curr_target.pose.position.y = 0; + curr_target.pose.position.z = 0; + + droneState = GROUND_IDLE; + + default_liftoff_pos.pose.position.x = curr_position.pose.position.x; + default_liftoff_pos.pose.position.y = curr_position.pose.position.y; + default_liftoff_pos.pose.position.z = 2; + + offb_set_mode.request.custom_mode = "OFFBOARD"; + arm_cmd.request.value = true; } void state_cb(const mavros_msgs::State::ConstPtr& msg){ @@ -154,13 +208,14 @@ void state_cb(const mavros_msgs::State::ConstPtr& msg){ } void updatePose(const geometry_msgs::PoseStamped::ConstPtr& inputPose){ - curr_position.position.x = (inputPose->pose).position.x; - curr_position.position.y = (inputPose->pose).position.y; - curr_position.position.z = (inputPose->pose).position.z; - curr_position.orientation.x = (inputPose->pose).orientation.x; - curr_position.orientation.y = (inputPose->pose).orientation.y; - curr_position.orientation.z = (inputPose->pose).orientation.z; - curr_position.orientation.w = (inputPose->pose).orientation.w; + curr_position = *inputPose; + // curr_position.pose.position.x = (inputPose->pose).position.x; + // curr_position.pose.position.y = (inputPose->pose).position.y; + // curr_position.pose.position.z = (inputPose->pose).position.z; + // curr_position.pose.orientation.x = (inputPose->pose).orientation.x; + // curr_position.pose.orientation.y = (inputPose->pose).orientation.y; + // curr_position.pose.orientation.z = (inputPose->pose).orientation.z; + // curr_position.pose.orientation.w = (inputPose->pose).orientation.w; } void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose){ @@ -173,6 +228,17 @@ void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose){ } void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ + curr_message = *inputMsg; + //handles input target + if(inputMsg->target.x != curr_target.pose.position.x || + inputMsg->target.y != curr_target.pose.position.y || + inputMsg->target.z != curr_target.pose.position.z){ + curr_target.pose.position.x = inputMsg->target.x; + curr_target.pose.position.y = inputMsg->target.y; + curr_target.pose.position.z = inputMsg->target.z; + } + + //handles input command std::string inputCmd = inputMsg->command.data; if(inputCmd == "E_STOP"){ droneState = SHUTTING_DOWN; From a056f6ee4684e99a3f1a316c9771c5768abc523d Mon Sep 17 00:00:00 2001 From: IseanB Date: Fri, 19 Aug 2022 21:41:17 -0700 Subject: [PATCH 23/33] Working Hovering, fixed few issues --- msg/dcontrol.msg | 2 +- src/src/singleDrone.cpp | 28 +++++++++++++++------------- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/msg/dcontrol.msg b/msg/dcontrol.msg index 548f7ad..70b9eb9 100644 --- a/msg/dcontrol.msg +++ b/msg/dcontrol.msg @@ -1,3 +1,3 @@ -# Commands: ESTOP, STOP, LIFT, LAND +# Commands: ESTOP, STOP, LIFT, LAND, CHECK std_msgs/String command geometry_msgs/Point target \ No newline at end of file diff --git a/src/src/singleDrone.cpp b/src/src/singleDrone.cpp index 469367c..2405aff 100644 --- a/src/src/singleDrone.cpp +++ b/src/src/singleDrone.cpp @@ -26,6 +26,7 @@ enum PossiableState{ mavros_msgs::State current_state; geometry_msgs::PoseStamped curr_position; +geometry_msgs::PoseStamped hover_position; geometry_msgs::Twist curr_velocity; geometry_msgs::PoseStamped curr_target; drone_control::dcontrol curr_message; @@ -68,6 +69,7 @@ int main(int argc, char **argv) setup(); std::string dPrefix = "uav" + static_cast(argv[1]) + "/"; + std::string uavName = "drone" + static_cast(argv[1]); ros::Time last_request = ros::Time::now(); // used for periodic messaging // all topics @@ -94,7 +96,7 @@ int main(int argc, char **argv) // mutli control subscribers ros::Subscriber multi_sub = nh.subscribe - ("drone1_cmds", 0, storeCommand); + (uavName + "_cmds", 0, storeCommand); //the setpoint publishing rate MUST be faster than 2Hz ros::Rate rate(20.0); @@ -114,23 +116,18 @@ int main(int argc, char **argv) last_request = ros::Time::now(); } }else if(droneState == LIFTING_OFF){ - if(ros::Time::now() - last_request > ros::Duration(5.0)){ - ROS_INFO("Lifting Off..."); - last_request = ros::Time::now(); - } 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(1.5))){ + (ros::Time::now() - last_request > ros::Duration(3))){ if( arming_client.call(arm_cmd) && arm_cmd.response.success){ - ROS_INFO("Vehicle armed"); + ROS_INFO(uavName, " armed"); } last_request = ros::Time::now(); } @@ -139,22 +136,22 @@ int main(int argc, char **argv) if(curr_target.pose.position.x == 0 && curr_target.pose.position.y == 0 && curr_target.pose.position.z == 0){ local_pos_pub.publish(default_liftoff_pos); if((reachedLocation(curr_position, default_liftoff_pos, .1)) && (isStationary(curr_velocity, .05))){ + hover_position = default_liftoff_pos; droneState = HOVERING; + ROS(uavName, " Hovering"); } } else{ local_pos_pub.publish(curr_target); if((reachedLocation(curr_position, curr_target, .1)) && (isStationary(curr_velocity, .05))){ + hover_position = curr_target; droneState = HOVERING; + ROS(uavName, " Hovering"); } } }else if(droneState == 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(ros::Time::now() - last_request > ros::Duration(30.0)){ ROS_INFO("In Transit..."); @@ -193,6 +190,10 @@ void setup(){ curr_target.pose.position.y = 0; curr_target.pose.position.z = 0; + hover_position.pose.position.x = 0; + hover_position.pose.position.y = 0; + hover_position.pose.position.z = 0; + droneState = GROUND_IDLE; default_liftoff_pos.pose.position.x = curr_position.pose.position.x; @@ -256,6 +257,7 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ ROS_INFO("STOP Error: Drone is already stationary."); } else; + hover_position = curr_position; droneState = HOVERING; } else if(inputCmd == "LAND"){ From c77dfe0f859a98b39fab333d9266c2dbdc9e79c7 Mon Sep 17 00:00:00 2001 From: IseanB Date: Sat, 20 Aug 2022 16:51:49 -0700 Subject: [PATCH 24/33] Added more clarity to msgs --- msg/dcontrol.msg | 2 +- msg/dresponse.msg | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/msg/dcontrol.msg b/msg/dcontrol.msg index 70b9eb9..1b13700 100644 --- a/msg/dcontrol.msg +++ b/msg/dcontrol.msg @@ -1,3 +1,3 @@ -# Commands: ESTOP, STOP, LIFT, LAND, CHECK +# 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 index 8aef1e6..6d515c8 100644 --- a/msg/dresponse.msg +++ b/msg/dresponse.msg @@ -1,2 +1,2 @@ -# Responses: States, ERROR, RECIVED, REACHED +# Responses: States, ERROR, RECEIVED, REACHED std_msgs/String response \ No newline at end of file From a5c1aeeaa697e496ac07693e33232a6cf5255b50 Mon Sep 17 00:00:00 2001 From: IseanB Date: Sat, 20 Aug 2022 16:52:38 -0700 Subject: [PATCH 25/33] Updated for multiple msgs compilation --- CMakeLists.txt | 6 ++++-- package.xml | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 932793f..b7ad9ea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -115,6 +115,7 @@ rospy std_msgs geometry_msgs mav_trajectory_generation +message_runtime # DEPENDS system_lib ) @@ -125,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} ) @@ -167,7 +168,8 @@ add_executable(single_control ## Add cmake target dependencies of the executable ## same as for the library above -add_dependencies(multi_control ${${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 diff --git a/package.xml b/package.xml index e858d79..4270371 100644 --- a/package.xml +++ b/package.xml @@ -37,7 +37,7 @@ - + message_generation From 5efcaad38dcdc8d17c0af470e5f3821215fb918b Mon Sep 17 00:00:00 2001 From: IseanB Date: Sat, 20 Aug 2022 16:52:55 -0700 Subject: [PATCH 26/33] Added code for transit commands --- src/src/multiDrone.cpp | 37 +++++++-- src/src/singleDrone.cpp | 178 +++++++++++++++++++++++++++++++++------- 2 files changed, 179 insertions(+), 36 deletions(-) diff --git a/src/src/multiDrone.cpp b/src/src/multiDrone.cpp index 9cc2307..5eaeb2d 100644 --- a/src/src/multiDrone.cpp +++ b/src/src/multiDrone.cpp @@ -1,5 +1,10 @@ #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) { @@ -10,19 +15,39 @@ int main(int argc, char **argv) ros::Publisher drone_cmd_pub = nh.advertise ("drone1_cmds", 0); + ros::Subscriber drone_info_sub = nh.subscribe + ("drone1_cmds", 0, storeInfo); drone_control::dcontrol msg; - msg.command.data = "LIFT"; - msg.target.z = 1; + bool done = false; while(ros::ok()){ - if(ros::Time::now() - last_request > ros::Duration(10.0)){ - ROS_INFO("info sent"); - drone_cmd_pub.publish(msg); - last_request = ros::Time::now(); + while(ros::Time::now() - last_request < ros::Duration(10.0)){ + if(ros::Time::now() - last_request >= ros::Duration(10.0)){ + ROS_INFO("info sent"); + msg.command.data = "LIFT"; + msg.target.z = 1; + drone_cmd_pub.publish(msg); + last_request = ros::Time::now(); + } + ros::spinOnce(); + rate.sleep(); + } + while(ros::Time::now() - last_request > ros::Duration(10.0)){ + if(ros::Time::now() - last_request >= ros::Duration(10.0)){ + msg.command.data = "IN_TRANSIT"; + last_request = ros::Time::now(); + } + ros::spinOnce(); + rate.sleep(); } 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/src/singleDrone.cpp b/src/src/singleDrone.cpp index 2405aff..868a916 100644 --- a/src/src/singleDrone.cpp +++ b/src/src/singleDrone.cpp @@ -12,6 +12,8 @@ #include #include #include "drone_control/dcontrol.h" +#include "drone_control/dresponse.h" +#include enum PossiableState{ GROUND_IDLE, @@ -22,24 +24,38 @@ enum PossiableState{ SHUTTING_DOWN // A drone is only momentarily in this state }; +struct TrajectoryGroupedInfo{ + mav_trajectory_generation::Trajectory trajectoryThis; + geometry_msgs::PoseStamped startPoint; + geometry_msgs::PoseStamped endPoint; + float trajectoryTime; + TrajectoryGroupedInfo(mav_trajectory_generation::Trajectory inputTraj, geometry_msgs::PoseStamped inputStart, geometry_msgs::PoseStamped inputEnd, float inputTime) : trajectoryThis(inputTraj), 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::PoseStamped hover_position; geometry_msgs::Twist curr_velocity; geometry_msgs::PoseStamped curr_target; drone_control::dcontrol curr_message; +geometry_msgs::PoseStamped hover_position; +drone_control::dresponse last_response; geometry_msgs::PoseStamped default_liftoff_pos; mavros_msgs::SetMode offb_set_mode; mavros_msgs::CommandBool arm_cmd; +std::string uavName; PossiableState droneState; +ros::Publisher multi_info_pub; +std::queue curr_trajectories; +std::queue curr_points; +float currTime; +float currTrajTime; /* Assumes drone is on ground. Initializes curr_position & curr_velocity values to 0 */ -void setup(); +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); @@ -56,6 +72,8 @@ void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose); /* Stores msgs from multi control node in last*/ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg); +void generateTraj(mav_trajectory_generation::Trajectory& inputTrajectory, int bx, int by, int bz, int ex, int ey, int ez); + int main(int argc, char **argv) { // initalizing node @@ -64,15 +82,14 @@ int main(int argc, char **argv) throw; } - ros::init(argc, argv, "drone" + static_cast(argv[1])); + ros::init(argc, argv, "drone" + static_cast(argv[1]) + "_control"); ros::NodeHandle nh; - setup(); + setup(nh, static_cast(argv[1])); std::string dPrefix = "uav" + static_cast(argv[1]) + "/"; - std::string uavName = "drone" + static_cast(argv[1]); ros::Time last_request = ros::Time::now(); // used for periodic messaging - // all topics + // all subs/services/pubs ros::Subscriber state_sub = nh.subscribe (dPrefix +"mavros/state", 0, state_cb); ros::Subscriber pos_sub = nh.subscribe @@ -94,8 +111,8 @@ int main(int argc, char **argv) ros::Publisher mav_pub = nh.advertise (dPrefix +"mavros/setpoint_raw/local", 20); - // mutli control subscribers - ros::Subscriber multi_sub = nh.subscribe + // mutli control sub/pub + ros::Subscriber multi_cmd_sub = nh.subscribe (uavName + "_cmds", 0, storeCommand); //the setpoint publishing rate MUST be faster than 2Hz @@ -127,7 +144,6 @@ int main(int argc, char **argv) (ros::Time::now() - last_request > ros::Duration(3))){ if( arming_client.call(arm_cmd) && arm_cmd.response.success){ - ROS_INFO(uavName, " armed"); } last_request = ros::Time::now(); } @@ -138,7 +154,8 @@ int main(int argc, char **argv) if((reachedLocation(curr_position, default_liftoff_pos, .1)) && (isStationary(curr_velocity, .05))){ hover_position = default_liftoff_pos; droneState = HOVERING; - ROS(uavName, " Hovering"); + last_response.response.data = "REACHED"; + multi_info_pub.publish(last_response); } } else{ @@ -146,14 +163,17 @@ int main(int argc, char **argv) if((reachedLocation(curr_position, curr_target, .1)) && (isStationary(curr_velocity, .05))){ hover_position = curr_target; droneState = HOVERING; - ROS(uavName, " Hovering"); + last_response.response.data = "REACHED"; + multi_info_pub.publish(last_response); } } }else if(droneState == HOVERING){ local_pos_pub.publish(hover_position); }else if(droneState == IN_TRANSIT){ - if(ros::Time::now() - last_request > ros::Duration(30.0)){ + //if need to update trajectory + // if(curr_trajectories.size() == 0 || curr_target != curr_trajectories.front.) + if(ros::Time::now() - last_request > ros::Duration(10.0)){ ROS_INFO("In Transit..."); last_request = ros::Time::now(); } @@ -165,12 +185,11 @@ int main(int argc, char **argv) } ros::spinOnce(); rate.sleep(); - } return 0; } -void setup(){ +void setup(ros::NodeHandle& nodehandler, std::string droneNum){ curr_position.pose.position.x = 0; curr_position.pose.position.y = 0; curr_position.pose.position.z = 0; @@ -195,6 +214,12 @@ void setup(){ hover_position.pose.position.z = 0; droneState = GROUND_IDLE; + multi_info_pub = nodehandler.advertise + (uavName + "_info", 0); + last_response.response.data = "NULL"; + + currTime = 0; + currTrajTime = 0; default_liftoff_pos.pose.position.x = curr_position.pose.position.x; default_liftoff_pos.pose.position.y = curr_position.pose.position.y; @@ -202,6 +227,7 @@ void setup(){ offb_set_mode.request.custom_mode = "OFFBOARD"; arm_cmd.request.value = true; + uavName = "drone" + droneNum; } void state_cb(const mavros_msgs::State::ConstPtr& msg){ @@ -210,13 +236,6 @@ void state_cb(const mavros_msgs::State::ConstPtr& msg){ void updatePose(const geometry_msgs::PoseStamped::ConstPtr& inputPose){ curr_position = *inputPose; - // curr_position.pose.position.x = (inputPose->pose).position.x; - // curr_position.pose.position.y = (inputPose->pose).position.y; - // curr_position.pose.position.z = (inputPose->pose).position.z; - // curr_position.pose.orientation.x = (inputPose->pose).orientation.x; - // curr_position.pose.orientation.y = (inputPose->pose).orientation.y; - // curr_position.pose.orientation.z = (inputPose->pose).orientation.z; - // curr_position.pose.orientation.w = (inputPose->pose).orientation.w; } void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose){ @@ -228,33 +247,65 @@ void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose){ curr_velocity.angular.z = inputPose->twist.angular.z; } +void generateTraj(mav_trajectory_generation::Trajectory& inputTrajectory, 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 + inputTrajectory.setSegments(segments); +} + void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ + last_response.response.data = "RECEIVED"; curr_message = *inputMsg; - //handles input target - if(inputMsg->target.x != curr_target.pose.position.x || - inputMsg->target.y != curr_target.pose.position.y || - inputMsg->target.z != curr_target.pose.position.z){ - curr_target.pose.position.x = inputMsg->target.x; - curr_target.pose.position.y = inputMsg->target.y; - curr_target.pose.position.z = inputMsg->target.z; - } //handles input command std::string inputCmd = inputMsg->command.data; - if(inputCmd == "E_STOP"){ + if(inputCmd == "SHUTOFF"){ droneState = SHUTTING_DOWN; } else if(inputCmd == "LIFT"){ if(droneState == GROUND_IDLE){ + if(inputMsg->target.x != curr_target.pose.position.x || + inputMsg->target.y != curr_target.pose.position.y || + inputMsg->target.z != curr_target.pose.position.z){ + curr_target.pose.position.x = inputMsg->target.x; + curr_target.pose.position.y = inputMsg->target.y; + curr_target.pose.position.z = inputMsg->target.z; + } 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 = curr_position; @@ -263,13 +314,80 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ 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"){// check logic + if(curr_trajectories.size() == 0){ + Eigen::Vector3d starting(curr_position.pose.position.x, curr_position.pose.position.y, curr_position.pose.position.z); + } else{ + TrajectoryGroupedInfo* last_traj = curr_trajectories.back(); + Eigen::Vector3d starting(last_traj->endPoint.pose.position.x, last_traj->endPoint.pose.position.y, last_traj->endPoint.pose.position.z); + } + mav_trajectory_generation::Trajectory outputTraj; + Eigen::Vector3d ending(inputMsg->target.x, inputMsg->target.y, inputMsg->target.z); + generateTraj(outputTraj, 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(outputTraj, startingPoint, endingPoint, static_cast(outputTraj.getSegmentTimes().at(0))); + curr_trajectories.push(outputGroupedInfo); + droneState = IN_TRANSIT; + } + else{ + // setup transit new + while(curr_trajectories.size() != 0){ + TrajectoryGroupedInfo* first_traj = curr_trajectories.front(); + delete first_traj; + curr_trajectories.pop(); + } + mav_trajectory_generation::Trajectory outputTraj; + 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); + generateTraj(outputTraj, 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(outputTraj, startingPoint, endingPoint, static_cast(outputTraj.getSegmentTimes().at(0))); + curr_trajectories.push(outputGroupedInfo); + + droneState = IN_TRANSIT; + } + } else{ ROS_INFO("Error: Invalid Command."); + last_response.response.data = "ERROR"; } + multi_info_pub.publish(last_response); } // void printTrajInfo(const mav_trajectory_generation::Segment::Vector& allSegments){ From c1ec47d3481ac6a6e3df73e7630aab37ffe9fd0e Mon Sep 17 00:00:00 2001 From: IseanB Date: Sat, 20 Aug 2022 21:26:46 -0700 Subject: [PATCH 27/33] Working/tested transit cmds --- src/src/multiDrone.cpp | 94 +++++++++++++++++++++++++++++++++++++---- src/src/singleDrone.cpp | 56 +++++++++++++++--------- 2 files changed, 120 insertions(+), 30 deletions(-) diff --git a/src/src/multiDrone.cpp b/src/src/multiDrone.cpp index 5eaeb2d..40bc129 100644 --- a/src/src/multiDrone.cpp +++ b/src/src/multiDrone.cpp @@ -14,32 +14,108 @@ int main(int argc, char **argv) ros::Rate rate(20.0); ros::Publisher drone_cmd_pub = nh.advertise - ("drone1_cmds", 0); + ("drone1/cmds", 0); ros::Subscriber drone_info_sub = nh.subscribe - ("drone1_cmds", 0, storeInfo); + ("drone1/info", 0, storeInfo); drone_control::dcontrol msg; - bool done = false; + 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()){ - while(ros::Time::now() - last_request < ros::Duration(10.0)){ - if(ros::Time::now() - last_request >= ros::Duration(10.0)){ - ROS_INFO("info sent"); + while(!lift){ + if(ros::Time::now() - last_request >= ros::Duration(15.0)){ + ROS_INFO("LIFT info sent"); msg.command.data = "LIFT"; msg.target.z = 1; drone_cmd_pub.publish(msg); last_request = ros::Time::now(); + lift = true; } ros::spinOnce(); rate.sleep(); } - while(ros::Time::now() - last_request > ros::Duration(10.0)){ - if(ros::Time::now() - last_request >= ros::Duration(10.0)){ - msg.command.data = "IN_TRANSIT"; + while(!transit7){ + if(!transit && ros::Time::now() - last_request >= ros::Duration(15.0)){ + ROS_INFO(" TRANSIT info sent"); + msg.command.data = "TRANSIT_ADD"; + msg.target.x = 1; + msg.target.y = 1; + msg.target.z = 1; + drone_cmd_pub.publish(msg); + last_request = ros::Time::now(); + transit = true; + } + else if(!transit2 && transit && ros::Time::now() - last_request >= ros::Duration(5.0)){ + ROS_INFO(" TRANSIT info sent"); + msg.command.data = "TRANSIT_ADD"; + msg.target.x = 2; + msg.target.y = 3; + msg.target.z = 5; + drone_cmd_pub.publish(msg); + last_request = ros::Time::now(); + transit2 = true; + } + else if(!transit3 && transit2 && ros::Time::now() - last_request >= ros::Duration(5.0)){ + ROS_INFO(" TRANSIT info sent"); + msg.command.data = "TRANSIT_ADD"; + msg.target.x = 2; + msg.target.y = 31; + msg.target.z = 5; + drone_cmd_pub.publish(msg); + last_request = ros::Time::now(); + transit3 = true; + } + else if(!transit4 && transit3 && ros::Time::now() - last_request >= ros::Duration(5.0)){ + ROS_INFO(" TRANSIT info sent"); + msg.command.data = "TRANSIT_NEW"; + msg.target.x = 21; + msg.target.y = 3; + msg.target.z = 5; + drone_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 info sent"); + msg.command.data = "TRANSIT_NEW"; + msg.target.x = 6; + msg.target.y = 2; + msg.target.z = 5; + drone_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 info sent"); + msg.command.data = "TRANSIT_ADD"; + msg.target.x = 2; + msg.target.y = 3; + msg.target.z = 5; + drone_cmd_pub.publish(msg); + last_request = ros::Time::now(); + transit6 = true; + } + else if(!transit7 && transit6 && ros::Time::now() - last_request >= ros::Duration(3.0)){ + ROS_INFO(" TRANSIT info sent"); + msg.command.data = "TRANSIT_NEW"; + msg.target.x = 2; + msg.target.y = 3; + msg.target.z = 5; + drone_cmd_pub.publish(msg); last_request = ros::Time::now(); + transit7 = true; } ros::spinOnce(); rate.sleep(); } + ROS_INFO("DONE"); ros::spinOnce(); rate.sleep(); } diff --git a/src/src/singleDrone.cpp b/src/src/singleDrone.cpp index 868a916..116190f 100644 --- a/src/src/singleDrone.cpp +++ b/src/src/singleDrone.cpp @@ -67,7 +67,7 @@ void updatePose(const geometry_msgs::PoseStamped::ConstPtr& inputPose); void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose); /* Prints crutial information about the inputted trajectory */ -// void printTrajInfo(const mav_trajectory_generation::Segment::Vector& allSegments); +void printTrajInfo(mav_trajectory_generation::Trajectory trajectory); /* Stores msgs from multi control node in last*/ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg); @@ -82,7 +82,7 @@ int main(int argc, char **argv) throw; } - ros::init(argc, argv, "drone" + static_cast(argv[1]) + "_control"); + ros::init(argc, argv, "drone" + static_cast(argv[1])); ros::NodeHandle nh; setup(nh, static_cast(argv[1])); @@ -113,7 +113,7 @@ int main(int argc, char **argv) // mutli control sub/pub ros::Subscriber multi_cmd_sub = nh.subscribe - (uavName + "_cmds", 0, storeCommand); + (uavName + "/cmds", 0, storeCommand); //the setpoint publishing rate MUST be faster than 2Hz ros::Rate rate(20.0); @@ -215,7 +215,7 @@ void setup(ros::NodeHandle& nodehandler, std::string droneNum){ droneState = GROUND_IDLE; multi_info_pub = nodehandler.advertise - (uavName + "_info", 0); + (uavName + "/info", 0); last_response.response.data = "NULL"; currTime = 0; @@ -333,11 +333,12 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ last_response.response.data = "ERROR"; } else if(inputCmd == "TRANSIT_ADD"){// check logic + Eigen::Vector3d starting; if(curr_trajectories.size() == 0){ - Eigen::Vector3d starting(curr_position.pose.position.x, curr_position.pose.position.y, curr_position.pose.position.z); + starting = Eigen::Vector3d(curr_position.pose.position.x, curr_position.pose.position.y, curr_position.pose.position.z); } else{ TrajectoryGroupedInfo* last_traj = curr_trajectories.back(); - Eigen::Vector3d starting(last_traj->endPoint.pose.position.x, last_traj->endPoint.pose.position.y, last_traj->endPoint.pose.position.z); + starting = Eigen::Vector3d(last_traj->endPoint.pose.position.x, last_traj->endPoint.pose.position.y, last_traj->endPoint.pose.position.z); } mav_trajectory_generation::Trajectory outputTraj; Eigen::Vector3d ending(inputMsg->target.x, inputMsg->target.y, inputMsg->target.z); @@ -355,6 +356,12 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ TrajectoryGroupedInfo* outputGroupedInfo = new TrajectoryGroupedInfo(outputTraj, startingPoint, endingPoint, static_cast(outputTraj.getSegmentTimes().at(0))); curr_trajectories.push(outputGroupedInfo); droneState = IN_TRANSIT; + ROS_INFO("TRANSIT ADD"); + printTrajInfo((curr_trajectories.back())->trajectoryThis); + std::cout << "Traj Time: " << (curr_trajectories.back())->trajectoryTime << std::endl; + std::cout << "Traj Queue Size: " << curr_trajectories.size() << std::endl; + std::cout << startingPoint << std::endl; + std::cout << endingPoint << std::endl; } else{ // setup transit new @@ -381,6 +388,12 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ curr_trajectories.push(outputGroupedInfo); droneState = IN_TRANSIT; + ROS_INFO("TRANSIT NEW"); + printTrajInfo((curr_trajectories.back())->trajectoryThis); + std::cout << "Traj Time: " << (curr_trajectories.back())->trajectoryTime << std::endl; + std::cout << "Traj Queue Size: " << curr_trajectories.size() << std::endl; + std::cout << startingPoint << std::endl; + std::cout << endingPoint << std::endl; } } else{ @@ -390,18 +403,19 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ multi_info_pub.publish(last_response); } -// 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 : "< Date: Sun, 21 Aug 2022 22:34:37 -0700 Subject: [PATCH 28/33] Changed parm types --- helper/computations.h | 6 +++--- helper/conversions.h | 2 +- helper/src/computations.cpp | 12 ++++++++++-- helper/src/conversions.cpp | 3 ++- 4 files changed, 16 insertions(+), 7 deletions(-) diff --git a/helper/computations.h b/helper/computations.h index b88e2c7..1f7eb70 100644 --- a/helper/computations.h +++ b/helper/computations.h @@ -4,21 +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 50f4166..c89eeb0 100644 --- a/helper/src/computations.cpp +++ b/helper/src/computations.cpp @@ -12,8 +12,8 @@ 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::PoseStamped& this_pos, const geometry_msgs::PoseStamped& 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..71123d1 100644 --- a/helper/src/conversions.cpp +++ b/helper/src/conversions.cpp @@ -1,6 +1,6 @@ #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; Eigen::VectorXd pos = s.evaluate(time, 0); Eigen::VectorXd vel = s.evaluate(time, 1); @@ -8,6 +8,7 @@ mavros_msgs::PositionTarget segmentToPoint(const mav_trajectory_generation::Segm result.header.stamp = ros::Time::now(); result.header.frame_id = "map"; + result.type_mask = 1024+2048; result.coordinate_frame = 1; From 1a8b25d4c1e28fbc6e8ab5d744cb3efbb06cec8f Mon Sep 17 00:00:00 2001 From: IseanB Date: Sun, 21 Aug 2022 22:35:16 -0700 Subject: [PATCH 29/33] Current work, need to fix updating points --- src/src/multiDrone.cpp | 169 +++++++++++++++++++--------------------- src/src/singleDrone.cpp | 137 ++++++++++++++++++++------------ 2 files changed, 170 insertions(+), 136 deletions(-) diff --git a/src/src/multiDrone.cpp b/src/src/multiDrone.cpp index 40bc129..da6a9a3 100644 --- a/src/src/multiDrone.cpp +++ b/src/src/multiDrone.cpp @@ -28,98 +28,93 @@ int main(int argc, char **argv) bool transit6 = false; bool transit7 = false; // bool transit2 = false; - while(ros::ok()){ - while(!lift){ - if(ros::Time::now() - last_request >= ros::Duration(15.0)){ - ROS_INFO("LIFT info sent"); - msg.command.data = "LIFT"; - msg.target.z = 1; - drone_cmd_pub.publish(msg); - last_request = ros::Time::now(); - lift = true; - } - ros::spinOnce(); - rate.sleep(); + 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; + drone_cmd_pub.publish(msg); + last_request = ros::Time::now(); + lift = true; } - while(!transit7){ - if(!transit && ros::Time::now() - last_request >= ros::Duration(15.0)){ - ROS_INFO(" TRANSIT info sent"); - msg.command.data = "TRANSIT_ADD"; - msg.target.x = 1; - msg.target.y = 1; - msg.target.z = 1; - drone_cmd_pub.publish(msg); - last_request = ros::Time::now(); - transit = true; - } - else if(!transit2 && transit && ros::Time::now() - last_request >= ros::Duration(5.0)){ - ROS_INFO(" TRANSIT info sent"); - msg.command.data = "TRANSIT_ADD"; - msg.target.x = 2; - msg.target.y = 3; - msg.target.z = 5; - drone_cmd_pub.publish(msg); - last_request = ros::Time::now(); - transit2 = true; - } - else if(!transit3 && transit2 && ros::Time::now() - last_request >= ros::Duration(5.0)){ - ROS_INFO(" TRANSIT info sent"); - msg.command.data = "TRANSIT_ADD"; - msg.target.x = 2; - msg.target.y = 31; - msg.target.z = 5; - drone_cmd_pub.publish(msg); - last_request = ros::Time::now(); - transit3 = true; - } - else if(!transit4 && transit3 && ros::Time::now() - last_request >= ros::Duration(5.0)){ - ROS_INFO(" TRANSIT info sent"); - msg.command.data = "TRANSIT_NEW"; - msg.target.x = 21; - msg.target.y = 3; - msg.target.z = 5; - drone_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 info sent"); - msg.command.data = "TRANSIT_NEW"; - msg.target.x = 6; - msg.target.y = 2; - msg.target.z = 5; - drone_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 info sent"); - msg.command.data = "TRANSIT_ADD"; - msg.target.x = 2; - msg.target.y = 3; - msg.target.z = 5; - drone_cmd_pub.publish(msg); - last_request = ros::Time::now(); - transit6 = true; - } - else if(!transit7 && transit6 && ros::Time::now() - last_request >= ros::Duration(3.0)){ - ROS_INFO(" TRANSIT info sent"); - msg.command.data = "TRANSIT_NEW"; - msg.target.x = 2; - msg.target.y = 3; - msg.target.z = 5; - drone_cmd_pub.publish(msg); - last_request = ros::Time::now(); - transit7 = true; - } - ros::spinOnce(); - rate.sleep(); + ros::spinOnce(); + rate.sleep(); + } + while(ros::ok() && !transit7){ + if(!transit && ros::Time::now() - last_request >= ros::Duration(20.0)){ + ROS_INFO(" TRANSIT info sent"); + msg.command.data = "TRANSIT_ADD"; + msg.target.x = 1; + msg.target.y = 1; + msg.target.z = 1; + drone_cmd_pub.publish(msg); + last_request = ros::Time::now(); + transit = true; + } + else if(!transit2 && transit && ros::Time::now() - last_request >= ros::Duration(15.0)){ + ROS_INFO(" TRANSIT info sent"); + msg.command.data = "TRANSIT_ADD"; + msg.target.x = -1; + msg.target.y = -2; + msg.target.z = 1; + drone_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 info sent"); + msg.command.data = "TRANSIT_ADD"; + msg.target.x = 0; + msg.target.y = 0; + msg.target.z = 1.5; + drone_cmd_pub.publish(msg); + last_request = ros::Time::now(); + transit3 = true; + } + else if(!transit4 && transit3 && ros::Time::now() - last_request >= ros::Duration(3.0)){ + ROS_INFO(" TRANSIT info sent"); + msg.command.data = "TRANSIT_NEW"; + msg.target.x = -1; + msg.target.y = 2; + msg.target.z = 1; + drone_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 info sent"); + msg.command.data = "TRANSIT_NEW"; + msg.target.x = 1; + msg.target.y = -2; + msg.target.z = 1; + drone_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 info sent"); + msg.command.data = "TRANSIT_ADD"; + msg.target.x = 3; + msg.target.y = -2; + msg.target.z = 1; + drone_cmd_pub.publish(msg); + last_request = ros::Time::now(); + transit6 = true; + } + else if(!transit7 && transit6 && ros::Time::now() - last_request >= ros::Duration(10.0)){ + ROS_INFO(" TRANSIT info sent"); + msg.command.data = "TRANSIT_NEW"; + msg.target.x = 4; + msg.target.y = 1; + msg.target.z = 1; + drone_cmd_pub.publish(msg); + last_request = ros::Time::now(); + transit7 = true; + ROS_INFO("DONE"); } - ROS_INFO("DONE"); ros::spinOnce(); rate.sleep(); } - return 0; } diff --git a/src/src/singleDrone.cpp b/src/src/singleDrone.cpp index 116190f..d446f93 100644 --- a/src/src/singleDrone.cpp +++ b/src/src/singleDrone.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include "drone_control/dcontrol.h" #include "drone_control/dresponse.h" #include @@ -36,7 +37,7 @@ struct TrajectoryGroupedInfo{ mavros_msgs::State current_state; geometry_msgs::PoseStamped curr_position; geometry_msgs::Twist curr_velocity; -geometry_msgs::PoseStamped curr_target; +mavros_msgs::PositionTarget curr_target;// not end target, dynamic changed drone_control::dcontrol curr_message; geometry_msgs::PoseStamped hover_position; @@ -50,9 +51,9 @@ PossiableState droneState; ros::Publisher multi_info_pub; std::queue curr_trajectories; -std::queue curr_points; float currTime; float currTrajTime; +const float timeStepSize = .1; /* Assumes drone is on ground. Initializes curr_position & curr_velocity values to 0 */ void setup(ros::NodeHandle& nodehandler,std::string droneNum); @@ -110,6 +111,8 @@ int main(int argc, char **argv) (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", 0); // mutli control sub/pub ros::Subscriber multi_cmd_sub = nh.subscribe @@ -148,31 +151,64 @@ int main(int argc, char **argv) last_request = ros::Time::now(); } } - - if(curr_target.pose.position.x == 0 && curr_target.pose.position.y == 0 && curr_target.pose.position.z == 0){ - local_pos_pub.publish(default_liftoff_pos); - if((reachedLocation(curr_position, default_liftoff_pos, .1)) && (isStationary(curr_velocity, .05))){ - hover_position = default_liftoff_pos; - droneState = HOVERING; - last_response.response.data = "REACHED"; - multi_info_pub.publish(last_response); - } + local_pos_pub.publish(default_liftoff_pos); + if((reachedLocation(curr_position, default_liftoff_pos, .1)) && (isStationary(curr_velocity, .05))){ + hover_position = default_liftoff_pos; + curr_target.position.x = hover_position.pose.position.x; + curr_target.position.y = hover_position.pose.position.y; + curr_target.position.z = hover_position.pose.position.z; + droneState = HOVERING; + currTrajTime = 0; + last_response.response.data = "REACHED"; + multi_info_pub.publish(last_response); } - else{ - local_pos_pub.publish(curr_target); - if((reachedLocation(curr_position, curr_target, .1)) && (isStationary(curr_velocity, .05))){ - hover_position = curr_target; - droneState = HOVERING; - 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(1.0)){ + ROS_INFO("HOVERING..."); + last_request = ros::Time::now(); } - - }else if(droneState == HOVERING){ local_pos_pub.publish(hover_position); }else if(droneState == IN_TRANSIT){ - //if need to update trajectory - // if(curr_trajectories.size() == 0 || curr_target != curr_trajectories.front.) + if(curr_trajectories.size() == 0){ // no traj loaded + hover_position = curr_position; + currTrajTime = 0; + currTime = 0; + curr_target.position.x = hover_position.pose.position.x; + curr_target.position.y = hover_position.pose.position.y; + curr_target.position.z = hover_position.pose.position.z; + droneState = HOVERING; + } + else{// at least one traj planned + TrajectoryGroupedInfo* first_trajectory = curr_trajectories.front(); + if(currTrajTime == 0){// trajectory not loaded + currTrajTime = first_trajectory->trajectoryTime; + currTime = 0; + curr_target = segmentToPoint((((first_trajectory->trajectoryThis).segments())[0]), currTime); + mav_pub.publish(curr_target); + currTime = timeStepSize; + } + else{// trajectory loaded + if(reachedLocation(curr_position, curr_target, .2)){// if near end of target + currTime += timeStepSize; + if(currTime > currTrajTime){ + currTime = currTrajTime; + } + curr_target = segmentToPoint((((first_trajectory->trajectoryThis).segments())[0]), currTime); + mav_pub.publish(curr_target); + if(currTime == currTrajTime){ + curr_trajectories.pop(); + delete first_trajectory; + currTrajTime = 0; + currTime = 0; + } + } + else{ + curr_target = segmentToPoint((((first_trajectory->trajectoryThis).segments())[0]), currTime); + mav_pub.publish(curr_target); + } + } + } + if(ros::Time::now() - last_request > ros::Duration(10.0)){ ROS_INFO("In Transit..."); last_request = ros::Time::now(); @@ -183,10 +219,11 @@ int main(int argc, char **argv) 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){ @@ -205,13 +242,13 @@ void setup(ros::NodeHandle& nodehandler, std::string droneNum){ curr_velocity.angular.y = 0; curr_velocity.angular.z = 0; - curr_target.pose.position.x = 0; - curr_target.pose.position.y = 0; - curr_target.pose.position.z = 0; + hover_position.pose.position.x = curr_position.pose.position.x; + hover_position.pose.position.y = curr_position.pose.position.x; + hover_position.pose.position.z = 2; - hover_position.pose.position.x = 0; - hover_position.pose.position.y = 0; - hover_position.pose.position.z = 0; + curr_target.position.x = hover_position.pose.position.x; + curr_target.position.y = hover_position.pose.position.y; + curr_target.position.z = hover_position.pose.position.z; droneState = GROUND_IDLE; multi_info_pub = nodehandler.advertise @@ -288,12 +325,15 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ } else if(inputCmd == "LIFT"){ if(droneState == GROUND_IDLE){ - if(inputMsg->target.x != curr_target.pose.position.x || - inputMsg->target.y != curr_target.pose.position.y || - inputMsg->target.z != curr_target.pose.position.z){ - curr_target.pose.position.x = inputMsg->target.x; - curr_target.pose.position.y = inputMsg->target.y; - curr_target.pose.position.z = inputMsg->target.z; + //if given a point, update default_liftoff_pos + if(inputMsg->target.x != curr_target.position.x || inputMsg->target.y != curr_target.position.y || inputMsg->target.z != curr_target.position.z){ + default_liftoff_pos.pose.position.x = inputMsg->target.x; + default_liftoff_pos.pose.position.y = inputMsg->target.y; + default_liftoff_pos.pose.position.z = inputMsg->target.z; + } + else{ + default_liftoff_pos.pose.position.x = curr_position.pose.position.x; + default_liftoff_pos.pose.position.y = curr_position.pose.position.y; } droneState = LIFTING_OFF; } @@ -309,6 +349,9 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ } else; hover_position = curr_position; + curr_target.position.x = hover_position.pose.position.x; + curr_target.position.y = hover_position.pose.position.y; + curr_target.position.z = hover_position.pose.position.z; droneState = HOVERING; } else if(inputCmd == "LAND"){ @@ -332,10 +375,11 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ ROS_INFO("TRANSIT Error: Wait for landing completion."); last_response.response.data = "ERROR"; } - else if(inputCmd == "TRANSIT_ADD"){// check logic + 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 SIZE STARTIN"); } 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); @@ -355,13 +399,10 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ TrajectoryGroupedInfo* outputGroupedInfo = new TrajectoryGroupedInfo(outputTraj, startingPoint, endingPoint, static_cast(outputTraj.getSegmentTimes().at(0))); curr_trajectories.push(outputGroupedInfo); + droneState = IN_TRANSIT; - ROS_INFO("TRANSIT ADD"); - printTrajInfo((curr_trajectories.back())->trajectoryThis); - std::cout << "Traj Time: " << (curr_trajectories.back())->trajectoryTime << std::endl; - std::cout << "Traj Queue Size: " << curr_trajectories.size() << std::endl; - std::cout << startingPoint << std::endl; - std::cout << endingPoint << std::endl; + last_response.response.data = "RECIEVED"; + std::cout << "Recived: " << curr_trajectories.size() << std::endl; } else{ // setup transit new @@ -370,6 +411,9 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ delete first_traj; curr_trajectories.pop(); } + currTrajTime = 0; + currTime = 0; + mav_trajectory_generation::Trajectory outputTraj; 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); @@ -388,12 +432,7 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ curr_trajectories.push(outputGroupedInfo); droneState = IN_TRANSIT; - ROS_INFO("TRANSIT NEW"); - printTrajInfo((curr_trajectories.back())->trajectoryThis); - std::cout << "Traj Time: " << (curr_trajectories.back())->trajectoryTime << std::endl; - std::cout << "Traj Queue Size: " << curr_trajectories.size() << std::endl; - std::cout << startingPoint << std::endl; - std::cout << endingPoint << std::endl; + last_response.response.data = "RECIEVED"; } } else{ From bb323c79accf5dd97cd95da21eb87292df170ab5 Mon Sep 17 00:00:00 2001 From: IseanB Date: Mon, 22 Aug 2022 15:53:00 -0700 Subject: [PATCH 30/33] Smoothed Trajectory --- src/src/multiDrone.cpp | 4 +- src/src/singleDrone.cpp | 179 +++++++++++++++++++--------------------- 2 files changed, 87 insertions(+), 96 deletions(-) diff --git a/src/src/multiDrone.cpp b/src/src/multiDrone.cpp index da6a9a3..6937365 100644 --- a/src/src/multiDrone.cpp +++ b/src/src/multiDrone.cpp @@ -46,7 +46,7 @@ int main(int argc, char **argv) msg.command.data = "TRANSIT_ADD"; msg.target.x = 1; msg.target.y = 1; - msg.target.z = 1; + msg.target.z = 2; drone_cmd_pub.publish(msg); last_request = ros::Time::now(); transit = true; @@ -56,7 +56,7 @@ int main(int argc, char **argv) msg.command.data = "TRANSIT_ADD"; msg.target.x = -1; msg.target.y = -2; - msg.target.z = 1; + msg.target.z = 2; drone_cmd_pub.publish(msg); last_request = ros::Time::now(); transit2 = true; diff --git a/src/src/singleDrone.cpp b/src/src/singleDrone.cpp index d446f93..f91fe4f 100644 --- a/src/src/singleDrone.cpp +++ b/src/src/singleDrone.cpp @@ -26,34 +26,35 @@ enum PossiableState{ }; struct TrajectoryGroupedInfo{ - mav_trajectory_generation::Trajectory trajectoryThis; + mav_trajectory_generation::Segment segmentThis; geometry_msgs::PoseStamped startPoint; geometry_msgs::PoseStamped endPoint; float trajectoryTime; - TrajectoryGroupedInfo(mav_trajectory_generation::Trajectory inputTraj, geometry_msgs::PoseStamped inputStart, geometry_msgs::PoseStamped inputEnd, float inputTime) : trajectoryThis(inputTraj), startPoint(inputStart), endPoint(inputEnd), trajectoryTime(inputTime) {}; + 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 end target, dynamic changed +mavros_msgs::PositionTarget curr_target;// not endpoint, dynamic changed drone_control::dcontrol curr_message; geometry_msgs::PoseStamped hover_position; drone_control::dresponse last_response; -geometry_msgs::PoseStamped default_liftoff_pos; 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 = .1; +const float timeStepSize = .05; /* Assumes drone is on ground. Initializes curr_position & curr_velocity values to 0 */ void setup(ros::NodeHandle& nodehandler,std::string droneNum); @@ -67,13 +68,13 @@ 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); +// /* 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); -void generateTraj(mav_trajectory_generation::Trajectory& inputTrajectory, int bx, int by, int bz, int ex, int ey, int ez); +mav_trajectory_generation::Segment generateTraj(int bx, int by, int bz, int ex, int ey, int ez); int main(int argc, char **argv) { @@ -87,7 +88,6 @@ int main(int argc, char **argv) ros::NodeHandle nh; setup(nh, static_cast(argv[1])); - std::string dPrefix = "uav" + static_cast(argv[1]) + "/"; ros::Time last_request = ros::Time::now(); // used for periodic messaging // all subs/services/pubs @@ -105,14 +105,12 @@ int main(int argc, char **argv) ros::ServiceClient end_flight_client = nh.serviceClient (dPrefix +"mavros/cmd/command"); - ros::Publisher local_pos_pub = nh.advertise - (dPrefix +"mavros/setpoint_position/local", 0); 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", 0); + (dPrefix +"curr_target", 5); // mutli control sub/pub ros::Subscriber multi_cmd_sub = nh.subscribe @@ -129,6 +127,9 @@ int main(int argc, char **argv) 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)){ @@ -140,6 +141,7 @@ int main(int argc, char **argv) (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 { @@ -147,16 +149,19 @@ int main(int argc, char **argv) (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(); } } - local_pos_pub.publish(default_liftoff_pos); - if((reachedLocation(curr_position, default_liftoff_pos, .1)) && (isStationary(curr_velocity, .05))){ - hover_position = default_liftoff_pos; - curr_target.position.x = hover_position.pose.position.x; - curr_target.position.y = hover_position.pose.position.y; - curr_target.position.z = hover_position.pose.position.z; + if(ros::Time::now() - last_request > ros::Duration(3)){ + std::cout << hover_position << std::endl; + 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"; @@ -173,19 +178,17 @@ int main(int argc, char **argv) hover_position = curr_position; currTrajTime = 0; currTime = 0; - curr_target.position.x = hover_position.pose.position.x; - curr_target.position.y = hover_position.pose.position.y; - curr_target.position.z = hover_position.pose.position.z; + curr_target.position = hover_position.pose.position; droneState = HOVERING; } else{// at least one traj planned - TrajectoryGroupedInfo* first_trajectory = curr_trajectories.front(); + 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->trajectoryThis).segments())[0]), currTime); + currTime = timeStepSize*5; // starts calculating at .5 seconds, to prevent float point error + curr_target = segmentToPoint(first_trajectory->segmentThis, currTime); mav_pub.publish(curr_target); - currTime = timeStepSize; } else{// trajectory loaded if(reachedLocation(curr_position, curr_target, .2)){// if near end of target @@ -193,17 +196,16 @@ int main(int argc, char **argv) if(currTime > currTrajTime){ currTime = currTrajTime; } - curr_target = segmentToPoint((((first_trajectory->trajectoryThis).segments())[0]), currTime); + curr_target = segmentToPoint(first_trajectory->segmentThis, currTime); mav_pub.publish(curr_target); if(currTime == currTrajTime){ curr_trajectories.pop(); - delete first_trajectory; + // delete first_trajectory; currTrajTime = 0; currTime = 0; } } else{ - curr_target = segmentToPoint((((first_trajectory->trajectoryThis).segments())[0]), currTime); mav_pub.publish(curr_target); } } @@ -223,48 +225,32 @@ int main(int argc, char **argv) ros::spinOnce(); rate.sleep(); } - return 0; } void setup(ros::NodeHandle& nodehandler, std::string droneNum){ - curr_position.pose.position.x = 0; - curr_position.pose.position.y = 0; - curr_position.pose.position.z = 0; - curr_position.pose.orientation.x = 0; - curr_position.pose.orientation.y = 0; - curr_position.pose.orientation.z = 0; - curr_position.pose.orientation.w = 0; - - curr_velocity.linear.x = 0; - curr_velocity.linear.y = 0; - curr_velocity.linear.z = 0; - curr_velocity.angular.x = 0; - curr_velocity.angular.y = 0; - curr_velocity.angular.z = 0; + 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.x; + hover_position.pose.position.y = curr_position.pose.position.y; hover_position.pose.position.z = 2; - curr_target.position.x = hover_position.pose.position.x; - curr_target.position.y = hover_position.pose.position.y; - curr_target.position.z = hover_position.pose.position.z; + 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; - default_liftoff_pos.pose.position.x = curr_position.pose.position.x; - default_liftoff_pos.pose.position.y = curr_position.pose.position.y; - default_liftoff_pos.pose.position.z = 2; - offb_set_mode.request.custom_mode = "OFFBOARD"; arm_cmd.request.value = true; - uavName = "drone" + droneNum; } void state_cb(const mavros_msgs::State::ConstPtr& msg){ @@ -284,7 +270,8 @@ void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose){ curr_velocity.angular.z = inputPose->twist.angular.z; } -void generateTraj(mav_trajectory_generation::Trajectory& inputTrajectory, int bx, int by, int bz, int ex, int ey, int ez){ +mav_trajectory_generation::Segment generateTraj(int bx, int by, int bz, int ex, int ey, int ez){ + ROS_INFO("GEN START"); Eigen::Vector3d startPoint(bx,by,bz); Eigen::Vector3d endPoint(ex,ey,ez); @@ -301,8 +288,8 @@ void generateTraj(mav_trajectory_generation::Trajectory& inputTrajectory, int bx vertices.push_back(end); std::vector segment_times; - const double v_max = 2.0; - const double a_max = 2.0; + const double v_max = 1.0; + const double a_max = 1.0; segment_times = estimateSegmentTimes(vertices, v_max, a_max); const int N = 10;// needs to be at least 10 for snap, 8 for jerk @@ -311,10 +298,13 @@ void generateTraj(mav_trajectory_generation::Trajectory& inputTrajectory, int bx opt.solveLinear(); opt.getSegments(&segments);// fills "segments" variable with segments of the path - inputTrajectory.setSegments(segments); + ROS_INFO("GEN END"); + return mav_trajectory_generation::Segment(segments[0]); } void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ + ros::Time last_request = ros::Time::now(); + ROS_INFO("STORE START"); last_response.response.data = "RECEIVED"; curr_message = *inputMsg; @@ -326,15 +316,11 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ else if(inputCmd == "LIFT"){ if(droneState == GROUND_IDLE){ //if given a point, update default_liftoff_pos - if(inputMsg->target.x != curr_target.position.x || inputMsg->target.y != curr_target.position.y || inputMsg->target.z != curr_target.position.z){ - default_liftoff_pos.pose.position.x = inputMsg->target.x; - default_liftoff_pos.pose.position.y = inputMsg->target.y; - default_liftoff_pos.pose.position.z = inputMsg->target.z; - } - else{ - default_liftoff_pos.pose.position.x = curr_position.pose.position.x; - default_liftoff_pos.pose.position.y = curr_position.pose.position.y; + 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{ @@ -348,10 +334,8 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ last_response.response.data = "ERROR"; } else; - hover_position = curr_position; - curr_target.position.x = hover_position.pose.position.x; - curr_target.position.y = hover_position.pose.position.y; - curr_target.position.z = hover_position.pose.position.z; + hover_position.pose = curr_position.pose; + curr_target.position = hover_position.pose.position; droneState = HOVERING; } else if(inputCmd == "LAND"){ @@ -379,14 +363,12 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ 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 SIZE STARTIN"); } 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); } - mav_trajectory_generation::Trajectory outputTraj; Eigen::Vector3d ending(inputMsg->target.x, inputMsg->target.y, inputMsg->target.z); - generateTraj(outputTraj, starting[0], starting[1], starting[2], ending[0], ending[1], ending[2]); + 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]; @@ -397,15 +379,24 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ endingPoint.pose.position.y = ending[1]; endingPoint.pose.position.z = ending[2]; - TrajectoryGroupedInfo* outputGroupedInfo = new TrajectoryGroupedInfo(outputTraj, startingPoint, endingPoint, static_cast(outputTraj.getSegmentTimes().at(0))); + TrajectoryGroupedInfo* outputGroupedInfo = new TrajectoryGroupedInfo(outputSegment, startingPoint, endingPoint, static_cast(outputSegment.getTime())); curr_trajectories.push(outputGroupedInfo); droneState = IN_TRANSIT; - last_response.response.data = "RECIEVED"; - std::cout << "Recived: " << curr_trajectories.size() << std::endl; } 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,.15) && !isStationary(curr_velocity, .25, .1)){ + 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; @@ -414,10 +405,9 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ currTrajTime = 0; currTime = 0; - mav_trajectory_generation::Trajectory outputTraj; - Eigen::Vector3d starting(curr_position.pose.position.x, curr_position.pose.position.y, curr_position.pose.position.z); + 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); - generateTraj(outputTraj, starting[0], starting[1], starting[2], ending[0], ending[1], ending[2]); + 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]; @@ -428,7 +418,7 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ endingPoint.pose.position.y = ending[1]; endingPoint.pose.position.z = ending[2]; - TrajectoryGroupedInfo* outputGroupedInfo = new TrajectoryGroupedInfo(outputTraj, startingPoint, endingPoint, static_cast(outputTraj.getSegmentTimes().at(0))); + TrajectoryGroupedInfo* outputGroupedInfo = new TrajectoryGroupedInfo(outputSegment, startingPoint, endingPoint, static_cast(outputSegment.getTime())); curr_trajectories.push(outputGroupedInfo); droneState = IN_TRANSIT; @@ -440,21 +430,22 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ last_response.response.data = "ERROR"; } multi_info_pub.publish(last_response); + std::cout << last_request.toSec() << std::endl; } -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 : "< Date: Mon, 22 Aug 2022 21:23:13 -0700 Subject: [PATCH 31/33] fine tuned traj --- helper/src/conversions.cpp | 2 +- src/src/multiDrone.cpp | 88 +++++++++++++++++++++++++------------- src/src/singleDrone.cpp | 25 +++++------ 3 files changed, 69 insertions(+), 46 deletions(-) diff --git a/helper/src/conversions.cpp b/helper/src/conversions.cpp index 71123d1..6c92c31 100644 --- a/helper/src/conversions.cpp +++ b/helper/src/conversions.cpp @@ -2,13 +2,13 @@ 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); result.header.stamp = ros::Time::now(); result.header.frame_id = "map"; - result.type_mask = 1024+2048; result.coordinate_frame = 1; diff --git a/src/src/multiDrone.cpp b/src/src/multiDrone.cpp index 6937365..9d71467 100644 --- a/src/src/multiDrone.cpp +++ b/src/src/multiDrone.cpp @@ -13,10 +13,14 @@ int main(int argc, char **argv) ros::Time last_request = ros::Time::now(); ros::Rate rate(20.0); - ros::Publisher drone_cmd_pub = nh.advertise - ("drone1/cmds", 0); - ros::Subscriber drone_info_sub = nh.subscribe - ("drone1/info", 0, storeInfo); + 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; @@ -33,7 +37,10 @@ int main(int argc, char **argv) ROS_INFO("LIFT info sent"); msg.command.data = "LIFT"; msg.target.z = 1; - drone_cmd_pub.publish(msg); + 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; } @@ -41,73 +48,94 @@ int main(int argc, char **argv) rate.sleep(); } while(ros::ok() && !transit7){ - if(!transit && ros::Time::now() - last_request >= ros::Duration(20.0)){ - ROS_INFO(" TRANSIT info sent"); + 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; - drone_cmd_pub.publish(msg); + 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(15.0)){ - ROS_INFO(" TRANSIT info sent"); + 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; - drone_cmd_pub.publish(msg); + 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 info sent"); + ROS_INFO(" TRANSIT ADD"); msg.command.data = "TRANSIT_ADD"; msg.target.x = 0; msg.target.y = 0; - msg.target.z = 1.5; - drone_cmd_pub.publish(msg); + 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(3.0)){ - ROS_INFO(" TRANSIT info sent"); + 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 = 1; - drone_cmd_pub.publish(msg); + 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 info sent"); + ROS_INFO(" TRANSIT NEW"); msg.command.data = "TRANSIT_NEW"; msg.target.x = 1; msg.target.y = -2; - msg.target.z = 1; - drone_cmd_pub.publish(msg); + 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 info sent"); + ROS_INFO(" TRANSIT ADD"); msg.command.data = "TRANSIT_ADD"; msg.target.x = 3; msg.target.y = -2; - msg.target.z = 1; - drone_cmd_pub.publish(msg); + 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(10.0)){ - ROS_INFO(" TRANSIT info sent"); + 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 = 4; - msg.target.y = 1; - msg.target.z = 1; - drone_cmd_pub.publish(msg); + 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"); diff --git a/src/src/singleDrone.cpp b/src/src/singleDrone.cpp index f91fe4f..9927bef 100644 --- a/src/src/singleDrone.cpp +++ b/src/src/singleDrone.cpp @@ -54,7 +54,7 @@ ros::Publisher local_pos_pub; std::queue curr_trajectories; float currTime; float currTrajTime; -const float timeStepSize = .05; +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); @@ -155,8 +155,7 @@ int main(int argc, char **argv) } } if(ros::Time::now() - last_request > ros::Duration(3)){ - std::cout << hover_position << std::endl; - last_request = ros::Time::now(); + last_request = ros::Time::now(); } local_pos_pub.publish(hover_position); if((reachedLocation(curr_position, hover_position, .1)) && (isStationary(curr_velocity, .05))){ @@ -168,7 +167,7 @@ int main(int argc, char **argv) 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(1.0)){ + if(ros::Time::now() - last_request > ros::Duration(5.0)){ ROS_INFO("HOVERING..."); last_request = ros::Time::now(); } @@ -186,12 +185,12 @@ int main(int argc, char **argv) if(currTrajTime == 0){// trajectory not loaded first_trajectory = curr_trajectories.front(); currTrajTime = first_trajectory->trajectoryTime; - currTime = timeStepSize*5; // starts calculating at .5 seconds, to prevent float point error + currTime = 0; curr_target = segmentToPoint(first_trajectory->segmentThis, currTime); mav_pub.publish(curr_target); } else{// trajectory loaded - if(reachedLocation(curr_position, curr_target, .2)){// if near end of target + if(reachedLocation(curr_position, curr_target, .35)){// if near end of target currTime += timeStepSize; if(currTime > currTrajTime){ currTime = currTrajTime; @@ -200,9 +199,8 @@ int main(int argc, char **argv) mav_pub.publish(curr_target); if(currTime == currTrajTime){ curr_trajectories.pop(); - // delete first_trajectory; + delete first_trajectory; currTrajTime = 0; - currTime = 0; } } else{ @@ -271,7 +269,6 @@ void updateVel(const geometry_msgs::TwistStamped::ConstPtr& inputPose){ } mav_trajectory_generation::Segment generateTraj(int bx, int by, int bz, int ex, int ey, int ez){ - ROS_INFO("GEN START"); Eigen::Vector3d startPoint(bx,by,bz); Eigen::Vector3d endPoint(ex,ey,ez); @@ -288,8 +285,8 @@ mav_trajectory_generation::Segment generateTraj(int bx, int by, int bz, int ex, vertices.push_back(end); std::vector segment_times; - const double v_max = 1.0; - const double a_max = 1.0; + 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 @@ -298,13 +295,11 @@ mav_trajectory_generation::Segment generateTraj(int bx, int by, int bz, int ex, opt.solveLinear(); opt.getSegments(&segments);// fills "segments" variable with segments of the path - ROS_INFO("GEN END"); return mav_trajectory_generation::Segment(segments[0]); } void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ ros::Time last_request = ros::Time::now(); - ROS_INFO("STORE START"); last_response.response.data = "RECEIVED"; curr_message = *inputMsg; @@ -363,6 +358,7 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ 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); @@ -391,7 +387,7 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ 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,.15) && !isStationary(curr_velocity, .25, .1)){ + while(!isFlat(curr_position,.05) && !isStationary(curr_velocity, .05, .05)){ local_pos_pub.publish(hover_position); ROS_INFO("STABLIZING"); ros::spinOnce(); @@ -430,7 +426,6 @@ void storeCommand(const drone_control::dcontrol::ConstPtr& inputMsg){ last_response.response.data = "ERROR"; } multi_info_pub.publish(last_response); - std::cout << last_request.toSec() << std::endl; } // void printTrajInfo(mav_trajectory_generation::Trajectory trajectory){ From c3cb7bc7f35e1557e30636c1923a812909a04341 Mon Sep 17 00:00:00 2001 From: Isean Bhanot Date: Tue, 23 Aug 2022 19:56:19 -0700 Subject: [PATCH 32/33] Updates & Grammar --- README.md | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 68b707c..9ac6191 100644 --- a/README.md +++ b/README.md @@ -56,7 +56,7 @@ This repository combines a trajectory planning, communication protocol, and robo --- ## 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. @@ -67,20 +67,26 @@ 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 Tip: Before launch run ```echo "export SVGA_VGPU10=0" >> ~/.bashrc``` and ```source ~/.bashrc```, to prevent VMWare REST Error below. [^8] -```roslaunch px4 mavros_posix_sitl.launch``` +```roslaunch drone_control fourDronesNodes.launch``` + +### Running drone# node(single drone control) +```rosrun drone_control single_control #(0-3)``` -### Running offboard_node -```rosrun simple_movements offboard_node``` +### 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 From ecdc4d493a6a6045fb3e16546f4fcdc28a92a4cb Mon Sep 17 00:00:00 2001 From: Isean Bhanot Date: Tue, 23 Aug 2022 19:58:14 -0700 Subject: [PATCH 33/33] Fixes highlighting --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index 9ac6191..25dd201 100644 --- a/README.md +++ b/README.md @@ -71,14 +71,19 @@ The test folder contains tests for the helper functions in the helper folder. So 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 Tip: Before launch run ```echo "export SVGA_VGPU10=0" >> ~/.bashrc``` and ```source ~/.bashrc```, to prevent VMWare REST Error below. [^8] +### 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)```