def make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose):
yaml_dict = {}
yaml_dict["test_scene_num"] = test_scene_num.data
yaml_dict["arm_name"] = arm_name.data
yaml_dict["object_name"] = object_name.data
yaml_dict["pick_pose"] = message_converter.convert_ros_message_to_dictionary(pick_pose)
yaml_dict["place_pose"] = message_converter.convert_ros_message_to_dictionary(place_pose)
return yaml_dict
def test_ros_message_with_3uint8_array(self):
from rospy_message_converter.msg import Uint8Array3TestMessage
from base64 import standard_b64encode
expected_data = "".join([chr(i) for i in [97, 98, 99, 100]])
message = Uint8Array3TestMessage(data=expected_data)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary["data"], standard_b64encode(expected_data))
def test_ros_message_with_string(self):
from std_msgs.msg import String
expected_dictionary = { 'data': 'Hello' }
message = String(data=expected_dictionary['data'])
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def test_ros_message_with_float64(self):
from std_msgs.msg import Float64
expected_dictionary = { 'data': struct.unpack('<d', '\x7F\xEF\xFF\xFF\xFF\xFF\xFF\xFD')[0] }
message = Float64(data=expected_dictionary['data'])
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def test_ros_message_with_child_message(self):
from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
expected_dictionary = {
'layout': {
'dim': [
{ 'label': 'Dimension1', 'size': 12, 'stride': 7 },
{ 'label': 'Dimension2', 'size': 24, 'stride': 14 }
],
'data_offset': 0
},
'data': [1.1, 2.2, 3.3]
}
dimension1 = MultiArrayDimension(
label = expected_dictionary['layout']['dim'][0]['label'],
size = expected_dictionary['layout']['dim'][0]['size'],
stride = expected_dictionary['layout']['dim'][0]['stride']
)
dimension2 = MultiArrayDimension(
label = expected_dictionary['layout']['dim'][1]['label'],
size = expected_dictionary['layout']['dim'][1]['size'],
stride = expected_dictionary['layout']['dim'][1]['stride']
)
layout = MultiArrayLayout(
dim = [dimension1, dimension2],
data_offset = expected_dictionary['layout']['data_offset']
)
multiArray = Float64MultiArray(
layout = layout,
data = expected_dictionary['data']
)
dictionary = message_converter.convert_ros_message_to_dictionary(multiArray)
self.assertEqual(dictionary, expected_dictionary)
def test_ros_message_with_int8(self):
from std_msgs.msg import Int8
expected_dictionary = { 'data': -0x7F }
message = Int8(data=expected_dictionary['data'])
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def test_ros_message_with_empty(self):
from std_msgs.msg import Empty
expected_dictionary = {}
message = Empty()
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def test_ros_message_with_uint64(self):
from std_msgs.msg import UInt64
expected_dictionary = { 'data': 0xFFFFFFFFFFFFFFFF }
message = UInt64(data=expected_dictionary['data'])
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def test_ros_message_with_char(self):
from std_msgs.msg import Char
expected_dictionary = { 'data': 99 }
message = Char(data=expected_dictionary['data'])
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def test_ros_message_with_array(self):
from rospy_message_converter.msg import TestArray
expected_dictionary = {
'data': [1.1, 2.2, 3.3]
}
message = TestArray(data = expected_dictionary['data'])
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def callback(msg):
data = dict()
data["to"] = trusted.split(' ')
data["from"] = self.name
data["topic"] = "/{}{}".format(self.name, topic)
data["type"] = msg_type
data["stamp"] = time.time()
data["msg"] = mc.convert_ros_message_to_dictionary(msg)
self.conn.send_message(data)
def test_ros_message_with_time(self):
from std_msgs.msg import Time
rospy.init_node('time_node')
now_time = rospy.Time.now()
expected_dictionary = {
'data': { 'secs': now_time.secs, 'nsecs': now_time.nsecs }
}
message = Time(data=now_time)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def handle_subscription(self, data, sub):
"""
Relay subscription data through ZMQ.
@param data: The data that is published under the subscribed topic.
@param sub: I{(str)} The subscription name.
"""
d = mc.convert_ros_message_to_dictionary(data)
print d
self.pub_socket.send_multipart([sub, json.dumps(d)])
def test_ros_message_with_time(self):
from std_msgs.msg import Time
from time import time
now_time = rospy.Time(time())
expected_dictionary = {
'data': { 'secs': now_time.secs, 'nsecs': now_time.nsecs }
}
message = Time(data=now_time)
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def convert_ros_message_to_json(message):
"""
Takes in a ROS message and returns a JSON-formatted string.
Example:
ros_message = std_msgs.msg.String(data="Hello, Robot")
json_message = convert_ros_message_to_json(ros_message)
"""
dictionary = message_converter.convert_ros_message_to_dictionary(message)
json_message = json.dumps(dictionary)
return json_message
def send_message(self, msg_type, topic_name, msg):
msg_dict = message_converter.convert_ros_message_to_dictionary(msg)
send_dict = dict()
data_dict = dict()
send_dict["route"] = "topic"
data_dict["topic_name"] = topic_name
data_dict["msg_type"] = msg_type
data_dict["msg"] = msg_dict
send_dict["data"] = data_dict
json_msg = json.dumps(send_dict)
self.pub.send(zmq.Message(json_msg))
return send_dict
def test_ros_message_with_duration(self):
from std_msgs.msg import Duration
duration = rospy.rostime.Duration(33, 25)
expected_dictionary = {
'data': {
'secs' : duration.secs,
'nsecs' : duration.nsecs
}
}
message = Duration(data=duration)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def test_ros_message_with_header(self):
from std_msgs.msg import Header
rospy.init_node('time_node')
now_time = rospy.Time.now()
expected_dictionary = {
'stamp': { 'secs': now_time.secs, 'nsecs': now_time.nsecs },
'frame_id' : 'my_frame',
'seq': 3
}
message = Header(
stamp = now_time,
frame_id = expected_dictionary['frame_id'],
seq = expected_dictionary['seq']
)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def test_ros_message_with_header(self):
from std_msgs.msg import Header
from time import time
now_time = rospy.Time(time())
expected_dictionary = {
'stamp': { 'secs': now_time.secs, 'nsecs': now_time.nsecs },
'frame_id' : 'my_frame',
'seq': 3
}
message = Header(
stamp = now_time,
frame_id = expected_dictionary['frame_id'],
seq = expected_dictionary['seq']
)
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def callback(msg):
if msg._connection_header["callerid"] == rospy.get_name():
return
data = dict()
data["To"] = trusted.split(' ')
data["From"] = self.name
if topic == "/tf":
data["Topic"] = topic
else:
data["Topic"] = "/{}{}".format(self.name, topic)
data["Type"] = msg_type
data["Stamp"] = time.time()
data["Private_key"] = self.private_key
if msg_type == "tf2_msgs/TFMessage":
for t in msg.transforms:
t = self.modify_stamped_message(t)
else:
msg = self.modify_stamped_message(msg)
data["Msg"] = mc.convert_ros_message_to_dictionary(msg)
self.conn[topic].send_message(data)
def test_ros_message_with_float64(self):
from std_msgs.msg import Float64
expected_dictionary = { 'data': 0x7FEFFFFFFFFFFFF }
message = Float64(data=expected_dictionary['data'])
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
# new_target_item.pose.pos[:] = [-0.5, -0.2, 0.1]
_task_list_msg.target_items.append(new_target_item)
new_target_item = task_packing_list.msg.ItemTarget()
new_target_item.info.name = "Hammer"
new_target_item.kind = task_packing_list.msg.Item.HAMMER
new_target_item.slot_id = 3
# new_target_item.info.id = 1000
new_target_item.info.description = "If you get hit it hurts otherwise it is fun."
# new_target_item.pose.pos[:] = [-0.5, -0.3, 0.1]
_task_list_msg.target_items.append(new_target_item)
if WITH_PUBLISHING:
task_list_publisher.publish(_task_list_msg)
msg_dict = message_converter.convert_ros_message_to_dictionary(new_target_item)
print msg_dict
msg_dict = message_converter.convert_ros_message_to_dictionary(_task_list_msg)
json_str = json.dumps(msg_dict, sort_keys=True, indent=2, separators=(",", ": "))
f = open(file_path, "w+")
f.write(json_str)
f.close()
print "\n#################\nFinished -> file was written %s\n\n" % file_path
print 60 * "#"
print json_str
f = open(file_path, "r+")
json_str = f.read()
def test_ros_message_with_byte(self):
from std_msgs.msg import Byte
expected_dictionary = { 'data': 5 }
message = Byte(data=expected_dictionary['data'])
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def onMessage(self, data):
self.on_message.emit(message_converter.convert_ros_message_to_dictionary(data))
def emit_new_goal(self, pose):
self.emit('target_pose', message_converter.convert_ros_message_to_dictionary(pose)['pose'])
def convert_ros_message_to_json(message):
dictionary = message_converter.convert_ros_message_to_dictionary(message)
json_message = json.dumps(dictionary)
return json_message
def onMessage(self, data):
self.on_message.emit(
message_converter.convert_ros_message_to_dictionary(data))
