def run(self):
FunctionUnit.init_node(self)
obstacle_receive = FunctionUnit.create_receive(self, "obstacle_detection", Twist, self.obstacle_on_received)
noise_receive = FunctionUnit.create_receive(self, "noise", Twist, self.noise_on_received)
self._cmd_send = FunctionUnit.create_send(self, 'cmd_vel', Twist)
thread.start_new_thread(self.start,())
FunctionUnit.spin(self)
def run(self):
FunctionUnit.init_node(self)
obstacle_receive = FunctionUnit.create_receive(
self, "obstacle_detection", Twist, self.obstacle_on_received)
noise_receive = FunctionUnit.create_receive(self, "noise", Twist,
self.noise_on_received)
self._cmd_send = FunctionUnit.create_send(self, 'cmd_vel', Twist)
thread.start_new_thread(self.start, ())
FunctionUnit.spin(self)
def run(self):
FunctionUnit.init_node(self)
test = swarm.msg.Broadcast()
obstacle_receive = FunctionUnit.create_receive(self, "obstacle_detection", Twist, self.obstacle_on_received)
noise_receive = FunctionUnit.create_receive(self, "noise", Twist, self.noise_on_received)
formation_receive = FunctionUnit.create_receive(self, "formation_position", Twist, self.formation_on_received)
robot_receive = FunctionUnit.create_receive(self, "robot_detect", Twist, self.robot_on_received)
self._cmd_send = FunctionUnit.create_send(self, 'cmd_vel', Twist)
thread.start_new_thread(self.start,())
FunctionUnit.spin(self)
def run(self):
FunctionUnit.init_node(self)
test = swarm.msg.Broadcast()
obstacle_receive = FunctionUnit.create_receive(
self, "obstacle_detection", Twist, self.obstacle_on_received)
noise_receive = FunctionUnit.create_receive(self, "noise", Twist,
self.noise_on_received)
formation_receive = FunctionUnit.create_receive(
self, "formation_position", Twist, self.formation_on_received)
robot_receive = FunctionUnit.create_receive(self, "robot_detect",
Twist,
self.robot_on_received)
self._cmd_send = FunctionUnit.create_send(self, 'cmd_vel', Twist)
thread.start_new_thread(self.start, ())
FunctionUnit.spin(self)
def neighbor_on_received(self, msg):
'''
for i in range(0,len(self._neighbor_list)):
if i == len(self._neighbor_list):
break
if not self.find(self._neighbor_list[i],msg.data):
self._sub_list[i].unregister()
del self._neighbor_list[i]
del self._sub_list[i]
del self._cb_list[i]
for j in range(i,len(self._cb_list)):
self._cb_list[j]._id -= 1
del self._neighbor_position[2*i+1]
del self._neighbor_position[2*i]
del self._neighbor_vel[2*i+1]
del self._neighbor_vel[2*i]
'''
for i in range(0, len(msg.data)):
if not self.find(msg.data[i], self._neighbor_list):
self._neighbor_position.append(0)
self._neighbor_position.append(0)
self._neighbor_vel.append(0)
self._neighbor_vel.append(0)
self._neighbor_list.append(msg.data[i])
self._cb_list.append(
NeiPosCallback(
len(self._neighbor_list) - 1, self._neighbor_position,
self._neighbor_vel))
self._sub_list.append(
FunctionUnit.create_receive(
self, "/robot_" + str(msg.data[i]) + "/position",
swarm.msg.Position,
self._cb_list[len(self._cb_list) - 1]))
'''
def run(self):
FunctionUnit.init_node(self)
self._send = FunctionUnit.create_send(self, 'cmd_vel', Twist)
self._mypos_rcv=FunctionUnit.create_receive(self, "position", swarm.msg.Position, self.position_on_received)
self._rcv=FunctionUnit.create_receive(self, "neighbor", swarm.msg.Neighbor, self.neighbor_on_received)
sendmsg=Twist()
sendmsg.linear.x=random.uniform(-1,1)
sendmsg.linear.y=random.uniform(-1,1)
while not rospy.is_shutdown():
time.sleep(0.1)
u_0 = self.f_g()[0]+self.f_d()[0] #+self.f_r()[0]
u_1 = self.f_g()[1]+self.f_d()[1] #+self.f_r()[1]
sendmsg.linear.x+=u_0
sendmsg.linear.y+=u_1
#self._send.send(sendmsg)
FunctionUnit.spin(self)
def neighbor_on_received(self,msg):
'''
for i in range(0,len(self._neighbor_list)):
if i == len(self._neighbor_list):
break
if not self.find(self._neighbor_list[i],msg.data):
self._sub_list[i].unregister()
del self._neighbor_list[i]
del self._sub_list[i]
del self._cb_list[i]
for j in range(i,len(self._cb_list)):
self._cb_list[j]._id -= 1
del self._neighbor_position[2*i+1]
del self._neighbor_position[2*i]
del self._neighbor_vel[2*i+1]
del self._neighbor_vel[2*i]
'''
for i in range(0,len(msg.data)):
if not self.find(msg.data[i],self._neighbor_list):
self._neighbor_position.append(0)
self._neighbor_position.append(0)
self._neighbor_vel.append(0)
self._neighbor_vel.append(0)
self._neighbor_list.append(msg.data[i])
self._cb_list.append(NeiPosCallback(len(self._neighbor_list)-1,self._neighbor_position,self._neighbor_vel))
self._sub_list.append(FunctionUnit.create_receive(self, "/robot_"+str(msg.data[i])+"/position", swarm.msg.Position, self._cb_list[len(self._cb_list)-1]))
'''
def run(self):
FunctionUnit.init_node(self)
num = rospy.get_param('~robotnum',4)
self._send = FunctionUnit.create_send(self, 'position_broadcast', swarm.msg.Broadcast)
self._cb_list=[]
for i in range(0,num):
self._pub_list.append(FunctionUnit.create_send(self, 'robot_'+str(i)+'/position', swarm.msg.Position))
self._position_list.append(-1)
self._position_list.append(-1)
self._adj_list.append([])
self._adj_pub_list.append(FunctionUnit.create_send(self, 'robot_'+str(i)+'/neighbor', swarm.msg.Neighbor))
for i in range(0,num):
self._cb_list.append(PosCallback(i,self._pub_list,self._position_list))
self._sub_list.append(FunctionUnit.create_receive(self, "robot_"+str(i)+"/base_pose_ground_truth", Odometry, self._cb_list[i]))
count = 1
while not rospy.is_shutdown():
time.sleep(0.1)
for i in range(0,num):
self._adj_list[i]=[]
for i in range(0,num):
for j in range(i,num):
if self.calculate_dist(i,j) < 4 and self.calculate_dist(i,j)>0:
self._adj_list[i].append(j)
self._adj_list[j].append(i)
for i in range(0,num):
neighbormsg=swarm.msg.Neighbor()
neighbormsg.data = self._adj_list[i]
self._adj_pub_list[i].send(neighbormsg)
print "send loop send loop" + str(count)
count +=1
FunctionUnit.spin(self)
def run(self):
FunctionUnit.init_node(self)
position_receive = FunctionUnit.create_receive(self, "formation_input", Odometry, self.position_on_received)
self._detect_send = FunctionUnit.create_send(self, 'formation_position', Twist)
self._target_x=rospy.get_param('~targetx',0)
self._target_y=rospy.get_param('~targety',0)
thread.start_new_thread(self.start_detect,())
FunctionUnit.spin(self)
def run(self):
FunctionUnit.init_node(self)
position_receive = FunctionUnit.create_receive(
self, "position", swarm.msg.Broadcast, self.position_on_received)
self._detect_send = FunctionUnit.create_send(self,
'obstacle_detection',
Twist)
thread.start_new_thread(self.start_detect, ())
FunctionUnit.spin(self)
def run(self):
FunctionUnit.init_node(self)
self._send = FunctionUnit.create_send(self, 'cmd_vel', Twist)
self._mypos_rcv = FunctionUnit.create_receive(
self, "position", swarm.msg.Position, self.position_on_received)
self._rcv = FunctionUnit.create_receive(self, "neighbor",
swarm.msg.Neighbor,
self.neighbor_on_received)
sendmsg = Twist()
sendmsg.linear.x = random.uniform(-1, 1)
sendmsg.linear.y = random.uniform(-1, 1)
while not rospy.is_shutdown():
time.sleep(0.1)
u_0 = self.f_g()[0] + self.f_d()[0] #+self.f_r()[0]
u_1 = self.f_g()[1] + self.f_d()[1] #+self.f_r()[1]
sendmsg.linear.x += u_0
sendmsg.linear.y += u_1
#self._send.send(sendmsg)
FunctionUnit.spin(self)
def run(self):
FunctionUnit.init_node(self)
num = rospy.get_param('~robotnum',4)
self._send = FunctionUnit.create_send(self, 'position_broadcast', swarm.msg.Broadcast)
self._cb_list=[]
#for i in range(0,num):
# self._pub_list.append(FunctionUnit.create_send(self, 'robot_'+str(i)+'/position', swarm.msg.Broadcast))
for i in range(0,num):
self._cb_list.append(PosCallback(i,self._send))
self._sub_list.append(FunctionUnit.create_receive(self, "robot_"+str(i)+"/base_pose_ground_truth", Odometry, self._cb_list[i]))
FunctionUnit.spin(self)
def run(self):
FunctionUnit.init_node(self)
self._id = rospy.get_param('~robot_id',0)
self._robotnum = rospy.get_param('~robot_num',0)
self._robot_list = []
for i in range (0,2*self._robotnum):
self._robot_list.append(0.0)
self._send = FunctionUnit.create_send(self, 'robot_detect', Twist)
self._rcv = FunctionUnit.create_receive(self, "/position_broadcast", swarm.msg.Broadcast, self.broadcast_on_received)
thread.start_new_thread(self.start_detect,())
FunctionUnit.spin(self)
def run(self):
FunctionUnit.init_node(self)
self._id = rospy.get_param('~robot_id', 0)
self._robotnum = rospy.get_param('~robot_num', 0)
self._robot_list = []
for i in range(0, 2 * self._robotnum):
self._robot_list.append(0.0)
self._send = FunctionUnit.create_send(self, 'robot_detect', Twist)
self._rcv = FunctionUnit.create_receive(self, "/position_broadcast",
swarm.msg.Broadcast,
self.broadcast_on_received)
thread.start_new_thread(self.start_detect, ())
FunctionUnit.spin(self)
def run(self):
FunctionUnit.init_node(self)
num = rospy.get_param('~robotnum', 4)
self._send = FunctionUnit.create_send(self, 'position_broadcast',
swarm.msg.Broadcast)
self._cb_list = []
for i in range(0, num):
self._pub_list.append(
FunctionUnit.create_send(self, 'robot_' + str(i) + '/position',
swarm.msg.Position))
self._position_list.append(-1)
self._position_list.append(-1)
self._adj_list.append([])
self._adj_pub_list.append(
FunctionUnit.create_send(self, 'robot_' + str(i) + '/neighbor',
swarm.msg.Neighbor))
for i in range(0, num):
self._cb_list.append(
PosCallback(i, self._pub_list, self._position_list))
self._sub_list.append(
FunctionUnit.create_receive(
self, "robot_" + str(i) + "/base_pose_ground_truth",
Odometry, self._cb_list[i]))
count = 1
while not rospy.is_shutdown():
time.sleep(0.1)
for i in range(0, num):
self._adj_list[i] = []
for i in range(0, num):
for j in range(i, num):
if self.calculate_dist(i, j) < 4 and self.calculate_dist(
i, j) > 0:
self._adj_list[i].append(j)
self._adj_list[j].append(i)
for i in range(0, num):
neighbormsg = swarm.msg.Neighbor()
neighbormsg.data = self._adj_list[i]
self._adj_pub_list[i].send(neighbormsg)
print "send loop send loop" + str(count)
count += 1
FunctionUnit.spin(self)
def run(self):
FunctionUnit.init_node(self)
position_receive = FunctionUnit.create_receive(self, "position", swarm.msg.Broadcast, self.position_on_received)
self._detect_send = FunctionUnit.create_send(self, 'obstacle_detection', Twist)
thread.start_new_thread(self.start_detect,())
FunctionUnit.spin(self)
def run(self):
FunctionUnit.init_node(self)
self._send = FunctionUnit.create_send(self, 'position', swarm.msg.Broadcast)
self._rcv=FunctionUnit.create_receive(self, "base_pose_ground_truth", Odometry, self.odom_on_received)
FunctionUnit.spin(self)