def __init__(self, node_name):
FunctionUnit.__init__(self, node_name)
self._x = 0.0
self._y = 0.0
self._sphere = 1.0
self._min_range = 0.3
self._max_speed = 1.0
def __init__(self, node_name):
FunctionUnit.__init__(self, node_name)
#self._robot=robot_name
self._x = 0.0#robot_x
self._y = 0.0#robot_y
self._vector_x = 0.0#vector from robot to formation position
self._vector_y = 0.0
def __init__(self, node_name):
FunctionUnit.__init__(self, node_name)
self._sub_list=[]
self._pub_list=[]
self._position_list=[]
self._adj_list=[]
self._adj_pub_list=[]
def __init__(self, node_name):
FunctionUnit.__init__(self, node_name)
#self._robot=robot_name
self._x = 0.0 #robot_x
self._y = 0.0 #robot_y
self._vector_x = 0.0 #vector from obstacle to robot
self._vector_y = 0.0
def __init__(self, node_name):
FunctionUnit.__init__(self, node_name)
#self._robot=robot_name
self._x = 0.0#robot_x
self._y = 0.0#robot_y
self._vector_x = 0.0#vector from obstacle to robot
self._vector_y = 0.0
def __init__(self, node_name):
FunctionUnit.__init__(self, node_name)
self._x = 0.0
self._y = 0.0
self._sphere = 1.0
self._min_range = 0.3
self._max_speed = 1.0
def __init__(self, node_name):
FunctionUnit.__init__(self, node_name)
self._sub_list = []
self._pub_list = []
self._position_list = []
self._adj_list = []
self._adj_pub_list = []
def __init__(self, node_name):
FunctionUnit.__init__(self, node_name)
#self._robot=robot_name
self._x = 0.0 #robot_x
self._y = 0.0 #robot_y
self._vector_x = 0.0 #vector from robot to formation position
self._vector_y = 0.0
def __init__(self, node_name):
FunctionUnit.__init__(self, node_name)
#self._robot=robot_name
self._obstacle_x = 0.0
self._obstacle_y = 0.0
self._max_speed = 1
self._min_speed = -1
self._noise_x = 0.0
self._noise_y = 0.0
def __init__(self, node_name):
FunctionUnit.__init__(self, node_name)
#self._robot=robot_name
self._obstacle_x = 0.0
self._obstacle_y = 0.0
self._max_speed = 1
self._min_speed = -1
self._noise_x =0.0
self._noise_y =0.0
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)
position_receive = FunctionUnit.create_receive(self, "base_pose_ground_truth", Odometry, self.position_on_received)
thread.start_new_thread(self.start,())
FunctionUnit.spin(self)
def run(self):
FunctionUnit.init_node(self)
self._low=rospy.get_param('~low',0)
self._up=rospy.get_param('~up',0)
self._rate=rospy.Rate(10)
self._noise_send = FunctionUnit.create_send(self, 'cmd_vel', Twist)
while not rospy.is_shutdown():
msg = Twist()
msg.linear.x=random.uniform(self._low,self._up)
msg.linear.y=random.uniform(self._low,self._up)
self._noise_send.send(msg)
self._rate.sleep()
def __init__(self, node_name):
FunctionUnit.__init__(self, node_name)
#self._robot=robot_name
self._obstacle_x = 0.0
self._obstacle_y = 0.0
self._max_speed = 1
self._min_speed = -1
self._noise_x = 0.0
self._noise_y = 0.0
self._formation_x = 0.0
self._formation_y = 0.0
self._robot_vx = 0.0 #already caclulated velocity vector
self._robot_vy = 0.0
def __init__(self, node_name):
FunctionUnit.__init__(self, node_name)
#self._robot=robot_name
self._obstacle_x = 0.0
self._obstacle_y = 0.0
self._max_speed = 1
self._min_speed = -1
self._noise_x =0.0
self._noise_y =0.0
self._formation_x =0.0
self._formation_y =0.0
self._robot_vx =0.0#already caclulated velocity vector
self._robot_vy =0.0
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 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 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)
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)
position_receive = FunctionUnit.create_receive(
self, "base_pose_ground_truth", Odometry,
self.position_on_received)
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)
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 __init__(self, node_name):
FunctionUnit.__init__(self, node_name)
self._sub_list = []
self._neighbor_position = []
self._neighbor_vel = []
self._neighbor_list = []
self._cb_list = []
self._position = [0, 0]
self._velocity = [0, 0]
#self._pub_list=[]
self._epsilon = 0.1 #segma_norm param
self._a = 5
self._b = 5
self._c = abs(self._a - self._b) / pow(4 * self._a * self._b, 0.5)
self._h = 0.2 #param: rho function for alpha agent
self._d = 7
self._r = 1.2 * self._d
self._r_alpha = self.segma_norm2([self._r, 0])
self._d_alpha = self.segma_norm2([self._d, 0])
self._c1 = 0.5
self._c2 = 0.5
def __init__(self, node_name):
FunctionUnit.__init__(self, node_name)
self._sub_list=[]
self._neighbor_position=[]
self._neighbor_vel=[]
self._neighbor_list=[]
self._cb_list=[]
self._position=[0,0]
self._velocity=[0,0]
#self._pub_list=[]
self._epsilon = 0.1 #segma_norm param
self._a = 5
self._b = 5
self._c = abs(self._a - self._b)/ pow(4*self._a*self._b,0.5)
self._h = 0.2 #param: rho function for alpha agent
self._d = 7
self._r = 1.2*self._d
self._r_alpha = self.segma_norm2([self._r,0])
self._d_alpha = self.segma_norm2([self._d,0])
self._c1=0.5
self._c2=0.5
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._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.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._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)
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)
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)
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)
def __init__(self, node_name):
FunctionUnit.__init__(self, node_name)
self._sub_list=[]
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 __init__(self, node_name):
FunctionUnit.__init__(self, node_name)