From c3f999a00b0d93b996bab5828f6de968ab5580d0 Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com>
Date: Fri, 27 May 2022 08:19:11 +0200
Subject: [PATCH] included logging for acceleration
---
.../coordination_formation_control_pkg/src/drone_node.cpp | 7 ++++++-
.../src/omniwheel_node.cpp | 6 +++++-
2 files changed, 11 insertions(+), 2 deletions(-)
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 20c1037..b873bbb 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -71,6 +71,8 @@ int main(int argc, char **argv) {
/* current implementaiton is in 2D*/
Eigen::MatrixXf uav_state_q(2,n_drones);
Eigen::MatrixXf uav_state_p(2,n_drones);
+ Eigen::MatrixXf uav_state_a(2,n_drones);
+
// obstacle_t *current_obs = new obstacle_t[n_obs]();
std::vector<obstacle_t> current_obs;
current_obs.resize(n_obs);
@@ -225,7 +227,8 @@ int main(int argc, char **argv) {
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);
-
+ uav_state_a(0, i-1) = current_state.a(0);
+ uav_state_a(1, i-1) = current_state.a(1);
}
/* update reference point*/
current_waypoint.q(0) = next_waypoint->pos[0] ; /*position 2 position 1 orientation yaw*/
@@ -345,6 +348,8 @@ int main(int argc, char **argv) {
logdata->current_pose[i].position.y = uav_state_q(1,i);
logdata->current_twist[i].linear.x = uav_state_p(0,i);
logdata->current_twist[i].linear.y = uav_state_p(1,i);
+ logdata->current_acc[i].linear.x = uav_state_a(0,i);
+ logdata->current_acc[i].linear.y = uav_state_a(1,i);
}
logdata->command_pose.x = current_waypoint.q(0);
logdata->command_pose.y = current_waypoint.q(1);
diff --git a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
index b698889..756bb0e 100644
--- a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
@@ -66,6 +66,7 @@ int main(int argc, char **argv) {
/* vector of ugvs*/
Eigen::MatrixXf ugv_state_q(2,n_ugvs);
Eigen::MatrixXf ugv_state_p(2,n_ugvs);
+ Eigen::MatrixXf ugv_state_a(2,n_ugvs);
/* obstacle = number of obstacle + the other ugvs*/
int n_obs_ugv = (n_obs) + (n_ugvs-1);
@@ -207,6 +208,8 @@ int main(int argc, char **argv) {
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);
+ ugv_state_a(0, i-1) = current_state.a(0);
+ ugv_state_a(1, i-1) = current_state.a(1);
}
@@ -315,7 +318,8 @@ int main(int argc, char **argv) {
logdata->current_pose[i].position.y = ugv_state_q(1,i);
logdata->current_twist[i].linear.x = ugv_state_p(0,i);
logdata->current_twist[i].linear.y = ugv_state_p(1,i);
-
+ logdata->current_acc[i].linear.x = ugv_state_a(0,i);
+ logdata->current_acc[i].linear.y = ugv_state_a(1,i);
}
logdata->command_pose.x = current_waypoint.q(0);
--
GitLab