def rosmsg_cmd_show(mode, full, alias='show'): import rosbag cmd = "ros%s" % (mode[1:]) parser = OptionParser(usage="usage: %s %s [options] <%s>" % (cmd, alias, full)) parser.add_option("-r", "--raw", dest="raw", default=False, action="store_true", help="show raw message text, including comments") parser.add_option("-b", "--bag", dest="bag", default=None, help="show message from .bag file", metavar="BAGFILE") options, arg = _stdin_arg(parser, full) if arg.endswith(mode): arg = arg[:-(len(mode))] # try to catch the user specifying code-style types and error if '::' in arg: parser.error( cmd + " does not understand C++-style namespaces (i.e. '::').\nPlease refer to msg/srv types as 'package_name/Type'." ) elif '.' in arg: parser.error( "invalid message type '%s'.\nPlease refer to msg/srv types as 'package_name/Type'." % arg) if options.bag: bag_file = options.bag if not os.path.exists(bag_file): raise ROSMsgException("ERROR: bag file [%s] does not exist" % bag_file) for topic, msg, t in rosbag.Bag(bag_file).read_messages(raw=True): datatype, _, _, _, pytype = msg if datatype == arg: if options.raw: print(pytype._full_text) else: context = genmsg.MsgContext.create_default() msgs = generate_dynamic(datatype, pytype._full_text) for t, msg in msgs.items(): context.register(t, msg._spec) print(spec_to_str(context, msgs[datatype]._spec)) break else: rospack = rospkg.RosPack() if '/' in arg: #package specified rosmsg_debug(rospack, mode, arg, options.raw) else: found_msgs = list(rosmsg_search(rospack, mode, arg)) if not found_msgs: print("Could not find msg '%s'" % arg, file=sys.stderr) return 1 for found in found_msgs: print("[%s]:" % found) rosmsg_debug(rospack, mode, found, options.raw)
def test_serialize_exception(): import genpy from genpy.dynamic import generate_dynamic msgs = generate_dynamic("gd_msgs/EasyInt32", "int32 data\n") assert ['gd_msgs/EasyInt32'] == list(msgs.keys()) m_cls = msgs['gd_msgs/EasyInt32'] m_instance = m_cls() m_instance.data = '1' # the type is incorrect buff = StringIO() try: m_instance.serialize(buff) assert False, 'This should have raised a genpy.SerializationError' except genpy.SerializationError: pass except Exception: assert False, 'This should have raised a genpy.SerializationError instead'
def rosmsg_cmd_show(mode, full, alias='show'): cmd = "ros%s"%(mode[1:]) parser = OptionParser(usage="usage: %s %s [options] <%s>"%(cmd, alias, full)) parser.add_option("-r", "--raw", dest="raw", default=False,action="store_true", help="show raw message text, including comments") parser.add_option("-b", "--bag", dest="bag", default=None, help="show message from .bag file", metavar="BAGFILE") options, arg = _stdin_arg(parser, full) if arg.endswith(mode): arg = arg[:-(len(mode))] # try to catch the user specifying code-style types and error if '::' in arg: parser.error(cmd+" does not understand C++-style namespaces (i.e. '::').\nPlease refer to msg/srv types as 'package_name/Type'.") elif '.' in arg: parser.error("invalid message type '%s'.\nPlease refer to msg/srv types as 'package_name/Type'." % arg) if options.bag: bag_file = options.bag if not os.path.exists(bag_file): raise ROSMsgException("ERROR: bag file [%s] does not exist"%bag_file) for topic, msg, t in rosbag.Bag(bag_file).read_messages(raw=True): datatype, _, _, _, pytype = msg if datatype == arg: if options.raw: print(pytype._full_text) else: context = genmsg.MsgContext.create_default() msgs = generate_dynamic(datatype, pytype._full_text) for t, msg in msgs.items(): context.register(t, msg._spec) print(spec_to_str(context, msgs[datatype]._spec)) break else: rospack = rospkg.RosPack() if '/' in arg: #package specified rosmsg_debug(rospack, mode, arg, options.raw) else: found_msgs = list(rosmsg_search(rospack, mode, arg)) if not found_msgs: print("Could not find msg '%s'" % arg, file=sys.stderr) return 1 for found in found_msgs: print("[%s]:"%found) rosmsg_debug(rospack, mode, found, options.raw)
def test_generate_dynamic(): import genpy from genpy.dynamic import generate_dynamic msgs = generate_dynamic("gd_msgs/EasyString", "string data\n") assert ['gd_msgs/EasyString'] == list(msgs.keys()) m_cls = msgs['gd_msgs/EasyString'] m_instance = m_cls() m_instance.data = 'foo' buff = StringIO() m_instance.serialize(buff) m_instance2 = m_cls().deserialize(buff.getvalue()) assert m_instance == m_instance2 try: char = unichr except NameError: char = chr m_instance.data = 'foo' + char(1234) buff = StringIO() m_instance.serialize(buff) m_instance2 = m_cls().deserialize(buff.getvalue()) if sys.hexversion < 0x03000000: # python 2 requires manual decode into unicode m_instance2.data = m_instance2.data.decode('utf-8') assert m_instance == m_instance2 # 'probot_msgs' is a test for #1183, failure if the package no longer exists msgs = generate_dynamic("gd_msgs/MoveArmState", """Header header probot_msgs/ControllerStatus status #Current arm configuration probot_msgs/JointState[] configuration #Goal arm configuration probot_msgs/JointState[] goal ================================================================================ MSG: std_msgs/Header #Standard metadata for higher-level flow data types #sequence ID: consecutively increasing ID uint32 seq #Two-integer timestamp that is expressed as: # * stamp.secs: seconds (stamp_secs) since epoch # * stamp.nsecs: nanoseconds since stamp_secs # time-handling sugar is provided by the client library time stamp #Frame this data is associated with # 0: no frame # 1: global frame string frame_id ================================================================================ MSG: probot_msgs/ControllerStatus # This message defines the expected format for Controller Statuss messages # Embed this in the feedback state message of highlevel controllers byte UNDEFINED=0 byte SUCCESS=1 byte ABORTED=2 byte PREEMPTED=3 byte ACTIVE=4 # Status of the controller = {UNDEFINED, SUCCESS, ABORTED, PREEMPTED, ACTIVE} byte value #Comment for debug string comment ================================================================================ MSG: probot_msgs/JointState string name float64 position float64 velocity float64 applied_effort float64 commanded_effort byte is_calibrated """) assert set(['gd_msgs/MoveArmState', 'probot_msgs/JointState', 'probot_msgs/ControllerStatus', 'std_msgs/Header']) == set(msgs.keys()) m_instance1 = msgs['std_msgs/Header']() # make sure default constructor works m_instance2 = msgs['std_msgs/Header'](stamp=genpy.Time.from_sec(time.time()), frame_id='foo-%s'%time.time(), seq=12390) _test_ser_deser(m_instance2, m_instance1) m_instance1 = msgs['probot_msgs/ControllerStatus']() m_instance2 = msgs['probot_msgs/ControllerStatus'](value=4, comment=str(time.time())) d = {'UNDEFINED':0,'SUCCESS':1,'ABORTED':2,'PREEMPTED':3,'ACTIVE':4} for k, v in d.items(): assert v == getattr(m_instance1, k) _test_ser_deser(m_instance2, m_instance1) m_instance1 = msgs['probot_msgs/JointState']() m_instance2 = msgs['probot_msgs/JointState'](position=time.time(), velocity=time.time(), applied_effort=time.time(), commanded_effort=time.time(), is_calibrated=2) _test_ser_deser(m_instance2, m_instance1) m_instance1 = msgs['gd_msgs/MoveArmState']() js = msgs['probot_msgs/JointState'] config = [] goal = [] # generate some data for config/goal for i in range(0, 10): config.append(js(position=time.time(), velocity=time.time(), applied_effort=time.time(), commanded_effort=time.time(), is_calibrated=2)) goal.append(js(position=time.time(), velocity=time.time(), applied_effort=time.time(), commanded_effort=time.time(), is_calibrated=2)) m_instance2 = msgs['gd_msgs/MoveArmState'](header=msgs['std_msgs/Header'](), status=msgs['probot_msgs/ControllerStatus'](), configuration=config, goal=goal) _test_ser_deser(m_instance2, m_instance1)