diff --git a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch index f1764b5ffcdd08264ccc8928da671fdeac28b485..407ec50e38f0dc5b0a9c8cb0ec696c2d4e069e50 100644 --- a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch +++ b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch @@ -14,7 +14,7 @@ </group> <group ns="uav_2"> - <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="2" launch-prefix="xterm -e "> + <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="2" launch-prefix="xterm -e gdb --args "> </node> </group> <group ns="uav_3"> diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1_trianglular_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_1_trianglular_formation_crazyflie_1_logdata.bag index 6a388155d749283cf6da36cea8f6623e4bad4edc..f935def08549ce95538cf126e91bc06c8d7f9762 100644 Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_1_trianglular_formation_crazyflie_1_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_1_trianglular_formation_crazyflie_1_logdata.bag differ diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1_trianglular_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_1_trianglular_formation_crazyflie_2_logdata.bag index ed6cec37554d918d3dafc28d3528bd415549863c..fc48456c5dd95b6e6d33aa537bfcd8512e5f95a0 100644 Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_1_trianglular_formation_crazyflie_2_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_1_trianglular_formation_crazyflie_2_logdata.bag differ diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1_trianglular_formation_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_1_trianglular_formation_crazyflie_3_logdata.bag index 78ff8ead0abcf0bfedfafd098d0eceebdd202e49..ed66c636d7a639a4a7955ec42ea130c1833f3a4f 100644 Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_1_trianglular_formation_crazyflie_3_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_1_trianglular_formation_crazyflie_3_logdata.bag differ diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1_trianglular_formation_crazyflie_4_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_1_trianglular_formation_crazyflie_4_logdata.bag deleted file mode 100644 index 31c590f1fac983b957377ac819a7454f35f9d06e..0000000000000000000000000000000000000000 Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_1_trianglular_formation_crazyflie_4_logdata.bag and /dev/null differ 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 f705aaff2b3e8de7ed75f82a46f3793649a3b387..d703a8562749e2018c99457b3208d9a834b17d57 100644 --- a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py +++ b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py @@ -42,12 +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 + acc_row_[ind*2] = msg.current_acc[ind].linear.x + acc_row_[ind*2+1] = msg.current_acc[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_); + acc_data.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]) @@ -63,8 +63,8 @@ t_np = np.array(t_) 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_); - +acc_data_np = np.array(acc_data); +print(acc_data_np) 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]); @@ -155,21 +155,21 @@ 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) + ax = acc_data_np[:,i*2]; + ay = acc_data_np[:,i*2+1] + axs4[0, 0].plot(t_np,ax ) + axs4[0, 1].plot(t_np, ay) + axs4[1, 0].plot(t_np, np.sqrt(np.multiply(ax,ax)+np.multiply(ay,ay))) + +axs4[0, 0].set_ylabel("ax(m)") +axs4[0, 0].set_xlabel("time(s)") +axs4[0, 0].legend(robot_names) +axs4[0, 1].set_ylabel("ay(m)") +axs4[0, 1].set_xlabel("time(s)") +axs4[0, 1].legend(robot_names) +axs4[1, 0].set_ylabel("a(m/s)") +axs4[1, 0].set_xlabel("time(s)") +axs4[1, 0].legend(robot_names) # multple robot plot for comparision 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 b873bbb8ec55073990602c08ca76f682daed1992..80d41a618ad635bac4054adf707b8c5c6861316c 100644 --- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp +++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp @@ -59,6 +59,7 @@ int main(int argc, char **argv) { logdata = new coordination_formation_control_pkg::logData(); logdata->current_pose.resize(n_drones); logdata->current_twist.resize(n_drones); + logdata->current_acc.resize(n_drones); /* current command*/ crazyswarm::Hover current_command; 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 756bb0ea801173331b7628dbe44cbcbfc03c69d2..23015271dfa0b4fb084532a7e2007bdefb2b8e3b 100644 --- a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp +++ b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp @@ -52,6 +52,7 @@ int main(int argc, char **argv) { logdata = new coordination_formation_control_pkg::logData(); logdata->current_pose.resize(n_ugvs); logdata->current_twist.resize(n_ugvs); + logdata->current_acc.resize(n_ugvs); /*current command*/ geometry_msgs::Twist current_command;