@@ -29,7 +29,7 @@ def get_goal(self, goal):
2929
3030 print ("New goal received: ({}, {})" .format (goal .pose .position .x , goal .pose .position .y ))
3131 self .target_received = True
32- self .target = [goal .pose .position .x , goal .pose .position .y ]
32+ # self.target = [goal.pose.position.x, goal.pose.position.y]
3333
3434 def send_velocity (self , frame_id , send_velocity_v ):
3535
@@ -46,87 +46,110 @@ def calculate_distance(self, point1, point2):
4646 distance = math .sqrt ((x2 - x1 )** 2 + (y2 - y1 )** 2 )
4747 return distance
4848
49+ # update all parameters of the boid
50+ def update_param (self , agent_msg , agent , boids ):
4951
50-
52+ agent .position_v = get_agent_position (agent_msg ) # current position of the agent
53+ agent .target_list = [boids [value ].position_v for value in agent .edges ]
54+ agent .position_error_list = [agent .target_list [i ]- agent .position_v for i in range (len (agent .edges ))]
55+ agent .pos_diff_list = [agent .position_error_list [i ].norm () for i in range (len (agent .edges ))]
56+ agent .arrive () # update the current velocity of the agent
57+ self .target_reached_list [agent .id ] = agent .target_reached
5158
5259 def odom_callback (self , msg ):
53-
60+
5461 frame_id = msg .header .frame_id
5562 id = frame_id [7 ]
5663
5764 if frame_id not in self .cmd_vel_pub :
5865 self .cmd_vel_pub [frame_id ] = rospy .Publisher ("{}cmd_vel" .format (frame_id [:9 ]), Twist , queue_size = 10 )
5966
60- self .agent = self .agents [int (id )]
61-
62- if self .target_received :
63- _ , nav_velocity_v , self .target_reached_list [int (id )] = self .agent .get_cmd_vel (msg , self .target , self .target_reached_list [int (id )])
64- if all (self .target_reached_list ):
65- self .target_received = False
66- self .target_reached_list = [False for i in range (self .num_of_robots )]
67-
68- print ("Target position reached!" )
69- else :
70- nav_velocity_v = Vector2 ()
71-
72- # """ ===== Total Velocity ======"""
73-
74- self .Total_velocity = self .nav_weight * nav_velocity_v
75- self .send_velocity (frame_id , self .Total_velocity )
76-
77-
78-
79-
80-
67+ self .agent = self .agents [int (id )] # taking instance of that robot
68+ self .agent .position_v = get_agent_position (msg )
8169
70+ if self .agent .edges != [None ]: # if not a stub born
8271
72+ if self .target_received :
73+ self .update_param (msg , self .agent , self .agents )
74+ if all (self .target_reached_list ):
75+ self .target_received = False
8376
77+ print ("Target position reached!" )
8478
79+ self .Total_velocity = self .nav_weight * self .agent .cur_vel_v
80+ self .send_velocity (frame_id , self .Total_velocity )
8581
8682
8783 def __init__ (self ):
8884
8985 # initialize
90- self .count = 1
9186 self .target_received = False
92-
87+ self . num_of_robots = rospy . get_param ( "/num_of_robots" )
9388
94- # fixed parameters
95- self .num_of_robots = rospy .get_param ("/num_of_robots" )
96- self .target = [0.0 , 0.0 ]
97- self .velocity = [0.0 , 0.0 ] # Initial velocity
98- self .initial_time = rospy .get_time ()
99- self .cmd_vel_pub = {}
100- self .target_reached_list = [False for i in range (self .num_of_robots )]
101-
89+ """ Edges and Adjacency matrix """
90+ # Adjacency matrix
91+ self .adj_mat = np .zeros ((self .num_of_robots , self .num_of_robots ))
92+ self .edges = {}
93+ self .edges [0 ] = [1 ] # self.edges[1] = [2, 3]: 1 listens to 2 and 3
94+ self .edges [1 ] = [2 ] # self.edges[2] = [0] : 2 listens to no one
95+ self .edges [2 ] = [None ] # self.edges[3] = [1, 3]: 3 listens to 1 and 3
96+
97+ """ Build adjacency matrix """
98+ for key , values in self .edges .items ():
99+ for value in values :
100+ if value == None : # stub born
101+ continue
102+ self .adj_mat [key , value ] = 1
103+
104+ """ List Parameters to be updated by all robots """
105+ self .cmd_vel_pub = {}
106+ self .target_reached_list = [False for i in range (self .num_of_robots )]
107+ self .target_lists = [list () for i in range (self .num_of_robots )]
108+ self .position_error_list = [list () for i in range (self .num_of_robots )]
109+ self .pos_diff_list = [list () for i in range (self .num_of_robots )]
110+
111+
102112 # Tunable parameters
103- self .max_speed = 0.6
113+ self .max_speed = 0.6
104114 self .slowing_radius = 1.0
105- self .search_radius = 2.0
115+ self .search_radius = 2.0
106116
107117 # Weights
108118 self .nav_weight = 1
119+ self .nav_velocity_v = Vector2 ()
109120 self .Total_velocity = Vector2 ()
110121
111-
112122 # self.map_subscriber = rospy.Subscriber("/map", nav_msgs.msg.OccupancyGrid, self.read_map_callback_2)
113123 self .move_goal_sub = rospy .Subscriber ("/move_base_simple/goal" , PoseStamped , self .get_goal )#None # TODO: subscriber to /move_base_simple/goal published by rviz
114124
115- rate = rospy .Rate (10 )
116- rate .sleep ()
117-
118- self .agents = [Boid (initial_velocity_x = 0.0 , initial_velocity_y = 0.0 , max_speed_mag = self .max_speed ,
119- slowing_radius = self .slowing_radius ) for _ in range (self .num_of_robots )]
125+
126+ self .agents = [Boid (self .adj_mat , self .edges [id ], id , self .position_error_list [id ], self .pos_diff_list [id ], self .target_lists [id ],
127+ self .max_speed , self .slowing_radius , self .search_radius ) for id in range (self .num_of_robots )]
128+
129+
130+ for key , values in self .edges .items ():
131+ if values == [None ]:
132+ self .target_reached_list [key ] = True
133+ self .target_lists [key ] = [None ]
134+ self .position_error_list [key ] = [0 ]
135+ self .pos_diff_list [key ] = [0 ]
136+ else :
137+ self .target_lists [key ] = [self .agents [value ].position_v for value in values ]
138+
120139
121140 [rospy .Subscriber ("robot_{}/odom" .format (i ), Odometry , self .odom_callback ) for i in range (self .num_of_robots )]
122141
123142
143+
124144
125145
126146if __name__ == '__main__' :
127147 try :
128148 rospy .init_node ('consensus_mrs_2' )
129149 robot = Robot_move ()
150+ # rate = rospy.Rate(10)
151+ # rate.sleep()
152+
130153 rospy .spin ()
131154
132155 except rospy .ROSInterruptException :
0 commit comments