Skip to content
Open
Show file tree
Hide file tree
Changes from 4 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
Empty file added Arch.yml
Empty file.
98 changes: 97 additions & 1 deletion src/rosdiscover/cli.py
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,10 @@

from .acme import AcmeGenerator
from .config import Config
from .interpreter import Interpreter
from .interpreter import Interpreter, NodeSummary

import roswire
import time

DESC = 'discovery of ROS architectures'
CONFIG_HELP = """R|A YAML file defining the configuration.
Expand Down Expand Up @@ -83,6 +86,90 @@ def rosservice_list(args) -> None:
print('\n'.join(sorted(services)))


def toString(line):
s = ''
l1 = list(line)
if len(l1) == 0:
return '[]'
for i in l1:
(fmt, topic) = i
s = s + "\n - format: " + fmt + "\n name: " + topic + "\n"
return s


def get_info(image, sources, environment, file, package):
rsw = roswire.ROSWire()
with rsw.launch(image, sources, environment=environment) as system:
with system.roscore() as ros:
ros.roslaunch(file,
package=package,
args={'gui': 'false'})

time.sleep(30)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Add this as an optional parameter, but default it to 30.

node_names = list(ros.nodes)
state = ros.state
topic_to_type = ros.topic_to_type
service_to_format = {}
for service_name in state.services:
service = ros.services[service_name]
service_to_format[service_name] = service.format.name

return node_names, state, topic_to_type, service_to_format


def create_dict(node_names, state, topic_to_type, service_to_format):
nodeSummaryDict = {}
for n in node_names:
p = []
for key in state.publishers:
pubs = state.publishers[key]
for i in pubs:
if i == n:
p.append((topic_to_type[key], key))
s = []
for key in state.subscribers:
subs = state.subscribers[key]
for i in subs:
if i == n:
s.append((topic_to_type[key], key))
serv = []
for key in service_to_format:
path = n + '/'
if path in key:
serv.append((service_to_format[key], key))
obj = NodeSummary('', n, '/', '', '', False, '', False, p, s, [], [], [], serv, [], [])
nodeSummaryDict.update({n: obj})
return nodeSummaryDict


def dynamic_analysis(args):
print(args)
with open(args.config.name, 'r') as r:
data = yaml.safe_load(r)
r.close()
f = open("Arch.yml", "w")
logger.enable('roswire')
image = data['image']
sources = data['sources']
if 'environment' in data:
environment = data['environment']
node_names, state, topic_to_type, service_to_format = get_info(image, sources, environment,
args.launchfile, args.package)
nodeSummaryDict = create_dict(node_names, state, topic_to_type, service_to_format)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think the following code can be done more simply, like we discussed yesterday

for i in nodeSummaryDict:
obj = nodeSummaryDict[i]
f.write(f"- action-clients: {toString(obj.action_clients)}\n ")
f.write(f"action-servers: {toString(obj.action_servers)}\n ")
f.write(f"filename: {obj.filename}\n fullname: {obj.fullname}\n ")
f.write(f"kind: {obj.kind}\n name: {obj.name}\n")
f.write(f"namespace: {obj.namespace}\n nodelet: {obj.nodelet}\n ")
f.write(f"package: {obj.package}\n placeholder: {obj.placeholder}\n ")
f.write(f"provides: {toString(obj.provides)}\n pubs: {toString(obj.pubs)}\n ")
f.write(f"reads: {toString(obj.reads)}\n subs: {toString(obj.subs)}\n ")
f.write(f"uses: {toString(obj.uses)}\n writes: {toString(obj.writes)}\n\n")
f.close()


class MultiLineFormatter(argparse.HelpFormatter):
def _split_lines(self, text, width):
if text.startswith('R|'):
Expand Down Expand Up @@ -113,6 +200,15 @@ def main() -> None:

p.set_defaults(func=rostopic_list)

p = subparsers.add_parser(
'dynamic',
help='Generates a dynamic analysis using rosnode list',
formatter_class=MultiLineFormatter)
p.add_argument('--package', type=str, help="package for roslaunch")
p.add_argument('--launchfile', type=str, help="launchfile for roslaunch")
p.add_argument('config', type=argparse.FileType('r'), help=CONFIG_HELP)
p.set_defaults(func=dynamic_analysis)

p = subparsers.add_parser(
'rosservice',
help='simulates the output of rosservice for a given configuration.',
Expand Down
12 changes: 5 additions & 7 deletions src/rosdiscover/models/gazebo_gui.py
Original file line number Diff line number Diff line change
@@ -1,11 +1,9 @@
from ..interpreter import model

@model('gazebo_ros', 'gzclient')

@model('gazebo_ros', 'gzclient')
def gazebo_gui(c):
c.pub('/gazebo_gui/parameter_description', 'dynamic_recongfigure/ConfigDescription')
c.pub('/gazebo_gui/parameter_updates', 'dynamic_reconfigure/Config')
c.provide('gazebo_gui/set_parameters', 'unknown type')
c.sub('/clock', 'rosgraph_msgs/Clock')


c.pub('/gazebo_gui/parameter_description', 'dynamic_recongfigure/ConfigDescription')
c.pub('/gazebo_gui/parameter_updates', 'dynamic_reconfigure/Config')
c.provide('gazebo_gui/set_parameters', 'unknown type')
c.sub('/clock', 'rosgraph_msgs/Clock')
12 changes: 6 additions & 6 deletions src/rosdiscover/models/joy.py
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
from ..interpreter import model
from ..interpreter import model

@model('joy', 'joy_node')

@model('joy', 'joy_node')
def joy(c):
c.pub('diagnostics', 'diagnostic_msgs/DiagnosticArray')
c.pub('joy', 'sensor_msgs/Joy')
c.sub('/clock', 'rosgraph_msgs/Clock')
c.sub('set_feedback', 'unknown type')
c.pub('diagnostics', 'diagnostic_msgs/DiagnosticArray')
c.pub('joy', 'sensor_msgs/Joy')
c.sub('/clock', 'rosgraph_msgs/Clock')
c.sub('set_feedback', 'unknown type')
17 changes: 8 additions & 9 deletions src/rosdiscover/models/twist_mux.py
Original file line number Diff line number Diff line change
@@ -1,13 +1,12 @@
from ..interpreter import model

@model('twist_mux', 'twist_mux')

@model('twist_mux', 'twist_mux')
def twist_mux(c):
c.pub('cmd_vel_out', 'geometry_msgs/Twist')
c.pub('diagnostics', 'diagnostic_msgs/DiagnosticArray')
c.sub('/clock', 'rosgraph_msgs/Clock')
c.sub('cmd_vel', 'unknown type')
c.sub('e_stop', 'unknown type')
c.sub('/move_base/cmd_vel', 'unknown type')
c.sub('/pad_teleop/cmd_vel', 'unknown type')

c.pub('cmd_vel_out', 'geometry_msgs/Twist')
c.pub('diagnostics', 'diagnostic_msgs/DiagnosticArray')
c.sub('/clock', 'rosgraph_msgs/Clock')
c.sub('cmd_vel', 'unknown type')
c.sub('e_stop', 'unknown type')
c.sub('/move_base/cmd_vel', 'unknown type')
c.sub('/pad_teleop/cmd_vel', 'unknown type')