converted data to numpy, not plottedt yet

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

real_robot/coordination_formation_control_pkg/scripts/plot_results.py

 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

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);