Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 8 additions & 8 deletions resources/cob_light.ros
Original file line number Diff line number Diff line change
@@ -1,18 +1,18 @@
PackageSet { package {
CatkinPackage cob_light { artifact {
PackageSet {
CatkinPackage cob_light {
Artifact cob_light {
node Node { name cob_light
serviceserver {
Node { name cob_light
ServiceServers {
ServiceServer { name 'set_light' service 'cob_light.SetLightMode'},
ServiceServer { name 'stop_mode' service 'cob_light.StopLightMode'}}
publisher {
Publishers {
Publisher { name '/diagnostics' message 'diagnostic_msgs.DiagnosticArray'},
Publisher { name 'marker' message 'visualization_msgs.Marker'},
Publisher { name 'debug' message 'std_msgs.ColorRGBA'},
Publisher { name 'debugMulti' message 'cob_light.ColorRGBAArray'}}
subscriber {
Subscribers {
Subscriber { name '/scan_unified' message 'sensor_msgs.LaserScan'},
Subscriber { name 'command' message 'cob_light.ColorRGBAArray'}}
actionserver {
ActionServers {
ActionServer { name 'set_light' action 'cob_light.SetLightModeAct'}}
}}}}}}
}}}}
2 changes: 1 addition & 1 deletion resources/robotino.rossystem
Original file line number Diff line number Diff line change
Expand Up @@ -35,4 +35,4 @@ RosSystem { Name 'robotino'
RosPublisher 'hokuyo/parameter_updates' { RefPublisher 'hokuyo_node.hokuyo_node.hokuyo_node.hokuyo/parameter_updates'}}
RosSrvClients{
RosServiceClient 'hokuyo/set_parameters' { RefClient 'hokuyo_node.hokuyo_node.hokuyo_node.hokuyo/set_parameters'},
RosServiceClient 'hokuyo/self_test' { RefClient 'hokuyo_node.hokuyo_node.hokuyo_node.hokuyo/self_test'}}} , ComponentInterface { name joy2 RosPublishers { RosPublisher joy { RefPublisher "joy.joy_node.joy_node.joy" } , RosPublisher diagnostics { RefPublisher "joy.joy_node.joy_node.diagnostics" } } } ) TopicConnections { TopicConnection joint_states { From ( "robotino_base.joint_states" ) To ( "robot_state_publisher.joint_states" ) } } }
RosServiceClient 'hokuyo/self_test' { RefClient 'hokuyo_node.hokuyo_node.hokuyo_node.hokuyo/self_test'}}} , ComponentInterface { name joy2 RosPublishers { RosPublisher joy { RefPublisher "joy.joy_node.joy_node.joy" } , RosPublisher diagnostics { RefPublisher "joy.joy_node.joy_node.diagnostics" } } } ) TopicConnections { TopicConnection joint_states { From ( "robotino_base.joint_states" ) To ( "robot_state_publisher.joint_states" ) } } }
Original file line number Diff line number Diff line change
Expand Up @@ -20,12 +20,15 @@

import rospy
import yaml
import os, rospkg


def ros_system_model_generator_test():
ros_system_model = RosSystemModelGenerator()
ros_system_model.setSystemName("test_system")
for key, value in yaml.load(open('../resources/rosparam_example.yaml')).iteritems():
rospack = rospkg.RosPack()
file_path=rospack.get_path("ros_model_parser")+'/resources/rosparam_example.yaml'
for key, value in yaml.load(open(os.path.join(file_path))).iteritems():
ros_system_model.addParameter(key,value)
ros_system_model.dump_java_ros_system_model("/tmp/test.rossystem")

Expand Down
6 changes: 3 additions & 3 deletions src/ros_model_generator/rosmodel_generator.py
Original file line number Diff line number Diff line change
Expand Up @@ -57,12 +57,12 @@ def dump_java_ros_model(self, ros_model_file):
outfile.write(ros_model_str)

def create_ros_model(self):
ros_model_str = "PackageSet { package { \n"
ros_model_str = "PackageSet {\n"
ros_model_str += " CatkinPackage "+self.package_name + " { "
ros_model_str += "artifact {\n"
ros_model_str += "\n"
ros_model_str += self.node.dump_java_ros_model()
ros_model_str = ros_model_str[:-2]
ros_model_str += "\n}}}}"
ros_model_str += "\n}}"
return True, ros_model_str


Expand Down
19 changes: 8 additions & 11 deletions src/ros_model_parser/rosmodel_parser.py
Original file line number Diff line number Diff line change
Expand Up @@ -66,11 +66,11 @@ def __init__(self, model, isFile=True):
lambda tokens: float(tokens[0])) | string_value | Keyword("false") | Keyword("true") | listStr | mapStr

_packageSet = Keyword("PackageSet").suppress()
_package = Keyword("package").suppress()
#_package = Keyword("package").suppress()
_catkin_pkg = Keyword("CatkinPackage").suppress()
_artifact = Keyword("artifact").suppress()
#_artifact = Keyword("artifact").suppress()
_artifacts = Keyword("Artifact").suppress()
_node = Keyword("node").suppress()
#_node = Keyword("node").suppress()
_nodes = Keyword("Node").suppress()
_name = CaselessKeyword("name").suppress()

