def shutdown(self):
rospy.loginfo("Stopping")
self.running = False
self.velocity = Twist()
self.pub.publish(self.velocity)
self.data = data_agent()
self.pub_data.publish(self.data)
def shutdown(self):
rospy.loginfo("Stopping")
self.running = False
self.velocity = Twist()
self.pub.publish(self.velocity)
self.data = data_agent()
self.pub_data.publish(self.data)
vec_1 = [self.it, self.mu_log]
vec_2 = [self.it, self.mu_hat_log]
with open(
'/home/s2504111/monte_carlo_simulations/output/withmu/AAA_convergence_mu_agent_2.csv',
'a') as new_file: #CHANGE W+ TO A (APPEND)
writer = csv.writer(new_file, delimiter=',')
writer.writerow(vec_1)
writer.writerow(vec_2)
def __init__(self, speed, desired_x_distance, activate_mu):
#print 'in init'
self.name = 'n_1'
self.min_range = -6
self.max_range = 6
self.speed = speed #in + x - direction
self.desired_x_distance = speed * desired_x_distance
self.error_margin_x = self.desired_x_distance * 0.1
self.error_margin_y = 0.02
self.running = True
rospy.on_shutdown(self.shutdown)
rospy.Subscriber(self.name + '/hokuyo_points', LaserScan,
self.calculate_z_values)
self.pub = rospy.Publisher(self.name + '/cmd_vel', Twist, queue_size=1)
self.pub_data = rospy.Publisher(self.name + '/processed_data',
data_agent,
queue_size=1)
self.velocity = Twist()
self.velocity.linear.x = self.speed
self.velocity.linear.y = 0
self.pub.publish(self.velocity)
self.error_x_direction_log = []
self.error_y_direction_log = []
self.store_linear_velocity_x = []
self.store_linear_velocity_y = []
self.msg_data = data_agent()
self.k = 0
self.temp_distance_d_t_vector = []
self.sum_distance_d_t = []
# initial conditions for mu
self.w_init = np.array([(1), (1), (1)])
self.omega = math.pi
self.xi_init = np.array([(1), (1), (1)])
self.R = np.array([(1), (1), (0)])
self.gain = 1 #GAIN
# initial conditions for mu_head
self.z_0 = np.array([(0), (0), (0)])
self.g_set_state = rospy.ServiceProxy("/gazebo/set_model_state",
SetModelState)
# ------------------ NEW INITIAL CONDITIONS COMPACT FORM ----------------------- #
self.merging_agent = 0
self.merging_agent_mu = 0
self.store_e_y = []
self.store_e_x_pos = []
self.store_e_x_neg = []
self.store_e_x_pos_mu = []
self.store_e_x_neg_mu = []
self.activate_mu = activate_mu
self.z_0 = [1, 1, 1]
self.merge = 0
self.mu_log = []
self.mu_hat_log = []
self.scale = 1
self.now = np.float64([rospy.get_time()])
self.old = np.float64([rospy.get_time()])
self.begin = np.float64([rospy.get_time()])
self.time = np.float64([])
self.time_log = np.array([])
self.d_t = np.float64([])
self.e_x_neg_log = []
self.e_x_neg_log_mu = []
self.e_x_neg_log_mu_hat = []