diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_1_logdata.bag index eccb7117fd4d31ba6a693f8bc6ecc6e5489a7e24..74e20c01a3248768a2ecd0511146b41c9ea5bf53 100644 Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_1_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_1_logdata.bag differ diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_2_logdata.bag index 8ee0186eb0b0d41eec31c80d66f04d9585362a95..9ea81e6d87d8792f9f0fd5bbb5816c54559ddffb 100644 Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_2_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_2_logdata.bag differ diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_3_logdata.bag index 027965c29d555ad7d39d362b32a0d27e04de7e8c..9514750b3d8a9eb85167f5475afc56d268d2658a 100644 Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_3_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_3_logdata.bag differ diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_1_logdata.bag index b90896bb766ee03d414658cfe71987d50b268d7d..35def27d1c69aa081d999250c8590d2032f5752f 100644 Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_1_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_1_logdata.bag differ diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_2_logdata.bag index 66fd6291d7740aa0fef43c5784d5ee3f3b773131..b66f35f94372af493b07a90d8014b3b2c3b5ef6f 100644 Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_2_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_2_logdata.bag differ diff --git a/real_robot/coordination_formation_control_pkg/scripts/plot_real_experiment_pgf_plot.py b/real_robot/coordination_formation_control_pkg/scripts/plot_real_experiment_pgf_plot.py index 862f361ae8e1131cc47a8ce620080993b7b0e7e6..6eb011322452d18d8f9ec49c9c89faed97ee67d5 100644 --- a/real_robot/coordination_formation_control_pkg/scripts/plot_real_experiment_pgf_plot.py +++ b/real_robot/coordination_formation_control_pkg/scripts/plot_real_experiment_pgf_plot.py @@ -24,8 +24,8 @@ from matplotlib import pyplot as plt ### obtain information from topic # basic configurations result_folder_name = "/home/spot/masters_thesis_stevedan/formation_coordination_uav_ugv/real_robot/coordination_formation_control_pkg/results" -file_name = "/experiment_6_formation_3_uav_2_ugv/result_crazyflie_1_logdata.bag" -topic_name = "crazyflie_1" + "result" +file_name = "/experiment_6_formation_3_uav_2_ugv/result_crazyflie_2_logdata.bag" +topic_name = "crazyflie_2" + "result" bag = rosbag.Bag(result_folder_name + file_name) N_uavs = 3 @@ -177,28 +177,29 @@ axis_traj.plot(position_x[-1,:], position_y[-1,:], ".",marker="*",markersize=10, if N_ugvs > 0: axis_traj.plot(ugv_position_x[0,:], ugv_position_y[0,:], ".",marker="s",markersize=12, markeredgecolor="black", markerfacecolor="black") axis_traj.plot(ugv_position_x[-1,:], ugv_position_y[-1,:], ".",marker="p",markersize=12, markeredgecolor="black", markerfacecolor="black") - axis_traj.plot(ugv_cluster_x[0,:], ugv_cluster_y[0,:], ".",marker="h",markersize=12, markeredgecolor="black", markerfacecolor="black") - axis_traj.plot(ugv_cluster_x[-1,:], ugv_cluster_y[-1,:], ".",marker="+",markersize=12, markeredgecolor="black", markerfacecolor="black") + # axis_traj.plot(ugv_cluster_x[0,:], ugv_cluster_y[0,:], ".",marker="h",markersize=12, markeredgecolor="black", markerfacecolor="black") + # axis_traj.plot(ugv_cluster_x[-1,:], ugv_cluster_y[-1,:], ".",marker="+",markersize=12, markeredgecolor="black", markerfacecolor="black") #plot final configuration -axis_traj.plot(np.append(position_x[-1,:],position_x[-1,0]) , np.append(position_y[-1,:],position_y[-1,0]),"r") -# if N_ugvs > 0: -# axis_traj.plot(np.append(ugv_position_x[-1,:],ugv_cluster_x[-1,:]) , np.append(ugv_position_y[-1,:],ugv_cluster_y[-1,:]),"k") -# axis_traj.plot(np.append(ugv_position_x[-1,:],ugv_cluster_x[-1,:]) , np.append(ugv_position_y[-1,:],ugv_cluster_y[-1,:]),"k") +# axis_traj.plot(np.append(position_x[-1,:],position_x[-1,0]) , np.append(position_y[-1,:],position_y[-1,0]),"r") +if N_ugvs > 0: + for i in range(N_ugvs): + axis_traj.plot(np.append(ugv_position_x[-1,i],ugv_cluster_x[-1,i]) , np.append(ugv_position_y[-1,i],ugv_cluster_y[-1,i]),"k") + axis_traj.plot(np.append(ugv_position_x[-1,i],ugv_cluster_x[-1,i]) , np.append(ugv_position_y[-1,i],ugv_cluster_y[-1,i]),"k") #plot trajectories color_array = ["c", "g","b", "y", "m", "C1", "C2"] # plot the uavs -for i in range(N_uavs): - c = color_array[i]; - axis_traj.plot(position_x[:,i], position_y[:,i], c,label="UAV_" + str(i+1)) +# for i in range(N_uavs): +# c = color_array[i]; +# axis_traj.plot(position_x[:,i], position_y[:,i], c,label="UAV_" + str(i+1)) for t in range(N_ugvs): c = color_array[t+N_uavs]; c1 = color_array[t+N_uavs+2]; - axis_traj.plot(ugv_position_x[:,t], ugv_position_y[:,t],c+"--",label="UGV_" + str(t+1)) - axis_traj.plot(ugv_cluster_x[:,t], ugv_cluster_y[:,t],c1+"--",label="Cluster_" + str(t+1)) + # axis_traj.plot(ugv_position_x[:,t], ugv_position_y[:,t],c+"--",label="UGV_" + str(t+1)) + axis_traj.plot(ugv_cluster_x[:,t], ugv_cluster_y[:,t],c1+"--",label="Cluster_" + str(t+1),marker="*",markersize=10) # plot the obstacle if n_obstacles >0: 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 89b1afbf7967840a07f9668428c61a58f5b4a4d6..169941d6df118e3a2278d0595d025bd085ecc95f 100644 --- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp +++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp @@ -410,7 +410,7 @@ int main(int argc, char **argv) { // std::cout << "vy: transformed: " << vxl << "vy transformed: " << vyl << std::endl; - vel_cmd_publisher.publish(current_command); + // vel_cmd_publisher.publish(current_command); } @@ -465,7 +465,7 @@ int main(int argc, char **argv) { logdata->ugv_current_pose[i].position.x = current_ugv_state.q(0); logdata->ugv_current_pose[i].position.y = current_ugv_state.q(1); logdata->ugv_cluster[i].position.x = next_waypoint->cluster_pos[i*2]; - // logdata->ugv_cluster[i].position.x = next_waypoint->cluster_pos[i*2+1]; + logdata->ugv_cluster[i].position.y = next_waypoint->cluster_pos[i*2+1]; } 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/mission_controller.cpp b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp index 3b0c4a7dc26ac683c9086cf0f1d7d6b9235ff51c..b44e84203514b03b4e73e9a8d7ac28a1c853bf0a 100644 --- a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp +++ b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp @@ -340,7 +340,7 @@ for (int i = 0; i ++; i < n_ugvs) /* send goals*/ if (dist < this->eps) // waypoint reached TODO, this is hard coded, not a good idea { - std::cout << "here" << std::endl; + // std::cout << "here" << std::endl; if(this->mantain_position == false) { this->current_waypoint.stop = true; @@ -528,7 +528,7 @@ Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen: } previous_centroids = centroids; // std::cout << "final centroids" << centroids << std::endl; -this->cluster_initialized = true; + this->cluster_initialized = true; return centroids; }