Expand All @@ -79,36 +79,33 @@ def __init__(self, model, isFile=True):
_topic_type= Keyword("message").suppress()

# Service Server
_service_svrs= Keyword("serviceserver").suppress()
_service_svrs= Keyword("ServiceServers").suppress()
_service_svr= Keyword("ServiceServer").suppress()
service_svr = Group( _service_svr + OCB + _name + name("name") + _srv_type + name("type") + CCB)
service_svrs = (_service_svrs + OCB + OneOrMore(service_svr + Optional(",").suppress()) + CCB)

# Service Client
_service_clis= Keyword("serviceclient").suppress()
_service_clis= Keyword("ServiceClients").suppress()
_service_cli= Keyword("ServiceClient").suppress()
service_cli = Group( _service_cli + OCB + _name + name("name") + _srv_type + name("type") + CCB)
service_clis = (_service_clis + OCB + OneOrMore(service_cli + Optional(",").suppress()) + CCB)

# Publisher
_pubs= Keyword("publisher").suppress()
_pubs= Keyword("Publishers").suppress()
_pub= Keyword("Publisher").suppress()
pub = Group( _pub + OCB + _name + name("name") + _topic_type + name("type") + CCB)
pubs = (_pubs + OCB + OneOrMore(pub + Optional(",").suppress()) + CCB)

# Subscriber
_subs= Keyword("subscriber").suppress()
# Subscriber
_subs= Keyword("Subscribers").suppress()
_sub= Keyword("Subscriber").suppress()
sub = Group( _sub + OCB + _name + name("name") + _topic_type + name("type") + CCB)
subs = (_subs + OCB + OneOrMore(sub + Optional(",").suppress()) + CCB)

self.rospkg_grammar = _packageSet + \
OCB + \
_package + OCB + \
_catkin_pkg + name("pkg_name") + OCB + \
_artifact + OCB + \
_artifacts + name("artifact_name") + OCB + \
_node + \
_nodes + OCB + _name + name("node_name") + \
Optional(service_svrs)("svr_servers") + \
Optional(pubs)("publishers") + \
Expand Down
16 changes: 10 additions & 6 deletions src/ros_model_parser/rossystem_parser.py
Original file line number Diff line number Diff line change
Expand Up @@ -39,17 +39,20 @@ def parseActionDict(string, location, tokens):

class RosSystemModelParser(object):
def __init__(self, model, isFile=True):
# OCB = Open Curly Bracket {}
# OCB = Open Curly Bracket {
# CCB = Close Curly Bracket }
# ORB = Open Round Bracket (
# CRB = Close Round Bracket )
# SQ = Single Quotes '
# DQ = Double Quotes "
# OSB = Open Square Bracket [
# CSB = Close Square Bracket ]

OCB, CCB, ORB, CRB, SQ, OSB, CSB = map(Suppress, "{}()'[]")
name = Optional(SQ) + Word(printables,
excludeChars="{},'") + Optional(SQ)

OCB, CCB, ORB, CRB, SQ, DQ, OSB, CSB = map(Suppress, "{}()'\"[]")

name = Optional(SQ) + Optional(DQ) + Word(printables,
excludeChars="{},'") + Optional(SQ) + Optional(DQ)

real = Combine(Word(nums) + '.' + Word(nums))

Expand Down Expand Up @@ -118,6 +121,7 @@ def __init__(self, model, isFile=True):
_g_parameters = Keyword("Parameters").suppress()
_g_parameter = Keyword("Parameter").suppress()
_type = Keyword("type").suppress()
_value = Keyword("value").suppress()

listStr << delimitedList(Group(OCB + delimitedList(values) + CCB))
mapStr << (OSB + delimitedList(Group(OCB + delimitedList((Group(
Expand All @@ -127,7 +131,7 @@ def __init__(self, model, isFile=True):
param_value << _value + (values | listStr)

parameter = Group(_parameter + name("param_name") +
OCB + _ref_parameter + name("param_path") + param_value("param_value") + CCB)
OCB + _ref_parameter + name("param_path") + Optional(param_value("param_value")) + CCB)
parameters = (_parameters + OCB +
OneOrMore(parameter + Optional(",").suppress()) + CCB)

Expand Down Expand Up @@ -169,7 +173,7 @@ def __init__(self, model, isFile=True):
OneOrMore(topic_connection + Optional(",").suppress()) + CCB)

g_parameter = Group(_g_parameter + OCB + _name + name("param_name") +
_type + name("value_type") + param_value("param_value") + CCB)
_type + name("value_type") + Optional(param_value("param_value")) + CCB)
g_parameters = (_g_parameters + OCB +
OneOrMore(g_parameter + Optional(",").suppress()) + CCB)

Expand Down