def write_planner_input_file(self, file_name):
s = ""
for obj in self._frames.values():
json_str = json_message_converter.convert_ros_message_to_json(obj)
s += json_str + "\n"
s += "\n"
for obj in self._locs.values():
json_str = json_message_converter.convert_ros_message_to_json(obj)
s += json_str + "\n"
s += "\n"
for obj in self._ovcs.values():
json_str = json_message_converter.convert_ros_message_to_json(obj)
s += json_str + "\n"
s += "\n"
for obj in self._cts.values():
json_str = json_message_converter.convert_ros_message_to_json(obj)
s += json_str + "\n"
s += "\n"
print(s)
with open(file_name + '.json', 'w') as text_file:
text_file.write(s)
def GModelStatescallback(data):
global g_model_states_file_lin
global g_model_states_file_pos
global g_model_states_file_clo
json_str1 = json_message_converter.convert_ros_message_to_json(
data.twist[3])
json.dump(json_str1, g_model_states_file_lin)
json_str2 = json_message_converter.convert_ros_message_to_json(
data.pose[3])
json.dump(json_str2, g_model_states_file_pos)
def bbox_for_sub(message):
json_str = json_message_converter.convert_ros_message_to_json(message)
obj_Locs = ast.literal_eval(json.loads(json_str).values()[0])
for o in obj_Locs:
o[1] = tuple(o[1])
o[0] = o[0].encode("ascii")
return (obj_Locs)
def save_transfrom_as_json(self, source_frame, target_frame):
# save transform from source to target as json file at the save_folder
save_path = os.path.join(self.camera_map, "{}_to_{}_{}.json".format(source_frame, target_frame, time.strftime("%Y%m%d-%H%M%S")))
rospy.loginfo("Saving transform at {}".format(save_path))
transform = self.tf_buffer.lookup_transform(source_frame, target_frame, rospy.Time(), rospy.Duration(1.0))
with open(save_path, "w") as json_file:
json.dump(json_message_converter.convert_ros_message_to_json(transform), json_file)
def callback_data(self, msg):
rospy.loginfo("subscribe kddisensor data")
kddisensor_data = json.loads(
json_message_converter.convert_ros_message_to_json(msg))
f = open(self.jsonfile_name, "w")
json.dump(kddisensor_data, f)
f.close()
def test_ros_message_with_string_unicode(self):
from std_msgs.msg import String
expected_json = '{"data": "Hello \\u00dcnicode"}'
message = String(data = u'Hello \u00dcnicode')
message = serialize_deserialize(message)
returned_json = json_message_converter.convert_ros_message_to_json(message)
self.assertEqual(returned_json, expected_json)
def __init__(self, connect_to='tcp://127.0.0.1:5555'):
"""
sublist:list of rostopics you want sent over
"""
#important zmq initialization stuff
self.zmq_context = zmq.SerializingContext()
self.zmq_socket = self.zmq_context.socket(zmq.REQ)
self.zmq_socket.connect(connect_to)
#convenient lambdas to use later on
self.rostojson = lambda x: json_message_converter.convert_ros_message_to_json(
x)
self.jsontoros = lambda topic, x: json_message_converter.convert_json_to_ros_message(
topic, x)
#Syncs to each frame
self.lidar_sub = rospy.Subscriber("/scan", LaserScan,
self.lidar_callback)
self.steer_sub = rospy.Subscriber(
"/vesc/high_level/ackermann_cmd_mux/output", AckermannDriveStamped,
self.steer_callback)
self.cam_sub = rospy.Subscriber("/usb_cam/image_raw", Image,
self.cam_callback)
#Hooks sends to the camera, so we need the latest of each observation
self.latest_obs = {}
m.patch()
self.bridge = CvBridge()
def callback(message):
global typ
returned_json = json_message_converter.convert_ros_message_to_json(message)
outbox_message = OutBoxMsg(header=message.header,
message_type=typ,
message_content=returned_json)
pub.publish(outbox_message)
def test_ros_message_with_string(self):
from std_msgs.msg import String
expected_json = '{"data": "Hello"}'
message = String(data='Hello')
returned_json = json_message_converter.convert_ros_message_to_json(
message)
self.assertEqual(returned_json, expected_json)
def test_ros_message_with_uint8_array(self):
from rospy_message_converter.msg import Uint8Array3TestMessage
input_data = "".join([chr(i) for i in [97, 98, 99, 100]])
expected_json = '{"data": "YWJjZA=="}' # base64encode("abcd") YWJjZA==
message = Uint8Array3TestMessage(data=input_data)
returned_json = json_message_converter.convert_ros_message_to_json(message)
self.assertEqual(returned_json, expected_json)
def socJsonCallBack(soc_data_):
global flag, socflag, filename
if soc_data_.code == 5 and soc_data_.action == 1:
filename = "guiji/guiji" + soc_data_.filename
socflag = 1
elif soc_data_.code == 3 or soc_data_.code == 4:
socflag = 0
flag = 0
elif soc_data_.code == 2:
print socflag
flag = 0
if socflag == 1:
#if soc_data_.status.status == 0:
fs = open(filename, "r") #+soc_data_.filename,"r")
a = fs.readlines()
global y5
y5 = {}
i = 0
ite = path()
for line in a[0:]:
seq = re.compile(" ")
lst = seq.split(line.strip())
p = pose()
ite.poses.append(p)
ite.poses[i].x = float(lst[0])
ite.poses[i].y = float(lst[1])
i = i + 1
path_json_ = json_message_converter.convert_ros_message_to_json(
ite)
y5 = path_json_
#print y5
fs.close()
def test_ros_message_with_string(self):
from std_msgs.msg import String
expected_json = '{"data": "Hello"}'
message = String(data = 'Hello')
message = serialize_deserialize(message)
returned_json = json_message_converter.convert_ros_message_to_json(message)
self.assertEqual(returned_json, expected_json)
def ros_callback(self, req, args):
#Example callback for services
settings = args[0]
channel = args[1]
connection = args[2]
try:
json_str = json_message_converter.convert_ros_message_to_json(req)
corr_id = str(uuid.uuid4())
with self.lock:
self.responses[corr_id] = None
channel.basic_publish(exchange=settings["exchange"], \
routing_key=settings["routing_key"], \
body=json_str, \
properties=pika.BasicProperties(
reply_to = settings["callback_queue"],
correlation_id = corr_id,
),
)
start_wait = datetime.datetime.utcnow()
while datetime.datetime.utcnow() < start_wait + datetime.timedelta(
seconds=10):
with self.lock:
response = self.responses[corr_id]
if response is not None:
del self.responses[corr_id]
resp = settings["service_type"]._response_class()
resp.response.data = response
return resp
connection.process_data_events()
except:
traceback.print_exc()
resp = settings["service_type"]._response_class()
resp.response.data = ""
return resp
def callback(_data, sensorType):
global inputArrays
# Using a library to convert ROS message into readable JSON format
inputArrays[
sensorType] = json_message_converter.convert_ros_message_to_json(_data)
complete = 1
# If any of the sensors has yet been tracked
for sensor in inputArrays:
if sensor == None:
complete = 0
# Should only send to server once all sensors has been tracked
if complete == 1:
try:
# Converting JSON content to string and sending it in its respective field
r = requests.post('http://192.168.1.100:3001/boat',
data={
"gps": str(inputArrays[0]),
"imu": str(inputArrays[1]),
"time": str(inputArrays[2]),
"wind": str(inputArrays[3]),
"sail": str(inputArrays[4]),
"rudder": str(inputArrays[5])
},
timeout=3.9)
print("SUCCESS: sensor input sent to database")
inputArrays = [None, None, None, None, None, None]
except requests.exceptions.RequestException as e:
print("FAILURE: sensor input failed to send to database")
inputArrays = [None, None, None, None, None, None]
def test_ros_message_with_3uint8_array(self):
from rospy_message_converter.msg import Uint8Array3TestMessage
input_data = [97, 98, 99]
expected_json = '{"data": "YWJj"}' # base64.b64encode("abc") is "YWJj"
message = Uint8Array3TestMessage(data=input_data)
message = serialize_deserialize(message)
returned_json = json_message_converter.convert_ros_message_to_json(message)
self.assertEqual(returned_json, expected_json)
def test_ros_message_with_3uint8_array(self):
from rospy_message_converter.msg import Uint8Array3TestMessage
input_data = "".join([chr(i) for i in [97, 98, 99]])
expected_json = '{"data": "YWJj"}' # base64.standard_b64encode("abc") is "YWJj"
message = Uint8Array3TestMessage(data=input_data)
message = serialize_deserialize(message)
returned_json = json_message_converter.convert_ros_message_to_json(message)
self.assertEqual(returned_json, expected_json)
def map2string(req):
message = req.grid
json_str = json_message_converter.convert_ros_message_to_json(message)
r = String()
print(json.dumps(json_str))
r.data = json.dumps(json_str)
print("Converting Map to String")
return r
def create_region_descriptor(self, region):
d = Descriptor()
d.type = self.data_type
d.data = json_message_converter.convert_ros_message_to_json(region)
d.ref = self.descriptor_ref
d.tags = self.description_tags
return d
def test_ros_message_with_uint8_array(self):
from rospy_message_converter.msg import Uint8Array3TestMessage
input_data = "".join([chr(i) for i in [97, 98, 99, 100]])
expected_json = '{"data": "YWJjZA=="}' # base64encode("abcd") YWJjZA==
message = Uint8Array3TestMessage(data=input_data)
returned_json = json_message_converter.convert_ros_message_to_json(
message)
self.assertEqual(returned_json, expected_json)
def test_ros_message_with_header(self):
from std_msgs.msg import Header
rospy.init_node('time_node')
now_time = rospy.Time.now()
expected_json = '{{"stamp": {{"secs": {0}, "nsecs": {1}}}, "frame_id": "my_frame", "seq": 3}}'\
.format(now_time.secs, now_time.nsecs)
message = Header(stamp = now_time, frame_id = 'my_frame', seq = 3)
returned_json = json_message_converter.convert_ros_message_to_json(message)
self.assertEqual(returned_json, expected_json)
def test_ros_message_with_header(self):
from std_msgs.msg import Header
rospy.init_node('time_node')
now_time = rospy.Time.now()
expected_json = '{{"stamp": {{"secs": {0}, "nsecs": {1}}}, "frame_id": "my_frame", "seq": 3}}'\
.format(now_time.secs, now_time.nsecs)
message = Header(stamp=now_time, frame_id='my_frame', seq=3)
returned_json = json_message_converter.convert_ros_message_to_json(
message)
self.assertEqual(returned_json, expected_json)
def save_pose(self):
try:
rospy.loginfo("[" + rospy.get_name() + "] " +
"... saving current pose:\n" + str(self.robotPoseSt))
jsonPose = json_message_converter.convert_ros_message_to_json(
self.robotPoseSt)
with open(self.saved_pose_uri, 'w+') as outfile:
outfile.write(jsonPose)
except AttributeError:
rospy.logwarn("[" + rospy.get_name() + "] " +
"No saved pose to store.")
def trajectoryCallback(self, trajectoryMessage):
messagecopy = do8Trajectory()
messagecopy.header = trajectoryMessage.header
messagecopy.trajectory = trajectoryMessage.trajectory[:30]
print('message is...')
# print(messagecopy)
jsonText = json_message_converter.convert_ros_message_to_json(
messagecopy)
# print (jsonText)
self.sendToRemote(jsonText)
return
def get_pose(mess):
json_str = json_message_converter.convert_ros_message_to_json(
waypoint_marker.getPose())
print json_str
print mess
#results = db.search(Todo.Category == 'Home')
#waypoint_db.insert({'mapname': mess.mapname, 'name' : mess.name, 'id': mess.seq, 'waypoint': json_str})
with open(os.path.join(MAP_PATH, mess.mapname + '.json')) as json_file:
data = json.load(json_file)
data[mess.name] = json_message_converter.convert_ros_message_to_json(
waypoint_marker.getPose())
with open(os.path.join(MAP_PATH, mess.mapname + '.json'), 'w') as fp:
json.dump(data, fp)
print json_str
return getpostResponse(waypoint_marker.getPose())
def simple_callback(self, data, args):
#Demo callback. Simply publishes the converted json on the same topic as recieved on.
settings = args[0]
channel = args[1]
try:
json_str = json_message_converter.convert_ros_message_to_json(data)
channel.basic_publish(exchange=settings["exchange"],
routing_key=settings["routing_key"],
body=json_str)
except:
traceback.print_exc()
def on_incoming_message(self, msg_data, topic_name):
# type: (AnyMsg, str) -> None
connection_header = msg_data._connection_header['type'].split('/')
ros_pkg = connection_header[0] + '.msg'
msg_type = connection_header[1]
#print 'Message type detected as ' + msg_type
msg_class = getattr(import_module(ros_pkg), msg_type)
msg = msg_class()
msg.deserialize(msg_data._buff)
json_msg = json_message_converter.convert_ros_message_to_json(msg)
self.publishers[topic_name].publish(json_msg)
def callback(data):
message = json_message_converter.convert_ros_message_to_json(data)
weather = json.loads(message).values()[0]
weather = ast.literal_eval(weather)
seconds = rospy.get_time()
dt = datetime.fromtimestamp(seconds)
print(
str(dt) + ' Test weather subscriber - the weather in ' +
str(weather['name']) + ' is ' +
str(weather['weather'][0]['description']))
def socJsonCallBack(soc_data_):
global flag, socflag, filename
#if soc_data_.code == 1:
#flag=1
#socflag=0
#time.sleep(2)
#f = open('map.json', 'w')
#item = {
# "map3d": y12,
#}
#e = {
# "code": 2,
# "data": item
#}
#d=json.dumps(e)
#socketSend(json.dumps(e))
#socketSend("end---")
#time.sleep(3)
#f.write(d)
#f.write('ctrmodeback : '+y7+' / '+'status : '+y8+' / '+'map3d : '+y1+' / '+'gps3d : '+y2+' / '+'v : '+y3+' / '+'pos : '+y4+' / '+'list : '+y6+' / '+'pathplan : '+y5+' / '+'panctrmodeback : '+y10+' / '+'panangle : '+y9+' / ')
#y6=''
#f.close()
if soc_data_.code == 5 and soc_data_.action == 1:
filename = "guiji/guiji" + soc_data_.filename
socflag = 1
elif soc_data_.code == 3 or soc_data_.code == 4:
socflag = 0
flag = 0
elif soc_data_.code == 2:
print socflag
flag = 0
if socflag == 1:
#if soc_data_.status.status == 0:
fs = open(filename, "r") #+soc_data_.filename,"r")
a = fs.readlines()
global y5
y5 = {}
i = 0
ite = path()
for line in a[0:]:
seq = re.compile(" ")
lst = seq.split(line.strip())
p = pose()
ite.poses.append(p)
ite.poses[i].x = float(lst[0])
ite.poses[i].y = float(lst[1])
i = i + 1
path_json_ = json_message_converter.convert_ros_message_to_json(
ite)
y5 = path_json_
#print y5
fs.close()
def save_transfrom_as_json(self, header, source_frame, target_frame):
# save transform from source to target as json file at the save_folder
transform = self.tf_buffer.lookup_transform(target_frame, source_frame,
rospy.Time(),
rospy.Duration(1.0))
with open(
os.path.join(
self.save_folder,
"{}_{}_to_{}.json".format(header, source_frame,
target_frame)),
"w") as json_file:
json.dump(
json_message_converter.convert_ros_message_to_json(transform),
json_file)
def _ros_cb(self, data, topic_name): # This is Ros topic call back
try:
(mqtt_topic, data_type) = self.publisher[topic_name]
except KeyError:
rospy.logwarn(
"[Ros_mqtt_bridge] Publisher Topic name: " + topic_name +
" doesn't exist. Need to init first. Ignore message.")
return
# Mqtt publish
self.mqtt_obj.publish(
mqtt_topic,
json_message_converter.convert_ros_message_to_json(data),
qos=0,
retain=False) # non-blocking msg
def publish(self):
rate = rospy.Rate(20)
count = 0
#Cound number of dirs already existant and make a new one at size + 1
dir_number = len(os.walk("images").next()[1])
save_path = os.path.join("images", "collection_" + str(dir_number))
#Make the dir
os.mkdir(save_path)
print("Saving to {}".format(save_path))
#Run this publisher indefinitely
garbage = 0
while not rospy.is_shutdown():
if self.image is not None and self.linear is not None and self.angular is not None:
rospy.loginfo("All vars initialised")
if abs(self.linear.x) > 0.001 and abs(self.angular.z) > 0.001:
garbage = 0
if garbage > 0:
print(
"Discarded {} images which were stationary".format(
garbage))
try:
rospy.logwarn("saving image %s in dir %s", count,
dir_number)
# cv2.imshow('image',self.image)
# cv2.waitKey(0)
image_path = os.path.join(save_path,
str(count) + ".jpg")
json_path = os.path.join(save_path,
str(count) + ".json")
json_str = json_message_converter.convert_ros_message_to_json(
self.twist)
rospy.loginfo(json_str)
#Write image file and json file to disk
cv2.imwrite(image_path, self.image)
with open(json_path, 'w') as outfile:
outfile.write(json_str)
# json.dump(json_str, outfile)
count += 1
except Exception as e:
rospy.logerr(e)
else:
garbage += 1
rate.sleep()
def globalJsonCallBack(global_data_):
global flag
#if flag==1:
global y5
ite = path()
y5 = {}
a = len(global_data_.poses)
for i in range(a):
p = pose()
ite.poses.append(p)
ite.poses[i].x = global_data_.poses[i].pose.position.x
ite.poses[i].y = global_data_.poses[i].pose.position.y
path_json_ = json_message_converter.convert_ros_message_to_json(ite)
y5 = path_json_
print y5
def callbackManual(data):
rospy.loginfo("agv_test_web.py-callbackManual()")
typeData = "geometry_msgs/Twist"
data = json_message_converter.convert_ros_message_to_json(data)
# rospy.loginfo("agv_main.py-data: %s", data)
action = agv_action()
action.action = 1
action.type = typeData
action.data = data
rospy.loginfo("agv_test_web.py-action.type: %s", action.type)
rospy.loginfo("agv_test_web.py-action.data: %s", action.data)
global pub
pub.publish(action)
rospy.loginfo("agv_test_web.py-Publisher to topic /agv_action")
def saveInterestPoint(self, data, type="simple_goal"):
itPoint = InterestPoint()
itPoint.label = "It"+str(self._index_label)
itPoint.type = type
itPoint.pose = data.pose
itPoint.arm_position = 0
self.sendNavOrderAction('NP', 'CleanRetryReplay', str(self._index_label), 30)
self._index_label+=1
f = open(self.CONFIG_PATH + str(itPoint.label) + '.coord', 'w+')
json_str = json_message_converter.convert_ros_message_to_json(itPoint)
f.write(json_str)
f.close()
rospy.loginfo('Successfully saved the interestPoint:' + str(json_str))
def format_from_ros_msg(self,ros_message):
return json_message_converter.convert_ros_message_to_json(ros_message)
var_length = msg_type.endswith("[]")
splits = msg_type.split("[")
if len(splits) > 2:
raise ValueError("Currently only support 1-dimensional array types: %s" % msg_type)
if var_length:
return msg_type[:-2], True, None
else:
try:
length = int(splits[1][:-1])
return splits[0], True, length
except ValueError:
raise ValueError("Invalid array dimension: [%s]" % splits[1][:-1])
else:
return msg_type, False, None
if __name__ == "__main__":
if len(sys.argv) < 2:
print "Usage : get_msg_spec.py <MESSAGE TYPE> [MESSAGE TYPE ...]"
sys.exit(0)
msg_types = sys.argv[1:]
search_path = get_search_path()
for msg_type in msg_types:
spec = get_spec(msg_type, search_path)
typedef = spec_to_typedef_msg(spec)
json_typedef = json_message_converter.convert_ros_message_to_json(typedef)
print (json_typedef)
