def get_fused_pose(self, req):
if req is None:
pose_resp = None
else:
pose = SE2_from_xytheta([req.x, req.y, req.theta])
pose = self.db_kinematics.step(self.encoder_ticks_left_delta_t, self.encoder_ticks_right_delta_t, pose)
trans, theta = translation_angle_from_SE2(pose)
pose_resp = PoseResponse(trans[0], trans[1], theta)
if not self.configure_flag:
self.pose = pose
self.configure_flag = True
if self.configure_flag:
self.pose = self.db_kinematics.step(self.encoder_ticks_left_delta_t, self.encoder_ticks_right_delta_t,
self.pose)
self.encoder_ticks_right_delta_t = 0
self.encoder_ticks_left_delta_t = 0
pose_SE3 = SE3_from_SE2(self.pose)
rot, trans = rotation_translation_from_SE3(pose_SE3)
quaternion_tf = quaternion_from_matrix(pose_SE3)
quaternion = Quaternion(quaternion_tf[0], quaternion_tf[1], quaternion_tf[2], quaternion_tf[3])
translation = Vector3(trans[0], trans[1], trans[2])
trafo = Transform(translation, quaternion)
self.tfs_msg.transform = trafo
if self.vel_left == 0.0 and self.vel_right == 0.0:
self.tfs_msg.header.stamp = rospy.Time.now()
else:
self.tfs_msg.header.stamp = self.encoder_timestamp
self.br.sendTransformMessage(self.tfs_msg)
self.pub_tf_enc_loc.publish(self.tfs_msg)
return pose_resp
def compose_maps(grid_spacing, tiles):
db = get_easy_algo_db()
tinfo = TransformationsInfo()
FRAME_GLOBAL = 'global'
partial = []
for tile in tiles:
cell = tile['cell']
name = tile['name']
rotation = tile['rotation']
tile_map = db.create_instance(FAMILY_SEGMAPS, name)
x = cell[0] * grid_spacing
y = cell[1] * grid_spacing
theta = np.deg2rad(rotation)
g = SE2_from_xytheta([x,y, theta])
tinfo.add_transformation(frame1=FRAME_GLOBAL, frame2=FRAME_TILE, g=g)
tile_map2 = tinfo.transform_map_to_frame(tile_map, FRAME_GLOBAL)
partial.append(tile_map2)
result = SegmentsMap.merge(partial)
return result
def __init__(self, node_name):
# Initialize the DTROS parent class
super(EncoderLocalizationNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC)
self.veh_name = rospy.get_namespace().strip("/")
self.radius = rospy.get_param(f'/{self.veh_name}/kinematics_node/radius', 100)
self.baseline = 0.0968
self.pose = SE2_from_xytheta([0.5, 0, np.pi])
self.db_kinematics = DuckiebotKinematics(radius=self.radius, baseline=self.baseline)
self.configure_flag = False
self.vel_left = 0.0
self.vel_right = 0.0
self.encoder_ticks_left_total = 0
self.encoder_ticks_left_delta_t = 0
self.encoder_ticks_right_total = 0
self.encoder_ticks_right_delta_t = 0
self.encoder_timestamp = rospy.Time.now()
self.max_number_ticks = 135
self.tfs_msg = TransformStamped()
self.tfs_msg.header.frame_id = 'map'
self.tfs_msg.header.stamp = self.encoder_timestamp
self.tfs_msg.child_frame_id = 'encoder_baselink'
self.br = tf.TransformBroadcaster()
self.sub_executed_commands = rospy.Subscriber(f'/{self.veh_name}/wheels_driver_node/wheels_cmd_executed',
WheelsCmdStamped, self.cb_executed_commands)
self.sub_encoder_ticks_left = rospy.Subscriber(f'/{self.veh_name}/left_wheel_encoder_node/tick',
WheelEncoderStamped, self.callback_left)
self.sub_encoder_ticks_right = rospy.Subscriber(f'/{self.veh_name}/right_wheel_encoder_node/tick',
WheelEncoderStamped, self.callback_right)
self.pub_tf_enc_loc = rospy.Publisher(f'/{self.veh_name}/{node_name}/transform_stamped',
TransformStamped, queue_size=10)
def encoder_localization_client(x, y, theta):
rospy.wait_for_service('encoder_localization')
try:
get_pose_map_base_enocer = rospy.ServiceProxy('encoder_localization',
Pose)
resp = get_pose_map_base_enocer(x, y, theta)
return SE2_from_xytheta([resp.x, resp.y, resp.theta])
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
def run(self):
while not rospy.is_shutdown():
if self.image is None:
continue
if not self.camera_info_received:
continue
if self.rectify_flag:
self.rectify_image()
list_pose_tag = self.detect_april_tag()
if not list_pose_tag:
if self.encoder_conf_flag:
trans_map_base_2D, theta_map_base_2D = translation_angle_from_SE2(
self.pose_map_base_SE2)
self.pose_map_base_SE2 = encoder_localization_client(
trans_map_base_2D[0], trans_map_base_2D[1],
theta_map_base_2D)
self.tfs_msg_map_base.transform = self.pose2transform(
SE3_from_SE2(self.pose_map_base_SE2))
self.tfs_msg_map_base.header.stamp = rospy.Time.now()
self.pub_tf_fused_loc.publish(self.tfs_msg_map_base)
self.br_map_base.sendTransformMessage(self.tfs_msg_map_base)
else:
pose_dta_dtc = self.pose_inverse(list_pose_tag[0])
pose_april_cam = self.T_a_dta @ pose_dta_dtc @ self.T_dtc_c
pose_map_base = self.pose_map_at @ pose_april_cam @ self.pose_cam_base
quat_map_base = quaternion_from_matrix(pose_map_base)
euler = euler_from_quaternion(quat_map_base)
theta = euler[2]
rot_map_base, translation_map_base = rotation_translation_from_SE3(
pose_map_base)
pose_map_base_SE2_enc = encoder_localization_client(
translation_map_base[0], translation_map_base[1], theta)
self.encoder_conf_flag = True
self.pose_map_base_SE2 = SE2_from_xytheta(
[translation_map_base[0], translation_map_base[1], theta])
self.tfs_msg_map_base.transform = self.pose2transform(
SE3_from_SE2(self.pose_map_base_SE2))
self.tfs_msg_map_base.header.stamp = rospy.Time.now()
self.pub_tf_fused_loc.publish(self.tfs_msg_map_base)
self.br_map_base.sendTransformMessage(self.tfs_msg_map_base)
self.tfs_msg_map_april.header.stamp = self.image_timestamp
self.static_tf_br_map_april.sendTransform(
self.tfs_msg_map_april)
self.tfs_msg_cam_base.header.stamp = self.image_timestamp
self.static_tf_br_cam_base.sendTransform(self.tfs_msg_cam_base)
self.tfs_msg_april_cam.transform = self.pose2transform(
pose_april_cam)
self.tfs_msg_april_cam.header.stamp = self.image_timestamp
self.br_april_cam.sendTransformMessage(self.tfs_msg_april_cam)
def new_episode(self):
''' Returns an episode tuple. By default it just
samples a random pose for the robot.
'''
p = self.start_pose
x = p[0]
y = p[1]
th = np.deg2rad(p[2])
vehicle_state = SE3_from_SE2(SE2_from_xytheta([x, y, th]))
id_episode = unique_timestamp_string()
return World.Episode(id_episode, vehicle_state)
def step(self, number_ticks_left, number_ticks_right, pose1: SE2) -> SE2:
d_l = 2 * np.pi * self.radius * number_ticks_left / self.max_number_ticks_encoder
d_r = 2 * np.pi * self.radius * number_ticks_right / self.max_number_ticks_encoder
d = (d_l + d_r) / 2
delta_theta = (d_r - d_l) / (self.baseline)
translation1 = translation_from_SE2(pose1)
theta1 = angle_from_SE2(pose1)
theta2 = theta1 + delta_theta
x2 = translation1[0] + d * math.cos(theta2)
y2 = translation1[1] + d * math.sin(theta2)
pose2 = SE2_from_xytheta([x2, y2, theta2])
return pose2
def get_base_scenario(scenario_name: str, nduckies: int, ntiles: int,
min_dist_from_other_duckie: float) -> Scenario:
tile_size = 0.585
themap = {"tiles": [], "tile_size": tile_size}
themap["tiles"] = [["asphalt"] * ntiles] * ntiles
area = RectangularArea([0, 0], [tile_size * ntiles, tile_size * ntiles])
robots = {}
L = tile_size * (ntiles / 2)
# x = L / 8
# y = L / 2
pose = SE2_from_xytheta([0.2, 0.2, np.deg2rad(45)])
vel = FriendlyVelocity(0.0, 0.0, 0.0)
robots["ego0"] = ScenarioRobotSpec(
color="blue",
configuration=RobotConfiguration(pose, vel),
controllable=True,
protocol=PROTOCOL_NORMAL,
description="",
)
yaml_str = yaml.dump(themap)
duckie_poses = sample_duckies(
nduckies,
[pose_from_friendly(robots["ego0"].configuration.pose)],
min_dist_from_robot=0.4,
min_dist_from_other_duckie=min_dist_from_other_duckie,
bounds=area,
)
duckies = {}
for i, pose in enumerate(duckie_poses):
fpose = friendly_from_pose(pose)
duckies[f"duckie{i}"] = ScenarioDuckieSpec("yellow", fpose)
payload = {"info": "fill here the yaml"}
ms = Scenario(
payload_yaml=yaml.dump(payload),
scenario_name=scenario_name,
environment=yaml_str,
robots=robots,
duckies=duckies,
player_robots=list(robots),
)
return ms
def get_base_scenario(scenario_name: str, nduckies: int,
ntiles: int) -> Scenario:
tile_size = 0.585
themap = {"tiles": [], "tile_size": tile_size}
themap["tiles"] = [["asphalt"] * ntiles] * ntiles
area = RectangularArea([0, 0], [tile_size * ntiles, tile_size * ntiles])
robots = {}
L = tile_size * (ntiles / 2)
x = L / 8
y = L / 2
pose = SE2_from_xytheta([x, y, 0])
vel = np.zeros((3, 3))
robots["ego0"] = ScenarioRobotSpec(
color="blue",
configuration=RobotConfiguration(pose, vel),
controllable=True,
protocol=PROTOCOL_NORMAL,
description="",
)
yaml_str = yaml.dump(themap)
duckie_poses = sample_duckies(
nduckies,
[robots["ego0"].configuration.pose],
min_dist_from_robot=0.4,
min_dist_from_other_duckie=0.3,
bounds=area,
)
duckies = {}
for i, pose in enumerate(duckie_poses):
duckies[f"duckie{i}"] = ScenarioDuckieSpec("yellow", pose)
ms = Scenario(
scenario_name=scenario_name,
environment=yaml_str,
robots=robots,
duckies=duckies,
player_robots=list(robots),
)
return ms
def __init__(self, node_name):
# Initialize the DTROS parent class
super(EncoderLocalizationNode, self).__init__(node_name=node_name,node_type=NodeType.GENERIC)
self.veh_name = rospy.get_namespace().strip("/")
self.radius = rospy.get_param(f'/{self.veh_name}/kinematics_node/radius', 100)
self.baseline = 0.0968
x_init = rospy.get_param(f'/{self.veh_name}/{node_name}/x_init', 100)
self.pose = SE2_from_xytheta([x_init, 0, np.pi])
self.db_kinematics = DuckiebotKinematics(radius=self.radius, baseline=self.baseline)
self.vel_left = 0.0
self.vel_right = 0.0
self.encoder_ticks_left_total = 0
self.encoder_ticks_left_delta_t = 0
self.encoder_ticks_right_total = 0
self.encoder_ticks_right_delta_t = 0
self.encoder_timestamp = rospy.Time.now()
self.max_number_ticks = 135
init_pose_SE3 = SE3_from_SE2(self.pose)
rot, trans = rotation_translation_from_SE3(init_pose_SE3)
quaternion_tf = quaternion_from_matrix(init_pose_SE3)
quaternion = Quaternion(quaternion_tf[0], quaternion_tf[1], quaternion_tf[2], quaternion_tf[3])
translation = Vector3(trans[0], trans[1], trans[2])
trafo = Transform(translation, quaternion)
self.tfs_msg = TransformStamped()
self.tfs_msg.header.frame_id = 'map'
self.tfs_msg.header.stamp = self.encoder_timestamp
self.tfs_msg.child_frame_id = 'encoder_baselink'
self.tfs_msg.transform = trafo
self.br = tf.TransformBroadcaster()
self.sub_executed_commands = rospy.Subscriber(f'/{self.veh_name}/wheels_driver_node/wheels_cmd_executed',
WheelsCmdStamped, self.cb_executed_commands)
self.sub_encoder_ticks_left = rospy.Subscriber(f'/{self.veh_name}/left_wheel_encoder_node/tick',
WheelEncoderStamped, self.callback_left)
self.sub_encoder_ticks_right = rospy.Subscriber(f'/{self.veh_name}/right_wheel_encoder_node/tick',
WheelEncoderStamped, self.callback_right)
self.pub_tf_enc_loc = rospy.Publisher(f'/{self.veh_name}/{node_name}/transform_stamped',
TransformStamped, queue_size=10)
def new_episode(self):
''' Returns an episode tuple. By default it just
samples a random pose for the robot.
'''
if self.start_poses is None:
x = np.random.uniform(self.bounds[0][0], self.bounds[0][1])
y = np.random.uniform(self.bounds[1][0], self.bounds[1][1])
th = np.random.uniform(0, np.pi * 2)
else:
nposes = len(self.start_poses)
if self.cur_pose is None:
self.cur_pose = np.random.randint(nposes)
else:
self.cur_pose = (self.cur_pose + 1) % nposes
p = self.start_poses[self.cur_pose]
x = p[0]
y = p[1]
th = np.deg2rad(p[2])
vehicle_state = SE3_from_SE2(SE2_from_xytheta([x, y, th]))
id_episode = unique_timestamp_string()
return World.Episode(id_episode, vehicle_state)
def __init__(self, node_name):
super(FusedLocalizationNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh_name = rospy.get_namespace().strip("/")
self.radius = rospy.get_param(
f'/{self.veh_name}/kinematics_node/radius', 100)
self.baseline = 0.0968
x_init = rospy.get_param(f'/{self.veh_name}/{node_name}/x_init', 100)
self.rectify_flag = rospy.get_param(
f'/{self.veh_name}/{node_name}/rectify', 100)
self.encoder_conf_flag = False
self.bridge = CvBridge()
self.image = None
self.image_timestamp = rospy.Time.now()
self.cam_info = None
self.camera_info_received = False
self.newCameraMatrix = None
self.at_detector = Detector(families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
trans_base_cam = np.array([0.0582, 0.0, 0.1072])
rot_base_cam = rotation_from_axis_angle(np.array([0, 1, 0]),
np.radians(15))
rot_cam_base = rot_base_cam.T
trans_cam_base = -rot_cam_base @ trans_base_cam
self.pose_cam_base = SE3_from_rotation_translation(
rot_cam_base, trans_cam_base)
self.tfs_msg_cam_base = TransformStamped()
self.tfs_msg_cam_base.header.frame_id = 'camera'
self.tfs_msg_cam_base.header.stamp = rospy.Time.now()
self.tfs_msg_cam_base.child_frame_id = 'at_baselink'
self.tfs_msg_cam_base.transform = self.pose2transform(
self.pose_cam_base)
self.static_tf_br_cam_base = tf2_ros.StaticTransformBroadcaster()
translation_map_at = np.array([0.0, 0.0, 0.08])
rot_map_at = np.eye(3)
self.pose_map_at = SE3_from_rotation_translation(
rot_map_at, translation_map_at)
self.tfs_msg_map_april = TransformStamped()
self.tfs_msg_map_april.header.frame_id = 'map'
self.tfs_msg_map_april.header.stamp = rospy.Time.now()
self.tfs_msg_map_april.child_frame_id = 'apriltag'
self.tfs_msg_map_april.transform = self.pose2transform(
self.pose_map_at)
self.static_tf_br_map_april = tf2_ros.StaticTransformBroadcaster()
self.tfs_msg_april_cam = TransformStamped()
self.tfs_msg_april_cam.header.frame_id = 'apriltag'
self.tfs_msg_april_cam.header.stamp = rospy.Time.now()
self.tfs_msg_april_cam.child_frame_id = 'camera'
self.br_april_cam = tf.TransformBroadcaster()
self.tfs_msg_map_base = TransformStamped()
self.tfs_msg_map_base.header.frame_id = 'map'
self.tfs_msg_map_base.header.stamp = rospy.Time.now()
self.tfs_msg_map_base.child_frame_id = 'fused_baselink'
self.br_map_base = tf.TransformBroadcaster()
self.pose_map_base_SE2 = SE2_from_xytheta([x_init, 0, np.pi])
R_x_c = rotation_from_axis_angle(np.array([1, 0, 0]), np.pi / 2)
R_z_c = rotation_from_axis_angle(np.array([0, 0, 1]), np.pi / 2)
R_y_a = rotation_from_axis_angle(np.array([0, 1, 0]), -np.pi / 2)
R_z_a = rotation_from_axis_angle(np.array([0, 0, 1]), np.pi / 2)
R_dtc_c = R_x_c @ R_z_c
R_a_dta = R_y_a @ R_z_a
self.T_dtc_c = SE3_from_rotation_translation(R_dtc_c,
np.array([0, 0, 0]))
self.T_a_dta = SE3_from_rotation_translation(R_a_dta,
np.array([0, 0, 0]))
self.sub_image_comp = rospy.Subscriber(
f'/{self.veh_name}/camera_node/image/compressed',
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1)
self.sub_cam_info = rospy.Subscriber(
f'/{self.veh_name}/camera_node/camera_info',
CameraInfo,
self.cb_camera_info,
queue_size=1)
self.pub_tf_fused_loc = rospy.Publisher(
f'/{self.veh_name}/{node_name}/transform_stamped',
TransformStamped,
queue_size=10)
def solve(self, G):
max_dist = self.params['max_dist']
min_nodes = self.params['min_nodes']
scale = self.params['scale']
results = {}
self.info('Loaded graph with %d nodes, %d edges.' %
(G.number_of_nodes(), G.number_of_edges()))
self.phase('compute:simplification')
landmarks_subgraph, how_to_reattach = \
simplify_graph_aggressive(G, max_dist=max_dist,
eprint=self.info, min_nodes=min_nodes)
self.info('Reduced graph with %d nodes, %d edges.' %
(landmarks_subgraph.number_of_nodes(),
landmarks_subgraph.number_of_edges()))
self.phase('compute:shortest_paths')
subpaths = all_pairs_shortest_path(landmarks_subgraph)
self.phase('compute:fully_connect')
landmarks_subgraph_full = \
compute_fully_connected_subgraph(landmarks_subgraph, paths=subpaths)
self.phase('compute:solve_by_reduction')
G_landmarks = solve_by_reduction(landmarks_subgraph_full,
scale=scale,
eprint=self.info)
self.phase('compute:placing other nodes')
G_all = reattach(G_landmarks, how_to_reattach)
solution = DiGraph(G)
for x in G.nodes():
solution.node[x]['pose'] = G_all.node[x]['pose']
head = sorted(solution.nodes())[0]
target = SE2_from_xytheta([0, 0, 0])
graph_fix_node(solution, head, target)
self.phase('stats:computing graph_errors')
# note that this is a dense graph
nlandmarks = landmarks_subgraph.number_of_nodes()
if nlandmarks < 100:
lgstats = graph_errors(constraints=landmarks_subgraph,
solution=G_landmarks)
self.info(graph_errors_print('landmark-gstats', lgstats))
results['lgstats'] = lgstats
gstats = graph_errors(constraints=solution, solution=solution)
self.phase('debug:printing')
self.info(graph_errors_print('all-gstats', gstats))
results['G_all'] = G_all
results['G_landmarks'] = G_landmarks
results['solution'] = solution
results['gstats'] = gstats
self.phase(None)
results['landmarks'] = landmarks_subgraph.nodes()
results['landmarks_subgraph'] = landmarks_subgraph
return results