From 7db4fcdc395f7d71df25e76d20e8c766fa914f55 Mon Sep 17 00:00:00 2001 From: Robin Shaun Date: Fri, 7 Aug 2020 12:48:15 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E7=BC=96=E9=98=9F=E7=AE=97?= =?UTF-8?q?=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- communication/multi_vehicle_communication.sh | 2 +- coordination/formation_demo/follower.py | 228 +++++++++++++++++- .../launch_generator/multi_vehicle.launch | 87 ------- .../param/dwa_local_planner_params.yaml | 2 +- 4 files changed, 219 insertions(+), 100 deletions(-) diff --git a/communication/multi_vehicle_communication.sh b/communication/multi_vehicle_communication.sh index 6a1bfb0..1a76d25 100644 --- a/communication/multi_vehicle_communication.sh +++ b/communication/multi_vehicle_communication.sh @@ -1,5 +1,5 @@ #!/bin/bash -iris_num=9 +iris_num=6 typhoon_h480_num=0 solo_num=0 plane_num=0 diff --git a/coordination/formation_demo/follower.py b/coordination/formation_demo/follower.py index 0e74722..3d37ddc 100644 --- a/coordination/formation_demo/follower.py +++ b/coordination/formation_demo/follower.py @@ -35,8 +35,8 @@ class Follower: self.formation_config = 'waiting' self.following_count = 0 self.Kp = 1 - self.velxy_max = 0.8 - self.velz_max = 0.8 + self.velxy_max = 0.6 #0.8 + self.velz_max = 1 self.following_local_pose = [PoseStamped() for i in range(self.uav_num)] self.following_local_pose_sub = [None]*self.uav_num self.arrive_count = 0 @@ -86,8 +86,30 @@ class Follower: self.cmd_pub.publish(self.offboard) self.info_pub.publish("Received") print("Follower"+str(self.id-1)+": Switch to Formation "+self.formation_config) - if not self.formation_config=='waiting': - self.L_matrix = self.get_L_central_matrix() + if self.formation_config=='waiting': + self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config]) #self.get_L_central_matrix() + else: + if self.first_formation: + self.first_formation=False + self.orig_formation=formation_dict[self.formation_config] + self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config]) + #self.L_matrix = self.get_L_central_matrix() + else: + #self.new_formation=self.get_new_formation(self.orig_formation,formation_dict[self.formation_config]) + self.adj_matrix = self.build_graph(self.orig_formation,formation_dict[self.formation_config]) + self.label_left = numpy.max(self.adj_matrix, axis=1) # init label for the left + self.label_right = numpy.array([0]*(self.uav_num-1)) # init label for the right set + + self.match_right = numpy.array([-1] *(self.uav_num-1)) + self.visit_left = numpy.array([0]*(self.uav_num-1)) + self.visit_right = numpy.array([0]*(self.uav_num-1)) + self.slack_right = numpy.array([100]*(self.uav_num-1)) + self.change_id = self.KM() + self.new_formation=self.get_new_formation(self.change_id,formation_dict[self.formation_config]) + self.L_matrix = self.get_L_matrix(self.new_formation) + #self.L_matrix = self.get_L_central_matrix() + self.orig_formation=self.new_formation + self.following_ids = numpy.argwhere(self.L_matrix[self.id,:] == 1) self.following_count = 0 for i in range(self.uav_num): @@ -96,19 +118,23 @@ class Follower: for following_id in self.following_ids: self.following_local_pose_sub[following_id[0]] = rospy.Subscriber(self.uav_type+'_'+str(following_id[0])+"/mavros/local_position/pose", PoseStamped , self.following_local_pose_callback,following_id[0]) self.following_count += 1 + if self.id==3: + print(self.L_matrix) + self.cmd_vel_enu.linear = Vector3(0, 0, 0) for following_id in self.following_ids: - self.cmd_vel_enu.linear.x += self.following_local_pose[following_id[0]].pose.position.x - self.local_pose.pose.position.x + formation_dict[self.formation_config][0, self.id-2] - self.cmd_vel_enu.linear.y += self.following_local_pose[following_id[0]].pose.position.y - self.local_pose.pose.position.y + formation_dict[self.formation_config][1, self.id-2] - self.cmd_vel_enu.linear.z += self.following_local_pose[following_id[0]].pose.position.z - self.local_pose.pose.position.z + formation_dict[self.formation_config][2, self.id-2] + self.cmd_vel_enu.linear.x += self.following_local_pose[following_id[0]].pose.position.x - self.local_pose.pose.position.x + formation_dict[self.formation_config][0, self.id-1] + self.cmd_vel_enu.linear.y += self.following_local_pose[following_id[0]].pose.position.y - self.local_pose.pose.position.y + formation_dict[self.formation_config][1, self.id-1] + self.cmd_vel_enu.linear.z += self.following_local_pose[following_id[0]].pose.position.z - self.local_pose.pose.position.z + formation_dict[self.formation_config][2, self.id-1] if not following_id[0] == 0: self.cmd_vel_enu.linear.x -= formation_dict[self.formation_config][0, following_id[0]-1] self.cmd_vel_enu.linear.y -= formation_dict[self.formation_config][1, following_id[0]-1] self.cmd_vel_enu.linear.z -= formation_dict[self.formation_config][2, following_id[0]-1] - self.cmd_vel_enu.linear.x = self.Kp * self.cmd_vel_enu.linear.x + self.avoid_vel.x - self.cmd_vel_enu.linear.y = self.Kp * self.cmd_vel_enu.linear.y + self.avoid_vel.y - self.cmd_vel_enu.linear.z = self.Kp * self.cmd_vel_enu.linear.z + self.avoid_vel.z + + self.cmd_vel_enu.linear.x = self.Kp * self.cmd_vel_enu.linear.x + self.avoid_vel.x + self.cmd_vel_enu.linear.y = self.Kp * self.cmd_vel_enu.linear.y + self.avoid_vel.y + self.cmd_vel_enu.linear.z = self.Kp * self.cmd_vel_enu.linear.z + self.avoid_vel.z if self.cmd_vel_enu.linear.x > self.velxy_max: self.cmd_vel_enu.linear.x = self.velxy_max @@ -131,13 +157,193 @@ class Follower: self.arrive_count = 0 rate.sleep() + def build_graph(self,orig_formation,change_formation): + distance=[[0 for i in range(self.uav_num-1)]for j in range(self.uav_num-1)] + for i in range(self.uav_num-1): + for j in range(self.uav_num-1): + distance[i][j]=numpy.linalg.norm(orig_formation[:,i]-change_formation[:,j]) + distance[i][j]=int(50-distance[i][j]) + return distance + + def find_path(self,i): + + self.visit_left[i] = True + for j, match_weight in enumerate(self.adj_matrix[i],start=0): + if self.visit_right[j]: + continue + gap = self.label_left[i] + self.label_right[j] - match_weight + if gap == 0 : + self.visit_right[j] = True + if self.match_right[j]==-1 or self.find_path(self.match_right[j]): + self.match_right[j] = i + return True + else: + self.slack_right[j]=min(gap,self.slack_right[j]) + #print('1',slack_right) + return False + + def KM(self): + + for i in range(self.uav_num-1): + #print(i) + self.slack_right = numpy.array([100]*(self.uav_num-1)) + while True: + self.visit_left = numpy.array([0]*(self.uav_num-1)) + self.visit_right = numpy.array([0]*(self.uav_num-1)) + if self.find_path(i): + break + d = numpy.inf + #print ('2',slack_right) + for j, slack in enumerate(self.slack_right): + if not self.visit_right[j] : + d = min(d,slack) + #print(d) + for k in range(self.uav_num-1): + if self.visit_left[k]: + self.label_left[k] -= d + if self.visit_right[k]: + self.label_right[k] += d + else: + self.slack_right[k] -=d + return self.match_right + + def get_new_formation(self,change_id,change_formation): + + + new_formation=numpy.zeros((3,self.uav_num-1)) + position=numpy.zeros((3,self.uav_num-1)) + change_id=[i + 1 for i in change_id] + #print (change_id) + for i in range(0,self.uav_num-1): + position[:,i]=change_formation[:,i] + + for i in range(0,self.uav_num-1): + for j in range(0,self.uav_num-1): + if (j+1)==change_id[i]: + new_formation[:,i]=position[:,j] + return new_formation + + #函数输入为相对leader的位置矩阵和无人机数量，输出为L矩阵 + def get_L_matrix(self,rel_posi): + + c_num=int((self.uav_num-1)/2) + min_num_index_list = [0]*c_num + + comm=[[]for i in range (self.uav_num)] + w=numpy.ones((self.uav_num,self.uav_num))*0 + nodes_next=[] + node_flag = [self.uav_num-1] + node_mid_flag=[] + + rel_d=[0]*(self.uav_num-1) + + + for i in range(0,self.uav_num-1): + + rel_d[i]=pow(rel_posi[0][i],2)+pow(rel_posi[1][i],2)+pow(rel_posi[2][i],2) + + c=numpy.copy(rel_d) + c.sort() + count=0 + + for j in range(0,c_num): + for i in range(0,self.uav_num-1): + if rel_d[i]==c[j]: + if not i in node_mid_flag: + min_num_index_list[count]=i + node_mid_flag.append(i) + count=count+1 + if count==c_num: + break + if count==c_num: + break + + + for j in range(0,c_num): + nodes_next.append(min_num_index_list[j]) + + comm[self.uav_num-1].append(min_num_index_list[j]) + + size_=len(node_flag) + + while (nodes_next!=[]) and (size_<(self.uav_num-1)): + + next_node= nodes_next[0] + nodes_next=nodes_next[1:] + min_num_index_list = [0]*c_num + node_mid_flag=[] + rel_d=[0]*(self.uav_num-1) + for i in range(0,self.uav_num-1): + + if i==next_node or i in node_flag: + + rel_d[i]=2000 + else: + + rel_d[i]=pow((rel_posi[0][i]-rel_posi[0][next_node]),2)+pow((rel_posi[1][i]-rel_posi[1][next_node]),2)+pow((rel_posi[2][i]-rel_posi[2][next_node]),2) + + c=numpy.copy(rel_d) + c.sort() + count=0 + + for j in range(0,c_num): + for i in range(0,self.uav_num-1): + if rel_d[i]==c[j]: + if not i in node_mid_flag: + min_num_index_list[count]=i + node_mid_flag.append(i) + count=count+1 + if count==c_num: + break + if count==c_num: + break + node_flag.append(next_node) + + size_=len(node_flag) + + for j in range(0,c_num): + + if min_num_index_list[j] in node_flag: + + nodes_next=nodes_next + + else: + if min_num_index_list[j] in nodes_next: + nodes_next=nodes_next + else: + nodes_next.append(min_num_index_list[j]) + + comm[next_node].append(min_num_index_list[j]) + + for i in range (0,self.uav_num): + for j in range(0,self.uav_num-1): + + if i==0: + if j in comm[self.uav_num-1]: + w[j+1][i]=1 + else: + w[j+1][i]=0 + else: + if j in comm[i-1]: + w[j+1][i]=1 + else: + w[j+1][i]=0 + + L=w + for i in range (0,self.uav_num): + + L[i][i]=-sum(w[i]) + + return L #central-station control def get_L_central_matrix(self): - L=numpy.zeros((self.uav_num,self.uav_num)) for i in range(1,self.uav_num): L[i][0]=1 L[i][i]=-1 + ''' + L=numpy.array([[-0.,0. , 0., 0., 0., 0.],[ 1. ,-1. , 0. , 0. , 0. , 0.],[ 1., 2,-2. , 0. , 0. , 0.],[ 1., 0., 1 ,-2. , 0. , 0.],[ 1. , 0. , 0. , 1 ,-2. , 0.],[ 1. , 0. , 0. , 0. , 1 ,-2.]]) + ''' return L if __name__ == '__main__': diff --git a/coordination/launch_generator/multi_vehicle.launch b/coordination/launch_generator/multi_vehicle.launch index a23d7e0..342aacf 100644 --- a/coordination/launch_generator/multi_vehicle.launch +++ b/coordination/launch_generator/multi_vehicle.launch @@ -192,92 +192,5 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/motion_planning/param/dwa_local_planner_params.yaml b/motion_planning/param/dwa_local_planner_params.yaml index 23fe7da..9d69d49 100644 --- a/motion_planning/param/dwa_local_planner_params.yaml +++ b/motion_planning/param/dwa_local_planner_params.yaml @@ -26,7 +26,7 @@ DWAPlannerROS: # Forward Simulation Parameters sim_time: 2.0 vx_samples: 20 - vy_samples: 0 + vy_samples: 20 vth_samples: 40 controller_frequency: 10.0