Skip to content
Snippets Groups Projects
Commit d3a4c8e3 authored by Stevedan Ogochukwu Omodolor's avatar Stevedan Ogochukwu Omodolor
Browse files

not finished swam controller

parent e7f668c7
Branches
No related tags found
No related merge requests found
Showing
with 138 additions and 12 deletions
...@@ -58,7 +58,7 @@ add_executable(mission_controller src/mission_controller.cpp src/robot_state.cpp ...@@ -58,7 +58,7 @@ add_executable(mission_controller src/mission_controller.cpp src/robot_state.cpp
add_dependencies(mission_controller ${PROJECT_NAME}_gencfg) add_dependencies(mission_controller ${PROJECT_NAME}_gencfg)
target_link_libraries(mission_controller ${catkin_LIBRARIES} ${Eigen_LIBRARIES}) 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}) target_link_libraries(drone_node ${catkin_LIBRARIES} ${Eigen_LIBRARIES})
......
...@@ -11,7 +11,7 @@ crazyflies: ...@@ -11,7 +11,7 @@ crazyflies:
channel: 80 channel: 80
initialPosition: [0.0, 0.0, 0.0] initialPosition: [0.0, 0.0, 0.0]
type: default type: default
# - id: 4 - id: 4
# channel: 80 channel: 80
# initialPosition: [0.0, 0.0, 0.0] initialPosition: [0.0, 0.0, 0.0]
# type: default type: default
n_drones: 3 n_drones: 4
hz_freq: 50 # controller frquency hz_freq: 50 # controller frquency
...@@ -7,6 +7,7 @@ ...@@ -7,6 +7,7 @@
#include "geometry_msgs/Pose.h" #include "geometry_msgs/Pose.h"
#include "crazyswarm/GenericLogData.h" #include "crazyswarm/GenericLogData.h"
#include "robot_state.h" #include "robot_state.h"
#include "tf/transform_datatypes.h"
// #include <message_filters/subscriber.h> // #include <message_filters/subscriber.h>
// #include <message_filters/time_synchronizer.h> // #include <message_filters/time_synchronizer.h>
......
...@@ -17,7 +17,7 @@ typedef enum robot_type ...@@ -17,7 +17,7 @@ typedef enum robot_type
/* robot state-ugv and agv*/ /* robot state-ugv and agv*/
typedef struct robot_state typedef struct robot_state
{ {
Vector3d q; /*position*/ Vector4d q; /*position 3 position 1 orientation yaw*/
Vector3d p; /* velocity*/ Vector3d p; /* velocity*/
}robot_state_t; }robot_state_t;
...@@ -27,6 +27,9 @@ typedef struct robot_ind ...@@ -27,6 +27,9 @@ typedef struct robot_ind
int id; /*d*/ int id; /*d*/
std::string robot_name; /* robot name*/ std::string robot_name; /* robot name*/
robot_type_t t; /*robot_type*/ 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; }robot_ind_t;
/* obstacle definition*/ /* obstacle definition*/
...@@ -37,6 +40,62 @@ typedef struct obstacle ...@@ -37,6 +40,62 @@ typedef struct obstacle
float radius; /*radius of the obstacle*/ float radius; /*radius of the obstacle*/
}obstacle_t; }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;
......
#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
...@@ -11,7 +11,7 @@ ...@@ -11,7 +11,7 @@
# Logging configuration (Use enable_logging to actually enable logging) # Logging configuration (Use enable_logging to actually enable logging)
genericLogTopics: ["position", "velocity"] genericLogTopics: ["position", "velocity"]
genericLogTopicFrequencies: [10,10] 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"] genericLogTopic_velocity_Variables: ["stateEstimateZ.vx", "stateEstimateZ.vy","stateEstimateZ.vz"]
# firmware parameters for all drones (use crazyflieTypes.yaml to set per type, or # firmware parameters for all drones (use crazyflieTypes.yaml to set per type, or
# allCrazyflies.yaml to set per drone) # allCrazyflies.yaml to set per drone)
......
...@@ -15,16 +15,16 @@ ...@@ -15,16 +15,16 @@
</node> </node>
</group> </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 name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="3" launch-prefix="xterm -e">
</node> </node>
</group> --> </group>
<!--
<group ns="uav_4"> <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 name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="4" launch-prefix="xterm -e">
</node> </node>
</group> --> </group>
......
...@@ -3,6 +3,7 @@ ...@@ -3,6 +3,7 @@
#include "ros/ros.h" #include "ros/ros.h"
#include "robot_state.h" #include "robot_state.h"
#include "shared_definitions.h" #include "shared_definitions.h"
#include "swamControllerAlg.h"
#include "coordination_formation_control_pkg/coordination.h" #include "coordination_formation_control_pkg/coordination.h"
#include "crazyswarm/Hover.h" #include "crazyswarm/Hover.h"
#include "crazyswarm/Land.h" #include "crazyswarm/Land.h"
...@@ -61,6 +62,9 @@ int main(int argc, char **argv) { ...@@ -61,6 +62,9 @@ int main(int argc, char **argv) {
/* TODO publisher to publish results and rosbag it*/ /* TODO publisher to publish results and rosbag it*/
/* intialize controller*/
/*create a list of drones to subscribe to*/ /*create a list of drones to subscribe to*/
std::vector<robotState *> uavs; std::vector<robotState *> uavs;
robot_ind_t uav_def; robot_ind_t uav_def;
......
...@@ -66,6 +66,14 @@ void robotState::stateCallbackUgvPose(const geometry_msgs::PoseConstPtr &pose_ms ...@@ -66,6 +66,14 @@ void robotState::stateCallbackUgvPose(const geometry_msgs::PoseConstPtr &pose_ms
this->state.q(0) = pose_msg->position.x; this->state.q(0) = pose_msg->position.x;
this->state.q(1) = pose_msg->position.y; this->state.q(1) = pose_msg->position.y;
this->state.q(2) = pose_msg->position.z; 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) void robotState::stateCallbackUavVel(const crazyswarm::GenericLogDataConstPtr &vel_msg)
{ {
...@@ -78,4 +86,6 @@ void robotState::stateCallbackUavPos(const crazyswarm::GenericLogDataConstPtr &p ...@@ -78,4 +86,6 @@ void robotState::stateCallbackUavPos(const crazyswarm::GenericLogDataConstPtr &p
this->state.q(0) = pose_msg->values[0]; this->state.q(0) = pose_msg->values[0];
this->state.q(1) = pose_msg->values[1]; this->state.q(1) = pose_msg->values[1];
this->state.q(2) = pose_msg->values[2]; this->state.q(2) = pose_msg->values[2];
this->state.q(3) = pose_msg->values[3]; // degres
} }
#include "swamControllerAlg.h"
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment