From 079d892afc975511a17412a22e5f17d4fc25d1bc Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com>
Date: Thu, 26 May 2022 11:35:33 +0200
Subject: [PATCH] converted data to numpy, not plottedt yet
---
.../README.md | 10 ++--
.../scripts/plot_results.py | 57 ++++++++++++++++++-
.../src/drone_node.cpp | 2 +-
3 files changed, 62 insertions(+), 7 deletions(-)
diff --git a/real_robot/coordination_formation_control_pkg/README.md b/real_robot/coordination_formation_control_pkg/README.md
index ec46318..0e9471c 100644
--- a/real_robot/coordination_formation_control_pkg/README.md
+++ b/real_robot/coordination_formation_control_pkg/README.md
@@ -14,8 +14,10 @@ TODO
implement modules
- [X] send command, take off, land
- [X] send command should include conversion to velocity
-. [ ] test omniwheels node
-- [ ] Implement swam controller
+- [X] test omniwheels node
+- [X] Implement swam controller
-- Dependencies https://github.com/ethz-asl/libnabo.git
-It uses this ibrary to implement a k nearest negibour search
+
+
+How to execute the plot result
+python plot_results.py ../results/experiment_1_trianglular_formation_crazyflie_1_logdata.bag crazyflie_1 3
diff --git a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
index 6634bc0..c07f746 100644
--- a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
+++ b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
@@ -1,8 +1,10 @@
import rosbag
import sys
+import numpy as np
+from matplotlib import pyplot as plt
-if(len(sys.argv) != 3):
+if(len(sys.argv) != 4):
print ("invalid number of arguments: " + str(len(sys.argv)))
print ("should be 2: 'plot_results.py' and 'bagName'")
print ("or just 1 : 'plot_results.py'")
@@ -10,9 +12,60 @@ if(len(sys.argv) != 3):
else:
bag_name = str(sys.argv[1]);
robot_name = str(sys.argv[2])
+ n_robots_logged = int(sys.argv[3])
+ print(n_robots_logged)
print("Plotting result from robot: " + robot_name + ", bag_name " + bag_name)
bag = rosbag.Bag(bag_name)
topic_name = robot_name+"result"
+pose_data = []
+vel_data = []
+command_pose_data = []
+command_twist_data = []
+input_data = []
+input_formation_data = []
+input_orientation_data = []
+input_navigation_data = []
+input_obstacle_data = []
+input_integration_data = []
+
+pose_row_ = [0] * n_robots_logged*2;
+twist_row_ = [0] * n_robots_logged*2;
+
+
+# converting data to numpy for easy plotting
for topics, msg, t in bag.read_messages(topics =[topic_name]):
- print(msg)
+ for ind,pose in enumerate(msg.current_pose):
+ pose_row_[ind*2] = pose.position.x
+ pose_row_[ind*2+1] = pose.position.y
+ twist_row_[ind*2] = msg.current_twist[ind].linear.x
+ twist_row_[ind*2+1] = msg.current_twist[ind].linear.y
+
+ pose_data.append(pose_row_);
+ vel_data.append(twist_row_);
+ command_pose_data.append([msg.command_pose.x, msg.command_pose.y, msg.command_pose.theta]);
+ command_twist_data.append([msg.command_twist.linear.x, msg.command_twist.linear.y]);
+ input_data.append([msg.input.x, msg.input.y])
+ input_formation_data.append([msg.input_formation.x, msg.input_formation.y])
+ input_orientation_data.append([msg.input_orientation.x, msg.input_orientation.y])
+ input_navigation_data.append([msg.input_navigation.x, msg.input_navigation.y])
+ input_obstacle_data.append([msg.input_obstacle.x, msg.input_obstacle.y])
+ input_integration_data.append([msg.input_integration.x, msg.input_integration.y])
bag.close()
+
+pose_data_np = np.array(pose_data);
+vel_data_np = np.array(vel_data);
+x_list = range(0,n_robots_logged*2,2);
+y_list = range(1,n_robots_logged*2,2);
+x_list_np = np.array(pose_data_np[:,x_list]);
+y_list_np = np.array(pose_data_np[:,y_list]);
+cent_np = np.vstack((np.mean(x_list_np, axis=1), np.mean(y_list_np,axis =1))).transpose()
+command_pose_np = np.array(command_pose_data);
+command_twist_np = np.array(command_twist_data);
+input_data_np = np.array(input_data);
+input_formation_data_np = np.array(input_formation_data);
+input_orientation_data_np = np.array(input_orientation_data);
+input_navigation_data_np = np.array(input_navigation_data);
+input_obstacle_data_np = np.array(input_obstacle_data);
+input_integration_data_np = np.array(input_integration_data);
+
+# Start plotting
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 5ee600f..20c1037 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -353,7 +353,7 @@ int main(int argc, char **argv) {
logdata->command_twist.linear.y = current_waypoint.p(1);
logdata->input.x = input(0,0);
logdata->input.y = input(1,0);
- logdata->input.x = input(0,1);
+ logdata->input_formation.x = input(0,1);
logdata->input_formation.y = input(1,1);
logdata->input_orientation.x = input(0,2);
logdata->input_orientation.y = input(1,2);
--
GitLab