added velocity and acceleration plot

real_robot/coordination_formation_control_pkg/msg/logData.msg

 std_msgs/Header header # time
 geometry_msgs/Pose[] current_pose
 geometry_msgs/Twist[] current_twist
+geometry_msgs/Twist[] current_acc
 geometry_msgs/Pose2D command_pose
 geometry_msgs/Twist command_twist
 geometry_msgs/Pose2D input

real_robot/coordination_formation_control_pkg/scripts/plot_results.py

@@ -17,8 +17,10 @@ else:
     print("Plotting result from robot: " + robot_name + ", bag_name " + bag_name)
 bag = rosbag.Bag(bag_name)
 topic_name = robot_name+"result"
+t_ = []
 pose_data = []
 vel_data = []
+acc_data = []
 command_pose_data = []
 command_twist_data = []
 input_data = []
@@ -30,6 +32,7 @@ input_integration_data = []
 
 pose_row_ = [0] * n_robots_logged*2;
 twist_row_ = [0] * n_robots_logged*2;
+acc_row_ = [0] * n_robots_logged*2;
 
 
 # converting data to numpy for easy plotting
@@ -39,9 +42,12 @@ for topics, msg, t in bag.read_messages(topics =[topic_name]):
         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
-
+        acc_row_[ind*2] = msg.current_acce[ind].linear.x
+        acc_row_[ind*2+1] = msg.current_acce[ind].linear.y
+    t_.append(msg.header.stamp.secs + msg.header.stamp.nsecs*1e-9);
     pose_data.append(pose_row_);
     vel_data.append(twist_row_);
+    acc_row_.append(acc_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])
@@ -52,13 +58,19 @@ for topics, msg, t in bag.read_messages(topics =[topic_name]):
     input_integration_data.append([msg.input_integration.x, msg.input_integration.y])
 bag.close()
 
+t_np = np.array(t_)
+# remove the offset
+t_np = t_np -t_np[0]
 pose_data_np = np.array(pose_data);
 vel_data_np = np.array(vel_data);
+acc_data_np = np.array(acc_row_);
+
 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()
+cent_pose_np = np.vstack((np.mean(x_list_np, axis=1), np.mean(y_list_np,axis =1))).transpose()
+cent_twist_np = np.vstack((np.mean(np.array(vel_data_np[:,x_list]), axis=1), np.mean(np.array(vel_data_np[:,y_list]),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);
@@ -69,3 +81,98 @@ input_obstacle_data_np = np.array(input_obstacle_data);
 input_integration_data_np = np.array(input_integration_data);
 
 # Start plotting
+####################### command vs centroid ###################################
+fig1, axs1 = plt.subplots(nrows=2, ncols=2, figsize=(7, 7))
+st = fig1.suptitle("Command swam state vc current swam state", fontsize="x-large")
+
+# vx
+axs1[0, 0].plot(t_np, command_pose_np[:,0])
+axs1[0, 0].plot(t_np, cent_pose_np[:,0])
+axs1[0, 0].set_ylabel("x(m)")
+axs1[0, 0].set_xlabel("time(s)")
+axs1[0, 0].legend(["command", "state"])
+
+
+axs1[0, 1].plot(t_np, command_pose_np[:,1])
+axs1[0, 1].plot(t_np, cent_pose_np[:,1])
+axs1[0, 1].set_ylabel("y(m)")
+axs1[0, 1].set_xlabel("time(s)")
+axs1[0, 1].legend(["command", "state"])
+
+
+axs1[1, 0].plot(t_np, command_twist_np[:,0])
+axs1[1, 0].plot(t_np, cent_twist_np[:,0])
+axs1[1, 0].set_ylabel("vx(m/s)")
+axs1[1, 0].set_xlabel("time(s)")
+axs1[1, 0].legend(["command", "state"])
+
+axs1[1, 1].plot(t_np, command_twist_np[:,1])
+axs1[1, 1].plot(t_np, cent_twist_np[:,1])
+axs1[1, 1].set_ylabel("Vy(m/s)")
+axs1[1, 1].set_xlabel("time(s)")
+axs1[1, 1].legend(["command", "state"])
+
+####################### Disance plot ###################################
+fig2 = plt.figure()
+ax2 = plt.axes()
+st = fig2.suptitle("Distance plot", fontsize="x-large")
+name_distance = [];
+for i in range(n_robots_logged):
+    for j in range(n_robots_logged):
+        if i != j:
+            name_distance.append("D" + str(i) + str(j))
+            dist_vec_np = pose_data_np[:,[i*2,i*2+1]]-pose_data_np[:,[j*2,j*2+1]]
+            dist_vec_np_norm = np.linalg.norm(dist_vec_np, axis =1)
+            ax2.plot(t_np,dist_vec_np_norm)
+
+ax2.legend(name_distance)
+####################### Velocity plot ###################################
+fig3, axs3 = plt.subplots(nrows=2, ncols=2, figsize=(7, 7))
+st = fig3.suptitle("Velocity ", fontsize="x-large")
+robot_names = []
+for i in range(n_robots_logged):
+    robot_names.append("robot_" + str(i+1))
+    vx = vel_data_np[:,i*2];
+    vy = vel_data_np[:,i*2+1]
+    axs3[0, 0].plot(t_np,vx )
+    axs3[0, 1].plot(t_np, vy)
+    axs3[1, 0].plot(t_np, np.sqrt(np.multiply(vx,vx)+np.multiply(vy,vy)))
+
+axs3[0, 0].set_ylabel("vx(m)")
+axs3[0, 0].set_xlabel("time(s)")
+axs3[0, 0].legend(robot_names)
+axs3[0, 1].set_ylabel("vy(m)")
+axs3[0, 1].set_xlabel("time(s)")
+axs3[0, 1].legend(robot_names)
+axs3[1, 0].set_ylabel("v(m/s)")
+axs3[1, 0].set_xlabel("time(s)")
+axs3[1, 0].legend(robot_names)
+
+
+####################### Acceleration plot ###################################
+fig4, axs4 = plt.subplots(nrows=2, ncols=2, figsize=(7, 7))
+st = fig3.suptitle("Accleration", fontsize="x-large")
+robot_names = []
+for i in range(n_robots_logged):
+    robot_names.append("robot_" + str(i+1))
+    vx = acc_data_np[:,i*2];
+    vy = acc_data_np[:,i*2+1]
+    axs3[0, 0].plot(t_np,vx )
+    axs3[0, 1].plot(t_np, vy)
+    axs3[1, 0].plot(t_np, np.sqrt(np.multiply(vx,vx)+np.multiply(vy,vy)))
+
+axs3[0, 0].set_ylabel("ax(m)")
+axs3[0, 0].set_xlabel("time(s)")
+axs3[0, 0].legend(robot_names)
+axs3[0, 1].set_ylabel("ay(m)")
+axs3[0, 1].set_xlabel("time(s)")
+axs3[0, 1].legend(robot_names)
+axs3[1, 0].set_ylabel("a(m/s)")
+axs3[1, 0].set_xlabel("time(s)")
+axs3[1, 0].legend(robot_names)
+
+
+# multple robot plot for comparision
+
+
+plt.show()