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)