def from_dict(cls, data, frame, lifetime):
"""Deserializes obstacle data into two MarkerArrays.
Args:
data: A dictionary.
frame: Frame ID of every Marker.
lifetime: Lifetime of every Marker in seconds.
Returns:
Tuple of two visualization_msgs/MarkerArray, MarkerArray) tuple.
The first is of moving obstacles, and the latter is of stationary
obstacles.
"""
# Generate base header.
header = Header()
header.stamp = rospy.get_rostime()
header.frame_id = frame
# Parse moving obstacles, and populate markers with spheres.
moving_obstacles = GeoSphereArrayStamped()
moving_obstacles.header = header
if "moving_obstacles" in data:
for obj in data["moving_obstacles"]:
# Moving obstacles are spheres.
obstacle = GeoSphere()
# Set scale as radius.
obstacle.radius = feet_to_meters(obj["sphere_radius"])
obstacle.center.latitude = obj["latitude"]
obstacle.center.longitude = obj["longitude"]
obstacle.center.altitude = feet_to_meters(obj["altitude_msl"])
moving_obstacles.spheres.append(obstacle)
# Parse stationary obstacles, and populate markers with cylinders.
stationary_obstacles = GeoCylinderArrayStamped()
stationary_obstacles.header = header
if "stationary_obstacles" in data:
for obj in data["stationary_obstacles"]:
# Stationary obstacles are cylinders.
obstacle = GeoCylinder()
# Set scale to define size.
obstacle.radius = feet_to_meters(obj["cylinder_radius"])
obstacle.height = feet_to_meters(obj["cylinder_height"])
obstacle.center.latitude = obj["latitude"]
obstacle.center.longitude = obj["longitude"]
stationary_obstacles.cylinders.append(obstacle)
return (moving_obstacles, stationary_obstacles)
评论列表
文章目录
