def source_odom_callback(self, msg): with self.lock: # calculate camera transform current_camera_to_base = self.calculate_camera_to_base_transform(msg.header.stamp) if current_camera_to_base == None: return if self.initial_matrix == None: self.initial_matrix = numpy.linalg.inv(current_camera_to_base) camera_relative_base_transformation = numpy.dot(self.initial_matrix, current_camera_to_base) # base_link transformation in camera coords # calculate offseted odometry source_odom_matrix = make_homogeneous_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z], [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) new_odom_matrix = camera_relative_base_transformation.dot(source_odom_matrix) # make odometry msg. twist is copied from source_odom new_odom = copy.deepcopy(msg) new_odom.header.frame_id = self.odom_frame new_odom.child_frame_id = self.base_link_frame new_odom.pose.pose.position = Point(*list(new_odom_matrix[:3, 3])) new_odom.pose.pose.orientation = Quaternion(*list(tf.transformations.quaternion_from_matrix(new_odom_matrix))) new_pose_cov_matrix = numpy.matrix(new_odom.pose.covariance).reshape(6, 6) rotation_matrix = camera_relative_base_transformation[:3, :3] new_pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot(new_pose_cov_matrix[:3, :3].dot(rotation_matrix)) new_pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(new_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix)) new_odom.pose.covariance = numpy.array(new_pose_cov_matrix).reshape(-1,).tolist() # publish self.pub.publish(new_odom) if self.publish_tf: broadcast_transform(self.broadcast, new_odom, self.invert_tf)
def base_odom_callback(self, msg): with self.lock: try: self.listener.waitForTransform(self.map_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(3)) (trans,rot) = self.listener.lookupTransform(self.map_frame, msg.header.frame_id, msg.header.stamp) # current map->base_odom transform except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return base_odom_homogeneous_matrix = make_homogeneous_matrix([getattr(msg.pose.pose.position, attr) for attr in ["x", "y", "z"]], [getattr(msg.pose.pose.orientation, attr) for attr in ["x", "y", "z", "w"]]) # base_odom -> body map_to_base_odom_homogeneous_matrix = make_homogeneous_matrix(trans, rot) # map -> base_odom slam_odom_matrix = map_to_base_odom_homogeneous_matrix.dot(base_odom_homogeneous_matrix) # map -> body self.slam_odom = Odometry() self.slam_odom.pose.pose.position = Point(*list(slam_odom_matrix[:3, 3])) self.slam_odom.pose.pose.orientation = Quaternion(*list(tf.transformations.quaternion_from_matrix(slam_odom_matrix))) self.slam_odom.header.stamp = msg.header.stamp self.slam_odom.header.frame_id = self.map_frame self.slam_odom.child_frame_id = msg.child_frame_id # calculate covariance # use covariance of base_odom.pose and only transform it to new coords from tf # TODO: calc covariance in correct way, but what is that ...? rotation_matrix = map_to_base_odom_homogeneous_matrix[:3, :3] original_pose_cov_matrix = numpy.matrix(msg.pose.covariance).reshape(6, 6) pose_cov_matrix = numpy.zeros((6, 6)) pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot(original_pose_cov_matrix[:3, :3].dot(rotation_matrix)) pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(original_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix)) self.slam_odom.pose.covariance = numpy.array(pose_cov_matrix).reshape(-1,).tolist() # twist is local and transform same as covariance new_velocity = numpy.dot(rotation_matrix, numpy.array([[msg.twist.twist.linear.x], [msg.twist.twist.linear.y], [msg.twist.twist.linear.z]])) new_omega = numpy.dot(rotation_matrix, numpy.array([[msg.twist.twist.angular.x], [msg.twist.twist.angular.y], [msg.twist.twist.angular.z]])) original_twist_cov_matrix = numpy.matrix(msg.twist.covariance).reshape(6, 6) twist_cov_matrix = numpy.zeros((6, 6)) twist_cov_matrix[:3, :3] = (rotation_matrix.T).dot(original_twist_cov_matrix[:3, :3].dot(rotation_matrix)) twist_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(original_twist_cov_matrix[3:6, 3:6].dot(rotation_matrix)) self.slam_odom.twist = TwistWithCovariance(Twist(Vector3(*new_velocity[:, 0]), Vector3(*new_omega[:, 0])), twist_cov_matrix.reshape(-1,).tolist()) if not self.pub_only_map_updated: self.pub.publish(self.slam_odom)
def calculate_camera_to_base_transform(self, stamp): try: (trans,rot) = self.listener.lookupTransform(self.camera_frame, self.base_link_frame, stamp) except: try: rospy.logwarn("[%s] failed to solve camera_to_base tf in %f. Use rospy.Time(0): %s to %s", rospy.get_name(), stamp.to_sec(), self.camera_frame, self.base_link_frame) (trans,rot) = self.listener.lookupTransform(self.camera_frame, self.base_link_frame, rospy.Time(0)) except: rospy.logwarn("[%s] failed to solve camera_to_base tf: %s to %s", rospy.get_name(), self.camera_frame, self.base_link_frame) return None camera_to_base_link = make_homogeneous_matrix(trans, rot) # camera -> base_link return camera_to_base_link
def source_odom_callback(self, msg): with self.lock: # calculate camera transform current_camera_to_base = self.calculate_camera_to_base_transform( msg.header.stamp) if current_camera_to_base is None: return if self.initial_matrix is None: self.initial_matrix = numpy.linalg.inv(current_camera_to_base) camera_relative_base_transformation = numpy.dot( self.initial_matrix, current_camera_to_base ) # base_link transformation in camera coords # calculate offseted odometry source_odom_matrix = make_homogeneous_matrix([ msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z ], [ msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w ]) new_odom_matrix = camera_relative_base_transformation.dot( source_odom_matrix) # make odometry msg. twist is copied from source_odom new_odom = copy.deepcopy(msg) new_odom.header.frame_id = self.odom_frame new_odom.child_frame_id = self.base_link_frame new_odom.pose.pose.position = Point(*list(new_odom_matrix[:3, 3])) new_odom.pose.pose.orientation = Quaternion(*list( tf.transformations.quaternion_from_matrix(new_odom_matrix))) new_pose_cov_matrix = numpy.matrix( new_odom.pose.covariance).reshape(6, 6) rotation_matrix = camera_relative_base_transformation[:3, :3] new_pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot( new_pose_cov_matrix[:3, :3].dot(rotation_matrix)) new_pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot( new_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix)) new_odom.pose.covariance = numpy.array( new_pose_cov_matrix).reshape(-1, ).tolist() # publish self.pub.publish(new_odom) if self.publish_tf: broadcast_transform(self.broadcast, new_odom, self.invert_tf)
def calculate_camera_to_base_transform(self, stamp): try: (trans, rot) = self.listener.lookupTransform(self.camera_frame, self.base_link_frame, stamp) except: try: rospy.logwarn( "[%s] failed to solve camera_to_base tf in %f. Use rospy.Time(0): %s to %s", rospy.get_name(), stamp.to_sec(), self.camera_frame, self.base_link_frame) (trans, rot) = self.listener.lookupTransform(self.camera_frame, self.base_link_frame, rospy.Time(0)) except: rospy.logwarn( "[%s] failed to solve camera_to_base tf: %s to %s", rospy.get_name(), self.camera_frame, self.base_link_frame) return None camera_to_base_link = make_homogeneous_matrix( trans, rot) # camera -> base_link return camera_to_base_link