def start(self, filepath):
rate = rospy.Rate(self.awake_freq)
sim_time = 0
while (not rospy.is_shutdown()):
# Log estimated loc data
for key, val in self.curr_est_locs.items():
if not key in self.est_locs_log.keys():
self.est_locs_log[key] = []
if not val is None:
# val = np.nan * np.ones(2,)
self.est_locs_log[key].append(val)
# Log sensor and target loc data
for l in self.listeners:
if not l.robot_pose is None:
self.sensor_locs[l.robot_name].append(toxy(l.robot_pose))
for l in self.target_listeners:
if not l.robot_pose is None:
self.target_locs[l.robot_name].append(toxy(l.robot_pose))
# Log waypoints
for key, val in self.curr_waypoints.items():
if not val is None:
self.waypoint_log[key].append(val)
self.save_data(filepath)
rate.sleep()
# After Ctrl+C is pressed, save the log data to pickle file.
self.save_data(filepath)
print('saving data at {}...'.format(filepath))
def record_and_calibrate(self,
robot_namespace,
target_namespace,
save_data=False,
fit_type='light_readings'):
rospy.init_node('calibrate_meas_coef', anonymous=True)
rpose_topic = "/vrpn_client_node/{}/pose".format(robot_namespace)
tpose_topic = "/vrpn_client_node/{}/pose".format(target_namespace)
robot_pose = rospy.Subscriber(rpose_topic, PoseStamped,
self.robot_pose_callback_)
target_pose = rospy.Subscriber(tpose_topic, PoseStamped,
self.target_pose_callback_)
light_sensor = rospy.Subscriber(
"/{}/sensor_readings".format(robot_namespace), Float32MultiArray,
self.light_callback_)
rate = rospy.Rate(self.awake_freq)
while (not rospy.is_shutdown()):
if not (self.robot_pose == None or self.target_pose == None
or self.light_readings == None):
print(self.robot_pose)
print('target:', self.target_pose)
print('light:', self.light_readings)
self.robot_loc_stack.append(toxy(self.robot_pose))
self.target_loc_stack.append(toxy(self.target_pose))
self.light_reading_stack.append(np.array(self.light_readings))
rate.sleep()
if save_data:
np.savetxt('robot_loc_{}.txt'.format(robot_namespace),
np.array(self.robot_loc_stack),
delimiter=',')
np.savetxt('target_loc_{}.txt'.format(target_namespace),
np.array(self.target_loc_stack),
delimiter=',')
np.savetxt('light_readings_{}.txt'.format(robot_namespace),
np.array(self.light_reading_stack),
delimiter=',')
print('Calculating Coefficients...')
np.savetxt(
'coefs_{}.txt'.format(robot_namespace),
cc(np.array(self.robot_loc_stack),
np.array(self.target_loc_stack),
np.array(self.light_reading_stack),
fit_type=fit_type))
def start(self):
rate=rospy.Rate(self.awake_freq)
sim_time = 0
while (not rospy.is_shutdown()):
print('sim_time',sim_time)
rate.sleep()
# Update the source pose informations.
for l in self.listeners:
if not(l.robot_pose==None):
l.robot_loc_stack.append(toxy(l.robot_pose))
qs=np.array([l.robot_loc_stack[-1] for l in self.listeners]).reshape(-1,2)
print('The Source is Moving')
self.waypoints = self.waypoint_generator.get_waypoints(qs)
self.waypoints=self.waypoints.reshape(-1,self.n_src,2)
for i in range(self.n_src):
out=Float32MultiArray()
out.data=self.waypoints[:,i,:].ravel()
self.waypoint_pub[self.source_names[i]].publish(out)
sim_time+=1/self.awake_freq
def start(self):
rate=rospy.Rate(self.awake_freq)
sim_time = 0
while (not rospy.is_shutdown()):
print('sim_time',sim_time)
rate.sleep()
print('real_time_controlling')
# Update the robot pose informations.
all_loc_received = True
for l in self.listeners:
if not(l.robot_pose==None):
l.robot_loc_stack.append(toxy(l.robot_pose))
else:
all_loc_received = False
if not all_loc_received:
continue
ps=np.array([l.robot_loc_stack[-1] for l in self.listeners]).reshape(-1,2)
# Start generating waypoints.
self.waypoints=self.get_waypoints()
self.waypoints=self.waypoints.reshape(-1,self.n_robots,2)
for i in range(self.n_robots):
out=Float32MultiArray()
out.data=self.waypoints[:,i,:].ravel()
self.waypoint_pub[self.robot_names[i]].publish(out)
sim_time+=1/self.awake_freq
def start(self):
rate = rospy.Rate(self.awake_freq)
try:
while not rospy.is_shutdown():
if not (self.listener.robot_pose == None):
self.listener.robot_loc_stack.append(
toxy(self.listener.robot_pose))
self.listener.robot_yaw_stack.append(
toyaw(self.listener.robot_pose))
if not self.all_waypoints is None:
vel_msg = self.get_next_vel()
self.vel_pub.publish(vel_msg)
# print("{} moving".format(self.robot_name))
rate.sleep()
except:
pass
finally:
print("{} Stoping".format(self.robot_name))
self.vel_pub.publish(stop_twist())
def start(self):
rate = rospy.Rate(self.awake_freq)
sim_time = 0
while (not rospy.is_shutdown()):
print('sim_time', sim_time)
rate.sleep()
print('real_time_controlling')
print("Scalar Readings", self.scalar_readings)
# Update the robot pose informations.
all_loc_received = True
for l in self.listeners:
if not (l.robot_pose == None or
not l.robot_name in list(self.scalar_readings.keys())):
l.robot_loc_stack.append(toxy(l.robot_pose))
else:
all_loc_received = False
if not all_loc_received:
continue
ps = np.array([l.robot_loc_stack[-1]
for l in self.listeners]).reshape(-1, 2)
scalar_readings = np.array(
[self.scalar_readings[l.robot_name] for l in self.listeners])
# Start generating waypoints.
# if True:
q = self.get_est_loc()
if not self.initial_movement_finished:
print('Performing Initial Movements'
) # R,ps,n_p,n_steps,max_linear_speed,dt,epsilon
self.waypoints,radius_reached = mutual_separation_path_planning(\
self.initial_movement_radius,ps,self.n_robots,\
self.planning_timesteps,\
self.max_linear_speed,\
self.planning_dt,\
scalar_readings,\
xlim = self.xlim,\
ylim = self.ylim)
# print('Reached',radius_reached)
self.initial_movement_finished = sim_time >= self.initial_movement_time
else:
# After the initial movement is completed, we switch to FIM gradient ascent.
if q is None:
print('Not received any estimation locs')
pass
else:
q = q.reshape(
-1, 2
) # By default, this is using the estimation returned by ekf.
C1s = []
C0s = []
ks = []
bs = []
for l in self.listeners:
C1s.append(l.C1)
C0s.append(l.C0)
ks.append(l.k)
bs.append(l.b)
if None in C1s or None in C0s or None in ks or None in bs:
print('Coefficients not fully yet received.')
else:
print('Dynamic Tracking')
# Feed in everything needed by the waypoint planner.
# f_dLdp=partial(analytic_dLdp,C1s=C1s,C0s=C0s,ks=ks,bs=bs)
f_dLdp = dLdp(C1s=C1s, C0s=C0s, ks=ks, bs=bs)
# f_dLdp=dSdp(C1s=C1s,C0s=C0s,ks=ks,bs=bs)
# self.waypoints=FIM_ascent_path_planning(f_dLdp,q,ps,self.n_robots,\
# self.planning_timesteps,\
# self.max_linear_speed,\
# self.planning_dt,\
# self.epsilon,\
# Rect2D(self.xlim,self.ylim))
self.waypoints = straight_line_path_planning(
q, ps, self.n_robots, self.planning_timesteps)
self.initial_movement_pub.publish(self.initial_movement_finished)
self.waypoints = self.waypoints.reshape(-1, self.n_robots, 2)
for i in range(self.n_robots):
out = Float32MultiArray()
out.data = self.waypoints[:, i, :].ravel()
self.waypoint_pub[self.robot_names[i]].publish(out)
sim_time += 1 / self.awake_freq