From c84a23ecb138416b92247f3cd0bf9ccfe4805ae8 Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com>
Date: Sun, 15 May 2022 11:51:11 +0200
Subject: [PATCH] mission controller finished,remaining swamcontroller
implementation
---
.../CMakeLists.txt | 7 +-
.../cfg/missionController.cfg | 7 +
.../config/missionConfig.yaml | 7 +
.../include/mission_controller.h | 9 +-
.../src/mission_controller.cpp | 191 ++++++++++++++++--
5 files changed, 205 insertions(+), 16 deletions(-)
diff --git a/real_robot/coordination_formation_control_pkg/CMakeLists.txt b/real_robot/coordination_formation_control_pkg/CMakeLists.txt
index cc3cc07..eacdbb6 100644
--- a/real_robot/coordination_formation_control_pkg/CMakeLists.txt
+++ b/real_robot/coordination_formation_control_pkg/CMakeLists.txt
@@ -19,6 +19,7 @@ find_package(catkin)
## System dependencies are found with CMake's conventions
#find_package(Boost REQUIRED COMPONENTS system)
find_package (Eigen3 REQUIRED)
+find_package(OpenCV REQUIRED)
## Generate dynamic reconfigure parameters in the 'cfg' folder
@@ -56,13 +57,13 @@ include_directories(
## The recommended prefix ensures that target names across packages don't collide
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})
+target_link_libraries(mission_controller ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${OpenCV_LIBS})
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} ${OpenCV_LIBS})
add_executable(omniwheel_node src/omniwheel_node.cpp src/robot_state.cpp src/swamControllerAlg.cpp)
-target_link_libraries(omniwheel_node ${catkin_LIBRARIES} ${Eigen_LIBRARIES})
+target_link_libraries(omniwheel_node ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${OpenCV_LIBS})
install(DIRECTORY
diff --git a/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg b/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg
index 9e16cab..153ee98 100644
--- a/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg
+++ b/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg
@@ -9,6 +9,13 @@ demo.add("shut_down", bool_t, 0,
demo.add("initialize_all_robot", bool_t, 0, "start all robot", False)
demo.add("start_formation", bool_t, 0, "Start formation controller", False)
demo.add("stop", bool_t, 0, "stop formation ", False)
+demo.add("mantain_position", bool_t, 0, "mantain goal centroid", False)
+demo.add("update_waypoint", bool_t, 0, "update waypoint", False)
+demo.add("x", double_t, 0, "desired goal", 0.0, -3,-3)
+demo.add("y", double_t, 0, "desired goal", 0.0, -3,-3)
+demo.add("vx", double_t, 0, "desired goal", 0.0,-3,-3)
+demo.add("vy", double_t, 0, "desired goal", 0.0, -3,-3)
+demo.add("orientation", double_t, 0, "desired goal", 0, 0,360)
exit(gen.generate(PACKAGE, "coordination_formation_control_pkg", "missionController"))
diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
index 177b3db..3c5843c 100644
--- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
@@ -4,3 +4,10 @@ hz_freq: 50 # controller frquency
include_obstacle: false
include_orientation: false
n_obs: 0
+
+# waypoint
+# x: 0
+# y: 0
+# orienation: 0
+# vx: 0
+# vy: 0
diff --git a/real_robot/coordination_formation_control_pkg/include/mission_controller.h b/real_robot/coordination_formation_control_pkg/include/mission_controller.h
index 3e8a40b..1a5005a 100644
--- a/real_robot/coordination_formation_control_pkg/include/mission_controller.h
+++ b/real_robot/coordination_formation_control_pkg/include/mission_controller.h
@@ -7,7 +7,11 @@
#include <coordination_formation_control_pkg/missionControllerConfig.h>
#include <coordination_formation_control_pkg/coordination.h>
#include <coordination_formation_control_pkg/waypoint.h>
-
+#include "opencv2/highgui.hpp"
+#include "opencv2/core.hpp"
+#include "opencv2/imgproc.hpp"
+using namespace cv;
+using namespace cv;
#define ENABLE_LATCH true
/* state machine mission_controller*/
@@ -30,6 +34,7 @@ class missionController
/* execute main process*/
void executeMainProcess();
void updateCofig(coordination_formation_control_pkg::missionControllerConfig &config, uint32_t level);
+ Eigen::MatrixXf compute_cluster(Eigen::MatrixXf uav_q, Eigen::MatrixXf ugv_q,int num_ugvs, int num_uavs);
private:
fsm_mc_t state;
@@ -37,6 +42,8 @@ class missionController
bool initialize_all_robot;
bool start_formation;
bool stop;
+ bool mantain_position;
+ bool start_publishing_waypoint;
ros::NodeHandle nh;
/*dynamic reconfigure*/
diff --git a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
index a1db334..d59a890 100644
--- a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
@@ -10,6 +10,7 @@ nh(nh)
this->initialize_all_robot = false;
this->start_formation = false;
this->stop = false;
+ this->mantain_position = true;
/*publisher*/
this->coordination_publisher = this->nh.advertise<coordination_formation_control_pkg::coordination>("coordination_config",1);
@@ -17,10 +18,9 @@ nh(nh)
this->current_configuration.init_formation = false;
this->current_configuration.initialize_robot = false;
this->waypoint_publisher = this->nh.advertise<coordination_formation_control_pkg::waypoint>("goal_waypoint",1);
- /* initialize drone list subscirbers*/
-
- /* initialize ugv list*/
-
+ this->current_waypoint.vel[0] = 0;
+ this->current_waypoint.vel[1] = 0;
+ this->start_publishing_waypoint = false;
/*dynamic_reconfigure*/
@@ -41,8 +41,11 @@ void missionController::executeMainProcess()
{
ros::Rate loop_rate(100);
/* create a list of drones*/
- int n_drones;
+ int n_drones, n_ugvs;
this->nh.getParam("/n_drones", n_drones);
+ this->nh.getParam("/n_ugvs", n_ugvs);
+
+
std::vector<robotState *> uavs;
robot_ind_t uav_def;
bool sucess_initialization;
@@ -61,18 +64,56 @@ void missionController::executeMainProcess()
EXIT_FAILURE;
}
}
+ /* create a list of ugvs*/
+ std::vector<robotState *> ugvs;
+ robot_ind_t ugv_def;
+ for (int i = 1; i <= n_ugvs; i++)
+ {
+ ugv_def.robot_name = "omniwheel";
+ ugv_def.t = OMNIWHEELS;
+ ugv_def.id = i;
+ sucess_initialization = false;
+ auto * ugv = new robotState(ugv_def, nh, sucess_initialization);
+ ugvs.push_back(ugv);
+ if (sucess_initialization == false)
+ {
+ ROS_ERROR("Initialization: ugv %d produced an error in mission controller", ugv_def.id);
+ EXIT_FAILURE;
+ }
+ }
+
+ Eigen::MatrixXf current_cluster(2,n_ugvs);
+ row_vector_2d_t current_centroid_vel,goal_centroid,current_centroid, error;
+ float dist;
while(ros::ok())
{
- // test print all the drones states
- // for(int i = 1; i <=n_drones; i++)
- // {
- // robot_state_t current_state;
- // current_state = uavs[i-1]->get_state();
- // std::cout << "id: " << uavs[i-1]->id << "state: " << current_state.q << std::endl;
- // }
+ Eigen::MatrixXf uav_state_q(2,n_drones);
+ Eigen::MatrixXf uav_state_p(2,n_drones);
+ Eigen::MatrixXf ugv_state_q(2,n_ugvs);
+ Eigen::MatrixXf ugv_state_p(2,n_ugvs);
+ for(int i = 1; i <=n_drones; i++)
+ {
+ robot_state_t current_state;
+ current_state = uavs[i-1]->get_state();
+ uav_state_q(0, i-1) =current_state.q(0);
+ uav_state_q(1, i-1) =current_state.q(1);
+ uav_state_p(0, i-1) = current_state.p(0);
+ uav_state_p(1, i-1) = current_state.p(1);
+ }
+ /*get current state of all the ugv*/
+ for(int i = 1; i <=n_ugvs; i++)
+ {
+ robot_state_t current_state;
+ current_state = ugvs[i-1]->get_state();
+ ugv_state_q(0, i-1) =current_state.q(0);
+ ugv_state_q(1, i-1) =current_state.q(1);
+ ugv_state_p(0, i-1) = current_state.p(0);
+ ugv_state_p(1, i-1) = current_state.p(1);
+
+ }
if(this->shut_down)
{
@@ -107,6 +148,9 @@ void missionController::executeMainProcess()
this->current_configuration.stop_demo = false;
ROS_INFO("Mission_controller State: START_FORMATION");
+ /*make sure that we mantain current entroid if no centorid has been sent*/
+ // this->current_waypoint.pos[0] = uav_state_q.row(0).mean();
+ // this->current_waypoint.pos[1] = uav_state_q.row(1).mean();
this->state = START_FORMATION;
@@ -130,13 +174,53 @@ void missionController::executeMainProcess()
case START_FORMATION:
/* compute waypoint formation*/
+ /* initial waypoint will be the centorid of the current configuration*/
+ current_centroid_vel(0) = uav_state_p.row(0).mean();
+ current_centroid_vel(1) = uav_state_p.row(1).mean();
+ current_centroid(0) = uav_state_q.row(0).mean();
+ current_centroid(1) = uav_state_q.row(1).mean();
+ goal_centroid(0) = current_waypoint.pos[0];
+ goal_centroid(1) = current_waypoint.pos[1];
+
+ error = (goal_centroid-current_centroid);
+ dist = error.norm();
/* compute cluster*/
+ current_cluster = this->compute_cluster(uav_state_q, ugv_state_q, n_ugvs,n_drones);
+
+ /* assign clusters*/
+ for(int i = 0; i < n_ugvs; i++)
+ {
+ this->current_waypoint.cluster_pos.push_back(current_cluster(0,i));
+ this->current_waypoint.cluster_pos.push_back(current_cluster(1,i));
+ }
+ this->current_waypoint.cluster_vel[0] = current_centroid_vel(0);
+ this->current_waypoint.cluster_vel[1] = current_centroid_vel(1);
+
/* send goals*/
+ if (dist < 0.1) // waypoint reached TODO, this is hard coded, not a good idea
+ {
+ if(this->mantain_position == false)
+ {
+ this->current_waypoint.stop = true;
+ }
+ else
+ {
+ this->current_waypoint.stop = false;
+ /* if we want to mantain centroid, vel to 0*/
+ this->current_waypoint.vel[0] = 0;
+ this->current_waypoint.vel[1] = 0;
+
+ }
+ }
/* update waypoints*/
+ if (start_publishing_waypoint)
+ {
+ waypoint_publisher.publish(this->current_waypoint);
+ }
if(this->stop)
{
@@ -193,8 +277,91 @@ void missionController::updateCofig(coordination_formation_control_pkg::missionC
this->shut_down = true;
config.shut_down = false;
}
+ if(config.update_waypoint)
+ {
+ config.update_waypoint = false;
+ current_waypoint.pos[0] = config.x;
+ current_waypoint.pos[1] = config.y;
+ current_waypoint.orientation = config.orientation;
+ current_waypoint.vel[0] = config.vx;
+ current_waypoint.vel[1] = config.vy;
+ this->mantain_position = config.mantain_position;
+ this->start_publishing_waypoint = true;
+
+
+ }
}
+Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen::MatrixXf ugv_q,int num_ugvs, int num_uavs)
+{
+ /* only implemented for two ugv*/
+ if(num_ugvs > 2)
+ {
+ ROS_INFO("n_ugv is only implemented for 2 ugv, should modify for more ugvs");
+
+ this->stop = true;
+
+ }
+ else
+ {
+ std::vector<Point2f> centers;
+ for (int i = 0; i <2; i++)
+ {
+ /* make intial ugv be the centroid */
+ Point2f center;
+ center.x = ugv_q(0,i);
+ center.y = ugv_q(1,i);
+ centers.push_back(center);
+ }
+ /* points*/
+ std::vector<Point2f> points;
+
+ for(int t = 0; t < num_uavs; t++)
+ {
+ Point2f point;
+ point.x = uav_q(0,t);
+ point.y = uav_q(1,t);
+ points.push_back(point);
+ }
+ Mat labels;
+ int cluster_cout = num_ugvs;
+ double compactness = kmeans(points, cluster_cout, labels,
+ TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 10, 1.0),
+ 10, KMEANS_PP_CENTERS, centers);
+
+ /* decide to change or not*/
+ /* we calculate the enegy to mantain the current confguration or change*/
+ Eigen::MatrixXf centroids(2,num_ugvs);
+ for(int p = 0; p <num_ugvs; p++)
+ {
+ centroids(0,p) = centers[p].x;
+ centroids(1,p) = centers[p].y;
+ }
+
+ float man1 = (centroids.col(0) - ugv_q.col(0)).norm();
+ float man2 = (centroids.col(1) - ugv_q.col(0)).norm();
+ float cha1 = (centroids.col(1) - ugv_q.col(1)).norm();
+ float cha2 = (centroids.col(0) - ugv_q.col(1)).norm();
+
+ if( cha1 < man1)
+ {
+ if(cha2 <man2)
+ {
+ Eigen::MatrixXf dummy_centroid(2,num_ugvs);
+ dummy_centroid = centroids;
+ /* switch centroids*/
+ centroids.col(0) = dummy_centroid.col(1);
+ centroids.col(1) = dummy_centroid.col(0);
+
+
+ }
+ }
+
+ return centroids;
+
+ }
+
+}
int main(int argc, char **argv)
{
--
GitLab