From d3a4c8e3dc31a7883eec2f58e2c23a6932d60dea Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com> Date: Sat, 14 May 2022 13:07:25 +0200 Subject: [PATCH] not finished swam controller --- .../CMakeLists.txt | 2 +- .../config/crazyfliesConfig.yaml | 8 +-- .../config/formationConfig.yaml | 0 .../config/missionConfig.yaml | 2 +- .../include/robot_state.h | 1 + .../include/shared_definitions.h | 61 ++++++++++++++++++- .../include/swamControllerAlg.h | 51 ++++++++++++++++ .../launch/bringup_crazyflie_servers.launch | 2 +- .../launch/mission_controller.launch | 8 +-- .../src/drone_node.cpp | 4 ++ .../src/robot_state.cpp | 10 +++ .../src/swamControllerAlg.cpp | 1 + 12 files changed, 138 insertions(+), 12 deletions(-) create mode 100644 real_robot/coordination_formation_control_pkg/config/formationConfig.yaml create mode 100644 real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h create mode 100644 real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp diff --git a/real_robot/coordination_formation_control_pkg/CMakeLists.txt b/real_robot/coordination_formation_control_pkg/CMakeLists.txt index 4e8db4f..fdca1de 100644 --- a/real_robot/coordination_formation_control_pkg/CMakeLists.txt +++ b/real_robot/coordination_formation_control_pkg/CMakeLists.txt @@ -58,7 +58,7 @@ add_executable(mission_controller src/mission_controller.cpp src/robot_state.cpp add_dependencies(mission_controller ${PROJECT_NAME}_gencfg) target_link_libraries(mission_controller ${catkin_LIBRARIES} ${Eigen_LIBRARIES}) -add_executable(drone_node src/drone_node.cpp src/robot_state.cpp) +add_executable(drone_node src/drone_node.cpp src/robot_state.cpp src/swamControllerAlg.cpp) target_link_libraries(drone_node ${catkin_LIBRARIES} ${Eigen_LIBRARIES}) diff --git a/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml index 8d7912f..b0351bc 100644 --- a/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml +++ b/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml @@ -11,7 +11,7 @@ crazyflies: channel: 80 initialPosition: [0.0, 0.0, 0.0] type: default -# - id: 4 -# channel: 80 -# initialPosition: [0.0, 0.0, 0.0] -# type: default +- id: 4 + channel: 80 + initialPosition: [0.0, 0.0, 0.0] + type: default diff --git a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml new file mode 100644 index 0000000..e69de29 diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml index f4f82b5..e915a40 100644 --- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml +++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml @@ -1,2 +1,2 @@ -n_drones: 3 +n_drones: 4 hz_freq: 50 # controller frquency diff --git a/real_robot/coordination_formation_control_pkg/include/robot_state.h b/real_robot/coordination_formation_control_pkg/include/robot_state.h index 3c446f2..4f9550b 100644 --- a/real_robot/coordination_formation_control_pkg/include/robot_state.h +++ b/real_robot/coordination_formation_control_pkg/include/robot_state.h @@ -7,6 +7,7 @@ #include "geometry_msgs/Pose.h" #include "crazyswarm/GenericLogData.h" #include "robot_state.h" +#include "tf/transform_datatypes.h" // #include <message_filters/subscriber.h> // #include <message_filters/time_synchronizer.h> diff --git a/real_robot/coordination_formation_control_pkg/include/shared_definitions.h b/real_robot/coordination_formation_control_pkg/include/shared_definitions.h index e6bd0e8..8934bb8 100644 --- a/real_robot/coordination_formation_control_pkg/include/shared_definitions.h +++ b/real_robot/coordination_formation_control_pkg/include/shared_definitions.h @@ -17,7 +17,7 @@ typedef enum robot_type /* robot state-ugv and agv*/ typedef struct robot_state { - Vector3d q; /*position*/ + Vector4d q; /*position 3 position 1 orientation yaw*/ Vector3d p; /* velocity*/ }robot_state_t; @@ -27,6 +27,9 @@ typedef struct robot_ind int id; /*d*/ std::string robot_name; /* robot name*/ robot_type_t t; /*robot_type*/ + double safe_ditance; /* important to prevent mistakenly putting a very small number*/ + double robot_size; /* also for safety reasons*/ + bool leader; }robot_ind_t; /* obstacle definition*/ @@ -37,6 +40,62 @@ typedef struct obstacle float radius; /*radius of the obstacle*/ }obstacle_t; +/* goal waypoint*/ +typedef struct waypoint +{ + Vector4d q; /*position 3 position 1 orientation yaw*/ + Vector3d p; /* velocity*/ + MatrixXf cluster; + +}waypoint_t; + +/* formation configuraiton*/ + + + + +/* formation gain*/ +typedef struct form_gains +{ + double c1_alpha; /*formation gains*/ + double c2_alpha; + double c1_beta; /* consesus gains*/ + double c2_beta; + double c1_gamma; /* navigation gains*/ + double c2_gamma; + double c1_theta; /* orientation gains*/ + double c1_delta; /* integral gains*/ + double c2_delta; + double c3_delta; + +}form_gains_t; + +/* formation parameters*/ +typedef struct form_param +{ + double d; /* distance between agent*/ + double formation_t; /* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/ + double k; + double ratio; /* ratio between dist inter and obstacle*/ + double eps; + double a; + double b; + double h_alpha; + double h_beta; + double d_obs; + double nav_type; /* convergence approach -1, parallel approach 2*/ + double integrator; /* single/ double integrator*/ + double dt; /* sample time*/ + double int_max; /* maximum integral*/ +}form_param_t; + +/* formation configuraiton*/ +typedef struct formation_config +{ + form_gains_t gain; + form_param_t param; +}formation_config_t; + diff --git a/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h b/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h new file mode 100644 index 0000000..2526e7b --- /dev/null +++ b/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h @@ -0,0 +1,51 @@ +#ifndef SWAM_CONTROLLER_ALG_UAV_H +#define SWAM_CONTROLLER_ALG_UAV_H + +#include "shared_definitions.h" + +typedef Eigen::Matrix<double, 2,5> input_vector_t; +/* input vector conatains the following + * totall input x y + * ofr debugging I print the remaining input + * input_formation + * input obstacle + * input navigation + * input orientation + */ + + typedef Eigen::Matrix <double, 2, 1>row_vector_2d_t; + +class swamControllerAlg +{ +private: + formation_config_t form_param; /*parameters of the formation*/ + row_vector_2d_t integral_error; + row_vector_2d_t previous_error; + robot_ind_t robot_id; + +public: + /* constructor*/ + swamControllerAlg(robot_ind_t indentification, formation_config_t formation_config); + /* destructor*/ + ~swamControllerAlg(); + /* includes the main controller*/ + input_vector_t controller(Eigen::MatrixXf *q, Eigen::MatrixXf *p, waypoint_t ref, obstacle_t* obs); + +protected: + /* configuration formation information like interagent distances*/ + void configure_formation(); + + + +}; + + + + + + + + + + +#endif diff --git a/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch b/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch index 689252c..0c023f1 100644 --- a/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch +++ b/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch @@ -11,7 +11,7 @@ # Logging configuration (Use enable_logging to actually enable logging) genericLogTopics: ["position", "velocity"] genericLogTopicFrequencies: [10,10] - genericLogTopic_position_Variables: ["stateEstimateZ.x", "stateEstimateZ.y","stateEstimateZ.z"] + genericLogTopic_position_Variables: ["stateEstimateZ.x", "stateEstimateZ.y","stateEstimateZ.z", "stateEstimate.yaw"] genericLogTopic_velocity_Variables: ["stateEstimateZ.vx", "stateEstimateZ.vy","stateEstimateZ.vz"] # firmware parameters for all drones (use crazyflieTypes.yaml to set per type, or # allCrazyflies.yaml to set per drone) diff --git a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch index 77d5b34..141806d 100644 --- a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch +++ b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch @@ -15,16 +15,16 @@ </node> </group> - <!-- <group ns="uav_3"> + <group ns="uav_3"> <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="3" launch-prefix="xterm -e"> </node> - </group> --> -<!-- + </group> + <group ns="uav_4"> <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="4" launch-prefix="xterm -e"> </node> - </group> --> + </group> diff --git a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp index c2d1b1f..879445d 100644 --- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp +++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp @@ -3,6 +3,7 @@ #include "ros/ros.h" #include "robot_state.h" #include "shared_definitions.h" +#include "swamControllerAlg.h" #include "coordination_formation_control_pkg/coordination.h" #include "crazyswarm/Hover.h" #include "crazyswarm/Land.h" @@ -61,6 +62,9 @@ int main(int argc, char **argv) { /* TODO publisher to publish results and rosbag it*/ + /* intialize controller*/ + + /*create a list of drones to subscribe to*/ std::vector<robotState *> uavs; robot_ind_t uav_def; diff --git a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp index 05caa22..4f33c59 100644 --- a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp +++ b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp @@ -66,6 +66,14 @@ void robotState::stateCallbackUgvPose(const geometry_msgs::PoseConstPtr &pose_ms this->state.q(0) = pose_msg->position.x; this->state.q(1) = pose_msg->position.y; this->state.q(2) = pose_msg->position.z; + double roll, pitch,yaw; + tf::Quaternion quat(pose_msg->orientation.x, + pose_msg->orientation.y, + pose_msg->orientation.z, + pose_msg->orientation.w); + + tf::Matrix3x3(quat).getRPY(roll, pitch,yaw); + this->state.q(3) = yaw*180.0/M_PI; // degres to mantain consistency } void robotState::stateCallbackUavVel(const crazyswarm::GenericLogDataConstPtr &vel_msg) { @@ -78,4 +86,6 @@ void robotState::stateCallbackUavPos(const crazyswarm::GenericLogDataConstPtr &p this->state.q(0) = pose_msg->values[0]; this->state.q(1) = pose_msg->values[1]; this->state.q(2) = pose_msg->values[2]; + this->state.q(3) = pose_msg->values[3]; // degres + } diff --git a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp new file mode 100644 index 0000000..fe76f27 --- /dev/null +++ b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp @@ -0,0 +1 @@ +#include "swamControllerAlg.h" -- GitLab