Source code for morse.middleware.ros.semantic_camera
import logging; logger = logging.getLogger("morse." + __name__)
import json
import rospy
from std_msgs.msg import String
from morse.middleware.ros import ROSPublisherTF
from morse.middleware.socket_datastream import MorseEncoder
[docs]class SemanticCameraPublisher(ROSPublisherTF):
""" Publish the data of the semantic camera as JSON in a ROS String message.
And send TF transform between '/map' and ``object.name``.
"""
ros_class = String
[docs] def initialize(self):
if not self.component_instance.relative:
self.default_frame_id = '/map'
ROSPublisherTF.initialize(self)
[docs] def default(self, ci='unused'):
for obj in self.data['visible_objects']:
# send tf-frame for every object
self.sendTransform(obj['position'], obj['orientation'],
self.get_time(), str(obj['name']), self.frame_id)
string = String()
string.data = json.dumps(self.data['visible_objects'], cls=MorseEncoder)
self.publish(string)
[docs]class SemanticCameraPublisherLisp(ROSPublisherTF):
""" Publish the data of the semantic camera as a ROS String message,
that contains a lisp-list (each field are separated by a space).
This function was designed for the use with CRAM and the Adapto group.
"""
ros_class = String
[docs] def initialize(self):
if not self.component_instance.relative:
self.default_frame_id = '/map'
ROSPublisherTF.initialize(self)
[docs] def default(self, ci='unused'):
string = String()
string.data = "("
for obj in self.data['visible_objects']:
description = obj['description'] or '-'
# send tf-frame for every object
self.sendTransform(obj['position'], obj['orientation'],
self.get_time(), str(obj['name']), self.frame_id)
# Build string from name, description, location and orientation in the global world frame
string.data += "(" + str(obj['name']) + " " + description + " " + \
str(obj['position'].x) + " " + \
str(obj['position'].y) + " " + \
str(obj['position'].z) + " " + \
str(obj['orientation'].x) + " " + \
str(obj['orientation'].y) + " " + \
str(obj['orientation'].z) + " " + \
str(obj['orientation'].w) + ")"
string.data += ")"
self.publish(string)