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, transform, time_stamp): """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. """ # 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 else: print("This should not be here! %s " % node_id0)
def add_vertex(self, node_id, theta, position, is_initial_floor_tag=False, fixed=False, time_stamp=0.0): """Adds a node with an ID given in the format outputted from the transform_listener and with a given pose to the g2o graph. TODO : add a more general add_vertex function that takes a 3D pose and not only a 2D one. Args: node_id: ID of the node whose pose should be added as a node in the graph, in the format <node_type>_<node_id>. theta: Angle of the 2D rotation around the z axis defines the pose of the node. position: 2D translation vector (x, y) that define the pose of the node. is_initial_floor_tag: True if the node is an initial floor tag, False otherwise. fixed: True if the node should be added as a fixed node (i.e., not optimizable) in the graph, False otherwise. time_stamp: Timestamp. """ # Adds (assigns) a timestamp to a node, by giving it a 'local index' # w.r.t. the node. node = self.get_node(node_id) added = node.add_timestamp(time_stamp) if (not added): print( "add_vertex did not add : node %s at time %f as already there" % (node_id, time_stamp)) else: # Translation vector, z coordinate does not vary. position = [position[0], position[1], 0.0] # Rotation around the z axis of and angle theta. R = g.rotation_from_axis_angle(np.array([0, 0, 1]), np.deg2rad(theta)) if (is_initial_floor_tag): # For initial floor tags, apply a further rotation of 180 # degrees around the x axis, so that the orientation of the z # axis is reverted. R2 = g.rotation_from_axis_angle(np.array([1, 0, 0]), np.deg2rad(180)) R = np.matmul(R, R2) # Add vertex with pose and ID in the right format to the g2o graph. vertex_pose = g2o.Isometry3d(R, position) vertex_index = node.get_g2o_index(time_stamp) self.graph.add_vertex(vertex_index, vertex_pose, fixed=fixed) if (is_initial_floor_tag): # For initial floor tags, add an edge between the reference # vertex and the newly-added vertex for the pose of the tag. self.graph.add_edge(vertex0_id=0, vertex1_id=vertex_index, measure=vertex_pose)
def handle_watchtower_message(self, node_id0, node_id1, transform, time_stamp): """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. """ # Get type of the object seen. type_of_object_seen = node_id1.split("_")[0] 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.pose_graph.add_edge(node_id0, node_id1, transform, time_stamp) 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.pose_graph.add_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.pose_graph.add_edge(node_id0, node_id1, transform, time_stamp)
def create_measure(self): t = self.position roll_angle = np.deg2rad(self.rotation[0]) pitch_angle = np.deg2rad(self.rotation[1]) yaw_angle = np.deg2rad(self.rotation[2]) R_roll = g.rotation_from_axis_angle(np.array([0, 1, 0]), roll_angle) R_pitch = g.rotation_from_axis_angle(np.array([1, 0, 0]), pitch_angle) R_yaw = g.rotation_from_axis_angle(np.array([0, 0, 1]), yaw_angle) R = np.matmul(R_roll, np.matmul(R_pitch, R_yaw)) self.measure = g2o.Isometry3d(R, t)
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 assert_orthogonal_test(): for s in directions_sequence(): axis = any_orthogonal_direction(s) angle = np.pi / 2 R = rotation_from_axis_angle(axis, angle) s2 = np.dot(R, s) assert_orthogonal(s, s2)
def test_distances_rotations(self): for axis, angle in axis_angle_sequence(): s = random_direction() R = rotation_from_axis_angle(axis, angle) s2 = np.dot(R, s) dist = geodesic_distance_on_sphere(s, s2) # Note: this is == only if axis is orthogonal to s assert dist <= angle
def test_rotation_from_axis_angle2(self): for axis, angle in axis_angle_sequence(): R1 = rotation_from_axis_angle(axis, angle) R2 = rotation_from_axis_angle2(axis, angle) if False: s1, a1 = axis_angle_from_rotation(R1) s2, a2 = axis_angle_from_rotation(R2) print('Origi: %s around %s' % (angle, axis)) print('First: %s around %s' % (a1, s1)) print('Secnd: %s around %s' % (a2, s2)) assert_allclose(R1, R2)
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)