def handle_watchtower_message(self, node_id0, node_id1, data, time_stamp, pose_error=None):
"""Processes a message containing the pose of an object seen by a
watchtower and adds an edge to the graph. If the object seen is a
Duckiebot, adjusts the pose accordingly.
Args:
node_id0: ID of the object (watchtower) that sees the April tag of the
other object.
node_id1: ID of the object whose April tag is seen by the watchtower.
transform: Transform contained in the ROS message.
time_stamp: Timestamp associated to the ROS message.
"""
transform = get_transform_from_data(data)
# Get type of the object seen.
type_of_object_seen = get_node_type(node_id1)
space_dev = self.default_variance["watchtower"]["position_deviation"]
angle_dev = self.default_variance["watchtower"]["angle_deviation"]
if(pose_error != None):
measure_information = create_info_matrix(
space_dev * pose_error, angle_dev * pose_error)
else:
measure_information = create_info_matrix(space_dev, angle_dev)
if (type_of_object_seen == "duckiebot"):
# print("watzchtower %s is seing duckiebot %s" %
# (node_id0, node_id1))
# In case of Duckiebot the pose needs to be adjusted to take into
# account the pose of the April tag w.r.t. the base frame of the
# Duckiebot.
t = [0.0, 0.0, 0.055]
z_angle = 90
x_angle = 178
z_angle = np.deg2rad(z_angle)
x_angle = np.deg2rad(x_angle)
R_z = g.rotation_from_axis_angle(np.array([0, 0, 1]), z_angle)
R_x = g.rotation_from_axis_angle(np.array([1, 0, 0]), x_angle)
R = np.matmul(R_x, R_z) # verified!
H_apriltag_to_base = g2o.Isometry3d(R, t)
transform = transform * H_apriltag_to_base.inverse()
# Add edge to the graph.
return self.resampler.handle_watchtower_edge(node_id0, node_id1, transform, time_stamp, measure_information=measure_information, is_duckiebot=True)
else:
# Add edge to the graph.
if node_id0 not in self.edge_counters:
self.edge_counters[node_id0] = dict()
if node_id1 not in self.edge_counters[node_id0]:
self.edge_counters[node_id0][node_id1] = 0
if(self.edge_counters[node_id0][node_id1] < self.max_number_same_edge):
self.resampler.handle_watchtower_edge(
node_id0, node_id1, transform, time_stamp)
self.edge_counters[node_id0][node_id1] += 1
else:
a = random.randint(0, self.rejection_sampling_int)
if(a == 0):
self.edge_counters[node_id0][node_id1] += 1
return self.resampler.handle_watchtower_edge(node_id0, node_id1, transform, time_stamp, measure_information=measure_information)
def handle_duckiebot_message(self,
node_id0,
node_id1,
data,
time_stamp,
pose_error=None):
"""Processes a message containing the pose of an object seen by a
Duckiebot and adds an edge to the graph. Note: we assume that a
Duckiebot cannot see the April tag of another Duckiebot, so no
adjustment based on the object seen is needed.
Args:
node_id0: ID of the object (Duckiebot) that sees the April tag of the
other object.
node_id1: ID of the object whose April tag is seen by the Duckiebot.
transform: Transform contained in the ROS message.
time_stamp: Timestamp associated to the ROS message.
"""
transform = get_transform_from_data(data)
space_dev = self.default_variance["duckiebot"]["position_deviation"]
angle_dev = self.default_variance["duckiebot"]["angle_deviation"]
if (pose_error != None):
measure_information = create_info_matrix(space_dev * pose_error,
angle_dev * pose_error)
else:
measure_information = create_info_matrix(space_dev, angle_dev)
# Get type of the object that sees the other object, for a sanity check.
type_of_object_seeing = node_id0.split("_")[0]
if (type_of_object_seeing == "duckiebot"):
# The pose needs to be adjusted to take into account the relative
# pose of the camera on the Duckiebot w.r.t. to the base frame of
# the Duckiebot.
t = [0.075, 0.0, 0.025]
# This angle is an estimate of the angle by which the plastic
# support that holds the camera is tilted.
x_angle = -102
z_angle = -90
x_angle = np.deg2rad(x_angle)
z_angle = np.deg2rad(z_angle)
R_x = g.rotation_from_axis_angle(np.array([1, 0, 0]), x_angle)
R_z = g.rotation_from_axis_angle(np.array([0, 0, 1]), z_angle)
R = np.matmul(R_z, R_x) # verified
H_base_to_camera = g2o.Isometry3d(R, t)
transform = H_base_to_camera * transform
# Add edge to the graph.
# return self.resampler.handle_duckiebot_edge(node_id0, node_id1, transform, time_stamp, measure_information)
else:
print("This should not be here! %s " % node_id0)
def handle_odometry_message(self, node_id, data, time_stamp):
"""Processes an odometry message, adding an edge to the graph and
keeping track of the last time stamp before each new odometry message
(needed to handle edges in the pose graph and connect nodes).
Args:
node_id: ID of the object sending the odometry message.
transform: Transform contained in the ROS message.
time_stamp: Timestamp associated to the ROS message.
"""
transform = get_transform_from_data(data)
space_dev = self.default_variance["odometry"]["position_deviation"]
angle_dev = self.default_variance["odometry"]["angle_deviation"]
measure_information = create_info_matrix(space_dev, angle_dev)
# Add edge to the graph.
return self.resampler.handle_odometry_edge(node_id, transform,
time_stamp,
measure_information)