def state_estimation(self, coordinates):
"""Evaluate the box state using a montecarlo-based approach.
Argument:
coordinates (float[]): points coordinates
"""
random.seed()
min_value = float('inf')
# Params
estimation_rate = rospy.get_param('estimation_rate')
delta_linear = 0.02
delta_theta = 0.01
# Estimation
for i in range(300):
x_c = round(random.uniform(self._state[0]- delta_linear, self._state[0] + delta_linear), 2)
y_c = round(random.uniform(self._state[1]- delta_linear, self._state[1] + delta_linear), 2)
theta = round(angle_normalization(random.uniform(self._state[2] - delta_theta,\
self._state[2] + delta_theta)), 2)
estimated_state = [x_c, y_c, theta]
new_value = self.__loss_function(estimated_state, coordinates)
min_value = min(min_value, new_value)
if new_value == min_value:
self._state = estimated_state
return self._state
def state_estimation(self, coordinates):
"""Evaluate the box state using a montecarlo-based approach.
Argument:
coordinates (float[]): points coordinates
"""
random.seed()
min_value = float('inf')
# Params
estimation_rate = rospy.get_param('estimation_rate')
delta_linear = 0.02
delta_theta = 0.01
# Estimation
for i in range(300):
x_c = round(
random.uniform(self._state[0] - delta_linear,
self._state[0] + delta_linear), 2)
y_c = round(
random.uniform(self._state[1] - delta_linear,
self._state[1] + delta_linear), 2)
theta = round(angle_normalization(random.uniform(self._state[2] - delta_theta,\
self._state[2] + delta_theta)), 2)
estimated_state = [x_c, y_c, theta]
new_value = self.__loss_function(estimated_state, coordinates)
min_value = min(min_value, new_value)
if new_value == min_value:
self._state = estimated_state
return self._state
def find_initial_guess(self, coordinates, robots_pose):
"""Find the initial guess using a montecarlo-based approach.
Argument:
coordinates (float[]): points coordinatesn
"""
# Get the box size
box_params = rospy.get_param('box')
box_length = box_params['length']
box_width = box_params['width']
max_length = max(box_length, box_width)
# Get info on the uncertainty area
rospy.wait_for_service('uncertainty_area_get_info')
info = rospy.ServiceProxy('uncertainty_area_get_info', GetUncertaintyAreaInfo)
try:
response = info(0)
except rospy.ServiceException:
pass
unc_area = BoxGeometry(2 * max_length, max_length,\
[response.pose[0], response.pose[1]],\
response.pose[2])
# Robot that found the box
discoverer_id = response.discoverer_id
discoverer_pose = robots_pose[discoverer_id]
# Evaluate the segment between the two other robot
indexes = [0, 1, 2]
indexes.remove(discoverer_id)
poses = [robots_pose[i] for i in indexes]
segment_between = Segment([poses[0][0],poses[0][1]],\
[poses[1][0],poses[1][1]])
half_way = np.array(segment_between.point(0.5))
segment_length = segment_between.length()
# Find the length of the side on which the other robots are attached
robot_radius = float(rospy.get_param('robot_radius'))
effective_length = segment_length - 2 * robot_radius
side_length = box_width
if abs(effective_length - box_width) < abs(effective_length - box_length):
side_length = box_length
# Find an uncertainty area for the center of the box
direction = np.array([np.cos(discoverer_pose[2]), np.sin(discoverer_pose[2])])
line_half_way = Line(half_way.tolist(), (half_way + direction).tolist())
side0 = Line(unc_area.vertex(0), unc_area.vertex(1))
intersection = line_half_way.intersect(side0)
center_guess = (np.array(intersection) + (side_length / 2) * direction).tolist()
theta_guess = discoverer_pose[2]
if side_length == box_width:
theta_guess -= np.pi / 2
# Estimate the initial guess
min_value = float('inf')
random.seed()
theta = angle_normalization(theta_guess)
for i in range(10000):
x_c = random.gauss(center_guess[0], 0.01)
y_c = random.gauss(center_guess[1], 0.01)
estimated_state = [x_c, y_c, theta]
new_value = self.__loss_function(estimated_state, coordinates)
min_value = min(min_value, new_value)
if new_value == min_value:
self._state = estimated_state
print estimated_state
print center_guess
print theta_guess
def find_initial_guess(self, coordinates, robots_pose):
"""Find the initial guess using a montecarlo-based approach.
Argument:
coordinates (float[]): points coordinatesn
"""
# Get the box size
box_params = rospy.get_param('box')
box_length = box_params['length']
box_width = box_params['width']
max_length = max(box_length, box_width)
# Get info on the uncertainty area
rospy.wait_for_service('uncertainty_area_get_info')
info = rospy.ServiceProxy('uncertainty_area_get_info',
GetUncertaintyAreaInfo)
try:
response = info(0)
except rospy.ServiceException:
pass
unc_area = BoxGeometry(2 * max_length, max_length,\
[response.pose[0], response.pose[1]],\
response.pose[2])
# Robot that found the box
discoverer_id = response.discoverer_id
discoverer_pose = robots_pose[discoverer_id]
# Evaluate the segment between the two other robot
indexes = [0, 1, 2]
indexes.remove(discoverer_id)
poses = [robots_pose[i] for i in indexes]
segment_between = Segment([poses[0][0],poses[0][1]],\
[poses[1][0],poses[1][1]])
half_way = np.array(segment_between.point(0.5))
segment_length = segment_between.length()
# Find the length of the side on which the other robots are attached
robot_radius = float(rospy.get_param('robot_radius'))
effective_length = segment_length - 2 * robot_radius
side_length = box_width
if abs(effective_length - box_width) < abs(effective_length -
box_length):
side_length = box_length
# Find an uncertainty area for the center of the box
direction = np.array(
[np.cos(discoverer_pose[2]),
np.sin(discoverer_pose[2])])
line_half_way = Line(half_way.tolist(),
(half_way + direction).tolist())
side0 = Line(unc_area.vertex(0), unc_area.vertex(1))
intersection = line_half_way.intersect(side0)
center_guess = (np.array(intersection) +
(side_length / 2) * direction).tolist()
theta_guess = discoverer_pose[2]
if side_length == box_width:
theta_guess -= np.pi / 2
# Estimate the initial guess
min_value = float('inf')
random.seed()
theta = angle_normalization(theta_guess)
for i in range(10000):
x_c = random.gauss(center_guess[0], 0.01)
y_c = random.gauss(center_guess[1], 0.01)
estimated_state = [x_c, y_c, theta]
new_value = self.__loss_function(estimated_state, coordinates)
min_value = min(min_value, new_value)
if new_value == min_value:
self._state = estimated_state
print estimated_state
print center_guess
print theta_guess
