swri_transform_util.origin_manager module

A module containing a simple class for publishing the Local XY origin as a PoseStamped.

exception swri_transform_util.origin_manager.InvalidFixException

Bases: Exception

Exception to throw when a GPSFix or NavSatFix message does not contain a valid fix.

class swri_transform_util.origin_manager.OriginManager(node, local_xy_frame, local_xy_frame_identity=None)

Bases: object

A simple class for publishing the Local XY origin as a PoseStamped.

Creates publishers to /local_xy_origin (geometry_msgs/PoseStamped) and /diagnostics (diagnostic_msgs/DiagnosticArray). Publishes an identity TF from the origin to support /tf<->/wgs84 conversions.

Usage (manual origin):

manager = OriginManager(“/map”) manager.set_origin(29.45196669, -98.61370577, 233.719) manager.start()

Usage (manual origin from parameter):

local_xy_origin = rospy.get_param(‘~local_xy_origin’) manager = OriginManager(“map”) origin_list = rospy.get_param(‘~local_xy_origins’) manager.set_origin_from_list(local_xy_origin, origin_list) manager.start()

Usage (auto origin from NavSatFix):

def navsat_callback(msg, params):

(manager, navsat_sub) = params try:

manager.set_origin_from_navsat(msg)

except InvalidFixException as e:

rospy.logwarn(e) return

finally:

navsat_sub.unregister()

manager = OriginManager(“/map”) navsat_sub = rospy.Subscriber(“fix”, NavSatFix, queue_size=2) navsat_sub.impl.add_callback(navsat_callback, (manager, navsat_sub)) manager.start()

Usage (auto origin from GPSFix):

def gps_callback(msg, params):

(manager, gps_sub) = params try:

manager.set_origin_from_gps(msg)

except InvalidFixException as e:

rospy.logwarn(e) return

finally:

gps_sub.unregister()

manager = OriginManager(“/map”) gps_sub = rospy.Subscriber(“gps”, GPSFix, queue_size=2) gps_sub.impl.add_callback(gps_callback, (manager, gps_sub)) manager.start()

Usage (auto origin from custom topic):

def navsat_callback(msg, params):

(manager, custom_sub) = params pos = (msg.latitude, msg.longitude, mgs.altitude) manager.set_origin_from_custom(pos, msg.header.stamp) custom_sub.unregister()

manager = OriginManager(“/map”) custom_sub = rospy.Subscriber(custom_topic, CustomMsg, queue_size=2) custom_sub.impl.add_callback(custom_callback, (manager, custom_sub)) manager.start()

publish_messages()

Publish identity transform and diagnostics.

This method can be called as a rospy timer callback.

set_origin(source, latitude, longitude, altitude, stamp=None)

Set the origin to the position described by the arguments and publish it.

All other set_ methods wrap this method.

Args:

source (string): The source of the origin. latitude (float): The latitude of the origin in degrees. longitude (float): The longitude of the origin in degrees. altitude (float): The altitude of the origin in meters.

Positive values correspond to altitude above the geoid.

stamp (rospy.Time): The time to use for the origin’s header.stamp field.

If the argument is None, the stamp is not set and defaults to rospy.Time(0). (default None).

set_origin_from_custom(pos, stamp)

Set the local origin from a custom object.

Args:

pos: Tuple with the local origin stamp: Msg header stamp if available

set_origin_from_dict(origin_dict)

Set the local origin from a dictionary.

Args:

origin_dict (dict): A dictionary containing the keys “latitude”, “longitude”, and

“altitude” with appropriate float values

Raises:

KeyError: If origin_dict does not contain all of the required keys.

set_origin_from_gps(msg)

Set the local origin from a gps_common.msg.GPSFix object.

Args:

msg (gps_common.msg.GPSFix): A GPSFix message with the local origin

Raises:

InvalidFixException: If msg.status.status is STATUS_NO_FIX

set_origin_from_list(origin_name, origin_list)

Set the local origin from a list of named origins.

Args:

origin_name (str): The name of the origin in origin_list to use origin_list (list): A list of dicts containing a name key and all keys required

by set_origin_from_dict()

Raises:

KeyError: If any origin in origins_list lacks a “name” key or if there exists

no origin in origins_list whose “name” is origin_name

set_origin_from_navsat(msg)

Set the local origin from a sensor_msgs.msg.NavSatFix object.

Args:

msg (sensor_msgs.msg.NavSatFix): A NavSatFix message with the local origin

Raises:

InvalidFixException: If msg.status.status is STATUS_NO_FIX

start()

Creates a timer that periodically publishes our messages.