From e20ec4af31ce851f8d710dc2cc32b9087b8d94a3 Mon Sep 17 00:00:00 2001 From: Cyano0 <36192489+Cyano0@users.noreply.github.com> Date: Mon, 31 Mar 2025 11:20:48 +0100 Subject: [PATCH 01/21] Replace repository content with ROS2 packages --- .gitignore | 101 ---- CHANGELOG.rst | 485 ----------------- CMakeLists.txt | 45 -- config/execute.yaml | 125 ----- config/map.yaml | 11 - config/rob_lindsey.yaml | 54 -- config/test.yaml | 84 --- launch/sentor.launch | 20 - launch/topic_mapping.launch | 12 - msg/MonitorArray.msg | 2 - msg/TopicMapArray.msg | 1 - package.xml | 46 -- scripts/sentor_node.py | 179 ------ scripts/test.py | 101 ---- scripts/topic_mapping_node.py | 71 --- sentor/CMakeLists.txt | 24 + sentor/config/test_monitor_config.yaml | 70 +++ sentor/package.xml | 22 + sentor/scripts/test_sentor.py | 185 +++++++ sentor/sentor/Executor.py | 500 +++++++++++++++++ sentor/sentor/MultiMonitor.py | 76 +++ sentor/sentor/ROSTopicFilter.py | 123 +++++ sentor/sentor/ROSTopicHz.py | 105 ++++ sentor/sentor/ROSTopicPub.py | 91 ++++ sentor/sentor/TopicMonitor.py | 259 +++++++++ {src => sentor}/sentor/__init__.py | 0 .../__pycache__/Executor.cpython-310.pyc | Bin 0 -> 13906 bytes .../__pycache__/MultiMonitor.cpython-310.pyc | Bin 0 -> 3217 bytes .../__pycache__/service_types.cpython-310.pyc | Bin 0 -> 5497 bytes sentor/sentor/service_types.py | 122 +++++ sentor/sentor/test_executer.py | 77 +++ sentor/setup.cfg | 8 + sentor/setup.py | 21 + sentor_msgs/CMakeLists.txt | 20 + {msg => sentor_msgs/msg}/Monitor.msg | 2 +- sentor_msgs/msg/MonitorArray.msg | 2 + {msg => sentor_msgs/msg}/SentorEvent.msg | 2 +- {msg => sentor_msgs/msg}/TopicMap.msg | 2 +- sentor_msgs/msg/TopicMapArray.msg | 1 + sentor_msgs/package.xml | 24 + {srv => sentor_msgs/srv}/GetTopicMaps.srv | 2 +- setup.py | 9 - src/sentor/CustomLambdaExample.py | 12 - src/sentor/CustomProcessExample.py | 17 - src/sentor/Executor.py | 481 ---------------- src/sentor/MultiMonitor.py | 69 --- src/sentor/ROSTopicFilter.py | 92 ---- src/sentor/ROSTopicHz.py | 146 ----- src/sentor/ROSTopicPub.py | 36 -- src/sentor/SafetyMonitor.py | 106 ---- src/sentor/TopicMapServer.py | 184 ------- src/sentor/TopicMapper.py | 277 ---------- src/sentor/TopicMonitor.py | 512 ------------------ 53 files changed, 1734 insertions(+), 3282 deletions(-) delete mode 100644 .gitignore delete mode 100644 CHANGELOG.rst delete mode 100644 CMakeLists.txt delete mode 100644 config/execute.yaml delete mode 100644 config/map.yaml delete mode 100644 config/rob_lindsey.yaml delete mode 100644 config/test.yaml delete mode 100644 launch/sentor.launch delete mode 100644 launch/topic_mapping.launch delete mode 100644 msg/MonitorArray.msg delete mode 100644 msg/TopicMapArray.msg delete mode 100644 package.xml delete mode 100755 scripts/sentor_node.py delete mode 100755 scripts/test.py delete mode 100755 scripts/topic_mapping_node.py create mode 100644 sentor/CMakeLists.txt create mode 100644 sentor/config/test_monitor_config.yaml create mode 100644 sentor/package.xml create mode 100755 sentor/scripts/test_sentor.py create mode 100644 sentor/sentor/Executor.py create mode 100644 sentor/sentor/MultiMonitor.py create mode 100644 sentor/sentor/ROSTopicFilter.py create mode 100644 sentor/sentor/ROSTopicHz.py create mode 100644 sentor/sentor/ROSTopicPub.py create mode 100644 sentor/sentor/TopicMonitor.py rename {src => sentor}/sentor/__init__.py (100%) create mode 100644 sentor/sentor/__pycache__/Executor.cpython-310.pyc create mode 100644 sentor/sentor/__pycache__/MultiMonitor.cpython-310.pyc create mode 100644 sentor/sentor/__pycache__/service_types.cpython-310.pyc create mode 100644 sentor/sentor/service_types.py create mode 100644 sentor/sentor/test_executer.py create mode 100644 sentor/setup.cfg create mode 100644 sentor/setup.py create mode 100644 sentor_msgs/CMakeLists.txt rename {msg => sentor_msgs/msg}/Monitor.msg (86%) create mode 100644 sentor_msgs/msg/MonitorArray.msg rename {msg => sentor_msgs/msg}/SentorEvent.msg (88%) rename {msg => sentor_msgs/msg}/TopicMap.msg (87%) create mode 100644 sentor_msgs/msg/TopicMapArray.msg create mode 100644 sentor_msgs/package.xml rename {srv => sentor_msgs/srv}/GetTopicMaps.srv (50%) delete mode 100644 setup.py delete mode 100644 src/sentor/CustomLambdaExample.py delete mode 100644 src/sentor/CustomProcessExample.py delete mode 100644 src/sentor/Executor.py delete mode 100644 src/sentor/MultiMonitor.py delete mode 100644 src/sentor/ROSTopicFilter.py delete mode 100644 src/sentor/ROSTopicHz.py delete mode 100644 src/sentor/ROSTopicPub.py delete mode 100644 src/sentor/SafetyMonitor.py delete mode 100644 src/sentor/TopicMapServer.py delete mode 100644 src/sentor/TopicMapper.py delete mode 100644 src/sentor/TopicMonitor.py diff --git a/.gitignore b/.gitignore deleted file mode 100644 index 7bbc71c..0000000 --- a/.gitignore +++ /dev/null @@ -1,101 +0,0 @@ -# Byte-compiled / optimized / DLL files -__pycache__/ -*.py[cod] -*$py.class - -# C extensions -*.so - -# Distribution / packaging -.Python -env/ -build/ -develop-eggs/ -dist/ -downloads/ -eggs/ -.eggs/ -lib/ -lib64/ -parts/ -sdist/ -var/ -wheels/ -*.egg-info/ -.installed.cfg -*.egg - -# PyInstaller -# Usually these files are written by a python script from a template -# before PyInstaller builds the exe, so as to inject date/other infos into it. -*.manifest -*.spec - -# Installer logs -pip-log.txt -pip-delete-this-directory.txt - -# Unit test / coverage reports -htmlcov/ -.tox/ -.coverage -.coverage.* -.cache -nosetests.xml -coverage.xml -*.cover -.hypothesis/ - -# Translations -*.mo -*.pot - -# Django stuff: -*.log -local_settings.py - -# Flask stuff: -instance/ -.webassets-cache - -# Scrapy stuff: -.scrapy - -# Sphinx documentation -docs/_build/ - -# PyBuilder -target/ - -# Jupyter Notebook -.ipynb_checkpoints - -# pyenv -.python-version - -# celery beat schedule file -celerybeat-schedule - -# SageMath parsed files -*.sage.py - -# dotenv -.env - -# virtualenv -.venv -venv/ -ENV/ - -# Spyder project settings -.spyderproject -.spyproject - -# Rope project settings -.ropeproject - -# mkdocs documentation -/site - -# mypy -.mypy_cache/ diff --git a/CHANGELOG.rst b/CHANGELOG.rst deleted file mode 100644 index d91c223..0000000 --- a/CHANGELOG.rst +++ /dev/null @@ -1,485 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package sentor -^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.2.0 (2021-11-18) ------------------- -* Merge pull request `#47 `_ from adambinch/sentor_devel - Node `test.py` publishes topics to test sentor functionality. -* Node `test.py` publishes topics to test sentor functionality. - Corresponding example config `test.yaml`. -* Merge pull request `#46 `_ from adambinch/sentor_devel - Sentor/monitors topic reports all conditions (not just safety critical ones) -* removed unnecessary IF statement -* improvement to example config -* fix -* minor change -* Action process reports the action name as well as the specification and also reports the terminal state when the goal has completed. -* minor improvement -* Sentor/monitors topic reports all conditions (not just safety critical ones) - New field added to msg definition indicating whether the condition is safety critical (or not) -* revert -* test -* Merge pull request `#45 `_ from adambinch/sentor_devel - More efficient initialisation of the reconf process -* More efficient initialisation of the reconf process -* Merge pull request `#44 `_ from francescodelduchetto/shell_features - Allowing to pass a shell command with shell=True in Popen -* Allowing to pass a shell command with shell=True in Popen to allow shell features -* Merge pull request `#43 `_ from adambinch/sentor_devel - Can process every Nth message for individual conditions. -* tidying -* Can process every Nth message for individual conditions. -* Merge branch 'master' of https://github.com/LCAS/sentor into sentor_devel -* Signal when params (in TopicMonitor) are contained in a class attribute dictionary rather than having individual class attributes for all its params. -* Merge pull request `#42 `_ from adambinch/sentor_devel - Can process every nth message when topic mapping. -* minor change -* Can process every nth message when topic mapping. -* Merge pull request `#41 `_ from adambinch/sentor_devel - Can process every nth message -* Can process every nth message -* Merge pull request `#40 `_ from adambinch/sentor_devel - Fix for topic mapper breaking when using numpy in the topic arg. -* Shape of the topic map is stored in the config -* Merge branch 'master' of https://github.com/LCAS/sentor into sentor_devel -* more fixes -* Fix for topic mapper breaking when using numpy in the topic arg. -* Merge pull request `#39 `_ from adambinch/sentor_devel - List of configs passed as single string to topic mapping node. -* List of configs passed as single string to topic mapping node. -* Merge pull request `#38 `_ from MikHut/master - multiple confgs tmule friendly version -* multiple confgs tmule friendly -* Merge pull request `#37 `_ from adambinch/sentor_devel - Can pass list of configs to the topic mapping node. The topic mapping… -* Can pass list of configs to the topic mapping node. The topic mapping node will concatenate these into a single config. -* Merge pull request `#36 `_ from adambinch/sentor_devel - Can now pass a list of config files, which sentor will concatenate into one config. -* Merge branch 'master' of https://github.com/LCAS/sentor into sentor_devel -* Can now pass a list of config files to sentor. Sentor will concatenate these into one config. -* Merge pull request `#35 `_ from adambinch/sentor_devel - Improvements -* tidying -* Improvements. - For the `reconf` process you can now set the value of a param in the config to `_default` which reconfigures the param back to its original state. - The argument for the param's namespace has been changed from `ns` to `namespace`. - For topic mapping you no longer set the map and base frames as a rosparam (i.e in a launch file) but in the config instead, allowing maps using different tf transforms to be created simultaneously. - The topic map msg now includes a field for the base frame (`child_frame_id`). - For topic mapping the `stat` arg for the standard deviation has been changed from `std` to `stdev`. - Some general tidying. -* Merge pull request `#34 `_ from adambinch/sentor_devel - Improvements correspoding with new sentor wiki. -* minor change -* Map frame and base frame for making topic maps are no longer hard-coded. - General improvements. -* Improved example config in preparation for the new sentor wiki. - Example of custom process included. - Minor improvements/tidying. -* Merge pull request `#33 `_ from adambinch/sentor_devel - Custom Lambda Expressions. -* Custom Lambda Expressions. - You can now create custom lambda expressions in python files instead of specifying them explicitly in the config. - See the first lambda expression in the config `example.yaml`, which points to a function `CustomLambda` from file `CustomLambda.py` located in package `sentor`. - These are to be used when a lambda expression is too complex to be specified in the config explicitly. -* Merge branch 'master' of https://github.com/LCAS/sentor into sentor_devel - # Conflicts: -* Merge pull request `#32 `_ from adambinch/sentor_devel - Can include an (optional) tags field for each condition in the sento… -* scrapped last commit -* Removed error code topic. Not necessary as the error code can be obtained from the monitors topic. -* Can include an (optional) tags field for each conditions in the sentor config. - The tags for each condition are published on the `/sentor/monitors` topic. -* Merge pull request `#31 `_ from adambinch/sentor_devel - Correction -* Correction - Forgot to use brackets when calling the stop monitor method of the multimonitor in the sentor node. -* Merge pull request `#30 `_ from yiannis88/toc_uint16 - uint8[] to uint16[] to correctly read the info TOC -* uint8[] to uint16[] to correctly read the info TOC -* Merge pull request `#29 `_ from adambinch/sentor_devel - New `/sentor/monitors` topic, to be used by TOC. -* Removed header from ErrorCode msg -* Added new topic `/sentor/error_code` that produces the binary array error code to be used by TOC. - If you want to know which element of the array goes with which condition then see `/sentor/monitor` -* New `/sentor/monitors` topic, for TOC. - Message type is `sentor.msg.MonitorArray`. - Lists each of the safety critical conditions being monitored. - Each item in the list has the following fields: - `topic` the topic being monitored - `expression` the expression on the topic being evaluated. - 'safe' evaluates to False if the expression is satisfied/violated, True if not. - The topic only updates when something changes, and is latched. - This topic can be used to produce the error codes for TOC. -* Merge pull request `#28 `_ from adambinch/sentor_devel - Corrected mistake in topic mapper. -* Corrected mistake in topic mapper. -* Merge pull request `#27 `_ from adambinch/sentor_devel - Topic Monitoring Improvements. -* Added topic tools as a package dependency. -* Topic Monitoring Improvements. - Can set the topic rate in the config. More convenient than throttling topics in launch files. - If the topic rate is not set, then sentor subscribes to the original topic (as it does normally). - Service names (for the `call` process) and topic names (for the `publish` process) can be retrieved from rosparams and environment variables. Sentor automatically checks the names provided in the config. - Processes are now not verbose by default. - Some minor improvements. -* Merge pull request `#26 `_ from adambinch/sentor_devel - Updated package xml and cmakelists. -* Topic throttling in now done using topic tools via Popen from subprocess. -* Removed rosthrottle from package xml which has no kinetic release -* Updated package xml and cmakelists. - Simplified topic map msg. - Some minor improvements. -* Merge pull request `#25 `_ from adambinch/sentor_devel - Topic Mapping Improvements -* Can now retrieve topic map limits from metric map yaml file - (see `map` arg in config) -* For topic mapping can set rate param in config to throttle topics. - Useful when mapping topics with high publication rates. -* Topic map stat is selected at initialisation for efficiency. -* Merge pull request `#24 `_ from adambinch/sentor_devel - Topic mapping decoupled from topic monitoring. -* Topic mapping decoupled from topic monitoring. - Topic mapping has its own node: `roslaunch sentor topic_mapping.launch` - Example config: `sentor/config/map.yaml` - Monitoring is unaffected by these changes. -* Decoupling topic mapping from topic monitoring -* Merge pull request `#23 `_ from francescodelduchetto/master - adding /sentor/rich_event topic for structured sentor events information -* adding /sentor/rich_event topic for structured sentor events information -* Merge pull request `#22 `_ from adambinch/sentor_devel - safety critical default messages are now errors rather than warnings. -* safety critical default messages are now errors rather than warnings. -* Merge pull request `#21 `_ from adambinch/sentor_devel - For the 'call' process, the service client is now created at runtime. -* For the 'call' process, the service client is now created at runtime. -* Merge pull request `#20 `_ from adambinch/sentor_devel - Sentor waits for a service (default=2.0) before calling it at runtime. -* Sentor waits for a service (default=2.0) before calling it at runtime. - Some minor adjustments to one of the example configs. -* Merge pull request `#19 `_ from adambinch/sentor_devel - Sentor can execute custom processes. -* Sentor can execute custom processes. - Sentor can import a class `myClass` from `myClass.py` and execute it as a process. - The package name from which the class is retrieved and the name of the class must be specified in the config. - The class should have an init method and a run method, where the run method is executed at runtime. - Optional args can be passed to both of those. - See `config/example.yaml` -* Merge pull request `#18 `_ from adambinch/sentor_devel - Minor fix -* Minor fix -* Merge pull request `#17 `_ from adambinch/sentor_devel - Fix -* No need to create a temp list every time the existence of a key in a dictionary is checked -* Problem when sentor fails to initialise a process (such as a service) - but tries to execute it at runtime (because of that process_indices arg in the config). - This is a fix but needs to be tested. -* Merge pull request `#16 `_ from adambinch/sentor_devel - Option of waiting for results of goal for the action process. -* Option of waiting for results of goal for the action process. -* Merge pull request `#15 `_ from adambinch/sentor_devel - numpy library can be used in the lambda expressions -* numpy library can be used in the lambda expressions -* Merge pull request `#14 `_ from adambinch/sentor_devel - Included an arg in the sentor launch file `safe_operation_timeout` so… -* Constraint on the new arg. Some minor improvements. -* Included an arg in the sentor launch file `safe_operation_timeout` so that - all safety critical systems have to be good for a certain amount of time - before operation is judged to be safe. -* Merge pull request `#13 `_ from adambinch/sentor_devel - The top-level arg `default notifications` can now be specified for th… -* The top-level arg `default notifications` can now be specified for the signal when condition, - and each lambda expression, separately. - Added myself as a maintainer/author in the package xml. -* Contributors: Adam Binch, MikHut, adambinch, francescodelduchetto, yiannis88 - -2.1.0 (2020-04-20) ------------------- -* Merge pull request `#11 `_ from adambinch/sentor_devel - A significant change to the way sentor executes processes, and how args are specified in the config. -* The format of the config is now backwards compatible - (sentor can handle two formats for the signal when condition). - The lindsey config has been reverted back to the previous version. -* A few minor improvements -* No longer using separate timers to handle critical and non-critical lambda expressions. - Reduces the number of threads used by sentor by the number of monitors specified in the config. -* The safety callback in the topic monitor is called in the run function, rather than a separate timer. - Reduces the number of threads used by sentor by the number of monitors specified in the config. -* Merge branch 'master' of github.com:LCAS/sentor into sentor_devel -* Merge pull request `#12 `_ from adambinch/fix - fix -* fix -* Added authorships -* Adjustments to example config -* The hz monitor is instantiated only when needed. -* minor change -* Merge branch 'master' of github.com:LCAS/sentor into sentor_devel -* Merge pull request `#10 `_ from adambinch/fix - small fix -* Merge branch 'master' into fix -* small fix -* Merge branch 'master' of github.com:LCAS/sentor into sentor_devel -* Specifying process indices for the signal when condition, and for each lambda expression, is now - the default method of executing processes. - The signal when condition now has child args `condition` (published/not published), - `timeout`, `safety_critical` `process_indices` and `repeat_exec`. - Each lambda expression now has child args `expression`, - `timeout`, `safety_critical` `when_published`, `process_indices` and `repeat_exec`. -* Merge pull request `#9 `_ from adambinch/sentor_mapping - Sentor can now map topic values. -* minor change -* Example config for the new features -* Added an alternative mode `alt_exec` for executing processes. For a topic monitor, each lambda - expression listed now has an optional arg `process_indices` in which - you can specify which set of process you want to execute when that particular - lambda expression is satisfied. -* fix -* Added another process `reconf` - sentor can now dynamic reconfigure. - Updated config. - The hz monitor is now only instantiated when it is needed. -* fix -* Minoir change -* Topic map can now be built as the standard deviation of topic args. - Added `stat` message field to custom topic map msg. - Some restructuring and minor improvements. -* minor change -* Minor improvements. -* Added service `/sentor/get_maps` that returns all map data. - Changed default publishing/plotting rate of maps to zero which disables publishing/plotting of maps. - Changed the way the topic map is discretised as the previous method was causing the map to be displaced. - Some structural changes and improvements to code. -* Merge branch 'sentor_mapping' of github.com:adambinch/sentor into sentor_mapping -* Auto safety tagging is set to True by default. - Can now make topic maps with other statistics (min and max) - A few minor improvements -* Auto safety tagging is set to True by default. - A few minor improvements -* Created a topic map server to deal with writing/plotting the topic maps, and other services on the maps. - The topic map can be now be a sum of the topic args (as well as the weighted mean). - Real time plots (and the plot) rate, is now specified in the sentor launch file. -* Topic maps are now saved in `home/.sentor_maps`. - Topic map message now gives extra information. -* Improvement to the way the topic map is discretised. - Better example config. - Generated example topic map, saved in `sentor/maps`. -* Default plotting rate is 1hz -* minor fix -* Sentor topic mapper can now generate real-time plots. New args in config. -* Created a class (TopicMapPublisher) for publishing topic maps. - The services start/stop monitor now starts/stops the safety monitor, topic mapper and topic map publisher. - Made a service `/sentor/write_maps` for writing topic maps - Renamed messages `Map` and `MapArray` as `TopicMap` and `TopicMapArray`, respectively. - All sentor services with srv `Empty` now return an empty response (`EmptyResponse`) - Some other fixes and minor changes. -* Improvement to the way the topic mapper handles exceptions. - Some other minor changes. -* Sentor can now map topic values. - A numpy array is created as a discretized representation of the metric map. - When a topic message is obtained, a user defined argument on the message is evaluated. - A weighted mean of this value is stored in an element of the array, where the indices of the element is - given by the 'map to baselink' tf transform. As more data from a location is gathered, the weighted mean - (and thus the 'topic map'), is updated. Any region of this topic map that - has not been explored will contain nans. - Sentor can now store the weighted mean of a topic value in an element of an array. - The index of the element corresponds to a location in the map. - The index of the array is chosen by looking up the transform between map and baselink. -* Contributors: Adam Binch, adambinch - -2.0.4 (2020-02-22) ------------------- -* Merge pull request `#8 `_ from adambinch/sentor_devel - New top-level arg `lambdas_when_published` that ensures that lambda e… -* Simplified code a little. Small change to the readme. -* Made latest changes thread safe. -* updated readme -* Fix -* New top-level arg `lambdas_when_published` that ensures that lambda expressions - can be satisfied only when the topic is being published. -* Merge pull request `#7 `_ from adambinch/sentor_devel - Sentor devel: New Features -* minor chnage to readme -* New Features: - By setting the arg `auto_safety_tagging` (see `sentor.launch`) to True - sentor will automatically set safe operation to True when all - safety critical condition across all monitors are unsatisfied. - If `auto_safety_tagging` is set to `False` then the (renamed) service - `/sentor/set_safety_tag` must be called. -* The safety monitor will automatically set safe operation to True - if all safety critical conditions across all monitors - are not violated. -* Merge pull request `#6 `_ from adambinch/sentor_devel - Sentor devel: Safety critical conditions are now affected by the `repeat_exec` arg. -* Safety critical conditions are now affected by the `repeat_exec` arg. -* moved this to the rasberry repo -* start of sentor config for thorvald -* Merge pull request `#5 `_ from adambinch/sentor_devel - New top level arg added that allows you to turn off the default notif… -* New top level arg added that allows you to turn off the default notifications. -* Merge branch 'adambinch-sentor_devel' -* Updated README.md to reflect the previous change. -* The arg `topic_latched` for the process `publish` is now optional (default='True') -* The arg `repeat_exec` now works with the `signal_when` conditions, as well as the lambda expressions. - Updated the README.md. -* minor change -* The `verbose` option for each process was meant to be optional but was not. Fixed now. - Improvement to the README.md. -* README.md correction -* correction to README.md -* Updated the README.md and the argument descriptions in the config. -* New arg for each process `verbose`. Setting to False will limit notifications to errors - whilst processes are executed. - Expanded the default config `execute` to include a safety critical lambda condition. - Tidied/removed unnecessary code. -* `repeat` is now a top level arg and has been renamed to `repeat_exec`. - If true then all processes under `execute` will be executed repeatedly (every `timeout`) seconds - whilst all lambda condition's are satisfied. -* Found a better way of repeating processes whilst lambdas are satisfied -* removed `oneshot` option as it was causing problems. Simplified code -* Improved the way errors are logged. - New top level arg `include` in config. Set to false to not include that monitor, - rather than commenting it out (for convenience). -* Fixed an issue that was causing processes to be executed immediately (without taking `timeout` into account). - Previously, processes will be executed when the lambda conditions are satisfied. But they would not execute again unless they become unsatisfied, then satisfied again. - This is desirable behaviour in a lot of cases but maybe not all. So we now have the option to execute repeatedly (every timeout seconds), whilst the lambda conditions are satisfied. - See the new top level arg `oneshot` in the config. -* When executing a log you can now include data from the topic that - is being monitored. -* Minor change -* minor change -* When sentor logs a call to a service it also logs the request. - When sentor logs that a goal for an action has been sent it also logs the goal. -* When actionlib goals or service calls fail, those events are logged as warnings rather than errors. -* Removed `message` from process keywords in config and replaced with a new process `log` - in which you can log messages. -* The `signal_when` condition in the config now also has a `safety_critical` tag. - Added a new thread to the example config `execute.yaml`. This thread calls the service `/sentor/reset_safety_tag`. - The key word `function` in the config has been changed to `expression`. - A few minor improvements to code. -* Added missing package dependencies. - Set default pub rate of the `/safe_operation` topic to 10 hz. -* You can now tag lambda expressions as `safety_critical`. - A new topic `/safe_operation` will publish `True` if all safety critical - lambda expressions are satisfied. If one is from any thread then - the topic will publish false until a new service `/sentor/reset` is set to `True`. - Due to the inclusion of the new tags the config `rob_lindsey.yaml` has been updated. - It should still functions exactly the same as before. -* The optional arg `user_msg` has been changed to `message`. - Important info added to the README.md -* The new features (publishing to topics, calling services etc) are now referred to as - 'processes' rather than 'actions' to avoid confusion with actionlib actions. -* Small chnage to the README.md -* correction to README.md -* correction to README.md -* Updated the README.md. - Renamed arg in config to be consistent with the naming of others. - Added arg descriptions to the config. - A couple of minor improvements to code. -* Renamed config - Removed unnecessary config - Small improvement to code -* Correction -* Tested with a multi thread config (`multi_thread.yaml`). Seems to work fine. - Shortened default log messages published to the `sentor/event` topic. - When executing actions using a simple action server sentor now provided feedback on the goal. - Renamed config. - Ros logs made during sentor initialisation are no longer published to the `sentor/event` topic. - Updated pacakge.xml - To test with multi thread config simply launch the launch file `sentor.launch`. - As before send the robot in simulation to WayPoint1. The robot will automatically navigate to - WayPoint45. In the mean time sentor will execute a shell command `cowsay moo`. When the robot reaches its goal - it will teleport back to x=0,y=0 and relocalise. -* Sentor can now execute basic shell commands using subprocess. - Renamed and updated config. - Needed to (rospy) sleep the sentor node in some places so that messages - can published to slack (by slackeros). - Some other minor changes. -* Minor changes -* Merge branch 'sentor_devel' of https://github.com/adambinch/sentor into sentor_devel - # Conflicts: - # config/action.yaml - # src/sentor/Executor.py -* Sentor now publishes new events to the topic `/sentor/event`. - Users can now set their own (string) messages to be publsihed to this topic. - Removed some unnecessary stuff. Some minor changes. -* Sentor now publishes new events to the topic `/sentor/event`. - Users can now set their own (string) messages to be publsihed to this topic. - Removed some unnecessary stuff. -* Sentor can now make clients and send goals for any action type. - Included the python package `math` in `ROSTopicFilter.py` so that - it can be used in the lambda functions. -* Sentor can now publish to topics. - Also, a new arg `lock_exec` in the config gives the option of locking out other threads - while the current one is executing its sequence of actions. -* rospy sleep now included in set of actions. - Tidied up my changes to `TopicMonitor` -* New top level arg `exec_once` in config. If True then actions will be - executed only the first time that the signal conditions are met. -* correction -* correction -* correction -* Sentor can now call services -* Contributors: Adam Binch, Lindsey User, Marc Hanheide, adambinch - -2.0.3 (2019-04-12) ------------------- -* Merge pull request `#3 `_ from francescodelduchetto/master - fix some bugs -* Merge branch 'master' into master -* Merge branch 'master' of https://github.com/francescodelduchetto/sentor -* fix various errors -* Contributors: Lindsey User, Marc Hanheide - -2.0.2 (2019-04-12) ------------------- -* Merge pull request `#2 `_ from francescodelduchetto/master - update readme with description of config file usage -* rospy spin instead of 'handmade' spin -* print also the message together with the expression -* Merge branch 'master' into master -* Merge pull request `#1 `_ from francescodelduchetto/2.0 - merge 2.0 to master -* Update README.md -* Update README.md -* Update README.md -* Update README.md -* Contributors: Marc Hanheide, francescodelduchetto - -2.0.1 (2019-01-19) ------------------- -* Merge pull request `#1 `_ from LCAS/2.0 - Merging 2.0 into master with some modifications for release -* prepare for installation -* prettier prints and longer sleep in loop to avoid None in hz -* added timeout for lambdas and not published -* first commmit version 2.0: yaml file for configuration, singaling also for published, lambda funcs are specified in the yaml as a string -* ehm -* remove logs -* Merge branch 'master' of https://github.com/francescodelduchetto/sentor -* check log to be rem -* another small bit -* remove logs and madd another check to avoid duplicate msg expr in the same list -* some debug logs -* more waiting -* fix bug -* better handling of satsfied expressions as we don't drop anymore expression satisfied very close in time -* Update README.md -* gitignore -* comments and readme -* bug in list inserting elements -* monitoring either the frequency or the expression on msgs -* Merge branch 'master' of github.com:francescodelduchetto/sentor -* tab -* Update README.md -* warning message more significative -* Merge branch 'master' of github.com:francescodelduchetto/sentor -* comment -* elifs instead of ifs -* explanation on usage of filtering -* added possiblity to filter the value of messages and get a warning when it is satisfied -* slightly better printing -* only one warning message when the topic is not published anymore; better terminal printing -* Delete ROSTopicHz.pyc -* Update README.md -* Update README.md -* initial commit -* Contributors: Lindsey User, Marc Hanheide, francescodelduchetto diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index f55de38..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(sentor) -find_package(catkin REQUIRED) - -find_package(catkin REQUIRED COMPONENTS - message_generation - std_msgs -) - -catkin_python_setup() - -add_message_files( - FILES - TopicMap.msg - TopicMapArray.msg - SentorEvent.msg - Monitor.msg - MonitorArray.msg -) - -add_service_files( - FILES - GetTopicMaps.srv -) - -generate_messages( - DEPENDENCIES - std_msgs -) - -catkin_package( - CATKIN_DEPENDS message_runtime -) - -install(PROGRAMS - scripts/sentor_node.py - scripts/topic_mapping_node.py - scripts/test.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -foreach (dir launch config) - install(DIRECTORY ${dir}/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) -endforeach(dir) diff --git a/config/execute.yaml b/config/execute.yaml deleted file mode 100644 index 86bbdfa..0000000 --- a/config/execute.yaml +++ /dev/null @@ -1,125 +0,0 @@ -# SEE THE README.MD FOR ARGUMENT DESCRIPTIONS - -- name: "/current_node" - signal_lambdas: - - expression: "lambda msg: msg.data == 't1-r1-c1'" - safety_critical: True - process_indices: [0,1,2,3,4,5] - repeat_exec: False - tags: ["navigation"] - when_published: False - - expression: CustomLambda - file: CustomLambdaExample - package: sentor - safety_critical: False - process_indices: [6,7,8,9,10,11,12,13] - repeat_exec: False - tags: ["navigation"] - when_published: False - execute: - - log: - message: "Navigating away from {}" - level: warn - msg_args: - - "msg.data" - - action: - verbose: True - namespace: "/topological_navigation" - package: "topological_navigation" - action_spec: "GotoNodeAction" - wait: False - goal_args: - - "goal.target = 't1-r1-c2'" - - reconf: - verbose: True - params: - - namespace: row_detector - name: position_error_threshold - value: -1 - - log: - message: "Row detection disabled" - level: warn - - shell: - verbose: True - cmd_args: - - "cowsay" - - "moo" - - custom: - verbose: True - name: CustomProcess - file: CustomProcessExample - package: sentor - init_args: - - "sentor custom process says Hello World! at initialisation" - run_args: - - "sentor custom process says Hello World! at runtime" - - reconf: - verbose: True - params: - - namespace: row_detector - name: position_error_threshold - value: "_default" - - log: - message: "Row detection enabled" - level: info - - call: - verbose: True - service_name: "/sentor/set_safety_tag" - timeout: 2.0 - service_args: - - "req.data = True" - - log: - message: "Teleporting the robot" - level: info - - call: - verbose: True - service_name: "/gazebo/set_model_state" - timeout: 2.0 - service_args: - - "req.model_state.model_name = 'thorvald_ii'" - - "req.model_state.pose.position.x = 18.0" - - "req.model_state.pose.position.y = -7.0" - - "req.model_state.pose.orientation.w = 1.0" - - sleep: - verbose: True - duration: 3.0 - - log: - message: "Relocalising the robot" - level: info - - publish: - verbose: True - topic_name: "/initialpose" - topic_latched: False - topic_args: - - "msg.header.frame_id = '/map'" - - "msg.pose.pose.position.x = 18.0" - - "msg.pose.pose.position.y = -7.0" - - "msg.pose.pose.orientation.w = 1.0" - timeout: 0.1 - default_notifications: True - include: True - - -- name: "/gps/fix" - signal_when: - condition: "not published" - timeout: 3.0 - safety_critical: True - process_indices: [0] - tags: ["GPS", "400"] - signal_lambdas: - - expression: "lambda msg : msg.status.status < 2" - timeout: 10.0 - safety_critical: True - process_indices: [1] - tags: ["GPS", "401"] - execute: - - log: - message: "GPS data not seen for 3 seconds!" - level: error - - log: - message: "GPS status < 2 for 10 seconds. Lost corrections." - level: error - default_notifications: False - include: False - diff --git a/config/map.yaml b/config/map.yaml deleted file mode 100644 index b856b43..0000000 --- a/config/map.yaml +++ /dev/null @@ -1,11 +0,0 @@ -- name: "/topic_name" - arg: "msg.data" - stat: "mean" - limits: [11.0, 25.0, -40.0, -8.0] - resolution: 1.5 - -- name: "/topic_name" - arg: "not msg.data" - stat: "stdev" - map: "/home/user/maps/map.yaml" - resolution: 1.5 diff --git a/config/rob_lindsey.yaml b/config/rob_lindsey.yaml deleted file mode 100644 index 854bfa8..0000000 --- a/config/rob_lindsey.yaml +++ /dev/null @@ -1,54 +0,0 @@ -# name: is the name of the topic to monitor -# signal_when: optional, can be either 'not published' or 'published'. Respectively, it will send a warning when the topic is not published or when it is. -# signal_lambdas: optional, it's a list of (pythonic) lambda expressions such that when they are satisfied a warning is sent -# timeout: optional (default=0), amount of time (in seconds) for which the signal has to be satisfied before sending the warning. - -- name : '/scan' - signal_when : 'not published' - -- name : '/head_xtion/rgb/image_color' - signal_when : 'not published' - -- name : '/head_xtion/depth/image_rect' - signal_when : 'not published' - -- name : '/tf' - signal_when : 'not published' - -- name : '/robot_pose' - signal_when : 'not published' - -- name : '/odom' - signal_when : 'not published' - -- name : '/battery_state' - signal_when : 'not published' - signal_lambdas : - - expression: 'lambda msg : msg.lifePercent < 10' - - expression: 'lambda msg : msg.lifePercent < 5' - -- name : '/motor_status' - signal_when : 'not published' - signal_lambdas : - - expression: 'lambda msg : msg.emergency_button_pressed == True' - - expression: 'lambda msg : msg.emergency_button_pressed == False' - timeout : 5 - -- name : '/motor_status' - signal_lambdas : - - expression: 'lambda msg : msg.bumper_pressed == True' - timeout : 60 - -- name : '/virtual_bumper_event' - signal_lambdas : - - expression: 'lambda msg : msg.freeRunStarted == True' - -- name : '/diagnostics_toplevel_state' - signal_when : 'not published' - signal_lambdas : - - expression: 'lambda msg : msg.level == 1' - - expression: 'lambda msg : msg.level == 2' - timeout : 3 - -#- name : '/interface/buttonPressedWhileDisabled' -# signal_when : 'published' diff --git a/config/test.yaml b/config/test.yaml deleted file mode 100644 index a580578..0000000 --- a/config/test.yaml +++ /dev/null @@ -1,84 +0,0 @@ -# sentor/test.py publishes these topics expr_1 and expr_2. -# call services /pub_expr_1 and /set_expr_1 to start/stop publishing and set the topic arg (True/False), respectively. Same for expr_2. - -- name : "/expr_1" - signal_when: - condition: "not published" - timeout: 1.0 - safety_critical: True - process_indices: [0] - tags: ["expression 1"] - signal_lambdas: - - expression: "lambda msg : msg.data == False" - timeout: 5.0 - safety_critical: True - autonomy_critical: True - process_indices: [1] - tags: ["expression 1"] - execute: - - log: - message: "expression 1 is not published" - level: "warn" - - log: - message: "expression 1 is False" - level: "warn" - default_notifications: False - - -- name : "/expr_2" - signal_when: - condition: "not published" - timeout: 2.0 - safety_critical: True - process_indices: [0] - tags: ["expression 2"] - signal_lambdas: - - expression: "lambda msg : msg.data == False" - timeout: 10.0 - safety_critical: True - autonomy_critical: False - process_indices: [1] - tags: ["expression 2"] - execute: - - log: - message: "expression 2 is not published" - level: "warn" - - log: - message: "expression 2 is False" - level: "warn" - default_notifications: False - - -- name: "/safe_operation" - signal_lambdas: - - expression: "lambda msg : msg.data == False" - process_indices: [0] - - expression: "lambda msg : msg.data == True" - process_indices: [1] - execute: - - log: - message: "Not safe to operate" - level: warn - - log: - message: "Safe to operate" - level: info - timeout: 0.1 - default_notifications: False - - -- name: "/pause_autonomous_operation" - signal_lambdas: - - expression: "lambda msg : msg.data == True" - process_indices: [0] - - expression: "lambda msg : msg.data == False" - process_indices: [1] - execute: - - log: - message: "Not safe to operate autonomously" - level: warn - - log: - message: "Safe to operate autonomously" - level: info - timeout: 0.1 - default_notifications: False - diff --git a/launch/sentor.launch b/launch/sentor.launch deleted file mode 100644 index 03824f2..0000000 --- a/launch/sentor.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/launch/topic_mapping.launch b/launch/topic_mapping.launch deleted file mode 100644 index 3548b34..0000000 --- a/launch/topic_mapping.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/msg/MonitorArray.msg b/msg/MonitorArray.msg deleted file mode 100644 index 7526263..0000000 --- a/msg/MonitorArray.msg +++ /dev/null @@ -1,2 +0,0 @@ -std_msgs/Header header -sentor/Monitor[] conditions diff --git a/msg/TopicMapArray.msg b/msg/TopicMapArray.msg deleted file mode 100644 index 48b6d20..0000000 --- a/msg/TopicMapArray.msg +++ /dev/null @@ -1 +0,0 @@ -sentor/TopicMap[] topic_maps diff --git a/package.xml b/package.xml deleted file mode 100644 index adc0817..0000000 --- a/package.xml +++ /dev/null @@ -1,46 +0,0 @@ - - - sentor - 2.2.0 - The sentor package, to monitor ROS messages - - Francesco Del Duchetto - Adam Binch - Marc Hanheide - - MIT - - - - Francesco Del Duchetto - Adam Binch - - catkin - message_generation - rospy - rostopic - rosservice - actionlib - std_msgs - std_srvs - tf - rosgraph - dynamic_reconfigure - topic_tools - - rospy - rostopic - rosservice - actionlib - std_msgs - std_srvs - message_runtime - tf - rosgraph - dynamic_reconfigure - topic_tools - - - - - diff --git a/scripts/sentor_node.py b/scripts/sentor_node.py deleted file mode 100755 index 2de3dfd..0000000 --- a/scripts/sentor_node.py +++ /dev/null @@ -1,179 +0,0 @@ -#!/usr/bin/env python -""" -@author: Francesco Del Duchetto (FDelDuchetto@lincoln.ac.uk) -@author: Adam Binch (abinch@sagarobotics.com) -""" -########################################################################################## -from __future__ import division -from sentor.TopicMonitor import TopicMonitor -from sentor.SafetyMonitor import SafetyMonitor -from sentor.MultiMonitor import MultiMonitor -from std_msgs.msg import String -from sentor.msg import SentorEvent -from std_srvs.srv import Empty, EmptyResponse -import pprint -import signal -import rospy -import time -import yaml -import sys -import os - -# TODO nice printing of frequency of the topic with curses -# TODO consider timeout - -unpublished_topics_indexes = [] -satisfied_filters_indexes = [] - -topic_monitors = [] - -event_pub = None -rich_event_pub = None - -def __signal_handler(signum, frame): - - def kill_monitors(): - for topic_monitor in topic_monitors: - topic_monitor.kill_monitor() - - safety_monitor.stop_monitor() - autonomy_monitor.stop_monitor() - multi_monitor.stop_monitor() - - def join_monitors(): - for topic_monitor in topic_monitors: - topic_monitor.join() - - kill_monitors() - join_monitors() - print("stopped.") - os._exit(signal.SIGTERM) - - -def stop_monitoring(_): - for topic_monitor in topic_monitors: - topic_monitor.stop_monitor() - - safety_monitor.stop_monitor() - autonomy_monitor.stop_monitor() - multi_monitor.stop_monitor() - - rospy.logwarn("sentor_node stopped monitoring") - ans = EmptyResponse() - return ans - - -def start_monitoring(_): - for topic_monitor in topic_monitors: - topic_monitor.start_monitor() - - safety_monitor.start_monitor() - autonomy_monitor.start_monitor() - multi_monitor.start_monitor() - - rospy.logwarn("sentor_node started monitoring") - ans = EmptyResponse() - return ans - - -def event_callback(string, type, msg="", nodes=[], topic=""): - if type == "info": - rospy.loginfo(string + '\n' + str(msg)) - elif type == "warn": - rospy.logwarn(string + '\n' + str(msg)) - elif type == "error": - rospy.logerr(string + '\n' + str(msg)) - - if event_pub is not None: - event_pub.publish(String("%s: %s" % (type, string))) - - if rich_event_pub is not None: - event = SentorEvent() - event.header.stamp = rospy.Time.now() - event.level = SentorEvent.INFO if type == "info" else SentorEvent.WARN if type == "warn" else SentorEvent.ERROR - event.message = string - event.nodes = nodes - event.topic = topic - rich_event_pub.publish(event) -########################################################################################## - - -########################################################################################## -if __name__ == "__main__": - signal.signal(signal.SIGINT, __signal_handler) - rospy.init_node("sentor") - - config_file = rospy.get_param("~config_file", "") - try: - items = [yaml.load(open(item, 'r') , Loader=yaml.FullLoader) for item in config_file.split(',')] - topics = [item for sublist in items for item in sublist] - except Exception as e: - rospy.logerr("No configuration file provided: %s" % e) - topics = [] - - stop_srv = rospy.Service('/sentor/stop_monitor', Empty, stop_monitoring) - start_srv = rospy.Service('/sentor/start_monitor', Empty, start_monitoring) - - event_pub = rospy.Publisher('/sentor/event', String, queue_size=10) - rich_event_pub = rospy.Publisher('/sentor/rich_event', SentorEvent, queue_size=10) - - safety_monitor = SafetyMonitor("safe_operation", "SAFE OPERATION", "thread_is_safe", "set_safety_tag", event_callback) - autonomy_monitor = SafetyMonitor("pause_autonomous_operation", "SAFE AUTONOMOUS OPERATION", "thread_is_auto", "set_autonomy_tag", event_callback, invert=True) - multi_monitor = MultiMonitor() - - topic_monitors = [] - print("Monitoring topics:") - for i, topic in enumerate(topics): - - include = True - if 'include' in topic: - include = topic['include'] - - if not include: - continue - - try: - topic_name = topic["name"] - except Exception as e: - rospy.logerr("topic name is not specified for entry %s" % topic) - continue - - rate = 0 - N = 0 - signal_when = {} - signal_lambdas = [] - processes = [] - timeout = 0 - default_notifications = True - - if 'rate' in topic: - rate = topic['rate'] - if 'N' in topic: - N = int(topic['N']) - if 'signal_when' in topic: - signal_when = topic['signal_when'] - if 'signal_lambdas' in topic: - signal_lambdas = topic['signal_lambdas'] - if 'execute' in topic: - processes = topic['execute'] - if 'timeout' in topic: - timeout = topic['timeout'] - if 'default_notifications' in topic: - default_notifications = topic['default_notifications'] - - topic_monitor = TopicMonitor(topic_name, rate, N, signal_when, signal_lambdas, processes, - timeout, default_notifications, event_callback, i) - - topic_monitors.append(topic_monitor) - safety_monitor.register_monitors(topic_monitor) - autonomy_monitor.register_monitors(topic_monitor) - multi_monitor.register_monitors(topic_monitor) - - time.sleep(1) - - # start monitoring - for topic_monitor in topic_monitors: - topic_monitor.start() - - rospy.spin() -########################################################################################## \ No newline at end of file diff --git a/scripts/test.py b/scripts/test.py deleted file mode 100755 index 13ef29a..0000000 --- a/scripts/test.py +++ /dev/null @@ -1,101 +0,0 @@ -#!/usr/bin/env python -""" -Created on Wed Dec 4 17:35:05 2019 - -@author: Adam Binch (abinch@sagarobotics.com) -""" -##################################################################################### -import rospy -from std_msgs.msg import Bool -from std_srvs.srv import SetBool, SetBoolResponse - - -class Tester(object): - - - def __init__(self): - - self.pub_expr_1 = True - self.pub_expr_2 = True - - self.expr_1 = True - self.expr_2 = True - - rospy.Service('/pub_expr_1', SetBool, self.func_pub_expr_1) - rospy.Service('/pub_expr_2', SetBool, self.func_pub_expr_2) - - rospy.Service('/set_expr_1', SetBool, self.set_expr_1) - rospy.Service('/set_expr_2', SetBool, self.set_expr_2) - - self.pub_1 = rospy.Publisher("/expr_1", Bool, queue_size=10) - self.pub_2 = rospy.Publisher("/expr_2", Bool, queue_size=10) - - self.rate = rospy.Rate(10) - - - def func_pub_expr_1(self, req): - - self.pub_expr_1 = req.data - - ans = SetBoolResponse() - ans.success = True - ans.message = "Publishing expr_1 = {}".format(req.data) - - return ans - - - def func_pub_expr_2(self, req): - - self.pub_expr_2 = req.data - - ans = SetBoolResponse() - ans.success = True - ans.message = "Publishing expr_2 = {}".format(req.data) - - return ans - - - def set_expr_1(self, req): - - self.expr_1 = req.data - - ans = SetBoolResponse() - ans.success = True - ans.message = "Set expr_1 = {}".format(req.data) - - return ans - - - def set_expr_2(self, req): - - self.expr_2 = req.data - - ans = SetBoolResponse() - ans.success = True - ans.message = "Set expr_2 = {}".format(req.data) - - return ans - - - def run(self): - - while not rospy.is_shutdown(): - - if self.pub_expr_1: - self.pub_1.publish(Bool(self.expr_1)) - - if self.pub_expr_2: - self.pub_2.publish(Bool(self.expr_2)) - - self.rate.sleep() -##################################################################################### - - -##################################################################################### -if __name__ == "__main__": - - rospy.init_node("tester") - - tester = Tester() - tester.run() -##################################################################################### diff --git a/scripts/topic_mapping_node.py b/scripts/topic_mapping_node.py deleted file mode 100755 index 94d1944..0000000 --- a/scripts/topic_mapping_node.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/env python -""" -@author: Adam Binch (abinch@sagarobotics.com) -""" -########################################################################################## -from sentor.TopicMapper import TopicMapper -from sentor.TopicMapServer import TopicMapServer - -import signal -import rospy -import time -import yaml -import os - - -def __signal_handler(signum, frame): - def kill_mappers(): - for topic_mapper in topic_mappers: - topic_mapper.stop_mapping() - topic_map_server.stop() - def join_mappers(): - for topic_mapper in topic_mappers: - topic_mapper.join() - kill_mappers() - join_mappers() - print("stopped.") - os._exit(signal.SIGTERM) -########################################################################################## - - -########################################################################################## -if __name__ == "__main__": - signal.signal(signal.SIGINT, __signal_handler) - rospy.init_node("topic_mapper") - - config_file = rospy.get_param("~config_file", "") - try: - items = [yaml.load(file(item, 'r')) for item in config_file.split(',')] - topics = [item for sublist in items for item in sublist] - except Exception as e: - rospy.logerr("No configuration file provided: %s" % e) - topics = [] - - topic_mappers = [] - print("Mapping topics:") - for i, topic in enumerate(topics): - try: - topic_name = topic["name"] - except Exception as e: - rospy.logerr("topic name is not specified for entry %s" % topic) - continue - - include = True - if "include" in topic.keys(): - include = topic["include"] - - if include: - topic_mappers.append(TopicMapper(topic, i)) - - time.sleep(1) - - map_pub_rate = rospy.get_param("~map_pub_rate", 0) - map_plt_rate = rospy.get_param("~map_plt_rate", 0) - topic_map_server = TopicMapServer(topic_mappers, map_pub_rate, map_plt_rate) - - # start mapping - for topic_mapper in topic_mappers: - topic_mapper.start() - - rospy.spin() -########################################################################################## \ No newline at end of file diff --git a/sentor/CMakeLists.txt b/sentor/CMakeLists.txt new file mode 100644 index 0000000..b3b0fb7 --- /dev/null +++ b/sentor/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.5) +project(sentor) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(sentor_msgs REQUIRED) + +# Install Python package +ament_python_install_package(${PROJECT_NAME}) + +# Install scripts +install(PROGRAMS + scripts/sentor_node.py + scripts/test_sentor.py + DESTINATION lib/${PROJECT_NAME} +) + +# Install config files +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME}/config +) + +ament_package() diff --git a/sentor/config/test_monitor_config.yaml b/sentor/config/test_monitor_config.yaml new file mode 100644 index 0000000..978dd1f --- /dev/null +++ b/sentor/config/test_monitor_config.yaml @@ -0,0 +1,70 @@ +monitors: +# - name: "/example_topic" +# message_type: "std_msgs/msg/Int32" +# rate: 10 # optional throttling (messages per second) +# N: 5 +# qos: +# reliability: "reliable" +# durability: "volatile" +# depth: 10 +# signal_when: +# condition: "published" +# timeout: 2.0 +# safety_critical: false +# signal_lambdas: +# - expression: "lambda x: x.data > 10" +# timeout: 3.0 +# safety_critical: true +# autonomy_critical: false +# tags: ["example"] +# execute: [] +# default_notifications: true + + - name: "/example_topic" + message_type: "std_msgs/msg/Int32" + rate: 10 + N: 5 + qos: + reliability: "reliable" + durability: "volatile" + depth: 10 + signal_when: + condition: "published" + timeout: 2.0 + safety_critical: false + signal_lambdas: + - expression: "lambda x: x.data > 10" + timeout: 3.0 + safety_critical: true + autonomy_critical: false + tags: ["example"] + process_indices: [0] # Add process indices here + execute: + - log: + message: "Lambda condition met: value > 10 on /example_topic" + level: "warn" + default_notifications: true + enable_internal_logs: false + + - name: "/test_topic_2" + message_type: "sensor_msgs/msg/Temperature" + rate: 2 + N: 5 + qos: + reliability: "best_effort" + durability: "transient_local" + depth: 5 + signal_when: + condition: "published" + timeout: 2 + safety_critical: False + signal_lambdas: + - expression: "lambda x: x.temperature < 15" + timeout: 2 + safety_critical: True + execute: [] + timeout: 10 + default_notifications: False + + + diff --git a/sentor/package.xml b/sentor/package.xml new file mode 100644 index 0000000..b078255 --- /dev/null +++ b/sentor/package.xml @@ -0,0 +1,22 @@ + + + sentor + 0.1.0 + Sentor Python-based monitoring logic + + Your Name + MIT + + ament_cmake + ament_cmake_python + + rclpy + sentor_msgs + + rclpy + sentor_msgs + + + ament_cmake + + diff --git a/sentor/scripts/test_sentor.py b/sentor/scripts/test_sentor.py new file mode 100755 index 0000000..b447f1e --- /dev/null +++ b/sentor/scripts/test_sentor.py @@ -0,0 +1,185 @@ +#!/usr/bin/env python3 +import rclpy +import yaml +import time +import signal +import os +from sentor.TopicMonitor import TopicMonitor +from sentor.MultiMonitor import MultiMonitor +from std_msgs.msg import String +from sentor_msgs.msg import SentorEvent +from std_srvs.srv import Empty +import importlib +from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy +from rclpy.executors import MultiThreadedExecutor + + +topic_monitors = [] +event_pub = None +rich_event_pub = None +multi_monitor = None + +def __signal_handler(signum, frame): + """ Gracefully stop all monitors on SIGINT. """ + for topic_monitor in topic_monitors: + topic_monitor.kill_monitor() + multi_monitor.stop_monitor() + print("Stopped monitoring.") + os._exit(signal.SIGTERM) + +# def stop_monitoring(_): +# """ Stop all monitoring activities. """ +# for topic_monitor in topic_monitors: +# topic_monitor.stop_monitor() +# multi_monitor.stop_monitor() +# return Empty.Response() + +# def start_monitoring(_): +# """ Start monitoring activities. """ +# for topic_monitor in topic_monitors: +# topic_monitor.start_monitor() +# multi_monitor.start_monitor() +# return Empty.Response() + +def start_monitoring(request, _): + print("[Service] Received start_monitoring request") + for topic_monitor in topic_monitors: + topic_monitor.start_monitor() + multi_monitor.start_monitor() + return Empty.Response() + +def stop_monitoring(request, _): + print("[Service] Received stop_monitoring request") + for topic_monitor in topic_monitors: + topic_monitor.stop_monitor() + multi_monitor.stop_monitor() + return Empty.Response() + +def resolve_qos(topic, multi_monitor): + return get_qos_profile(topic["qos"]) if "qos" in topic else None + +def resolve_msg_type(topic, multi_monitor): + return get_message_type(topic["message_type"]) if "message_type" in topic else None + +def get_message_type(type_str): + """Import the actual message type class from a string like 'std_msgs/msg/Int32'.""" + if not type_str: + return None + try: + package, msg_name = type_str.split('/msg/') + module = importlib.import_module(f"{package}.msg") + return getattr(module, msg_name) + except Exception as e: + print(f"[ERROR] Failed to import message type '{type_str}': {e}") + return None +def get_qos_profile(qos_dict): + """Convert YAML QoS dict into actual QoSProfile object.""" + reliability = ReliabilityPolicy.RELIABLE + durability = DurabilityPolicy.VOLATILE + history = HistoryPolicy.KEEP_LAST + depth = 10 + + if qos_dict.get("reliability", "").lower() == "best_effort": + reliability = ReliabilityPolicy.BEST_EFFORT + if qos_dict.get("durability", "").lower() == "transient_local": + durability = DurabilityPolicy.TRANSIENT_LOCAL + if qos_dict.get("history", "").lower() == "keep_all": + history = HistoryPolicy.KEEP_ALL + if "depth" in qos_dict: + depth = qos_dict["depth"] + + return QoSProfile( + reliability=reliability, + durability=durability, + history=history, + depth=depth + ) + +def event_callback(message, level="info", msg=None, nodes=None, topic_name=None): + prefix = f"[{level.upper()}]" + print(f"{prefix} {message}") + +def main(): + global multi_monitor + rclpy.init() + node = rclpy.create_node("test_sentor") + + node.get_logger().info("Registering start/stop monitoring services...") + node.create_service(Empty, "start_monitoring", start_monitoring) + node.create_service(Empty, "stop_monitoring", stop_monitoring) + + config_file = node.declare_parameter("config_file", "config/test_monitor_config.yaml").value + topics = [] + + try: + items = [yaml.safe_load(open(item, 'r')) for item in config_file.split(',')] + for item in items: + if isinstance(item, dict) and "monitors" in item: + topics.extend(item["monitors"]) + elif isinstance(item, list): + topics.extend(item) + elif isinstance(item, dict): + topics.append(item) + except Exception as e: + node.get_logger().error(f"Error loading config file: {e}") + rclpy.shutdown() + return + + + multi_monitor = MultiMonitor() + + node.get_logger().info("Registering topic monitors:") + node.get_logger().info(f"Loaded topics from config:\n{topics}") + for i, topic in enumerate(topics): + if not isinstance(topic, dict): + continue + if topic.get("include", True) is False: + continue + + topic_name = topic.get("name") + if not topic_name: + continue + + qos_profile = resolve_qos(topic, multi_monitor) + msg_type = resolve_msg_type(topic, multi_monitor) + + node.get_logger().info(f"[Monitor-{i}] Topic: {topic_name}") + node.get_logger().info(f"[Monitor-{i}] Msg Type: {msg_type}") + node.get_logger().info(f"[Monitor-{i}] QoS Profile: {qos_profile}") + + topic_monitor = TopicMonitor( + topic_name=topic_name, + msg_type=msg_type, + qos_profile=qos_profile, + rate=topic.get("rate", 0), + N=topic.get("N", 0), + signal_when_config=topic.get("signal_when", {}), + signal_lambdas_config=topic.get("signal_lambdas", []), + processes=topic.get("execute", []), + timeout=topic.get("timeout", 0), + default_notifications=topic.get("default_notifications", True), + # event_callback=None, + event_callback=event_callback, + thread_num=i, + enable_internal_logs=topic.get("enable_internal_logs", True) + ) + + topic_monitors.append(topic_monitor) + multi_monitor.register_monitor(topic_monitor) + + # for topic_monitor in topic_monitors: + # topic_monitor.start() + + executor = MultiThreadedExecutor() + executor.add_node(node) # test_sentor + + # Also add each TopicMonitor's node + for topic_monitor in topic_monitors: + executor.add_node(topic_monitor.get_node()) + + executor.add_node(multi_monitor) + + executor.spin() + +if __name__ == "__main__": + main() diff --git a/sentor/sentor/Executor.py b/sentor/sentor/Executor.py new file mode 100644 index 0000000..e5c91de --- /dev/null +++ b/sentor/sentor/Executor.py @@ -0,0 +1,500 @@ +#!/usr/bin/env python3 +""" +Created on Thu Nov 21 10:30:22 2019 +@author: Adam Binch (abinch@sagarobotics.com) + +Converted from ROS1 to ROS2 2025 +@author: Zhuoling Huang + +Description: +This script defines an Executor node in ROS2 that handles various tasks such as: +- Calling ROS2 services +- Publishing to topics +- Executing ROS2 actions +- Running shell commands +- Logging messages +- Dynamically reconfiguring parameters +- Managing thread locks +""" +##################################################################################### +# Import necessary ROS2 libraries +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +from rclpy.publisher import Publisher +from rclpy.subscription import Subscription +from rclpy.service import Service +from rclpy.parameter import Parameter +from rcl_interfaces.msg import SetParametersResult + +# Import standard Python libraries +import os +import numpy as np +import math +import subprocess +import time +from threading import Lock +import subprocess + +# Import function for getting ROS2 service types dynamically +from sentor.service_types import get_service_type + +def _import(location, name): + """ + Dynamically imports a module and returns the specified attribute. + + Args: + - location (str): The module's location. + - name (str): The attribute to fetch from the module. + + Returns: + - The imported attribute. + """ + mod = __import__(location, fromlist=[name]) + return getattr(mod, name) + +class Executor(Node): + """ + Executor node that manages different types of processes in ROS2. + """ + def __init__(self, config, event_cb): + """ + Initializes the Executor node. + + Args: + - config (list): List of process configurations. + - event_cb (function): Callback function for event logging. + """ + super().__init__('executor_node') + self.config = config + self.event_cb = event_cb + + self.init_err_str = "Unable to initialize process of type '{}': {}" + self._lock = Lock() + self.processes = [] + + for process in config: + process_type = list(process.keys())[0] + + if process_type == "call": + self.init_call(process) + elif process_type == "publish": + self.init_publish(process) + elif process_type == "action": + self.init_action(process) + elif process_type == "sleep": + self.init_sleep(process) + elif process_type == "shell": + self.init_shell(process) + elif process_type == "log": + self.init_log(process) + elif process_type == "reconf": + self.init_reconf(process) + elif process_type == "lock_acquire": + self.init_lock_acquire(process) + elif process_type == "lock_release": + self.init_lock_release(process) + elif process_type == "custom": + self.init_custom(process) + else: + self.event_cb(f"Process of type '{process_type}' not supported", "warn") + self.processes.append("not_initialized") + + self.default_indices = range(len(self.processes)) + + def init_call(self, process): + """ + Initializes a ROS2 action client. + """ + try: + service_name = process["call"]["service_name"] + service_name = self.get_param_name(service_name) + + # FIX: Get the service type manually + service_class = get_service_type(service_name) + + timeout_srv = process["call"].get("timeout", 1.0) + + req = service_class.Request() # FIX: Correct way to create a service request in ROS2 + for arg in process["call"]["service_args"]: + exec(arg) + + d = { + "name": "call", + "verbose": self.is_verbose(process["call"]), + "def_msg": (f"Calling service '{service_name}'", "info", req), + "func": "self.call(**kwargs)", + "kwargs": { + "service_name": service_name, + "service_class": service_class, + "req": req, + "verbose": self.is_verbose(process["call"]), + "timeout_srv": timeout_srv, + } + } + + self.processes.append(d) + + except Exception as e: + self.event_cb(self.init_err_str.format("call", str(e)), "warn") + self.processes.append("not_initialized") + + def get_topic_type(self, topic_name): + """ Automatically retrieves the topic type using the ROS2 CLI """ + try: + result = subprocess.run(["ros2", "topic", "type", topic_name], capture_output=True, text=True) + topic_type = result.stdout.strip() + if topic_type: + return topic_type.replace("/", ".msg.") # Convert ROS2 format to Python import format + else: + raise ValueError(f"Unknown topic type for {topic_name}") + except Exception as e: + raise ValueError(f"Failed to retrieve topic type: {str(e)}") + + def init_publish(self, process): + try: + topic_name = self.get_param_name(process["publish"]["topic_name"]) + msg_type = self.get_topic_type(topic_name) + + pub = self.create_publisher(msg_type, topic_name, 10) + + msg = msg_type() + for arg in process["publish"]["topic_args"]: + exec(arg) + + d = { + "name": "publish", + "verbose": self.is_verbose(process["publish"]), + "def_msg": (f"Publishing to topic '{topic_name}'", "info", msg), + "func": "self.publish(**kwargs)", + "kwargs": {"pub": pub, "msg": msg}, + } + + self.processes.append(d) + + except Exception as e: + self.event_cb(self.init_err_str.format("publish", str(e)), "warn") + self.processes.append("not_initialized") + + def init_action(self, process): + try: + namespace = process["action"]["namespace"] + package = process["action"]["package"] + spec = process["action"]["action_spec"] + + action_spec = _import(package+".action", spec) + goal_class = _import(package+".action", spec[:-6] + "Goal") + + action_client = ActionClient(self, action_spec, namespace) + goal = goal_class() + for arg in process["action"]["goal_args"]: + exec(arg) + + d = { + "name": "action", + "verbose": self.is_verbose(process["action"]), + "def_msg": (f"Sending goal for '{namespace}' action with spec '{spec}'", "info", goal), + "func": "self.action(**kwargs)", + "kwargs": { + "namespace": namespace, + "spec": spec, + "action_client": action_client, + "goal": goal, + "verbose": self.is_verbose(process["action"]), + "wait": process["action"].get("wait", False), + } + } + + self.processes.append(d) + + except Exception as e: + self.event_cb(self.init_err_str.format("action", str(e)), "warn") + self.processes.append("not_initialized") + + def init_reconf(self, process): + try: + params = process["reconf"]["params"] + namespaces = set(param["namespace"] for param in params) + + for param in params: + self.declare_parameter(f"{param['namespace']}.{param['name']}", param["value"]) + + default_config = { + namespace: self.get_parameters_by_prefix(namespace) for namespace in namespaces + } + + default_params = [ + default_config[param["namespace"]][param["name"]].value for param in params + ] + + d = { + "name": "reconf", + "verbose": self.is_verbose(process["reconf"]), + "def_msg": (f"Reconfiguring parameters: {params}", "info", ""), + "func": "self.reconf(**kwargs)", + "kwargs": {"params": params, "default_params": default_params}, + } + + self.processes.append(d) + + except Exception as e: + self.event_cb(self.init_err_str.format("reconf", str(e)), "warn") + self.processes.append("not_initialized") + + def init_lock_acquire(self, process): + """ Initializes a process that acquires a thread lock """ + try: + d = { + "name": "lock_acquire", + "verbose": False, + "func": "self.lock_acquire()", + "kwargs": {} + } + self.processes.append(d) + except Exception as e: + self.event_cb(self.init_err_str.format("lock_acquire", str(e)), "warn") + self.processes.append("not_initialized") + + def init_lock_release(self, process): + """ Initializes a process that releases a thread lock """ + try: + d = { + "name": "lock_release", + "verbose": False, + "func": "self.lock_release()", + "kwargs": {} + } + self.processes.append(d) + except Exception as e: + self.event_cb(self.init_err_str.format("lock_release", str(e)), "warn") + self.processes.append("not_initialized") + + def init_custom(self, process): + """ Initializes a custom process from a package and module """ + try: + package = process["custom"]["package"] + name = process["custom"]["name"] + + _file = process["custom"].get("file", name) + custom_proc = _import(f"{package}.{_file}", name) + + cp = custom_proc(*process["custom"].get("init_args", [])) + + d = { + "name": "custom", + "verbose": self.is_verbose(process["custom"]), + "def_msg": (f"Executing custom process '{name}' from package '{package}'", "info", ""), + "func": "self.custom(**kwargs)", + "kwargs": { + "cp": cp, + "args": process["custom"].get("run_args", None) + } + } + + self.processes.append(d) + except Exception as e: + self.event_cb(self.init_err_str.format("custom", str(e)), "warn") + self.processes.append("not_initialized") + + def init_sleep(self, process): + """ Initializes a process that will sleep for a given duration """ + try: + d = { + "name": "sleep", + "verbose": self.is_verbose(process["sleep"]), + "def_msg": (f"Sentor sleeping for {process['sleep']['duration']} seconds", "info", ""), + "func": "self.sleep(**kwargs)", + "kwargs": { + "duration": process["sleep"]["duration"] + } + } + self.processes.append(d) + except Exception as e: + self.event_cb(self.init_err_str.format("sleep", str(e)), "warn") + self.processes.append("not_initialized") + + def init_shell(self, process): + """ Initializes a process that will execute a shell command """ + try: + d = { + "name": "shell", + "verbose": self.is_verbose(process["shell"]), + "def_msg": (f"Executing shell command {process['shell']['cmd_args']}", "info", ""), + "func": "self.shell(**kwargs)", + "kwargs": { + "cmd_args": process["shell"]["cmd_args"], + "shell_features": process["shell"].get("shell_features", False) + } + } + self.processes.append(d) + except Exception as e: + self.event_cb(self.init_err_str.format("shell", str(e)), "warn") + self.processes.append("not_initialized") + + def init_log(self, process): + """ Initializes a logging process """ + try: + d = { + "name": "log", + "verbose": False, + "func": "self.log(**kwargs)", + "kwargs": { + "message": process["log"]["message"], + "level": process["log"]["level"], + "msg_args": process["log"].get("msg_args", None) + } + } + self.processes.append(d) + except Exception as e: + self.event_cb(self.init_err_str.format("log", str(e)), "warn") + self.processes.append("not_initialized") + + def execute(self, msg=None, process_indices=None): + """ + Executes the configured processes. + """ + self.msg = msg + if process_indices is None: + indices = self.default_indices + else: + indices = process_indices + + for index in indices: + process = self.processes[index] + if process == "not_initialized": + continue + + try: + if process["verbose"] and "def_msg" in process: + self.event_cb(process["def_msg"][0], process["def_msg"][1], process["def_msg"][2]) + + kwargs = process["kwargs"] + eval(process["func"]) + + except Exception as e: + self.event_cb(f"Unable to execute process of type '{process['name']}': {str(e)}", "warn") + + def get_param_name(self, name): + """ Retrieves a parameter's value, checking environment variables and ROS parameters """ + env_name = os.environ.get(name) + if env_name is not None: + return env_name + + if self.has_parameter(name): + return self.get_parameter(name).value + + return name + + def is_verbose(self, process): + """ Checks if verbosity is enabled for a process """ + return process.get("verbose", False) + + def sleep(self, duration): + """ Sleeps for a given duration in seconds """ + self.get_logger().info(f"Sleeping for {duration} seconds...") + time.sleep(duration) # FIX: Replaced `rclpy.sleep()` with `time.sleep()` + + def call(self, service_name, service_class, req, verbose, timeout_srv): + """ Calls a ROS2 service """ + self.get_logger().info(f"Waiting for service '{service_name}'...") + client = self.create_client(service_class, service_name) + + if not client.wait_for_service(timeout_sec=timeout_srv): + self.event_cb(f"Service '{service_name}' not available", "warn") + return + + future = client.call_async(req) + rclpy.spin_until_future_complete(self, future) + + if future.done(): + response = future.result() + if verbose and response: + self.event_cb(f"Call to service '{service_name}' succeeded", "info", req) + else: + self.event_cb(f"Call to service '{service_name}' failed", "warn", req) + else: + self.event_cb(f"Call to service '{service_name}' did not complete", "warn") + + + def publish(self, pub, msg): + """ Publishes a message to a ROS2 topic """ + pub.publish(msg) + + def action(self, namespace, spec, action_client, goal, verbose, wait): + """ Sends a goal to a ROS2 action server """ + self.action_namespace = namespace + self.spec = spec + self.goal = goal + self.verbose_action = verbose + + future = action_client.send_goal_async(goal) + rclpy.spin_until_future_complete(self, future) + + if wait: + result_future = future.result().get_result_async() + rclpy.spin_until_future_complete(self, result_future) + + def shell(self, cmd_args, shell_features): + """ Executes a shell command """ + process = subprocess.Popen(cmd_args, + shell=shell_features, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE) + + stdout, stderr = process.communicate() + print(stdout.decode()) + + if stderr: + self.event_cb(f"Unable to execute shell commands {cmd_args}: {stderr.decode()}", "warn") + + def log(self, message, level, msg_args): + """ Logs a custom message """ + msg = self.msg + if msg is not None and msg_args is not None: + args = [eval(arg) for arg in msg_args] + self.event_cb(f"CUSTOM MSG: {message.format(*args)}", level) + else: + self.event_cb(f"CUSTOM MSG: {message}", level) + + def reconf(self, params, default_params): + """ Updates parameters dynamically in ROS2 """ + try: + self.set_parameters([ + Parameter(f"{param['namespace']}.{param['name']}", Parameter.Type.DOUBLE, + param["value"] if param["value"] != "_default" else default_param) + for param, default_param in zip(params, default_params) + ]) + except Exception as e: + self.event_cb(f"Unable to reconfigure parameters: {str(e)}", "warn") + + def lock_acquire(self): + """ Acquires a thread lock """ + self._lock.acquire() + + def lock_release(self): + """ Releases a thread lock """ + self._lock.release() + + def custom(self, cp, args): + """ Executes a custom process """ + cp.run(*args) if args is not None else cp.run() + + def goal_cb(self, status, result): + """ Handles action goal completion """ + if self.verbose_action and status == 3: + self.event_cb(f"Goal succeeded for '{self.action_namespace}' action with specification '{self.spec}'", "info", self.goal) + elif status == 2: + self.event_cb(f"Goal preempted for '{self.action_namespace}' action with specification '{self.spec}'", "warn", self.goal) + elif status != 3: + self.event_cb(f"Goal failed for '{self.action_namespace}' action with specification '{self.spec}'. Status is {status}", "warn", self.goal) + + +def main(args=None): + rclpy.init(args=args) + executor = Executor([], print) + rclpy.spin(executor) + executor.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/sentor/sentor/MultiMonitor.py b/sentor/sentor/MultiMonitor.py new file mode 100644 index 0000000..5c07deb --- /dev/null +++ b/sentor/sentor/MultiMonitor.py @@ -0,0 +1,76 @@ +from threading import Event +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, HistoryPolicy +from sentor_msgs.msg import Monitor, MonitorArray # Ensure this message is defined + +class MultiMonitor(Node): + def __init__(self, node_name='multi_monitor'): + super().__init__(node_name) + + # Declare and get safety publication rate from parameters + self.declare_parameter("safety_pub_rate", 10.0) + rate = self.get_parameter("safety_pub_rate").value + + self.topic_monitors = [] + self._stop_event = Event() + self.error_code = [] + + self.monitors_pub = None # Initialized dynamically when first monitor is registered + self.timer = self.create_timer(1.0 / rate, self.callback) + + def get_default_qos(self): + """Fallback QoS profile if not provided by monitor.""" + qos = QoSProfile( + depth=1, + reliability=QoSReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL + ) + self.get_logger().warn("[MultiMonitor] No QoS provided; using fallback default QoS.") + return qos + + def register_monitor(self, topic_monitor): + """Register a monitor and set QoS for MonitorArray publisher.""" + self.get_logger().info(f"[MultiMonitor] Registering monitor for topic: {topic_monitor.topic_name}") + + if not self.topic_monitors: + qos_profile = topic_monitor.qos_profile or self.get_default_qos() + self.monitors_pub = self.create_publisher(MonitorArray, "/sentor/monitors", qos_profile) + + self.topic_monitors.append(topic_monitor) + + def callback(self): + """Periodic check and publish monitor states.""" + if not self._stop_event.is_set() and self.monitors_pub is not None: + error_code_new = [ + monitor.conditions[expr]["satisfied"] + for monitor in self.topic_monitors + for expr in monitor.conditions + ] + + if error_code_new != self.error_code: + self.error_code = error_code_new + + msg = MonitorArray() + msg.header.stamp = self.get_clock().now().to_msg() + count = 0 + + for monitor in self.topic_monitors: + for expr in monitor.conditions: + condition = Monitor() + condition.topic = monitor.topic_name + condition.condition = expr + condition.safety_critical = monitor.conditions[expr]["safety_critical"] + condition.autonomy_critical = monitor.conditions[expr]["autonomy_critical"] + condition.satisfied = self.error_code[count] + condition.tags = monitor.conditions[expr]["tags"] + msg.conditions.append(condition) + count += 1 + + self.monitors_pub.publish(msg) + + def stop_monitor(self): + self._stop_event.set() + + def start_monitor(self): + self._stop_event.clear() diff --git a/sentor/sentor/ROSTopicFilter.py b/sentor/sentor/ROSTopicFilter.py new file mode 100644 index 0000000..eed76ca --- /dev/null +++ b/sentor/sentor/ROSTopicFilter.py @@ -0,0 +1,123 @@ +#!/usr/bin/env python3 +""" +Created on Thu Nov 21 10:30:22 2019 +@author: Adam Binch (abinch@sagarobotics.com) + +Converted from ROS1 to ROS2 2025 +@author: Zhuoling Huang + +Description: +Topic name is needed. (Message type and QoS can be detected automatically) +""" +##################################################################################### +#!/usr/bin/env python3 +import rclpy +import importlib +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy + +class ROSTopicFilter: + def __init__(self, parent_node: Node, topic_name, lambda_fn_str, config, throttle_val, stop_event=None): + self.node = parent_node + self.topic_name = topic_name + self.lambda_fn_str = lambda_fn_str + self.config = config + self.throttle_val = max(throttle_val, 1) + self.throttle_counter = 0 + self._stop_event = stop_event # <- SAFE default + + self.sat_callbacks = [] + self.unsat_callbacks = [] + + # Compile lambda function + try: + self.lambda_fn = eval(self.lambda_fn_str) + except Exception as e: + self.node.get_logger().error(f"[ROSTopicFilter] Lambda error: {e}") + self.lambda_fn = None + + # Detect message type + self.msg_type = self.get_message_type(self.topic_name) + if not self.msg_type: + self.node.get_logger().warn(f"[ROSTopicFilter] Could not detect message type for {self.topic_name}") + return + + # Detect QoS + qos = self.get_publisher_qos(self.topic_name) + self.node.get_logger().info(f"[ROSTopicFilter] Using QoS: {qos}") + + # Create subscription + self.subscription = self.node.create_subscription( + self.msg_type, + self.topic_name, + self.callback_filter_throttled, + qos + ) + self.node.get_logger().info(f"[ROSTopicFilter] Subscribed to {self.topic_name}") + + def get_message_type(self, topic_name): + topic_types = self.node.get_topic_names_and_types() + for topic, types in topic_types: + if topic == topic_name: + msg_type_str = types[0] + self.node.get_logger().info(f"[ROSTopicFilter] Detected msg type: {msg_type_str}") + try: + pkg, msg = msg_type_str.split('/msg/') + module = importlib.import_module(f"{pkg}.msg") + return getattr(module, msg) + except Exception as e: + self.node.get_logger().error(f"[ROSTopicFilter] Failed to import: {e}") + return None + + def get_publisher_qos(self, topic_name): + infos = self.node.get_publishers_info_by_topic(topic_name) + if not infos: + self.node.get_logger().warn(f"[ROSTopicFilter] No QoS info found for {topic_name}, using default.") + return QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=10, + durability=DurabilityPolicy.VOLATILE + ) + info = infos[0] + return QoSProfile( + reliability=info.qos_profile.reliability, + durability=info.qos_profile.durability, + history=info.qos_profile.history, + depth=info.qos_profile.depth + ) + + def callback_filter(self, msg): + if self._stop_event and self._stop_event.is_set(): + return + + if self.lambda_fn is None: + return + try: + result = self.lambda_fn(msg) + self.node.get_logger().info(f"[ROSTopicFilter] Lambda result: {result}") + if result: + for cb in self.sat_callbacks: + cb(self.lambda_fn_str, msg, self.config) + else: + for cb in self.unsat_callbacks: + cb(self.lambda_fn_str) + except Exception as e: + self.node.get_logger().error(f"[ROSTopicFilter] Lambda evaluation error: {e}") + + def callback_filter_throttled(self, msg): + if self._stop_event and self._stop_event.is_set(): + return + if self.throttle_counter % self.throttle_val == 0: + self.callback_filter(msg) + self.throttle_counter = 1 + else: + self.throttle_counter += 1 + + def register_satisfied_cb(self, func): + self.sat_callbacks.append(func) + + def register_unsatisfied_cb(self, func): + self.unsat_callbacks.append(func) + +##################################################################################### \ No newline at end of file diff --git a/sentor/sentor/ROSTopicHz.py b/sentor/sentor/ROSTopicHz.py new file mode 100644 index 0000000..cbc0c94 --- /dev/null +++ b/sentor/sentor/ROSTopicHz.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python +""" +@author: Francesco Del Duchetto (FDelDuchetto@lincoln.ac.uk) +@author: Adam Binch (abinch@sagarobotics.com) + +Converted from ROS1 to ROS2 2025 +@author: Zhuoling Huang + +""" +##################################################################################### +import rclpy +from rclpy.qos import QoSProfile +from threading import Lock +import math +from threading import Event + +class ROSTopicHz: + def __init__(self, node, topic_name, window_size=1000, throttle_val=1, stop_event=None): + self.node = node + self.topic_name = topic_name + self.window_size = window_size + self.throttle_val = throttle_val if throttle_val > 0 else 1 # Avoid division by zero + self.throttle = 0 + + self.lock = Lock() + self.last_printed_time = None + self.prev_time = None + self.times = [] + self.msg_tn = None + self.node.get_logger().info(f"[ROSTopicHz] Initialized for {self.topic_name} with window {self.window_size}") + # self._stop_event = Event() + # Accept stop_event from outside or create internally + self._stop_event = stop_event if stop_event is not None else Event() + self.subscription = None + self.enabled = True + self.last_msg_time = None + + def start_monitoring(self, msg_type, qos_profile): + if not self.subscription: + self.subscription = self.node.create_subscription( + msg_type, self.topic_name, self.callback_hz, qos_profile + ) + self.times.clear() + self.last_msg_time = None + self.enabled = True + self.node.get_logger().info(f"[ROSTopicHz] Monitoring started for {self.topic_name}") + + def stop_monitoring(self): + if self.subscription: + self.node.destroy_subscription(self.subscription) + self.subscription = None + self.enabled = False + self.node.get_logger().info(f"[ROSTopicHz] Monitoring stopped for {self.topic_name}") + + # def callback_hz(self, msg): + # if not self.enabled: + # return + # now = self.node.get_clock().now().nanoseconds / 1e9 + # if self.last_msg_time is not None: + # delta = now - self.last_msg_time + # self.times.append(delta) + # if len(self.times) > self.window_size: + # self.times.pop(0) + # self.last_msg_time = now + + def callback_hz(self, msg): + # if self._stop_event and self._stop_event.is_set(): + # return + now = self.node.get_clock().now().nanoseconds / 1e9 + with self.lock: + if self.last_msg_time is not None: + dt = now - self.last_msg_time + self.times.append(dt) + if len(self.times) > self.window_size: + self.times.pop(0) + self.last_msg_time = now + + def get_hz(self): + if not self.enabled or len(self.times) == 0: + return None + if len(self.times) < 2: + return None + import statistics + mean = sum(self.times) / len(self.times) + stddev = statistics.stdev(self.times) if len(self.times) > 1 else 0.0 + return 1.0 / mean, min(self.times), max(self.times), stddev, len(self.times) + + def callback_hz_throttled(self, msg): + self.throttle += 1 + if self.throttle % self.throttle_val == 0: + self.callback_hz(msg) + + def print_hz(self, logger): + stats = self.get_hz() + if not stats: + logger.info(f"[HzMonitor] No Hz stats yet. Waiting for more data.") + return + + rate, min_d, max_d, std_d, n = stats + logger.info( + f"[HzMonitor] Rate={rate:.2f} Hz | Min={min_d:.3f}s | Max={max_d:.3f}s | Std={std_d:.4f}s | N={n}" + ) + + + diff --git a/sentor/sentor/ROSTopicPub.py b/sentor/sentor/ROSTopicPub.py new file mode 100644 index 0000000..87636fb --- /dev/null +++ b/sentor/sentor/ROSTopicPub.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python +""" +@author: Francesco Del Duchetto (FDelDuchetto@lincoln.ac.uk) +@author: Adam Binch (abinch@sagarobotics.com) + +Converted from ROS1 to ROS2 2025 +@author: Zhuoling Huang +""" +##################################################################################### +# sentor/ROSTopicPub.py + +import importlib +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy +import rclpy + + +class ROSTopicPub: + def __init__(self, node, topic_name, msg_type=None, qos_profile=None, throttle_val=1): + self.node = node + self.topic_name = topic_name + self.pub_callbacks = [] + self.throttle_val = throttle_val if throttle_val > 0 else 1 + self.throttle = self.throttle_val + + # Auto detect message type if not provided + if msg_type is None: + msg_type = self._detect_msg_type() + self.msg_type = msg_type + + # Auto detect QoS if not provided + if qos_profile is None: + qos_profile = self._detect_qos_profile() + self.qos_profile = qos_profile + + # Create subscription + self.subscription = self.node.create_subscription( + self.msg_type, + self.topic_name, + self.callback_pub_throttled, + self.qos_profile + ) + + self.node.get_logger().info(f"[ROSTopicPub] Subscribed to {self.topic_name}") + + def _detect_msg_type(self): + topic_types = self.node.get_topic_names_and_types() + for topic, types in topic_types: + if topic == self.topic_name and types: + type_str = types[0] + self.node.get_logger().info(f"[ROSTopicPub] Detected msg type: {type_str}") + package, msg = type_str.split('/msg/') + module = importlib.import_module(f"{package}.msg") + return getattr(module, msg) + self.node.get_logger().warn(f"[ROSTopicPub] Could not detect message type for {self.topic_name}, using fallback.") + return None + + def _detect_qos_profile(self): + infos = self.node.get_publishers_info_by_topic(self.topic_name) + if infos: + info = infos[0] + qos = QoSProfile( + reliability=info.qos_profile.reliability, + durability=info.qos_profile.durability, + history=info.qos_profile.history, + depth=info.qos_profile.depth + ) + self.node.get_logger().info(f"[ROSTopicPub] Using auto-detected QoS: {qos}") + return qos + else: + self.node.get_logger().warn(f"[ROSTopicPub] No QoS info found, using fallback default.") + return QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=10 + ) + + def callback_pub(self, msg): + self.node.get_logger().debug(f"[ROSTopicPub] Received message on {self.topic_name}") + for func in self.pub_callbacks: + func(msg) + + def callback_pub_throttled(self, msg): + if self.throttle % self.throttle_val == 0: + self.callback_pub(msg) + self.throttle = 1 + else: + self.throttle += 1 + + def register_published_cb(self, func): + self.pub_callbacks.append(func) + diff --git a/sentor/sentor/TopicMonitor.py b/sentor/sentor/TopicMonitor.py new file mode 100644 index 0000000..1e9525f --- /dev/null +++ b/sentor/sentor/TopicMonitor.py @@ -0,0 +1,259 @@ +from threading import Thread, Event, Lock +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy +from rclpy.timer import Timer +from sentor.ROSTopicFilter import ROSTopicFilter +from sentor.ROSTopicHz import ROSTopicHz +from sentor.ROSTopicPub import ROSTopicPub +from sentor.Executor import Executor +import time + +class TopicMonitor(Thread): + def __init__( + self, + topic_name, + msg_type, + qos_profile, + rate, + N, + signal_when_config, + signal_lambdas_config, + processes, + timeout, + default_notifications, + event_callback, + thread_num, + enable_internal_logs=True # <-- new optional param + ): + """ + :param enable_internal_logs: If True, prints extra developer logs (like 'Lambda satisfied' traces). + """ + super().__init__() + + # Basic fields + self.topic_name = topic_name + self.msg_type = msg_type + self.qos_profile = qos_profile + self.rate = rate + self.N = N + self.signal_when_config = signal_when_config + self.signal_lambdas_config = signal_lambdas_config + self.processes = processes + self.timeout = timeout if timeout > 0 else 0.1 + self.default_notifications = default_notifications + self._event_callback = event_callback + self.thread_num = thread_num + + # Developer log toggle + self.enable_internal_logs = enable_internal_logs + + # Thread-control + concurrency + self._stop_event = Event() + self._lock = Lock() + + # Condition tracking + self.conditions = {} + self.sat_expr_timer = {} + + # Create the ROS2 Node + self.node = Node(f"topic_monitor_{thread_num}") + self.debug(f"[TopicMonitor] Created for topic: {self.topic_name}") + + # QoS fallback + if self.qos_profile is None: + self.node.get_logger().warn(f"[TopicMonitor] No QoS profile for {self.topic_name}, using fallback QoS") + self.qos_profile = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=10 + ) + + # Optionally set up an Executor if needed + if processes: + self.executor = Executor(processes, event_callback) + else: + self.executor = None + + # Parse signal_when config + self.process_signal_config() + + # Create ROSTopicHz for frequency monitoring + self.hz_monitor = ROSTopicHz(self.node, self.topic_name, 1000, self.N) + self.hz_monitor.start_monitoring(self.msg_type, self.qos_profile) + + # Periodically log frequency + self.node.create_timer(5.0, self.log_hz_stats) + + # Set up ROSTopicPub if signal_when == "published" + if self.signal_when_cfg.get("signal_when", "").lower() == "published": + self.debug(f"[TopicMonitor] Setting up ROSTopicPub for {self.topic_name}") + self.pub_monitor = ROSTopicPub( + node=self.node, + topic_name=self.topic_name, + msg_type=self.msg_type, + qos_profile=self.qos_profile, + throttle_val=self.N + ) + self.pub_monitor.register_published_cb(self.published_cb) + + # Build lambda filters + for i, signal_lambda in enumerate(self.signal_lambdas_config): + config = self.process_lambda_config(signal_lambda) + expr = config["expr"] + + # Track condition + self.conditions[expr] = { + "satisfied": False, + "safety_critical": config["safety_critical"], + "autonomy_critical": config["autonomy_critical"], + "tags": config.get("tags", []) + } + + # Create ROSTopicFilter + filter_monitor = ROSTopicFilter( + self.node, self.topic_name, expr, config, throttle_val=self.N, + ) + filter_monitor.register_satisfied_cb(self.lambda_satisfied_cb) + filter_monitor.register_unsatisfied_cb(self.lambda_unsatisfied_cb) + + def debug(self, msg): + """Helper method: only logs if enable_internal_logs=True.""" + if self.enable_internal_logs: + self.node.get_logger().info(msg) + + def process_signal_config(self): + """Parses the 'signal_when_config' to fill up self.signal_when_cfg dict and create conditions.""" + self.signal_when_cfg = { + "signal_when": self.signal_when_config.get("condition", ""), + "timeout": self.signal_when_config.get("timeout", self.timeout), + "safety_critical": self.signal_when_config.get("safety_critical", False), + "autonomy_critical": self.signal_when_config.get("autonomy_critical", False), + "default_notifications": self.signal_when_config.get("default_notifications", self.default_notifications), + "tags": self.signal_when_config.get("tags", []) + } + if self.signal_when_cfg["timeout"] <= 0: + self.signal_when_cfg["timeout"] = self.timeout + + # If there's a condition, create an entry in self.conditions + if self.signal_when_cfg["signal_when"]: + self.conditions[self.signal_when_cfg["signal_when"]] = { + "satisfied": False, + "safety_critical": self.signal_when_cfg["safety_critical"], + "autonomy_critical": self.signal_when_cfg["autonomy_critical"], + "tags": self.signal_when_cfg["tags"] + } + + def process_lambda_config(self, signal_lambda): + """Parses each signal_lambda config to unify structure and fallback timeouts.""" + config = { + "expr": signal_lambda.get("expression", ""), + "timeout": signal_lambda.get("timeout", self.timeout), + "safety_critical": signal_lambda.get("safety_critical", False), + "autonomy_critical": signal_lambda.get("autonomy_critical", False), + "default_notifications": signal_lambda.get("default_notifications", self.default_notifications), + "repeat_exec": signal_lambda.get("repeat_exec", False), + "tags": signal_lambda.get("tags", []) + } + if config["timeout"] <= 0: + config["timeout"] = self.timeout + return config + + def lambda_satisfied_cb(self, expr, msg, config): + if expr in self.sat_expr_timer: + return + + def on_timeout(): + self.debug(f"[TopicMonitor] Lambda satisfied: {expr}") + self.conditions[expr]["satisfied"] = True + + if config.get("default_notifications"): + message = f"Lambda '{expr}' satisfied on topic {self.topic_name}" + level = "error" if config.get("safety_critical") else "warn" + + if self._event_callback: + self._event_callback(message, level, msg=msg, topic_name=self.topic_name) + + if level == "error": + self.node.get_logger().error(message) + else: + self.node.get_logger().warn(message) + + # Here, mimic ROS1 by calling execute() when the condition is met. + if not config.get("repeat_exec", False): + # process_indices should be provided in your YAML for this lambda. + self.execute(msg, config.get("process_indices")) + # Optionally, if repeat_exec is enabled, you could set up a repeat timer. + + with self._lock: + self.sat_expr_timer[expr] = self.node.create_timer( + config["timeout"], lambda: self._on_lambda_timeout(expr, on_timeout) + ) + + def _on_lambda_timeout(self, expr, callback): + """Helper to cancel the timer and then run the provided callback.""" + with self._lock: + if expr in self.sat_expr_timer: + self.sat_expr_timer[expr].cancel() + del self.sat_expr_timer[expr] + callback() + + def lambda_unsatisfied_cb(self, expr): + """Called when the lambda condition is no longer satisfied.""" + with self._lock: + if expr in self.sat_expr_timer: + self.sat_expr_timer[expr].cancel() + del self.sat_expr_timer[expr] + if expr in self.conditions: + self.conditions[expr]["satisfied"] = False + + def published_cb(self, msg): + """Called when a 'published' message is received.""" + self.debug(f"[TopicMonitor] Topic {self.topic_name} is published") + if "published" in self.conditions: + self.conditions["published"]["satisfied"] = True + + def log_hz_stats(self): + """Timer callback to log frequency stats from ROSTopicHz.""" + if not self.hz_monitor.enabled: + return + stats = self.hz_monitor.get_hz() + if stats: + rate, min_d, max_d, std_d, n = stats + if self.enable_internal_logs: + self.node.get_logger().info( + f"[HzMonitor] Rate={rate:.2f} Hz | Min={min_d:.3f}s | Max={max_d:.3f}s | Std={std_d:.4f}s | N={n}" + ) + else: + if self.enable_internal_logs: + self.node.get_logger().info("[HzMonitor] No Hz stats yet. Waiting for more data.") + + def execute(self, msg=None, process_indices=None): + """Triggers the executor to run configured processes if available.""" + if self.processes and self.executor: + self.executor.execute(msg, process_indices) + + def run(self): + """Background thread spin loop for the TopicMonitor's node.""" + while True: + if not self._stop_event.is_set(): + rclpy.spin_once(self.node, timeout_sec=0.1) + else: + time.sleep(0.1) + + def stop_monitor(self): + self._stop_event.set() + self.hz_monitor.stop_monitoring() + + def start_monitor(self): + self._stop_event.clear() + self.node.get_logger().info(f"[TopicMonitor] Restarting monitor for {self.topic_name}") + self.hz_monitor.start_monitoring(self.msg_type, self.qos_profile) + + def get_node(self): + """Returns the node so it can be added to an executor externally.""" + return self.node + + def event_callback(message, level="info", msg=None, topic_name=None): + print(f"[{level.upper()}] {message}") + diff --git a/src/sentor/__init__.py b/sentor/sentor/__init__.py similarity index 100% rename from src/sentor/__init__.py rename to sentor/sentor/__init__.py diff --git a/sentor/sentor/__pycache__/Executor.cpython-310.pyc b/sentor/sentor/__pycache__/Executor.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..7d9e147ec8cd9172569c70c747b487548fd640c4 GIT binary patch literal 13906 zcmbtbU2Gi3ec$if`{aixN}^ z?4BfUj%pIRwbQ~*(xh!rTPXR^ph622=?8+MEfAm&ZD0D3r(*jOpg;k?1OeQ>IE~fc z|3ABTdnYLt&?9zsc6MfVX8zwF?3Bv|4Zr`Hys`QpCpGOq=^^{G@bE0I;HM~rwyX(V z7%hES$K7lh>*lhlQ=8SY*6n4R>vqdo&n@S;?zG(X{BoY_xmIDlxLoAA+bXS>m&;tw zw<_xs%M)BTT9eC@{EeyQX}<4T-o^KsY`qt*b>wmHIZUf7{K&3Ejx!}*ridYx9Y zz3RQxtG8DRAL7up#i|}#iyh(PUV5$(HaqQ;t)|}&@ti-?yU=O|YrdqHOT7z0LpHnA z1<$!9UtVc8Xk7kGUDntAko)ag^25P%aMllcEt<=ErPH{C`{b%0)&e!O7H)L?hC$0n zekSgO6hIa&*oz|6`dVLK1wy=QY#O27H~RV(4zy~}!d==iRv1=|xKOJ#*Sj4V)@pGM zAE}3-teUag>NM)KzRCy4;0I1J}rpc0s@3zfkxZ&{}F(D>#FNi5Ije8NOHiLUf>=v`Q zm&G2j7x#*ITFi<4K*0%dK-`CxNpZh;0QV{3i3f3?76-*cxbG4Vi$`#u5s!*_+;@vZ zqKf;hcuYKw`yO#vJc0XOu^^7%zE3jcp)Z5t=Cqw9cdU#l5_sa7)ul~!nP0pc}hmC9USe?fcm`lcDW zeRDg%qiumL?9(=_z9lRW*x|mmUFe%M)8)Nrv(ff6+KQPr2W`)yt(0lYq3w%kD`(nV zw4Fm+CDWEi+gH#wk!dTSZ4GUcnYJR@B-*AjZ6&nz(KelFE2Hgq(6(#Prfr*|(zo6R z6S-lE$sGe{qFu~w&xk3tt8jNXOD$Xa7M;}|q2a8j7vr*@pk}QN+Krw#->zS1`9$~4 zb~9|&Tg}M#y0X*ogTU*octmO5{8u;Uk9%L;lmLSkTa9|F73aE&#$%_>#2?#1%lEsu zt@(IuwmPe^BmG9Fy%Lu|OP6Z(#^qj9`pJ{@TYf$8W2eyz!p?d$b!Pmd(Zsyh?u1^@ z>k@hTBDSv9WjmfgV{N!vQQeD=_PV}|U63+%0YY}@yUo?u^{;>~YmEzWiQo8A)`C#V zJZ;3T(YoS%vRyxjE#e8Wb;;id;ye%Fy{fV4hKu|n*>!;*)Yg+a@89KiIt0}{If<#H zIddGDru0nI5Opv(-)`4$i`bUnz2HMFA4eekl{)w%wkyC&gQ^`{f!|t@4`W~Q5!zfX z*;QOhD$F!*g6Y<3jaEGfYBgqNa+-!bgo4Hf&!8yj4*eM}O4pdgRnlGk)2|t$qR$x7 zR7%eaDL6)0H648s*Gc-^&rn2X`XK9lkaZwY5c9UyH+FQzw72w4Q@!0bi23apAYyad z+|f4eWR$&61F<`sPH6R=Eh6cI0w}+oyx-A<(>FjUIS`85w^vM&2jejNcD4_gK;Z*h z6hE*xb08aMJJ-(Axa4eVx%am zk-v$to9=a&W>B@O`cJz;@ca3%58v12Y7kAG1ksTQ zPDXk2AM5k6)oibHqFn^~1;W%rk3Dt?G_o30tF|Oi#^v-Y4D;BO{^d9av45ch+AAg> ztp)PROE4_9jcGRTOqYe+EsFvdj$>T1`?;34DlEN07XpD1b z{mVU{=!ketTxbThD>=qewjMrWlX*XvA{O|)Ic;5I7JgQ0ejg|q;+u0qJ*&wkeg zHX9cW##Xa$%KgAxOE@C8W4r;J2yNh_{=Rm@*wVkD1IL_=r-5S@a4e5^`7qbFwg&G- zVbi*9F$NW*U-O=WdV!;A(&9Ee>4&ljRu_2T8Zq;3v!OV5FCd(uMpo-izVfmcTe1@z zi*4=|Tg2ecK@F-j>fNv>{Th(4+XL`I|62I5ek8INfCvlGp7ZTX?atM9c6NZviyk;# zZ?>RBLWHEt$n-hxEmqyQ5cDpj{K%BOcI*V9z=y!NvG8u3lYX~VZ}@TH^?Ix4pOUg8 z0Gl6lVkHvYB%^;9NQ8TyEd$12aA_h+8qIvbUWvPLR2T`a#XRmVoT|Ji_h5hW0*Virz7q&AN7|hz`HWzl1myGbIU4ai9m_1Ur9^6G zIFk{ZXoNx?tjZUuw?_rDm6H4os{1J7$p)J|wPXTF?nQd{8WjURqLAbfY9kIJzeEM& zOqPAn24WZAV;Lyds8f@QB`VHQaR=OyhwP;?Phq#?g0uh z??@IufsBb<1wgIQF}xHDnufEH4%LRh_Y$HqA`WNM*~wZHtpe)(1i2b5S=2o zN5dNjwvGnjW^9>25C`Z6Cd1;@P0%eL8vQ&A%BLCKlCJ>WAQ&Mi3#?ZbZ`cCD4dSv4 zacT8)eW23zps$(}>U7JzPql7$iBncMgtP!c9K3(qV{7Fl>sXHari z1|^Hvi;3_=jalE!q9p4s&Xdduy2LVaC@w=|aNhxiHr>l-6-U2lY z!{TzXlm=VGu|;2D$to|S)9qMK)}S=07~=RV^mq@5UdqTxT7q1sLQ+ApDK7A?S^Ud@ zT0<&+HReBoF5kfwd>#c!N%|u!FBuM`r;?(xkey13QjdymeGFQd$k4*fC@rWn86gE1 z(-UK%tp^{Yh+fZ592 z4l7kb1B_wtBUcBX5$^2X_&UtJH*=dhR<7;SUWd2nvLSSGZ!j7zvKYK+#KyvtvCR{X z3&#A3Rln_D>&j<-M3a(Z!uZ;(7R`?WP1!9AC&(Mp3Z7XQj8f>XFR<1a+gC_#-?nB4 zvl4Fenwzv{m)87_1|1)U{CADZaLMq>;FQ@r&?w0x6x1$k)Vk7NXZ|Vx& zhZ4SU0wz!hmsa!@M?m(D* z+C}Xb|NEPVD}7Csml&7EZv zySdtv#LgJhe0;|}mcp?XDZZruIpkYd@)cUeV0{Yh#(7f#$~biTDdB^IKsy|K;GPAs zo8eFV#lnDh2XX^TkXuxYN@1+-Yt+gn0G*aRisEl@1q&#M;Xr3I9@Yy`L zHe|q79TUo9j7S~D2qPl&PnEilh;2sf?iFTJ6V?Y7DRdVTm5zsQc|`m+5x*KK61D+VE4OK0he8LVkZVl*ZuFG*^1uNTc=dq~CS0qBkW1`3 zq4sbMYLW2BbK%MRu+r0s{g?Hz9ID>5hUT1n8QtU;sqm;ERcrBP?jD2-W*Hh`qYxS* zvOI?QeuC>G63vhZY*_&y|2yqeU{fVl)!ZmCW|wu{+PoeFn)c$)YqC0INnRH|eBPPPnY2=p_$4Fcpw`q9W zz%uyu;M^cjHAI?CIvB*6NxB}iIUa#t@Ne!I?`fM3Yi>iqmFt@r1+j)Q56!J0iV$F> zzB3pJk1#ND0>W%EF&#*lrB03t&T3l9$V9$b;~0XlD1V#px@7X-{p`BkbammDi_2`Gr? zjm7jPu+iwo7JceAjwo3(Bk6F@JTPmF z40mSNXlrp%Ed;`@(~#eyY41vDpTXOo;R;AEG~kwlFT`#4e1Yxu6FZcj+NArweiJ*gKl%N{9(Sc$XEK-CZfMPlFc(8eaH%(o*KrUjIq*cNG zWoB7qY=t+*}cyb?Ya0V6tA~dQap%EDYv77daTzJ|bvPHmRRN>7G zTL3)u5N*cr1MZG{MT~XHfglXO)&Rm~hOo7%ogrR@4I7~ec z7@6@rlo`U9L2!*BT#3H$sSx%#kl{lla>T)dBDGFzM9$Gp_o~Aoz>dC--Nw3LD!(1D zl<+6p4CaEt|~p_F_NbIU)VV$^f72zMoWChpN{yCdk2VBR?bJxMMFdZOt9 z98^rxgf{5WHPqdyaAuFxN+;}Mi z=GzImkRjyaKuX+6>nTMKjDZ;elU3GZlVFhCkCmT}rkU{YG6S6)8~})F`?1}E=d-2! zJH)UTtET)z43mFE#rLTgmG_{~{|}g+#EgftU?N9y!GLo}w|)twCkv)wEF=m1tS*j3 zIbF;(2!{L{9yqdz0Ep0fnk*6{-89=^rSJv8h<)=sT!qC_j>N~gt`+ElX+W(3z4)<<@hP-IHlDO(@JAz7!5%y}3$9>!F2%8f@}3Z>1GClGN5Sfd&Dabkfq>D@#! zXBs(+b4ds-wo&n~sdxkH%6D)H#<^r^&k+0{#)OEF;m<;F0YL`ffdTj6qZ~D>7>C)b zZ2fmMshAP+0g~Y(lxkucx=!NakOc)3$SJ{&WSI3mU6%SVl$HN&N0)n0gOhkizi6YC z&Hm`DcXkj2AiXL{te6ixHq8%vjWxeqLxTI5L3e z-ieJ)fK96rOf_+NtsV@`z_>hOwX^99`eUns)i)P|RfK3vwI)yRIXcD?DkTIPEf7%& zM>*z(=dJJzBd12#Luw138cosHp&x_8?ck)UX0+g)qzwmNbH!8Pr)Id}H3Mv)HjJbf z$orw0#Ta@TwmWeKF#4g{Gq!Bv@-6?Xxg~h4`2OxqO#8}}% ze;(gQXa})I)`HTFeizN-rVnXJ zFpf8!zH<@GN|^8J!9#@5FoEbiZ0ME_i4Zc8`wrr`IG@d2NHJLr1`Cb+E$vne1MVkMPZ%WxOSIg0?cM{PxvMV`wmPTHq-EY9- zn;v)CIv-i{IBBv2(G;8*L9U|~oITZo{8OB`0&Y18nl0pwPmA)Jf*py+icf*?XcOZt@Jn)n0_F(M5pb`20oI8Wrq>j7wFTcs5p%RnHS3E z?Wj3mRKZW*X=8I^gfxrZYlL5S~&LpX51ja;TpuPoVVt(iJq$C^JP;;$GP z#-0Xxa(I#ch$bvtyu7ohq{1BuIq+Vignt<&L_65I#aK?klN8GxIS&lafmZ|T`;z|;uN16_2y-fs| zwZ^dnMicqx=q3MxiU#%Pd^{C|Rd|$QMvJ;gDgr8K@=WOCFYy?clW__~#u0{O8kP>K-Iz@5$FT>@6 zU>Z(;+om)zQi)_0e8eG(7u*OOir%oYgX|NWJVY*CRVSMB)H+Cdj#(MNBtmQX6%-uo z2Jej?yj{Z?u}uOmG7`;Z5vt9XX{>#wgY2K!I`i_GQ}7hP#_6>Y^beu-cH!|2V+WpD zWO6ZCBM8NE>wENRwo;C#i;zE}+88?162Cn>X{18n&-rBuepJiwbn8}@<+FuB=$!uO zpa(3U4jP)N%CRGj#Y41`?__hlh1ey9V4$;--~*$F4r{+h$4YL31fkP+qpejt+T*>_ zS*1fxTs$JW`%|J_3;Y`{)GecMUfc3mZF{uj9oI z9SzMoqeTg*p~69uX2KyuPkia*`K5EOz3RQX^x|<3@F#Us<+gkYwG=dCh6#82HuW6) zKG{7t==%j!DCS(z59qT-R2rZ@)-l2i2{hE1@t*K5n#6&!O7xZnw<0KDi{ogCR|Yr? zHQ(4+fSL~}8BL=%5==o0u+|9#g7KY|dz)=^#(b)=jqX50Io}mP#UQIRiVbj=BrOF> z3QNurFqv2y(Iwb17SFc6A7HAykFJZv?ntO z!Pp!hVn?BJefKdpQUa6s3m`&YjjeM?1Bsm%UOWH%E2q%N7_dQa%4wvAHO_H>5JKD} zfRL`~XpVW{6By^W5M_F30Fv8+Wkc*ZIHY~miy}72v`qE%U_kgb{0?bq6-RPnGir7- zi4G)5s)=L%3hF9;atCXLV*1)O&_sn zMH=|NI@-a6e}G+q|Yx+mlUD);yFNlc=7^zxO_*okcS*Ql`d0EoQ4| zKDzJroQ>uR$wwNP_51(va1I2O{q-*Y-vdm*c~--tDV|>G*vZ`G`2}wY{|?ZD4N1nZ zE&m#mD3<CsdFvuKbr|^>KcxO8p{TOldMCkeH!x&`}cUs7TGfpca>tq~ZqLkZEEu zOSBWzZ<-3y=7?m1r%||M@jvPwbW83&_aXOww}QX@?vy)Sn09B~qHDP}?iO0hZl0g5 zNsH$g5hg^8gdU9Ov^`e#z@pxTDi<27T1XKzXaLGz0`*QObq?>K;xUPmmx}%;gozGX z7pb|FSKAEvYx3{;nQWuDB#_lAI~$xjAG^U?4@rzy+hBJ@u*fw}QtQV-Z>`sx?SCe? zlYFLhRPq&-bomn=2C(9Ntya|u-nm*$bQ(BAUPys$vjxi4m$cWoq#Bgo%PB+gglZb* z$&n@yPpeiWUJm~GMv}G$<(tgCg~ZyFq{R)UNTrbyl{5wXWJtzXL84Vad7zTGirl6# zcDuI@v(acHPKHd>HP%<<59n21v0|9WD$#zBgiI-z0jCP{KftIHrc;prO2tqI;u@h% s?4D3aPaZcdAP`%+>#B^Tv{d#Y*^Hc|ZX(my(teBn3WwYr(mZedAKEe!3IG5A literal 0 HcmV?d00001 diff --git a/sentor/sentor/__pycache__/MultiMonitor.cpython-310.pyc b/sentor/sentor/__pycache__/MultiMonitor.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..98c68b5059c24a698d071c1b3f2110e699d15ced GIT binary patch literal 3217 zcmZ`*&2J+~6|d?qx7%&UnaK>B0IOc@vS2{Yen=n<(J-T#XhE7W+07o1(b9CfDo#7? zZl|g|SzC@cB+^Jod*FaLtdNr9{3rYkbwix?hQt90iCx|+f5aX{TlHM;)z|y{Rh9L6 zK;Zkb{ny@GZxHf#WL7^1m`Bj$5C|iT=A?`NR!#?2*P__Bb9>-)oq^kR4e#XMpwg|F zzMK1lYPUKFx&b905$3VV6=4-_UE19mt1+KduV^>qbrzgj;wL=3B(RPin$`-xf>TMS zMYYo@8E;uS__#bg5an@}bA1>3Pe&p-%JNK&56V1C$1v61f58g{b1TowB2%T%;k@%$ zh-94Nma89{h@X52gp)31q-(M8B{`PnIfrW4w?}c{;=85A%54hM>-X;E54CkvV0|mdJ7q$xewW%G4u%fOS%$?ddHq6^#-qiXwxo|+>Wxo7dE7bt<$mYW z_}QU*MtkRa6pIiNh%wl>S>baM@f3B-I*Xjb@Q^< zgFz=Nj?0#-9m(@!T}A8T0?g75dQ;cVN*NF5DeQqGvXZ@`x*Ese11aP975VpXcTdUz z-(_G^Uc!{!q)fp|k(?iadgEB8yhvbSd@gsTNO$??;Iksh!DH!Z(&HeVg=YS@8jkjG zEE-_oJ~Rm>PeK~nA@wYudNi{6iU4P>y~3k}iA% zoOA8o5cwyAKK^FG@H$Z-1g3m0eu_zy2UwE^oYQ+Ha4n^O9VF#J6@v2lHWYT##Tv)WI|g?;wxvjbT3t?M_YvwQ(qLTEDZ z6m}->ue$s;FT|)QFe%#Y==lt7Wt5hqLOqDqqJW@)Q8C&zxKbGv{nskFA~1JmzU6&523TzVRG+4@E2J*lF=>PJ%}dF z4%E?_s7@Vm$laVTmaxjjUB1a`XDQYmi zwY^eUcFqe^bmA`Vb`$!PUbY+g=Eq_+rjaz5LfH7ug?V7X@ZCqu} z+r{1<5>xnwVD*a~Aiskq9{|~*f#p%h^1i`uAAF^fsrq9#y*hscR+h>Qj3Zz zZeGQA00J_+1A-W~V51!SVH}^0l6*E|E-djw+!tdao+9}Oh_27C&6MYP9Ixp0Hcl~7 zMBK$kt_*?rr13NVzVG_)G;M(+YYsqn+0f8N`5q8O`T&=VegW?zWdZb21sB@ zj1Ncvsv%CZi)|;`FO(QZ!!j#WbX*E^f0;KB;D(ckp6s+d?TR!Xj`6XODWeFy7lv7( zL&o8vFUK*i(7rqwDOR2r5QJx-k9JVJVy3yJoI#Qme?Xnl&JfZR?s2%&1%e#<7stUn zCYlZL9HoIdziqZdvvqYQfY-^&!>OuH;BmwNK4x#NVD_MBL+P3dHN|bzH)n}oz)maL iElWxJd$Y(s#6*|(f%x$7FtuVgZSwno`t^W1-~1Oo6G!L( literal 0 HcmV?d00001 diff --git a/sentor/sentor/__pycache__/service_types.cpython-310.pyc b/sentor/sentor/__pycache__/service_types.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..7c3160fc6999bc0b14682729e79fe9614a6deb33 GIT binary patch literal 5497 zcmb7|S9cr76~}?$5Clnp1i{`Us0(VKsNPAji83wGBxO4j+k2M4l0>w#i|h_b6GwST z@(ui$_|Xq}$`?rQz1K~T6DLlb-ktitvj73oQqHk>@WafVe&^2o2Xd3imI(g6Jn-V` zVh_G4Q~!@4Glq{pf>a?=h}sbu711cAnayN_XpmfR*(e&dt-+4TCebA0BChk?Zk8>g zMYf7onGgwWYlJN+lCn*->3j_NcF`_VA|*RShqg7rmKJH*DLQpNZgUq~SLHQvjpiY*i|g`+xFN^Hm>d`5 zy1diADJR5)oD`FCN=(UVF)e4rjJzdo$yqTgZ;RXVG4Yt36LY$Lm;JcBBkt&Qw|!U6 zi+P>yv7e9&VnL^S?M1mHmgGHgPd+K0luwDL1 zX62RZZD}id?3{3&mE!8UXCa|H*S0M$=%it4`K7HA25jGH^f6WsnQob0Vbi4cvgA6J zqx_xrSw~skikY{34A(}puJs)BFIY79nq`ir?G-Uz|DyHAzQbv=AhozzRQa`9yC4;M zp=QDS+Qtio$)dUHxV|dp;j=}{URknyg_*RVY-!WQ?EIh!L)~;;J7`++imR)Zm*qjr zjHPBQcivQMK`KnIdS+>@n19?X1qoOd+;wHm+oltA-YXU0WK^$JK^LtPYo@bmg&UAY zi}4M!Xq(G+X!3*JdK>n}@r$%$m?#RurAC7UmV_OebM?exK{LuQg@wDbK`WB;ZqcE9 z&{mhB!j5^*rOmbqDD`KZ6&LF?H!P~#fd%8cQiF!XbYo%1K@)XE%W9&{qUy1*I>NCn zTrG)KAa=GZ`9Tt6)Ma~jWXF{& zi+R(wx3aMyHto4`ZqD0-6GB_6CQY?2^(M}{z7_B#cWy4|S%SlC3p2%(uu#OGUvyTp zEkWbWbVhfhPF45(pW!={!ge?{wxOB}` zK_VO_Y!Cm?Dkk6t&m@P#&^RSaYKso?G`VMnLMYo9@W+a_t^GQ0IYHxM$=q~;)RJXe zCD&8q%f4$PxM~(06CCCZ3mtXClln}Vd9plidiUY;Y=d`({6Sl#Lj&RCsl^RveC4A+^&e?hJ3bTm_^+U(2>nu#7or8GqF{q!AXg1wqvBvp zHG)kl2F6tr*sS7Ui)sd2RSTFIQpN57?)A!G6^T4yb-`Pz``XY7iV&L*O1Y4DMBXz>L}p?o%0XzuNag^d%bU zfZC7zL3IE;qz;0I)gkbRIt(6FN5HH)3LaBg@VGh#o>0fZlj;O`N}U9A>J&Jla^Pt- z0*u7Nky zb#P4G0LRrBcvFpo6Y3^7sU~P7HL0f5v^uS3)Gc*fom4~W1U`pUe_`sq$(N$~LaxfKIU7gZa7eDoEjha9bq3YewPJ<2zTDeM zu?!sVs$>~HX;!(nQlhq1t5VRN%U4y)K&m2J<(_cBn#G_A`{j<^cE8*on${}XgnlH8zhx8 z3k9Q46Eb+yhmsizQdUmtQnl$Cn5kcG)?+S1&C2nd3grxSRxy!P%QZ^SvvOxOTahb6 znx%5PHhLs#MhVwH+_|o0joQ^$Oy=@h*S0r<`ZlZ57FzxCfbOnl#wJ$f4b!J4;U>3L z_R_Gmz#%xO3==Xuw^(k|xe{rdo@9DwUf1c$m8wcdb$s|?#U(#L>~y0p?Goy;Qjr7? z-l+_Ps+7`eZVq>t{1LCV!nRi&u~Lz}a#s#9tyK>Wt;!y(x}U0{5`Cdx>#8A&=v>rU z+VxO%hJjrM!`v1?%xeYuXV|um0%hRku`VmyUUGe_9Lp(}iu>_R5@=xB2w_@$MFdf0 znKZDna=a5QbXN2+C(=O_>jZ3Y%9i0k3d>E1bkf3dBj#q^uMDW0!qpW$#C}!Atn`%9 z3(0Fm4EH6Lvq%xK&nx6OLq`@my$?aG_`GB}n1WB5m`&nDsERBaWJ4+Fk4s)cIhQHw&xbP6W6fAVNS>Db)rMRvu(|uO= z+b?(5mr*Q)AwF1d-o5wzJMZlk*UMyY zd!+4N1A!A72{A$wAx>x}(63vsm5?AL32g*=76Aw$?l*iSe>I7m1|I82~dig%QdB^)ChC!8SY2M)b$!dK1+rA`w@ z3G_7b&JxZM&J!*WE)p&gE)%X0t`e>ht`lw$#t7qtn*@3od6R@G!ZcxqaEmZYxD5m? zPn!0+HRXA(ho2*fidq_^mO*PZhj@Bl#Jhv~uRoVtbESpLWdo{GLhzYx9zo=po6D95 zjr8+)c6&wF*z|M0mw!}oZgKq1(%l7|J+0f}MRcmP6{K`f)>+BrK7NEp7~@g)U(VQn zcmw-4=j>m+k^Pg$*z3HB{e#EZ-+43p8*gEM<*n>5Ji-3Vlk88tjs20gvp?_@`#taA zzvF54Ti(fj!@JmPyqo=+_po2_UiK>QW54A6>=%51y}}3C&-oDh86RdZ^F8dRd@uV6 z&#)i!ee6ekf9!|+0Q&(y$iB}HvG4K2EZ|4jcllBF9iC<1=EvB#_;L14eu902pJZR> zr`Xqcj(wGnu&?mb?8|(VeTkomeUYDKU*PB1=lOZ|IevkCmS1F_;g{H_`DOMgeuaIK zUuB=**VxDTb@nlSgME~bv5)X^_F;aLeTYx65AsR=0XD^=5C4n5KfV-?wlQxW-n)}E zf_A!aT*r|9s(;G&HuND22K1vM{7FaJTzN(43Of&AkgBMCt%vO+84T850=Ur}6F*OD z)!y`Z4LWP@h*~$>hdAi5o;PI)qOohjI}dlzS^p-euKFP-K1kce6)V4$M~~aydB`mg zq)7hp`^+d<($y}adm%{BWxL%l1JfgKi6G&a8@0L*xif-9DR0+m&5Q?Y`ysZr%2e(Gn8Ag1UJdVd&8% z6(rQkXly>T!MX6`|s2eQk*(mz!+bgSeLN{BGCL!JBt?k=R9|wJp)!{BR wi(i#)VcoW_ + sentor_msgs + 0.1.0 + Sentor message package + + Your Name + MIT + + ament_cmake + rosidl_default_generators + rosidl_default_runtime + + std_msgs + std_srvs + + std_msgs + std_srvs + + rosidl_interface_packages + + + ament_cmake + + diff --git a/srv/GetTopicMaps.srv b/sentor_msgs/srv/GetTopicMaps.srv similarity index 50% rename from srv/GetTopicMaps.srv rename to sentor_msgs/srv/GetTopicMaps.srv index 7e9b712..3754f8b 100644 --- a/srv/GetTopicMaps.srv +++ b/sentor_msgs/srv/GetTopicMaps.srv @@ -1,4 +1,4 @@ std_msgs/Empty empty --- -sentor/TopicMapArray topic_maps +sentor_msgs/TopicMapArray topic_maps bool success diff --git a/setup.py b/setup.py deleted file mode 100644 index 92cb37a..0000000 --- a/setup.py +++ /dev/null @@ -1,9 +0,0 @@ -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['sentor'], - package_dir={'': 'src'}) - -setup(**setup_args) diff --git a/src/sentor/CustomLambdaExample.py b/src/sentor/CustomLambdaExample.py deleted file mode 100644 index aec888e..0000000 --- a/src/sentor/CustomLambdaExample.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python -""" -Created on Fri Nov 20 11:35:22 2020 - -@author: Adam Binch (abinch@sagarobotics.com) -""" -######################################################################################################### -# SENTOR CUSTOM LAMBDA - -def CustomLambda(msg): - return msg.data == "t1-r1-c2" -######################################################################################################### \ No newline at end of file diff --git a/src/sentor/CustomProcessExample.py b/src/sentor/CustomProcessExample.py deleted file mode 100644 index 253ad0c..0000000 --- a/src/sentor/CustomProcessExample.py +++ /dev/null @@ -1,17 +0,0 @@ -#!/usr/bin/env python -""" -Created on Mon Nov 23 16:26:27 2020 - -@author: Adam Binch (abinch@sagarobotics.com) -""" -######################################################################################################### -# SENTOR CUSTOM PROCESS - -class CustomProcess(object): - - def __init__(self, message): - print(message) - - def run(self, message): - print(message) -######################################################################################################### \ No newline at end of file diff --git a/src/sentor/Executor.py b/src/sentor/Executor.py deleted file mode 100644 index 78e93d9..0000000 --- a/src/sentor/Executor.py +++ /dev/null @@ -1,481 +0,0 @@ -#!/usr/bin/env python -""" -Created on Thu Nov 21 10:30:22 2019 - -@author: Adam Binch (abinch@sagarobotics.com) -""" -##################################################################################### -import rospy, rosservice, rostopic, actionlib, subprocess -import dynamic_reconfigure.client -import os, numpy, math -from threading import Lock - - -def _import(location, name): - mod = __import__(location, fromlist=[name]) - return getattr(mod, name) - - -class Executor(object): - - - def __init__(self, config, event_cb): - - self.config = config - self.event_cb = event_cb - - self.init_err_str = "Unable to initialise process of type '{}': {}" - self._lock = Lock() - self.processes = [] - - for process in config: - - process_type = list(process.keys())[0] - - if process_type == "call": - self.init_call(process) - - elif process_type == "publish": - self.init_publish(process) - - elif process_type == "action": - self.init_action(process) - - elif process_type == "sleep": - self.init_sleep(process) - - elif process_type == "shell": - self.init_shell(process) - - elif process_type == "log": - self.init_log(process) - - elif process_type == "reconf": - self.init_reconf(process) - - elif process_type == "lock_acquire": - self.init_lock_acquire(process) - - elif process_type == "lock_release": - self.init_lock_release(process) - - elif process_type == "custom": - self.init_custom(process) - - else: - self.event_cb("Process of type '{}' not supported".format(process_type), "warn") - self.processes.append("not_initialised") - - self.default_indices = range(len(self.processes)) - - - def init_call(self, process): - - try: - service_name = process["call"]["service_name"] - service_name = self.get_name(service_name) - - service_class = rosservice.get_service_class_by_name(service_name) - - timeout_srv = 1.0 - if "timeout" in process["call"]: - timeout_srv = process["call"]["timeout"] - - req = service_class._request_class() - for arg in process["call"]["service_args"]: exec(arg) - - d = {} - d["name"] = "call" - d["verbose"] = self.is_verbose(process["call"]) - d["def_msg"] = ("Calling service '{}'".format(service_name), "info", req) - d["func"] = "self.call(**kwargs)" - d["kwargs"] = {} - d["kwargs"]["service_name"] = service_name - d["kwargs"]["service_class"] = service_class - d["kwargs"]["req"] = req - d["kwargs"]["verbose"] = self.is_verbose(process["call"]) - d["kwargs"]["timeout_srv"] = timeout_srv - - self.processes.append(d) - - except Exception as e: - self.event_cb(self.init_err_str.format("call", str(e)), "warn") - self.processes.append("not_initialised") - - - def init_publish(self, process): - - try: - topic_name = process["publish"]["topic_name"] - topic_name = self.get_name(topic_name) - - topic_latched = False - if "topic_latched" in process["publish"]: - topic_latched = process["publish"]["topic_latched"] - - msg_class, real_topic, _ = rostopic.get_topic_class(topic_name) - pub = rospy.Publisher(real_topic, msg_class, latch=topic_latched, - queue_size=10) - - msg = msg_class() - for arg in process["publish"]["topic_args"]: exec(arg) - - d = {} - d["name"] = "publish" - d["verbose"] = self.is_verbose(process["publish"]) - d["def_msg"] = ("Publishing to topic '{}'".format(topic_name), "info", msg) - d["func"] = "self.publish(**kwargs)" - d["kwargs"] = {} - d["kwargs"]["pub"] = pub - d["kwargs"]["msg"] = msg - - self.processes.append(d) - - except Exception as e: - self.event_cb(self.init_err_str.format("publish", str(e)), "warn") - self.processes.append("not_initialised") - - - def init_action(self, process): - - try: - namespace = process["action"]["namespace"] - package = process["action"]["package"] - spec = process["action"]["action_spec"] - - action_spec = _import(package+".msg", spec) - goal_class = _import(package+".msg", spec[:-6] + "Goal") - - rospy.sleep(1.0) - - action_client = actionlib.SimpleActionClient(namespace, action_spec) - wait = action_client.wait_for_server(rospy.Duration(5.0)) - if not wait: - e = "Action server with namespace '{}' and action specification '{}' not available".format(namespace, spec) - self.event_cb(self.init_err_str.format("action", e), "warn") - self.processes.append("not_initialised") - return - - goal = goal_class() - for arg in process["action"]["goal_args"]: exec(arg) - - d = {} - d["name"] = "action" - d["verbose"] = self.is_verbose(process["action"]) - d["def_msg"] = ("Sending goal for '{}' action with specification '{}'".format(namespace, spec), "info", goal) - d["func"] = "self.action(**kwargs)" - d["kwargs"] = {} - d["kwargs"]["namespace"] = namespace - d["kwargs"]["spec"] = spec - d["kwargs"]["action_client"] = action_client - d["kwargs"]["goal"] = goal - d["kwargs"]["verbose"] = self.is_verbose(process["action"]) - - d["kwargs"]["wait"] = False - if "wait" in process["action"]: - d["kwargs"]["wait"] = process["action"]["wait"] - - self.processes.append(d) - - except Exception as e: - self.event_cb(self.init_err_str.format("action", str(e)), "warn") - self.processes.append("not_initialised") - - - def init_sleep(self, process): - - try: - d = {} - d["name"] = "sleep" - d["verbose"] = self.is_verbose(process["sleep"]) - d["def_msg"] = ("Sentor sleeping for {} seconds".format(process["sleep"]["duration"]), "info", "") - d["func"] = "self.sleep(**kwargs)" - d["kwargs"] = {} - d["kwargs"]["duration"] = process["sleep"]["duration"] - - self.processes.append(d) - - except Exception as e: - self.event_cb(self.init_err_str.format("sleep", str(e)), "warn") - self.processes.append("not_initialised") - - - def init_shell(self, process): - - try: - d = {} - d["name"] = "shell" - d["verbose"] = self.is_verbose(process["shell"]) - d["def_msg"] = ("Executing shell commands {}".format(process["shell"]["cmd_args"]), "info", "") - d["func"] = "self.shell(**kwargs)" - d["kwargs"] = {} - d["kwargs"]["cmd_args"] = process["shell"]["cmd_args"] - d["kwargs"]["shell_features"] = process["shell"]["shell_features"] if "shell_features" in process["shell"] else False - - self.processes.append(d) - - except Exception as e: - self.event_cb(self.init_err_str.format("shell", str(e)), "warn") - self.processes.append("not_initialised") - - - def init_log(self, process): - - try: - d = {} - d["name"] = "log" - d["verbose"] = False - d["func"] = "self.log(**kwargs)" - d["kwargs"] = {} - d["kwargs"]["message"] = process["log"]["message"] - d["kwargs"]["level"] = process["log"]["level"] - - if "msg_args" in process["log"]: - d["kwargs"]["msg_args"] = process["log"]["msg_args"] - else: - d["kwargs"]["msg_args"] = None - - self.processes.append(d) - - except Exception as e: - self.event_cb(self.init_err_str.format("log", str(e)), "warn") - self.processes.append("not_initialised") - - - def init_reconf(self, process): - - try: - params = process["reconf"]["params"] - namespaces = set([param["namespace"] for param in params]) - - default_config = {} - for namespace in namespaces: - rcnfclient = dynamic_reconfigure.client.Client(namespace, timeout=2.0) - default_config[namespace] = rcnfclient.get_configuration() - - default_params = [default_config[param["namespace"]][param["name"]] for param in params] - - d = {} - d["name"] = "reconf" - d["verbose"] = self.is_verbose(process["reconf"]) - d["def_msg"] = ("Reconfiguring parameters: {}".format(params), "info", "") - d["func"] = "self.reconf(**kwargs)" - d["kwargs"] = {} - d["kwargs"]["params"] = params - d["kwargs"]["default_params"] = default_params - - self.processes.append(d) - - except Exception as e: - self.event_cb(self.init_err_str.format("reconf", str(e)), "warn") - self.processes.append("not_initialised") - - - def init_lock_acquire(self, process): - - try: - d = {} - d["name"] = "lock_acquire" - d["verbose"] = False - d["func"] = "self.lock_acquire()" - d["kwargs"] = {} - - self.processes.append(d) - - except Exception as e: - self.event_cb(self.init_err_str.format("lock_acquire", str(e)), "warn") - self.processes.append("not_initialised") - - - def init_lock_release(self, process): - - try: - d = {} - d["name"] = "lock_release" - d["verbose"] = False - d["func"] = "self.lock_release()" - d["kwargs"] = {} - - self.processes.append(d) - - except Exception as e: - self.event_cb(self.init_err_str.format("lock_release", str(e)), "warn") - self.processes.append("not_initialised") - - - def init_custom(self, process): - - try: - package = process["custom"]["package"] - name = process["custom"]["name"] - - _file = name - if "file" in process["custom"]: - _file = process["custom"]["file"] - - custom_proc = _import("{}.{}".format(package, _file), name) - - if "init_args" in process["custom"]: - args = process["custom"]["init_args"] - cp = custom_proc(*args) - else: - cp = custom_proc() - - d = {} - d["name"] = "custom" - d["verbose"] = self.is_verbose(process["custom"]) - d["def_msg"] = ("Executing custom process '{}' from package '{}'".format(name, package), "info", "") - d["func"] = "self.custom(**kwargs)" - d["kwargs"] = {} - d["kwargs"]["cp"] = cp - - d["kwargs"]["args"] = None - if "run_args" in process["custom"]: - d["kwargs"]["args"] = process["custom"]["run_args"] - - self.processes.append(d) - - except Exception as e: - self.event_cb(self.init_err_str.format("custom", str(e)), "warn") - self.processes.append("not_initialised") - - - def get_name(self, name): - - env_name = os.environ.get(name) - if env_name is not None: - name = env_name - - if rospy.has_param(name): - name = rospy.get_param(name) - - return name - - - def is_verbose(self, process): - - verbose = False - if "verbose" in process: - verbose = process["verbose"] - - return verbose - - - def execute(self, msg=None, process_indices=None): - - self.msg = msg - - if process_indices is None: - indices = self.default_indices - else: - indices = process_indices - - for index in indices: - rospy.sleep(0.1) # needed when using slackeros - - process = self.processes[index] - if process == "not_initialised": - continue - - try: - if process["verbose"] and "def_msg" in process: - self.event_cb(process["def_msg"][0], process["def_msg"][1], process["def_msg"][2]) - - kwargs = process["kwargs"] - eval(process["func"]) - - except Exception as e: - self.event_cb("Unable to execute process of type '{}': {}".format(process["name"], str(e)), "warn") - - - def call(self, service_name, service_class, req, verbose, timeout_srv): - - rospy.wait_for_service(service_name, timeout=timeout_srv) - service_client = rospy.ServiceProxy(service_name, service_class) - resp = service_client(req) - - if verbose and resp.success: - self.event_cb("Call to service '{}' succeeded".format(service_name), "info", req) - elif not resp.success: - self.event_cb("Call to service '{}' failed".format(service_name), "warn", req) - - - def publish(self, pub, msg): - pub.publish(msg) - - - def action(self, namespace, spec, action_client, goal, verbose, wait): - - self.action_namespace = namespace - self.spec = spec - self.goal = goal - self.verbose_action = verbose - - action_client.send_goal(goal, self.goal_cb) - - if wait: - action_client.wait_for_result() - - - def sleep(self, duration): - rospy.sleep(duration) - - - def shell(self, cmd_args, shell_features): - - process = subprocess.Popen(cmd_args, - shell=shell_features, - stdout=subprocess.PIPE, - stderr=subprocess.PIPE) - - stdout, stderr = process.communicate() - print(stdout) - - if stderr: - self.event_cb("Unable to execute shell commands {}: {}".format(cmd_args, stderr), "warn") - - - def log(self, message, level, msg_args): - - msg = self.msg - if msg is not None and msg_args is not None: - args = [eval(arg) for arg in msg_args] - self.event_cb("CUSTOM MSG: " + message.format(*args), level) - else: - self.event_cb("CUSTOM MSG: " + message, level) - - - def reconf(self, params, default_params): - - for param, default_param in zip(params, default_params): - rcnfclient = dynamic_reconfigure.client.Client(param["namespace"], timeout=1.0) - - if param["value"] != "_default": - value = param["value"] - else: - value = default_param - - rcnfclient.update_configuration({param["name"]: value}) - - - def lock_acquire(self): - self._lock.acquire() - - - def lock_release(self): - self._lock.release() - - - def custom(self, cp, args): - cp.run(*args) if args is not None else cp.run() - - - def goal_cb(self, status, result): - - if self.verbose_action and status == 3: - self.event_cb("Goal succeeded for '{}' action with specification '{}'".format(self.action_namespace, self.spec), "info", self.goal) - elif status == 2: - self.event_cb("Goal preempted for '{}' action with specification '{}'".format(self.action_namespace, self.spec), "warn", self.goal) - elif status != 3: - self.event_cb("Goal failed for '{}' action with specification '{}'. Status is {}".format(self.action_namespace, self.spec, status), "warn", self.goal) -##################################################################################### \ No newline at end of file diff --git a/src/sentor/MultiMonitor.py b/src/sentor/MultiMonitor.py deleted file mode 100644 index e79e2d5..0000000 --- a/src/sentor/MultiMonitor.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python -""" -Created on Fri Dec 6 08:51:15 2019 - -@author: Adam Binch (abinch@sagarobotics.com) -""" -##################################################################################### -from __future__ import division -import rospy -from sentor.msg import Monitor, MonitorArray -from threading import Event - - -class MultiMonitor(object): - - - def __init__(self): - - rate = rospy.get_param("~safety_pub_rate", 10.0) - - self.topic_monitors = [] - self._stop_event = Event() - self.error_code = [] - - self.monitors_pub = rospy.Publisher("/sentor/monitors", MonitorArray, latch=True, queue_size=1) - rospy.Timer(rospy.Duration(1.0/rate), self.callback) - - - def register_monitors(self, topic_monitor): - self.topic_monitors.append(topic_monitor) - - - def callback(self, event=None): - - if not self._stop_event.isSet(): - - error_code_new = [monitor.conditions[expr]["satisfied"] for monitor in self.topic_monitors for expr in monitor.conditions] - - if error_code_new != self.error_code: - self.error_code = error_code_new - - conditions = MonitorArray() - conditions.header.stamp = rospy.Time.now() - - count = 0 - for monitor in self.topic_monitors: - topic_name = monitor.topic_name - - for expr in monitor.conditions: - condition = Monitor() - condition.topic = topic_name - condition.condition = expr - condition.safety_critical = monitor.conditions[expr]["safety_critical"] - condition.autonomy_critical = monitor.conditions[expr]["autonomy_critical"] - condition.satisfied = self.error_code[count] - condition.tags = monitor.conditions[expr]["tags"] - conditions.conditions.append(condition) - count+=1 - - self.monitors_pub.publish(conditions) - - - def stop_monitor(self): - self._stop_event.set() - - - def start_monitor(self): - self._stop_event.clear() -##################################################################################### \ No newline at end of file diff --git a/src/sentor/ROSTopicFilter.py b/src/sentor/ROSTopicFilter.py deleted file mode 100644 index 228c9c2..0000000 --- a/src/sentor/ROSTopicFilter.py +++ /dev/null @@ -1,92 +0,0 @@ -#!/usr/bin/env python -""" -@author: Francesco Del Duchetto (FDelDuchetto@lincoln.ac.uk) -@author: Adam Binch (abinch@sagarobotics.com) -""" -##################################################################################### -from __future__ import division -import rospy, math, numpy -# imported the packages math and numpy so that they can be used in the lambda expressions - -def _import(location, name): - mod = __import__(location, fromlist=[name]) - return getattr(mod, name) - - -class ROSTopicFilter(object): - - def __init__(self, topic_name, lambda_fn_str, config, throttle_val): - - self.topic_name = topic_name - self.lambda_fn_str = lambda_fn_str - self.config = config - self.throttle_val = throttle_val - self.throttle = self.throttle_val - - self.lambda_fn = None - try: - if config["file"] is not None and config["package"] is not None: - self.lambda_fn = _import("{}.{}".format(config["package"], config["file"]), self.lambda_fn_str) - else: - self.lambda_fn = eval(self.lambda_fn_str) - except Exception as e: - rospy.logerr("Error evaluating lambda function %s : %s" % (self.lambda_fn_str, e)) - - self.filter_satisfied = False - self.unread_satisfied = False - self.value_read = False - self.sat_callbacks = [] - self.unsat_callbacks = [] - - - def callback_filter(self, msg): - - if self.lambda_fn is None: - return - - try: - self.filter_satisfied = self.lambda_fn(msg) - except Exception as e: - rospy.logwarn("Exception while evaluating %s: %s" % (self.lambda_fn_str, e)) - - # if the last value was read: set value_read to False - if self.value_read: - self.value_read = False - elif self.filter_satisfied: - self.unread_satisfied = True - # notify the listeners - - if self.filter_satisfied: - for func in self.sat_callbacks: - func(self.lambda_fn_str, msg, self.config) - else: - for func in self.unsat_callbacks: - func(self.lambda_fn_str) - - - def callback_filter_throttled(self, msg): - - if (self.throttle % self.throttle_val) == 0: - self.callback_filter(msg) - self.throttle = 1 - else: - self.throttle += 1 - - - def is_filter_satisfied(self): - self.value_read = True - - if self.unread_satisfied: - self.unread_satisfied = False - return True - - return self.filter_satisfied - - - def register_satisfied_cb(self, func): - self.sat_callbacks.append(func) - - - def register_unsatisfied_cb(self, func): - self.unsat_callbacks.append(func) -##################################################################################### \ No newline at end of file diff --git a/src/sentor/ROSTopicHz.py b/src/sentor/ROSTopicHz.py deleted file mode 100644 index 7e21742..0000000 --- a/src/sentor/ROSTopicHz.py +++ /dev/null @@ -1,146 +0,0 @@ -#!/usr/bin/env python -""" -@author: Francesco Del Duchetto (FDelDuchetto@lincoln.ac.uk) -@author: Adam Binch (abinch@sagarobotics.com) - -Modified from https://github.com/strawlab/ros_comm/blob/master/tools/rostopic/src/rostopic.py -""" -##################################################################################### -import rospy -import threading -import math - - -class ROSTopicHz(object): - """ - ROSTopicHz receives messages for a topic and computes frequency stats - """ - def __init__(self, topic_name, window_size, throttle_val, filter_expr=None): - self.lock = threading.Lock() - self.last_printed_tn = 0 - self.msg_t0 = -1. - self.msg_tn = 0 - self.times =[] - self.filter_expr = filter_expr - self.topic_name = topic_name - - # can't have infinite window size due to memory restrictions - if window_size < 0: - window_size = 50000 - self.window_size = window_size - - self.throttle_val = throttle_val - self.throttle = self.throttle_val - - - def callback_hz(self, m): - """ - ros sub callback - @param m: Message instance - @type m: roslib.message.Message - """ - # #694: ignore messages that don't match filter - if self.filter_expr is not None and not self.filter_expr(m): - return - with self.lock: - curr_rostime = rospy.get_rostime() - - # time reset - if curr_rostime.is_zero(): - if len(self.times) > 0: - print("time has reset, resetting counters") - self.times = [] - return - - curr = curr_rostime.to_sec() - if self.msg_t0 < 0 or self.msg_t0 > curr: - self.msg_t0 = curr - self.msg_tn = curr - self.times = [] - else: - self.times.append(curr - self.msg_tn) - self.msg_tn = curr - - #only keep statistics for the last 10000 messages so as not to run out of memory - if len(self.times) > self.window_size - 1: - self.times.pop(0) - - - def callback_hz_throttled(self, m): - - if (self.throttle % self.throttle_val) == 0: - self.callback_hz(m) - self.throttle = 1 - else: - self.throttle += 1 - - - def print_hz(self): - """ - print the average publishing rate to screen - """ - if not self.times: - return - elif self.msg_tn == self.last_printed_tn: - print("no new messages") - return - with self.lock: - #frequency - - # kwc: In the past, the rate decayed when a publisher - # dies. Now, we use the last received message to perform - # the calculation. This change was made because we now - # report a count and keep track of last_printed_tn. This - # makes it easier for users to see when a publisher dies, - # so the decay is no longer necessary. - - n = len(self.times) - #rate = (n - 1) / (rospy.get_time() - self.msg_t0) - mean = sum(self.times) / n - rate = 1./mean if mean > 0. else 0 - - #std dev - std_dev = math.sqrt(sum((x - mean)**2 for x in self.times) /n) - - # min and max - max_delta = max(self.times) - min_delta = min(self.times) - - self.last_printed_tn = self.msg_tn - print("average rate: %.3f\n\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s"%(rate, min_delta, max_delta, std_dev, n+1)) - - - def get_hz(self): - """ - return the average publishing rate to screen - """ - if not self.times: - return - elif self.msg_tn == self.last_printed_tn: - # print("no new messages") - return None - with self.lock: - #frequency - - # kwc: In the past, the rate decayed when a publisher - # dies. Now, we use the last received message to perform - # the calculation. This change was made because we now - # report a count and keep track of last_printed_tn. This - # makes it easier for users to see when a publisher dies, - # so the decay is no longer necessary. - - n = len(self.times) - #rate = (n - 1) / (rospy.get_time() - self.msg_t0) - mean = sum(self.times) / n - rate = 1./mean if mean > 0. else 0 - - #std dev - std_dev = math.sqrt(sum((x - mean)**2 for x in self.times) /n) - - # min and max - max_delta = max(self.times) - min_delta = min(self.times) - - self.last_printed_tn = self.msg_tn - return rate -##################################################################################### \ No newline at end of file diff --git a/src/sentor/ROSTopicPub.py b/src/sentor/ROSTopicPub.py deleted file mode 100644 index 3a6a78c..0000000 --- a/src/sentor/ROSTopicPub.py +++ /dev/null @@ -1,36 +0,0 @@ -#!/usr/bin/env python -""" -@author: Francesco Del Duchetto (FDelDuchetto@lincoln.ac.uk) -@author: Adam Binch (abinch@sagarobotics.com) - -""" -##################################################################################### -import rospy - - -class ROSTopicPub(object): - - def __init__(self, topic_name, throttle_val): - - self.topic_name = topic_name - self.pub_callbacks = [] - self.throttle_val = throttle_val - self.throttle = self.throttle_val - - def callback_pub(self, msg): - - for func in self.pub_callbacks: - func("'published'") - - def callback_pub_throttled(self, msg): - - if (self.throttle % self.throttle_val) == 0: - self.callback_pub(msg) - self.throttle = 1 - else: - self.throttle += 1 - - def register_published_cb(self, func): - - self.pub_callbacks.append(func) -##################################################################################### \ No newline at end of file diff --git a/src/sentor/SafetyMonitor.py b/src/sentor/SafetyMonitor.py deleted file mode 100644 index 1629a84..0000000 --- a/src/sentor/SafetyMonitor.py +++ /dev/null @@ -1,106 +0,0 @@ -#!/usr/bin/env python -""" -Created on Fri Dec 6 08:51:15 2019 - -@author: Adam Binch (abinch@sagarobotics.com) -""" -##################################################################################### -from __future__ import division -import rospy -from std_msgs.msg import Bool -from std_srvs.srv import SetBool, SetBoolResponse -from threading import Event - - -class SafetyMonitor(object): - - - def __init__(self, topic, event_msg, attr, srv, event_cb, invert=False): - - timeout = rospy.get_param("~safe_operation_timeout", 10.0) - rate = rospy.get_param("~safety_pub_rate", 10.0) - self.auto_tagging = rospy.get_param("~auto_safety_tagging", True) - - if timeout > 0: - self.timeout = timeout - else: - self.timeout = 0.1 - - self.attr = attr - self.event_cb = event_cb - self.invert = invert - self.topic_monitors = [] - - self.timer = None - self.safe_operation = False - self.safe_msg_sent = False - self.unsafe_msg_sent = False - - self._stop_event = Event() - - self.event_msg = event_msg + ": " - - self.safety_pub = rospy.Publisher(topic, Bool, queue_size=10) - rospy.Timer(rospy.Duration(1.0/rate), self.safety_pub_cb) - - rospy.Service("/sentor/" + srv, SetBool, self.srv_cb) - - - def register_monitors(self, topic_monitor): - self.topic_monitors.append(topic_monitor) - - - def safety_pub_cb(self, event=None): - - if not self._stop_event.isSet(): - - if self.topic_monitors: - threads_are_safe = [getattr(monitor, self.attr) for monitor in self.topic_monitors] - - if self.auto_tagging and all(threads_are_safe) and self.timer is None: - self.timer = rospy.Timer(rospy.Duration.from_sec(self.timeout), self.timer_cb, oneshot=True) - - if not all(threads_are_safe): - if self.timer is not None: - self.timer.shutdown() - self.timer = None - - self.safe_operation = False - if not self.unsafe_msg_sent: - self.event_cb(self.event_msg + "FALSE", "error") - self.safe_msg_sent = False - self.unsafe_msg_sent = True - - if self.invert: - self.safety_pub.publish(Bool(not self.safe_operation)) - else: - self.safety_pub.publish(Bool(self.safe_operation)) - - - def timer_cb(self, event=None): - - self.safe_operation = True - if not self.safe_msg_sent: - self.event_cb(self.event_msg + "TRUE", "info") - self.safe_msg_sent = True - self.unsafe_msg_sent = False - - - def srv_cb(self, req): - - self.safe_operation = req.data - - ans = SetBoolResponse() - ans.success = True - ans.message = self.event_msg + "{}".format(req.data) - - return ans - - - def stop_monitor(self): - self._stop_event.set() - - - def start_monitor(self): - self._stop_event.clear() -##################################################################################### \ No newline at end of file diff --git a/src/sentor/TopicMapServer.py b/src/sentor/TopicMapServer.py deleted file mode 100644 index 6cd6216..0000000 --- a/src/sentor/TopicMapServer.py +++ /dev/null @@ -1,184 +0,0 @@ -#!/usr/bin/env python -""" -Created on Mon Mar 2 10:57:11 2020 - -@author: Adam Binch (abinch@sagarobotics.com) -""" -########################################################################################## -from __future__ import division -import rospy, numpy as np -import os, uuid, pickle, yaml -import matplotlib.pyplot as plt - -from threading import Event -from sentor.msg import TopicMap, TopicMapArray -from sentor.srv import GetTopicMaps, GetTopicMapsResponse -from std_srvs.srv import Trigger, TriggerResponse -from std_srvs.srv import Empty, EmptyResponse - - -class TopicMapServer(object): - - - def __init__(self, topic_mappers, map_pub_rate, map_plt_rate): - - self.base_dir = os.path.join(os.path.expanduser("~"), ".sentor_maps") - if not os.path.exists(self.base_dir): - os.mkdir(self.base_dir) - - self.topic_mappers = topic_mappers - - self._stop_event = Event() - - rospy.Service("/sentor/write_maps", Trigger, self.write_maps) - rospy.Service("/sentor/get_maps", GetTopicMaps, self.get_maps) - rospy.Service("/sentor/clear_maps", Empty, self.clear_maps) - rospy.Service("/sentor/stop_mapping", Empty, self.stop_mapping) - rospy.Service("/sentor/start_mapping", Empty, self.start_mapping) - - if map_pub_rate > 0: - if map_pub_rate > 1: - map_pub_rate = 1 - self.maps_pub = rospy.Publisher('/sentor/topic_maps', TopicMapArray, queue_size=10) - rospy.Timer(rospy.Duration(1.0/map_pub_rate), self.publish_maps) - -# if map_plt_rate > 0: -# if map_plt_rate > 1: -# map_plt_rate = 1 -# rospy.Timer(rospy.Duration(1.0/map_plt_rate), self.plot_maps) - - - def write_maps(self, req): - - message = "Saving maps: " - for mapper in self.topic_mappers: - if mapper.is_instantiated: - - map_dir = os.path.join(self.base_dir, str(uuid.uuid4())) - os.mkdir(map_dir) - - pickle.dump(mapper.map, open(map_dir + "/topic_map.pkl", "wb")) - - with open(map_dir + "/config.yaml",'w') as f: - yaml.dump(mapper.config, f, default_flow_style=False) - - message = message + map_dir + " " - - ans = TriggerResponse() - ans.success = True - ans.message = message - return ans - - - def get_maps(self, req): - - topic_maps = TopicMapArray() - topic_maps = self.fill_msg(topic_maps) - - ans = GetTopicMapsResponse() - ans.topic_maps = topic_maps - ans.success = True - return ans - - - def clear_maps(self, req): - - for mapper in self.topic_mappers: - if mapper.is_instantiated: - mapper.init_map() - - ans = EmptyResponse() - return ans - - - def stop_mapping(self, req): - self.stop() - - rospy.logwarn("topic_mapping_node stopped mapping") - ans = EmptyResponse() - return ans - - - def start_mapping(self, req): - self.start() - - rospy.logwarn("topic_mapping_node started mapping") - ans = EmptyResponse() - return ans - - - def publish_maps(self, event=None): - - if not self._stop_event.isSet(): - - topic_maps = TopicMapArray() - topic_maps = self.fill_msg(topic_maps) - self.maps_pub.publish(topic_maps) - - - def plot_maps(self, event=None): - # broke after move to ubuntu 18 - - if not self._stop_event.isSet(): - - _id = 0 - for mapper in self.topic_mappers: - if mapper.is_instantiated: - - fig_id = "thread " + str(_id) + ": " + mapper.topic_name + " " + mapper.config["arg"] + " " + mapper.config["stat"] - - _map = mapper.map - masked_map = np.ma.array(_map, mask=np.isnan(_map)) - - plt.pause(0.1) - plt.figure(fig_id); plt.clf() - plt.imshow(masked_map.T, interpolation="spline16", origin="lower", - extent=mapper.config["limits"]) - plt.colorbar() - plt.gca().set_aspect("equal", adjustable="box") - plt.tight_layout() - - _id += 1 - - - def fill_msg(self, topic_maps): - - for mapper in self.topic_mappers: - if mapper.is_instantiated: - - map_msg = TopicMap() - map_msg.header.stamp = rospy.Time.now() - map_msg.header.frame_id = mapper.map_frame - map_msg.child_frame_id = mapper.base_frame - map_msg.topic_name = mapper.topic_name - map_msg.topic_arg = mapper.config["arg"] - map_msg.stat = mapper.config["stat"] - map_msg.resolution = mapper.config["resolution"] - map_msg.shape = mapper.shape - map_msg.limits = mapper.config["limits"] - - topic_map = np.ndarray.tolist(np.ravel(mapper.map)) - map_msg.topic_map = topic_map - - topic_maps.topic_maps.append(map_msg) - - return topic_maps - - - def stop(self): - - self._stop_event.set() - - for mapper in self.topic_mappers: - if mapper.is_instantiated: - mapper.stop_mapping() - - - def start(self): - - self._stop_event.clear() - - for mapper in self.topic_mappers: - if mapper.is_instantiated: - mapper.start_mapping() -########################################################################################## \ No newline at end of file diff --git a/src/sentor/TopicMapper.py b/src/sentor/TopicMapper.py deleted file mode 100644 index 493c4c2..0000000 --- a/src/sentor/TopicMapper.py +++ /dev/null @@ -1,277 +0,0 @@ -#!/usr/bin/env python -""" -Created on Tue Feb 25 08:55:41 2020 - -@author: Adam Binch (abinch@sagarobotics.com) -""" -########################################################################################## -from __future__ import division -from threading import Thread, Event -from cv2 import imread - -import rospy, rostopic, tf -import numpy as np, numpy -import math -import yaml, os, subprocess - - -class bcolors: - HEADER = '\033[95m' - OKBLUE = '\033[94m' - OKGREEN = '\033[92m' - WARNING = '\033[93m' - FAIL = '\033[91m' - ENDC = '\033[0m' - BOLD = '\033[1m' - UNDERLINE = '\033[4m' -########################################################################################## - - -########################################################################################## -class TopicMapper(Thread): - - - def __init__(self, config, thread_num): - Thread.__init__(self) - - self.config = config - self.thread_num = thread_num - self.topic_name = config["name"] - - self.map_frame = "map" - if "map_frame" in config: - self.map_frame = config["map_frame"] - - self.base_frame = "base_link" - if "base_frame" in config: - self.base_frame = config["base_frame"] - - self.config["map_frame"] = self.map_frame - self.config["base_frame"] = self.base_frame - - self.set_limits() - self.config["limits"] = [self.x_min, self.x_max, self.y_min, self.y_max] - - self.x_bins = np.arange(self.x_min, self.x_max, config["resolution"]) - self.y_bins = np.arange(self.y_min, self.y_max, config["resolution"]) - - self.nx = self.x_bins.shape[0] + 1 - self.ny = self.y_bins.shape[0] + 1 - - self.shape = [self.nx, self.ny] - self.config["shape"] = self.shape - - self.init_map() - - self.stat = self.set_stat() - - self._stop_event = Event() - - self.tf_listener = tf.TransformListener() - - self.is_instantiated = self.instantiate() - - - def set_limits(self): - - if "map" in self.config: - map_config = yaml.load(file(self.config["map"], 'r')) - dims = imread(os.path.dirname(self.config["map"]) + "/" + map_config["image"]).shape - - self.x_min, self.x_max = map_config["origin"][0], map_config["origin"][0] + (map_config["resolution"] * dims[0]) - self.y_min, self.y_max = map_config["origin"][1], map_config["origin"][1] + (map_config["resolution"] * dims[1]) - - if "limits" in self.config: - self.x_min, self.x_max = self.config["limits"][:2] - self.y_min, self.y_max = self.config["limits"][2:] - - if "map" not in self.config and "limits" not in self.config: - rospy.logerr("No topic map limits specified") - exit() - - - def init_map(self): - - self.obs = np.zeros((self.nx, self.ny)) - self.map = np.zeros((self.nx, self.ny)) - self.map[:] = np.nan - - if self.config["stat"] == "stdev": - self.wma = np.zeros((self.nx, self.ny)) - self.wma[:] = np.nan - - - def set_stat(self): - - if self.config["stat"] == "mean": - stat = self._mean - - elif self.config["stat"] == "sum": - stat = self._sum - - elif self.config["stat"] == "min": - stat = self._min - - elif self.config["stat"] == "max": - stat = self._max - - elif self.config["stat"] == "stdev": - stat = self._stdev - - else: - rospy.logerr("Statistic of type '{}' not supported".format(self.config["stat"])) - exit() - - return stat - - - def _mean(self, z, N): - return self.weighted_mean(z, self.topic_arg, N) - - - def _sum(self, z, N): - return z + self.topic_arg - - - def _min(self, z, N): - return np.min([z, self.topic_arg]) - - - def _max(self, z, N): - return np.max([z, self.topic_arg]) - - - def _stdev(self, z, N): - wm = self.wma[self.ix, self.iy] - if np.isnan(wm): wm=0 - - wm = self.weighted_mean(wm, self.topic_arg, N) - self.wma[self.ix, self.iy] = wm - - return np.sqrt(self.weighted_mean(z**2, (wm-self.topic_arg)**2, N)) - - - def weighted_mean(self, m, x, N): - return (1/N) * ((m * (N-1)) + x) - - - def instantiate(self): - - try: - msg_class, real_topic, _ = rostopic.get_topic_class(self.topic_name, blocking=False) - topic_type, _, _ = rostopic.get_topic_type(self.topic_name, blocking=False) - except rostopic.ROSTopicException: - rospy.logerr("Topic {} type cannot be determined, or ROS master cannot be contacted".format(self.topic_name)) - return False - - if real_topic is None: - rospy.logerr("Topic {} is not published".format(self.topic_name)) - return False - - print("Mapping topic arg "+ bcolors.OKGREEN + self.config["arg"] + bcolors.ENDC +" on topic "+ bcolors.OKBLUE + self.topic_name + bcolors.ENDC + '\n') - - rate = 0 - if "rate" in self.config: - rate = self.config["rate"] - - if rate > 0: - COMMAND_BASE = ["rosrun", "topic_tools", "throttle"] - subscribed_topic = "/sentor/mapping/" + str(self.thread_num) + real_topic - - command = COMMAND_BASE + ["messages", real_topic, str(rate), subscribed_topic] - subprocess.Popen(command, stdout=open(os.devnull, "wb")) - else: - subscribed_topic = real_topic - - self.throttle_val = 0 - if "N" in self.config: - self.throttle_val = int(self.config["N"]) - - self.throttle = self.throttle_val - if self.throttle_val <= 0: - cb = self.topic_cb - else: - cb = self.topic_cb_throttled - - rospy.Subscriber(subscribed_topic, msg_class, cb) - - return True - - - def topic_cb(self, msg): - - if not self._stop_event.isSet(): - - try: - x, y = self.get_transform() - except: - rospy.logwarn("Failed to get a tf transform between {} and {}".format(self.map_frame, self.base_frame)) - return - - if self.x_min <= x <= self.x_max and self.y_min <= y <= self.y_max: - valid_arg = self.process_arg(msg) - - if valid_arg: - self.update_map(x, y) - - - def topic_cb_throttled(self, msg): - - if (self.throttle % self.throttle_val) == 0: - self.topic_cb(msg) - self.throttle = 1 - else: - self.throttle += 1 - - - def get_transform(self): - - now = rospy.Time(0) - self.tf_listener.waitForTransform(self.map_frame, self.base_frame, now, rospy.Duration(1.0)) - (trans,rot) = self.tf_listener.lookupTransform(self.map_frame, self.base_frame, now) - - return trans[0], trans[1] - - - def process_arg(self, msg): - - try: - self.topic_arg = eval(self.config["arg"]) - except Exception as e: - rospy.logwarn("Exception while evaluating '{}': {}".format(self.config["arg"], e)) - return False - - valid_arg = True - if isinstance(self.topic_arg, bool): - self.topic_arg = int(self.topic_arg) - elif not isinstance(self.topic_arg, float) and not isinstance(self.topic_arg, int): - rospy.logwarn("Topic arg '{}' of {} on topic '{}' cannot be processed".format(self.config["arg"], type(self.topic_arg), self.topic_name)) - valid_arg = False - - return valid_arg - - - def update_map(self, x, y): - - ix = np.digitize(x, self.x_bins) - iy = np.digitize(y, self.y_bins) - self.ix = ix - self.iy = iy - - self.obs[ix, iy] += 1 - N = self.obs[ix, iy] - - z = self.map[ix, iy] - if np.isnan(z): z=0 - - z = self.stat(z, N) - self.map[ix, iy] = z - - - def stop_mapping(self): - self._stop_event.set() - - - def start_mapping(self): - self._stop_event.clear() -########################################################################################## \ No newline at end of file diff --git a/src/sentor/TopicMonitor.py b/src/sentor/TopicMonitor.py deleted file mode 100644 index 30a2e71..0000000 --- a/src/sentor/TopicMonitor.py +++ /dev/null @@ -1,512 +0,0 @@ -#!/usr/bin/env python -""" -@author: Francesco Del Duchetto (FDelDuchetto@lincoln.ac.uk) -@author: Adam Binch (abinch@sagarobotics.com) -""" -##################################################################################### -from sentor.ROSTopicHz import ROSTopicHz -from sentor.ROSTopicFilter import ROSTopicFilter -from sentor.ROSTopicPub import ROSTopicPub -from sentor.Executor import Executor - -from threading import Thread, Event, Lock -import socket -import rostopic -import rosgraph -import rospy -import time -import subprocess -import os - -class bcolors: - HEADER = '\033[95m' - OKBLUE = '\033[94m' - OKGREEN = '\033[92m' - WARNING = '\033[93m' - FAIL = '\033[91m' - ENDC = '\033[0m' - BOLD = '\033[1m' - UNDERLINE = '\033[4m' -########################################################################################## - - -########################################################################################## -class TopicMonitor(Thread): - - - def __init__(self, topic_name, rate, N, signal_when_config, signal_lambdas_config, processes, - timeout, default_notifications, event_callback, thread_num): - Thread.__init__(self) - - self.topic_name = topic_name - self.rate = rate - self.N = N - self.signal_when_config = signal_when_config - self.signal_lambdas_config = signal_lambdas_config - self.processes = processes - if timeout > 0: - self.timeout = timeout - else: - self.timeout = 0.1 - self.default_notifications = default_notifications - self._event_callback = event_callback - self.thread_num = thread_num - - self.independent_tags = rospy.get_param("~independent_tags", False) - - self.signal_when_is_safe = True - self.lambdas_are_safe = True - self.thread_is_safe = True - - self.signal_when_is_auto = True - self.lambdas_are_auto = True - self.thread_is_auto = True - - self.nodes = [] - self.sat_crit_expressions = [] - self.sat_auto_expressions = [] - self.sat_expressions_timer = {} - self.sat_expr_repeat_timer = {} - self.conditions = {} - - self.process_signal_config() - - if processes: - self.executor = Executor(processes, self.event_callback) - - self._stop_event = Event() - self._killed_event = Event() - self._lock = Lock() - - self.pub_monitor = None - self.hz_monitor = None - self.is_topic_published = True - self.is_instantiated = False - self.is_instantiated = self._instantiate_monitors() - - - def _instantiate_monitors(self): - if self.is_instantiated: return True - - try: - msg_class, real_topic, _ = rostopic.get_topic_class(self.topic_name, blocking=False) - topic_type, _, _ = rostopic.get_topic_type(self.topic_name, blocking=False) - except rostopic.ROSTopicException as e: - self.event_callback("Topic %s type cannot be determined, or ROS master cannot be contacted" % self.topic_name, "warn") - return False - - if real_topic is None: - self.event_callback("Topic %s is not published" % self.topic_name, "warn") - if self.signal_when_cfg["signal_when"].lower() == 'not published' and self.signal_when_cfg["safety_critical"]: - self.signal_when_is_safe = False - return False - - # if rate > 0 set in config then throttle topic at that rate - if self.rate > 0: - COMMAND_BASE = ["rosrun", "topic_tools", "throttle"] - subscribed_topic = "/sentor/monitoring/" + str(self.thread_num) + real_topic - - command = COMMAND_BASE + ["messages", real_topic, str(self.rate), subscribed_topic] - subprocess.Popen(command, stdout=open(os.devnull, "wb")) - else: - subscribed_topic = real_topic - - # find out topic publishing nodes - master = rosgraph.Master(rospy.get_name()) - try: - pubs, _ = rostopic.get_topic_list(master=master) - # filter based on topic - pubs = [x for x in pubs if x[0] == real_topic] - nodes = [] - for _, _, _nodes in pubs: - nodes += _nodes - self.nodes = nodes - except socket.error: - self.event_callback("Could not retrieve nodes for topic %s" % self.topic_name, "warn") - - # Do we need a hz monitor? - hz_monitor_required = False - if self.signal_when_cfg["signal_when"].lower() == 'not published': - hz_monitor_required = True - for signal_lambda in self.signal_lambdas_config: - if "when_published" in signal_lambda: - if signal_lambda["when_published"]: - hz_monitor_required = True - - if hz_monitor_required: - self.hz_monitor = self._instantiate_hz_monitor(subscribed_topic, self.topic_name, msg_class) - - if self.signal_when_cfg["signal_when"].lower() == 'published': - print("Signaling 'published' for "+ bcolors.OKBLUE + self.topic_name + bcolors.ENDC +" initialized") - self.pub_monitor = self._instantiate_pub_monitor(subscribed_topic, self.topic_name, msg_class) - self.pub_monitor.register_published_cb(self.published_cb) - - if self.signal_when_cfg["safety_critical"]: - self.signal_when_is_safe = False - - elif self.signal_when_cfg["signal_when"].lower() == 'not published': - print("Signaling 'not published' for "+ bcolors.BOLD + str(self.signal_when_cfg["timeout"]) + " seconds" + bcolors.ENDC +" for " + bcolors.OKBLUE + self.topic_name + bcolors.ENDC +" initialized") - - if len(self.signal_lambdas_config): - print("Signaling expressions for "+ bcolors.OKBLUE + self.topic_name + bcolors.ENDC + ":") - - self.lambda_monitor_list = [] - for signal_lambda in self.signal_lambdas_config: - - lambda_fn_str = signal_lambda["expression"] - lambda_config = self.process_lambda_config(signal_lambda) - - if lambda_fn_str != "": - print("\t" + bcolors.OKGREEN + lambda_fn_str + bcolors.ENDC + " ("+ bcolors.BOLD+"timeout: %s seconds" % lambda_config["timeout"] + bcolors.ENDC +")") - lambda_monitor = self._instantiate_lambda_monitor(subscribed_topic, msg_class, lambda_fn_str, lambda_config) - - # register cb that notifies when the lambda function is True - lambda_monitor.register_satisfied_cb(self.lambda_satisfied_cb) - lambda_monitor.register_unsatisfied_cb(self.lambda_unsatisfied_cb) - - self.lambda_monitor_list.append(lambda_monitor) - print("") - - self.is_instantiated = True - - return True - - - def event_callback(self, string, type, msg=""): - self._event_callback(string, type, msg, self.nodes, self.topic_name) - - - def process_signal_config(self): - - self.signal_when_cfg = {} - self.signal_when_cfg["signal_when"] = "" - self.signal_when_cfg["timeout"] = self.timeout - self.signal_when_cfg["safety_critical"] = False - self.signal_when_cfg["autonomy_critical"] = False - self.signal_when_cfg["default_notifications"] = self.default_notifications - self.signal_when_cfg["process_indices"] = None - self.signal_when_cfg["repeat_exec"] = False - self.signal_when_cfg["tags"] = [] - self.signal_when_cfg["N"] = self.N - - if type(self.signal_when_config) is str: - self.signal_when_cfg["signal_when"] = self.signal_when_config - elif type(self.signal_when_config) is dict: - - if "condition" in self.signal_when_config: - self.signal_when_cfg["signal_when"] = self.signal_when_config["condition"] - if "timeout" in self.signal_when_config: - self.signal_when_cfg["timeout"] = self.signal_when_config["timeout"] - if "safety_critical" in self.signal_when_config: - self.signal_when_cfg["safety_critical"] = self.signal_when_config["safety_critical"] - if "autonomy_critical" in self.signal_when_config: - self.signal_when_cfg["autonomy_critical"] = self.signal_when_config["autonomy_critical"] - if "default_notifications" in self.signal_when_config: - self.signal_when_cfg["default_notifications"] = self.signal_when_config["default_notifications"] - if "process_indices" in self.signal_when_config: - self.signal_when_cfg["process_indices"] = self.signal_when_config["process_indices"] - if "repeat_exec" in self.signal_when_config: - self.signal_when_cfg["repeat_exec"] = self.signal_when_config["repeat_exec"] - if "tags" in self.signal_when_config: - self.signal_when_cfg["tags"] = self.signal_when_config["tags"] - if "N" in self.signal_when_config: - self.signal_when_cfg["N"] = int(self.signal_when_config["N"]) - - if self.signal_when_cfg["timeout"] <= 0: - self.signal_when_cfg["timeout"] = 0.1 - - # for publishing to sentor/monitors - if self.signal_when_cfg["signal_when"].lower() == "not published" \ - or self.signal_when_cfg["signal_when"].lower() == "published": - d = {} - d["satisfied"] = False - d["safety_critical"] = self.signal_when_cfg["safety_critical"] - d["autonomy_critical"] = self.signal_when_cfg["autonomy_critical"] - d["tags"] = self.signal_when_cfg["tags"] - self.conditions[self.signal_when_cfg["signal_when"]] = d - - - def process_lambda_config(self, signal_lambda): - - lambda_config = {} - lambda_config["expr"] = "" - lambda_config["file"] = None - lambda_config["package"] = None - lambda_config["timeout"] = self.timeout - lambda_config["safety_critical"] = False - lambda_config["autonomy_critical"] = False - lambda_config["default_notifications"] = self.default_notifications - lambda_config["when_published"] = False - lambda_config["process_indices"] = None - lambda_config["repeat_exec"] = False - lambda_config["tags"] = [] - lambda_config["N"] = self.N - - if "expression" in signal_lambda: - lambda_config["expr"] = signal_lambda["expression"] - if "file" in signal_lambda: - lambda_config["file"] = signal_lambda["file"] - if "package" in signal_lambda: - lambda_config["package"] = signal_lambda["package"] - if "timeout" in signal_lambda: - lambda_config["timeout"] = signal_lambda["timeout"] - if "safety_critical" in signal_lambda: - lambda_config["safety_critical"] = signal_lambda["safety_critical"] - if "autonomy_critical" in signal_lambda: - lambda_config["autonomy_critical"] = signal_lambda["autonomy_critical"] - if "default_notifications" in signal_lambda: - lambda_config["default_notifications"] = signal_lambda["default_notifications"] - if "when_published" in signal_lambda: - lambda_config["when_published"] = signal_lambda["when_published"] - if "process_indices" in signal_lambda: - lambda_config["process_indices"] = signal_lambda["process_indices"] - if "repeat_exec" in signal_lambda: - lambda_config["repeat_exec"] = signal_lambda["repeat_exec"] - if "tags" in signal_lambda: - lambda_config["tags"] = signal_lambda["tags"] - if "N" in signal_lambda: - lambda_config["N"] = int(signal_lambda["N"]) - - if lambda_config["timeout"] <= 0: - lambda_config["timeout"] = 0.1 - - # for publishing to sentor/monitors - if lambda_config["expr"]: - d = {} - d["satisfied"] = False - d["safety_critical"] = lambda_config["safety_critical"] - d["autonomy_critical"] = lambda_config["autonomy_critical"] - d["tags"] = lambda_config["tags"] - self.conditions[lambda_config["expr"]] = d - - return lambda_config - - - def _instantiate_hz_monitor(self, subscribed_topic, topic_name, msg_class): - hz = ROSTopicHz(topic_name, 1000, self.signal_when_cfg["N"]) - - if self.signal_when_cfg["N"] <= 0: - cb = hz.callback_hz - else: - cb = hz.callback_hz_throttled - - rospy.Subscriber(subscribed_topic, msg_class, cb) - - return hz - - - def _instantiate_pub_monitor(self, subscribed_topic, topic_name, msg_class): - pub = ROSTopicPub(topic_name, self.signal_when_cfg["N"]) - - if self.signal_when_cfg["N"] <= 0: - cb = pub.callback_pub - else: - cb = pub.callback_pub_throttled - - rospy.Subscriber(subscribed_topic, msg_class, cb) - - return pub - - - def _instantiate_lambda_monitor(self, subscribed_topic, msg_class, lambda_fn_str, lambda_config): - filter = ROSTopicFilter(self.topic_name, lambda_fn_str, lambda_config, lambda_config["N"]) - - if lambda_config["N"] <= 0: - cb = filter.callback_filter - else: - cb = filter.callback_filter_throttled - - rospy.Subscriber(subscribed_topic, msg_class, cb) - - return filter - - - def run(self): - # if the topic was not published initially then no monitor is running - # but, maybe now it is published - if not self.is_instantiated: - if not self._instantiate_monitors(): - return - else: - self.is_instantiated = True - - def cb(_): - if self.signal_when_cfg["signal_when"].lower() == 'not published': - self.conditions[self.signal_when_cfg["signal_when"]]["satisfied"] = True - - if self.signal_when_cfg["safety_critical"]: - self.signal_when_is_safe = False - if self.signal_when_cfg["autonomy_critical"]: - self.signal_when_is_auto = False - if self.signal_when_cfg["default_notifications"] and self.signal_when_cfg["safety_critical"]: - self.event_callback("SAFETY CRITICAL: Topic %s is not published anymore" % self.topic_name, "error") - elif self.signal_when_cfg["default_notifications"]: - self.event_callback("Topic %s is not published anymore" % self.topic_name, "warn") - if not self.signal_when_cfg["repeat_exec"]: - self.execute(process_indices=self.signal_when_cfg["process_indices"]) - - def repeat_cb(_): - if self.signal_when_cfg["signal_when"].lower() == 'not published': - self.execute(process_indices=self.signal_when_cfg["process_indices"]) - - timer = None - timer_repeat = None - while not self._killed_event.isSet(): - while not self._stop_event.isSet(): - - self.thread_is_safe = self.signal_when_is_safe and self.lambdas_are_safe - if not self.independent_tags: - self.thread_is_auto = self.thread_is_safe and self.signal_when_is_auto and self.lambdas_are_auto - else: - self.thread_is_auto = self.signal_when_is_auto and self.lambdas_are_auto - - # check it is still published (None if not) - if self.hz_monitor is not None: - rate = self.hz_monitor.get_hz() - - if rate is None and self.is_topic_published: - self.is_topic_published = False - - timer = rospy.Timer(rospy.Duration.from_sec(self.signal_when_cfg["timeout"]), cb, oneshot=True) - - if self.signal_when_cfg["repeat_exec"]: - timer_repeat = rospy.Timer(rospy.Duration.from_sec(self.signal_when_cfg["timeout"]), repeat_cb, oneshot=False) - - if rate is not None: - self.is_topic_published = True - - if self.signal_when_cfg["signal_when"].lower() == 'not published': - self.conditions[self.signal_when_cfg["signal_when"]]["satisfied"] = False - - if self.signal_when_cfg["safety_critical"]: - self.signal_when_is_safe = True - - if self.signal_when_cfg["autonomy_critical"]: - self.signal_when_is_auto = True - - if timer is not None: - timer.shutdown() - timer = None - - if self.signal_when_cfg["repeat_exec"]: - if timer_repeat is not None: - timer_repeat.shutdown() - timer_repeat = None - - time.sleep(0.3) - time.sleep(1) - - - def lambda_satisfied_cb(self, expr, msg, config): - - def ProcessLambda(timer_dict): - process_lambda = True - if config["when_published"] and not self.is_topic_published: - process_lambda = False - timer_dict = self.kill_timer(timer_dict, config["expr"]) - return process_lambda, timer_dict - - if not self._stop_event.isSet(): - if not expr in self.sat_expressions_timer: - - def cb(_): - process_lambda, self.sat_expressions_timer = ProcessLambda(self.sat_expressions_timer) - if process_lambda: - if config["safety_critical"]: - self.lambdas_are_safe = False - self.sat_crit_expressions.append(config["expr"]) - - if config["autonomy_critical"]: - self.lambdas_are_auto = False - self.sat_auto_expressions.append(config["expr"]) - - self.conditions[config["expr"]]["satisfied"] = True - if config["default_notifications"]: - if config["safety_critical"]: - self.event_callback("SAFETY CRITICAL: Expression '%s' for %s seconds on topic %s satisfied" % (expr, config["timeout"], self.topic_name), "error", msg) - else: - self.event_callback("Expression '%s' for %s seconds on topic %s satisfied" % (expr, config["timeout"], self.topic_name), "warn", msg) - - if not config["repeat_exec"]: - self.execute(msg, config["process_indices"]) - - self._lock.acquire() - self.sat_expressions_timer.update({expr: rospy.Timer(rospy.Duration.from_sec(config["timeout"]), cb, oneshot=True)}) - self._lock.release() - - if config["repeat_exec"]: - if not expr in self.sat_expr_repeat_timer: - - def repeat_cb(_): - process_lambda, self.sat_expr_repeat_timer = ProcessLambda(self.sat_expr_repeat_timer) - if process_lambda: - self.execute(msg, config["process_indices"]) - self.sat_expr_repeat_timer = self.kill_timer(self.sat_expr_repeat_timer, config["expr"]) - - self._lock.acquire() - self.sat_expr_repeat_timer.update({expr: rospy.Timer(rospy.Duration.from_sec(config["timeout"]), repeat_cb, oneshot=True)}) - self._lock.release() - - - def lambda_unsatisfied_cb(self, expr): - if not self._stop_event.isSet(): - if expr in self.sat_expressions_timer: - self.sat_expressions_timer = self.kill_timer(self.sat_expressions_timer, expr) - self.conditions[expr]["satisfied"] = False - - if expr in self.sat_expr_repeat_timer: - self.sat_expr_repeat_timer = self.kill_timer(self.sat_expr_repeat_timer, expr) - - if expr in self.sat_crit_expressions: - self.sat_crit_expressions.remove(expr) - - if expr in self.sat_auto_expressions: - self.sat_auto_expressions.remove(expr) - - if not self.sat_crit_expressions: - self.lambdas_are_safe = True - - if not self.sat_auto_expressions: - self.lambdas_are_auto = True - - - def published_cb(self, msg): - if not self._stop_event.isSet(): - self.conditions[self.signal_when_cfg["signal_when"]]["satisfied"] = True - if self.signal_when_cfg["safety_critical"]: - self.signal_when_is_safe = False - if self.signal_when_cfg["autonomy_critical"]: - self.signal_when_is_auto = False - if self.signal_when_cfg["default_notifications"] and self.signal_when_cfg["safety_critical"]: - self.event_callback("SAFETY CRITICAL: Topic %s is published " % (self.topic_name), "error") - elif self.signal_when_cfg["default_notifications"]: - self.event_callback("Topic %s is published " % (self.topic_name), "warn") - #self.execute(msg, self.signal_when_cfg["process_indices"]) - - - def kill_timer(self, timer_dict, expr): - self._lock.acquire() - timer_dict[expr].shutdown() - timer_dict.pop(expr) - self._lock.release() - return timer_dict - - - def execute(self, msg=None, process_indices=None): - if self.processes: - rospy.sleep(0.1) # needed when using slackeros - self.executor.execute(msg, process_indices) - - - def stop_monitor(self): - self._stop_event.set() - - - def start_monitor(self): - self._stop_event.clear() - - - def kill_monitor(self): - self.stop_monitor() - self._killed_event.set() -########################################################################################## \ No newline at end of file From 719f67d211557ec97484829684b06c80a748ec58 Mon Sep 17 00:00:00 2001 From: Cyano0 <36192489+Cyano0@users.noreply.github.com> Date: Mon, 19 May 2025 16:10:12 +0100 Subject: [PATCH 02/21] Update node monitor --- sentor/config/test_monitor_config.yaml | 160 +++++--- sentor/scripts/sentor_node.py | 102 +++++ sentor/scripts/test_sentor.py | 522 ++++++++++++++++++------- sentor/sentor/CustomLambdaExample.py | 49 +++ sentor/sentor/CustomProcessExample.py | 49 +++ sentor/sentor/MultiMonitor.py | 261 ++++++++++++- sentor/sentor/NodeMonitor.py | 85 ++++ sentor/sentor/ROSTopicFilter.py | 14 +- sentor/sentor/ROSTopicHz.py | 56 ++- sentor/sentor/SafetyMonitor.py | 128 ++++++ sentor/sentor/TopicMonitor.py | 326 +++++++-------- sentor/sentor/test_executer.py | 77 ---- 12 files changed, 1371 insertions(+), 458 deletions(-) create mode 100755 sentor/scripts/sentor_node.py create mode 100644 sentor/sentor/CustomLambdaExample.py create mode 100644 sentor/sentor/CustomProcessExample.py create mode 100644 sentor/sentor/NodeMonitor.py create mode 100644 sentor/sentor/SafetyMonitor.py delete mode 100644 sentor/sentor/test_executer.py diff --git a/sentor/config/test_monitor_config.yaml b/sentor/config/test_monitor_config.yaml index 978dd1f..ab5c860 100644 --- a/sentor/config/test_monitor_config.yaml +++ b/sentor/config/test_monitor_config.yaml @@ -1,70 +1,138 @@ -monitors: -# - name: "/example_topic" -# message_type: "std_msgs/msg/Int32" -# rate: 10 # optional throttling (messages per second) -# N: 5 -# qos: -# reliability: "reliable" -# durability: "volatile" -# depth: 10 -# signal_when: -# condition: "published" -# timeout: 2.0 -# safety_critical: false -# signal_lambdas: -# - expression: "lambda x: x.data > 10" -# timeout: 3.0 -# safety_critical: true -# autonomy_critical: false -# tags: ["example"] -# execute: [] -# default_notifications: true +monitors: #[] + # - name: "/example_topic" + # message_type: "std_msgs/msg/Int32" + # rate: 10 + # N: 5 + # qos: + # reliability: "reliable" + # durability: "volatile" + # depth: 10 + # signal_when: + # condition: "published" + # timeout: 2.0 + # safety_critical: false + # signal_lambdas: + # - expression: "CustomLambda" + # file: "CustomLambdaExample" + # package: "sentor" + # safety_critical: False + # process_indices: [1] + # repeat_exec: False + # tags: ["navigation"] + # when_published: False + # execute: + # - log: + # message: "Lambda condition met: value > 10 on /example_topic" + # level: "warn" + # default_notifications: true + # enable_internal_logs: false - - name: "/example_topic" - message_type: "std_msgs/msg/Int32" - rate: 10 + + # - name: "/not_published_topic" + # message_type: "std_msgs/msg/Int32" + # rate: 10 + # N: 5 + # qos: + # reliability: "best effort" + # durability: "volatile" + # depth: 10 + # signal_when: + # condition: "not published" + # timeout: 2.0 + # safety_critical: false + # signal_lambdas: + # - expression: "lambda x: x.data > 10" + # timeout: 3.0 + # safety_critical: true + # autonomy_critical: false + # tags: ["test"] + # process_indices: [0] # Add process indices here + # execute: + # - log: + # message: "Lambda condition met: value > 10 on /example_topic" + # level: "warn" + # default_notifications: true + # enable_internal_logs: false + + # - name: "/test_topic_2" + # message_type: "sensor_msgs/msg/Temperature" + # rate: 2 + # N: 5 + # qos: + # reliability: "best_effort" + # durability: "transient_local" + # depth: 5 + # signal_when: + # condition: "published" + # timeout: 2 + # safety_critical: False + # signal_lambdas: + # - expression: "lambda x: x.temperature < 15" + # timeout: 2 + # safety_critical: True + # process_indices: [2] + # execute: [] + # timeout: 10 + # default_notifications: False + + - name: "/front_camera/camera_info" + message_type: "sensor_msgs/msg/CameraInfo" + rate: 8 N: 5 qos: reliability: "reliable" - durability: "volatile" - depth: 10 + durability: "VOLATILE" + depth: 5 + signal_when: condition: "published" - timeout: 2.0 - safety_critical: false + timeout: 30 + safety_critical: false # ← leave false if this “published” check shouldn’t gate safety‐beat + autonomy_critical: false # ← set true if you want mere “published” to gate warning‐beat + signal_lambdas: - - expression: "lambda x: x.data > 10" - timeout: 3.0 - safety_critical: true - autonomy_critical: false - tags: ["example"] - process_indices: [0] # Add process indices here - execute: - - log: - message: "Lambda condition met: value > 10 on /example_topic" - level: "warn" - default_notifications: true - enable_internal_logs: false + - expression: "lambda x: x.height < 480" + timeout: 2 + safety_critical: True # ← height‐check lambda counts for safety‐beat + autonomy_critical: False # ← but does *not* count for warning‐beat + process_indices: [2] + execute: [] + timeout: 10 + default_notifications: False - - name: "/test_topic_2" - message_type: "sensor_msgs/msg/Temperature" - rate: 2 + - name: "/back_camera/camera_info" + message_type: "sensor_msgs/msg/CameraInfo" + rate: 8 N: 5 qos: - reliability: "best_effort" - durability: "transient_local" + reliability: "reliable" + durability: "VOLATILE" depth: 5 signal_when: condition: "published" timeout: 2 safety_critical: False + autonomy_critical: false signal_lambdas: - - expression: "lambda x: x.temperature < 15" + - expression: "lambda x: x.height == 480" timeout: 2 safety_critical: True + autonomy_critical: True + process_indices: [2] execute: [] timeout: 10 default_notifications: False +node_monitors: + - name: "front_camera_camera_controller" + timeout: 2.0 + safety_critical: true + autonomy_critical: false + + - name: "back_camera_camera_controller" + timeout: 1.0 + safety_critical: false + autonomy_critical: true + diff --git a/sentor/scripts/sentor_node.py b/sentor/scripts/sentor_node.py new file mode 100755 index 0000000..487a70e --- /dev/null +++ b/sentor/scripts/sentor_node.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python +import rclpy +import yaml +import time +import signal +import os +from sentor.TopicMonitor import TopicMonitor +from sentor.TopicMonitor import TopicMonitor +from sentor.MultiMonitor import MultiMonitor +from std_msgs.msg import String +from sentor_msgs.msg import SentorEvent +from std_srvs.srv import Empty + +topic_monitors = [] +event_pub = None +rich_event_pub = None + +def __signal_handler(signum, frame): + """ Gracefully stop all monitors on SIGINT. """ + for topic_monitor in topic_monitors: + topic_monitor.kill_monitor() + multi_monitor.stop_monitor() + print("stopped.") + os._exit(signal.SIGTERM) + +def stop_monitoring(_): + """ Stop all monitoring activities. """ + for topic_monitor in topic_monitors: + topic_monitor.stop_monitor() + multi_monitor.stop_monitor() + return Empty.Response() + +def start_monitoring(_): + """ Start monitoring activities. """ + for topic_monitor in topic_monitors: + topic_monitor.start_monitor() + multi_monitor.start_monitor() + return Empty.Response() + +if __name__ == "__main__": + signal.signal(signal.SIGINT, __signal_handler) + rclpy.init() + node = rclpy.create_node("sentor") + + # 🔹 Load Configuration + config_file = node.declare_parameter("config_file", "").value + topics = [] + + try: + items = [yaml.safe_load(open(item, 'r')) for item in config_file.split(',')] + topics = [item for sublist in items for item in sublist] + except Exception as e: + node.get_logger().error(f"No configuration file provided: {e}") + + stop_srv = node.create_service(Empty, '/sentor/stop_monitor', stop_monitoring) + start_srv = node.create_service(Empty, '/sentor/start_monitor', start_monitoring) + + event_pub = node.create_publisher(String, '/sentor/event', 10) + rich_event_pub = node.create_publisher(SentorEvent, '/sentor/rich_event', 10) + + multi_monitor = MultiMonitor() # 🔹 MultiMonitor no longer creates monitors! + + topic_monitors = [] + print("Monitoring topics:") + for i, topic in enumerate(topics): + if topic.get("include", True) is False: + continue # Skip topics marked as "include: False" + + try: + topic_name = topic["name"] + except KeyError: + node.get_logger().error(f"Topic name not specified for entry: {topic}") + continue + + # Extract parameters with defaults + topic_monitor = TopicMonitor( + topic_name=topic_name, + msg_type=multi_monitor.get_message_type(topic["message_type"]), + qos_profile=multi_monitor.get_qos_profile(topic.get("qos", {})), + rate=topic.get("rate", 0), + N=topic.get("N", 0), + signal_when=topic.get("signal_when", {}), + signal_lambdas=topic.get("signal_lambdas", []), + processes=topic.get("execute", []), + timeout=topic.get("timeout", 0), + default_notifications=topic.get("default_notifications", True), + event_callback=None, + thread_num=i, + ) + + topic_monitors.append(topic_monitor) + multi_monitor.register_monitors(topic_monitor) # 🔹 We now pass monitors to MultiMonitor + + time.sleep(1) + + # Start monitoring + for topic_monitor in topic_monitors: + topic_monitor.start() + + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() diff --git a/sentor/scripts/test_sentor.py b/sentor/scripts/test_sentor.py index b447f1e..bdd1ef8 100755 --- a/sentor/scripts/test_sentor.py +++ b/sentor/scripts/test_sentor.py @@ -1,185 +1,415 @@ #!/usr/bin/env python3 -import rclpy +""" +Created on 20 May 2025 + +@author: Zhuoling Huang +""" +import os import yaml -import time import signal -import os -from sentor.TopicMonitor import TopicMonitor -from sentor.MultiMonitor import MultiMonitor -from std_msgs.msg import String -from sentor_msgs.msg import SentorEvent -from std_srvs.srv import Empty -import importlib -from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy + +import rclpy +from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor +from std_srvs.srv import Empty +from sentor.MultiMonitor import MultiMonitor +from sentor.TopicMonitor import TopicMonitor +from sentor.NodeMonitor import NodeMonitor +from sentor.SafetyMonitor import SafetyMonitor +# ─── Globals ──────────────────────────────────────────────────────────────── topic_monitors = [] -event_pub = None -rich_event_pub = None -multi_monitor = None - -def __signal_handler(signum, frame): - """ Gracefully stop all monitors on SIGINT. """ - for topic_monitor in topic_monitors: - topic_monitor.kill_monitor() - multi_monitor.stop_monitor() - print("Stopped monitoring.") - os._exit(signal.SIGTERM) +node_monitors = [] +multi_monitor = None -# def stop_monitoring(_): -# """ Stop all monitoring activities. """ -# for topic_monitor in topic_monitors: -# topic_monitor.stop_monitor() -# multi_monitor.stop_monitor() -# return Empty.Response() +def shutdown_handler(signum, frame): + for tm in topic_monitors: + tm.stop_monitor() + for nm in node_monitors: + nm.stop_monitor() + if multi_monitor: + multi_monitor.stop_monitor() + rclpy.shutdown() -# def start_monitoring(_): -# """ Start monitoring activities. """ -# for topic_monitor in topic_monitors: -# topic_monitor.start_monitor() -# multi_monitor.start_monitor() -# return Empty.Response() +signal.signal(signal.SIGINT, shutdown_handler) +# ─── Services ─────────────────────────────────────────────────────────────── def start_monitoring(request, _): - print("[Service] Received start_monitoring request") - for topic_monitor in topic_monitors: - topic_monitor.start_monitor() + for tm in topic_monitors: tm.start_monitor() + for nm in node_monitors: nm.start_monitor() multi_monitor.start_monitor() return Empty.Response() def stop_monitoring(request, _): - print("[Service] Received stop_monitoring request") - for topic_monitor in topic_monitors: - topic_monitor.stop_monitor() + for tm in topic_monitors: tm.stop_monitor() + for nm in node_monitors: nm.stop_monitor() multi_monitor.stop_monitor() return Empty.Response() -def resolve_qos(topic, multi_monitor): - return get_qos_profile(topic["qos"]) if "qos" in topic else None - -def resolve_msg_type(topic, multi_monitor): - return get_message_type(topic["message_type"]) if "message_type" in topic else None +def event_callback(msg, level="info", **_): + print(f"[{level.upper()}] {msg}") +# ─── Helpers ──────────────────────────────────────────────────────────────── def get_message_type(type_str): - """Import the actual message type class from a string like 'std_msgs/msg/Int32'.""" - if not type_str: - return None - try: - package, msg_name = type_str.split('/msg/') - module = importlib.import_module(f"{package}.msg") - return getattr(module, msg_name) - except Exception as e: - print(f"[ERROR] Failed to import message type '{type_str}': {e}") - return None -def get_qos_profile(qos_dict): - """Convert YAML QoS dict into actual QoSProfile object.""" - reliability = ReliabilityPolicy.RELIABLE - durability = DurabilityPolicy.VOLATILE - history = HistoryPolicy.KEEP_LAST - depth = 10 - - if qos_dict.get("reliability", "").lower() == "best_effort": - reliability = ReliabilityPolicy.BEST_EFFORT - if qos_dict.get("durability", "").lower() == "transient_local": - durability = DurabilityPolicy.TRANSIENT_LOCAL - if qos_dict.get("history", "").lower() == "keep_all": - history = HistoryPolicy.KEEP_ALL - if "depth" in qos_dict: - depth = qos_dict["depth"] - - return QoSProfile( - reliability=reliability, - durability=durability, - history=history, - depth=depth - ) + pkg, msg = type_str.split('/msg/') + module = __import__(f"{pkg}.msg", fromlist=[msg]) + return getattr(module, msg) -def event_callback(message, level="info", msg=None, nodes=None, topic_name=None): - prefix = f"[{level.upper()}]" - print(f"{prefix} {message}") +def get_qos_profile(qos): + from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy + rel = ReliabilityPolicy.BEST_EFFORT if qos.get("reliability","").lower()=="best_effort" else ReliabilityPolicy.RELIABLE + dur = DurabilityPolicy.TRANSIENT_LOCAL if qos.get("durability","").lower()=="transient_local" else DurabilityPolicy.VOLATILE + hist= HistoryPolicy.KEEP_ALL if qos.get("history","").lower()=="keep_all" else HistoryPolicy.KEEP_LAST + return QoSProfile(reliability=rel, durability=dur, history=hist, depth=qos.get("depth",10)) +# ─── main() ───────────────────────────────────────────────────────────────── def main(): - global multi_monitor + global topic_monitors, node_monitors, multi_monitor + rclpy.init() - node = rclpy.create_node("test_sentor") - - node.get_logger().info("Registering start/stop monitoring services...") - node.create_service(Empty, "start_monitoring", start_monitoring) - node.create_service(Empty, "stop_monitoring", stop_monitoring) - - config_file = node.declare_parameter("config_file", "config/test_monitor_config.yaml").value - topics = [] - - try: - items = [yaml.safe_load(open(item, 'r')) for item in config_file.split(',')] - for item in items: - if isinstance(item, dict) and "monitors" in item: - topics.extend(item["monitors"]) - elif isinstance(item, list): - topics.extend(item) - elif isinstance(item, dict): - topics.append(item) - except Exception as e: - node.get_logger().error(f"Error loading config file: {e}") - rclpy.shutdown() - return + driver = rclpy.create_node("test_sentor") + + # load config + cfg_path = driver.declare_parameter("config_file","~/config/test_monitor_config.yaml")\ + .get_parameter_value().string_value + cfg_path = os.path.expanduser(cfg_path) + with open(cfg_path,'r') as f: + cfg = yaml.safe_load(f) or {} + + topics_cfg = cfg.get("monitors", []) + nodes_cfg = cfg.get("node_monitors", []) + # start/stop services + driver.create_service(Empty, "start_monitoring", start_monitoring) + driver.create_service(Empty, "stop_monitoring", stop_monitoring) + # multi_monitor multi_monitor = MultiMonitor() - node.get_logger().info("Registering topic monitors:") - node.get_logger().info(f"Loaded topics from config:\n{topics}") - for i, topic in enumerate(topics): - if not isinstance(topic, dict): - continue - if topic.get("include", True) is False: - continue - - topic_name = topic.get("name") - if not topic_name: - continue - - qos_profile = resolve_qos(topic, multi_monitor) - msg_type = resolve_msg_type(topic, multi_monitor) - - node.get_logger().info(f"[Monitor-{i}] Topic: {topic_name}") - node.get_logger().info(f"[Monitor-{i}] Msg Type: {msg_type}") - node.get_logger().info(f"[Monitor-{i}] QoS Profile: {qos_profile}") - - topic_monitor = TopicMonitor( - topic_name=topic_name, - msg_type=msg_type, - qos_profile=qos_profile, - rate=topic.get("rate", 0), - N=topic.get("N", 0), - signal_when_config=topic.get("signal_when", {}), - signal_lambdas_config=topic.get("signal_lambdas", []), - processes=topic.get("execute", []), - timeout=topic.get("timeout", 0), - default_notifications=topic.get("default_notifications", True), - # event_callback=None, - event_callback=event_callback, - thread_num=i, - enable_internal_logs=topic.get("enable_internal_logs", True) - ) + # 1) TopicMonitors + for idx, t in enumerate(topics_cfg): + # ensure every condition—and every lambda—has a tags list + t.setdefault("signal_when", {}).setdefault("tags", []) + for lam in t.get("signal_lambdas", []): + lam.setdefault("tags", []) - topic_monitors.append(topic_monitor) - multi_monitor.register_monitor(topic_monitor) + tm = TopicMonitor( + topic_name = t["name"], + msg_type = get_message_type(t["message_type"]), + qos_profile = get_qos_profile(t.get("qos",{})), + rate = t["rate"], + N = t["N"], + signal_when_config = t.get("signal_when", {}), + signal_lambdas_config = t.get("signal_lambdas", []), + processes = t.get("execute", []), + timeout = t.get("timeout", 0.1), + default_notifications = t.get("default_notifications", True), + event_callback = event_callback, + thread_num = idx, + enable_internal_logs = True, + ) + topic_monitors.append(tm) + multi_monitor.register_monitor(tm) - # for topic_monitor in topic_monitors: - # topic_monitor.start() + # 2) NodeMonitors + for n in nodes_cfg: + nm = NodeMonitor( + target_node_name = n["name"], + timeout = n["timeout"], + event_callback = event_callback, + safety_critical = n.get("safety_critical", False), + autonomy_critical = n.get("autonomy_critical", False), + poll_rate = n.get("poll_rate", 1.0), + ) + node_monitors.append(nm) - executor = MultiThreadedExecutor() - executor.add_node(node) # test_sentor + # 3) safety heartbeat (safety_critical) + safety_hb = SafetyMonitor( + topic = 'safety/heartbeat', + event_msg = 'Heartbeat', + attr = 'is_alive', + srv_name = 'heartbeat_override', + event_cb = event_callback, + invert = False, + ) + # register both topic & node monitors + for tm in topic_monitors: + if tm.signal_when_cfg.get("safety_critical") or \ + any(l.get("safety_critical") for l in tm.signal_lambdas_config): + safety_hb.register_monitor(tm) + for nm in node_monitors: + if nm.safety_critical: + safety_hb.register_monitor(nm) - # Also add each TopicMonitor's node - for topic_monitor in topic_monitors: - executor.add_node(topic_monitor.get_node()) + # 4) warning heartbeat (autonomy_critical) + warning_hb = SafetyMonitor( + topic = 'warning/heartbeat', + event_msg = 'Warning-beat', + attr = 'is_autonomy_alive', + srv_name = 'warning_override', + event_cb = event_callback, + invert = False, + ) + for tm in topic_monitors: + if tm.signal_when_cfg.get("autonomy_critical") or \ + any(l.get("autonomy_critical") for l in tm.signal_lambdas_config): + warning_hb.register_monitor(tm) + for nm in node_monitors: + if nm.autonomy_critical: + warning_hb.register_monitor(nm) + # 5) spin everything together + executor = MultiThreadedExecutor() + executor.add_node(driver) executor.add_node(multi_monitor) + executor.add_node(safety_hb) + executor.add_node(warning_hb) + for tm in topic_monitors: + executor.add_node(tm.get_node()) + for nm in node_monitors: + executor.add_node(nm.node) executor.spin() + rclpy.shutdown() if __name__ == "__main__": main() + + + +# #!/usr/bin/env python3 +# import rclpy +# import yaml +# import time +# import signal +# import os +# from sentor.TopicMonitor import TopicMonitor +# from sentor.MultiMonitor import MultiMonitor +# from sentor.SafetyMonitor import SafetyMonitor +# from std_msgs.msg import String +# from sentor_msgs.msg import SentorEvent +# from std_srvs.srv import Empty +# import importlib +# from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy +# from rclpy.executors import MultiThreadedExecutor + + +# topic_monitors = [] +# event_pub = None +# rich_event_pub = None +# multi_monitor = None + +# def __signal_handler(signum, frame): +# """ Gracefully stop all monitors on SIGINT. """ +# for topic_monitor in topic_monitors: +# topic_monitor.kill_monitor() +# multi_monitor.stop_monitor() +# print("Stopped monitoring.") +# os._exit(signal.SIGTERM) + +# # def stop_monitoring(_): +# # """ Stop all monitoring activities. """ +# # for topic_monitor in topic_monitors: +# # topic_monitor.stop_monitor() +# # multi_monitor.stop_monitor() +# # return Empty.Response() + +# # def start_monitoring(_): +# # """ Start monitoring activities. """ +# # for topic_monitor in topic_monitors: +# # topic_monitor.start_monitor() +# # multi_monitor.start_monitor() +# # return Empty.Response() + +# def start_monitoring(request, _): +# print("[Service] Received start_monitoring request") +# for topic_monitor in topic_monitors: +# topic_monitor.start_monitor() +# multi_monitor.start_monitor() +# return Empty.Response() + +# def stop_monitoring(request, _): +# print("[Service] Received stop_monitoring request") +# for topic_monitor in topic_monitors: +# topic_monitor.stop_monitor() +# multi_monitor.stop_monitor() +# return Empty.Response() + +# def resolve_qos(topic, multi_monitor): +# return get_qos_profile(topic["qos"]) if "qos" in topic else None + +# def resolve_msg_type(topic, multi_monitor): +# return get_message_type(topic["message_type"]) if "message_type" in topic else None + +# def get_message_type(type_str): +# """Import the actual message type class from a string like 'std_msgs/msg/Int32'.""" +# if not type_str: +# return None +# try: +# package, msg_name = type_str.split('/msg/') +# module = importlib.import_module(f"{package}.msg") +# return getattr(module, msg_name) +# except Exception as e: +# print(f"[ERROR] Failed to import message type '{type_str}': {e}") +# return None +# def get_qos_profile(qos_dict): +# """Convert YAML QoS dict into actual QoSProfile object.""" +# reliability = ReliabilityPolicy.RELIABLE +# durability = DurabilityPolicy.VOLATILE +# history = HistoryPolicy.KEEP_LAST +# depth = 10 + +# if qos_dict.get("reliability", "").lower() == "best_effort": +# reliability = ReliabilityPolicy.BEST_EFFORT +# if qos_dict.get("durability", "").lower() == "transient_local": +# durability = DurabilityPolicy.TRANSIENT_LOCAL +# if qos_dict.get("history", "").lower() == "keep_all": +# history = HistoryPolicy.KEEP_ALL +# if "depth" in qos_dict: +# depth = qos_dict["depth"] + +# return QoSProfile( +# reliability=reliability, +# durability=durability, +# history=history, +# depth=depth +# ) + +# def event_callback(message, level="info", msg=None, nodes=None, topic_name=None): +# prefix = f"[{level.upper()}]" +# print(f"{prefix} {message}") + +# def main(): +# global multi_monitor +# rclpy.init() +# node = rclpy.create_node("test_sentor") + +# node.get_logger().info("Registering start/stop monitoring services...") +# node.create_service(Empty, "start_monitoring", start_monitoring) +# node.create_service(Empty, "stop_monitoring", stop_monitoring) + +# config_file = node.declare_parameter("config_file", "config/test_monitor_config.yaml").value +# topics = [] + +# try: +# items = [yaml.safe_load(open(item, 'r')) for item in config_file.split(',')] +# for item in items: +# if isinstance(item, dict) and "monitors" in item: +# topics.extend(item["monitors"]) +# elif isinstance(item, list): +# topics.extend(item) +# elif isinstance(item, dict): +# topics.append(item) +# except Exception as e: +# node.get_logger().error(f"Error loading config file: {e}") +# rclpy.shutdown() +# return + +# multi_monitor = MultiMonitor() + +# node.get_logger().info("Registering topic monitors:") +# node.get_logger().info(f"Loaded topics from config:\n{topics}") +# for i, topic in enumerate(topics): +# if not isinstance(topic, dict): +# continue +# if topic.get("include", True) is False: +# continue + +# topic_name = topic.get("name") +# if not topic_name: +# continue + +# qos_profile = resolve_qos(topic, multi_monitor) +# msg_type = resolve_msg_type(topic, multi_monitor) + +# node.get_logger().info(f"[Monitor-{i}] Topic: {topic_name}") +# node.get_logger().info(f"[Monitor-{i}] Msg Type: {msg_type}") +# node.get_logger().info(f"[Monitor-{i}] QoS Profile: {qos_profile}") + +# topic_monitor = TopicMonitor( +# topic_name=topic_name, +# msg_type=msg_type, +# qos_profile=qos_profile, +# rate=topic.get("rate", 0), +# N=topic.get("N", 0), +# signal_when_config=topic.get("signal_when", {}), +# signal_lambdas_config=topic.get("signal_lambdas", []), +# processes=topic.get("execute", []), +# timeout=topic.get("timeout", 0), +# default_notifications=topic.get("default_notifications", True), +# event_callback=event_callback, +# thread_num=i, +# enable_internal_logs=topic.get("enable_internal_logs", True) +# ) + +# topic_monitors.append(topic_monitor) +# multi_monitor.register_monitor(topic_monitor) + +# # # Create SafetyMonitor +# # safety_monitor = SafetyMonitor( +# # topic='safety/heartbeat', +# # event_msg='Heartbeat', +# # attr='is_alive', +# # srv_name='heartbeat_override', +# # event_cb=event_callback, +# # invert=False, +# # ) + +# # # Register TopicMonitors with SafetyMonitor +# # for tm in topic_monitors: +# # # signal_when critical OR any lambda critical? +# # sw_crit = tm.signal_when_cfg.get('safety_critical', False) +# # lambdas_crit = any(l.get('safety_critical', False) for l in tm.signal_lambdas_config) +# # if sw_crit or lambdas_crit: +# # safety_monitor.register_monitor(tm) + + +# executor = MultiThreadedExecutor() +# executor.add_node(node) # test_sentor + +# # ────────────────────────────────────────────────────────────────────────────── +# # (1) your existing safety monitor… +# safety_monitor = SafetyMonitor( +# topic='safety/heartbeat', +# event_msg='Heartbeat', +# attr='is_alive', +# srv_name='heartbeat_override', +# event_cb=event_callback, +# invert=False, +# ) +# for tm in topic_monitors: +# sw_crit = tm.signal_when_cfg.get('safety_critical', False) +# lam_crit = any(l.get('safety_critical', False) for l in tm.signal_lambdas_config) +# if sw_crit or lam_crit: +# safety_monitor.register_monitor(tm) +# executor.add_node(safety_monitor) +# # ────────────────────────────────────────────────────────────────────────────── + +# # (2) NEW: warning‐level heartbeat monitor +# warning_monitor = SafetyMonitor( +# topic='warning/heartbeat', +# event_msg='Warning‐beat', +# attr='is_autonomy_alive', +# srv_name='warning_override', +# event_cb=event_callback, +# invert=False, +# ) +# for tm in topic_monitors: +# # pick up anything marked autonomy_critical in signal_when or lambdas +# sw_warn = tm.signal_when_cfg.get('autonomy_critical', False) +# lam_warn = any(l.get('autonomy_critical', False) for l in tm.signal_lambdas_config) +# if sw_warn or lam_warn: +# warning_monitor.register_monitor(tm) +# executor.add_node(warning_monitor) + +# for topic_monitor in topic_monitors: +# executor.add_node(topic_monitor.get_node()) + +# executor.add_node(multi_monitor) +# # executor.add_node(safety_monitor) # ✅ Add SafetyMonitor to executor + +# executor.spin() + +# if __name__ == "__main__": +# main() \ No newline at end of file diff --git a/sentor/sentor/CustomLambdaExample.py b/sentor/sentor/CustomLambdaExample.py new file mode 100644 index 0000000..f536e49 --- /dev/null +++ b/sentor/sentor/CustomLambdaExample.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 +""" +CustomLambdaExample.py + +This module defines a custom lambda function for SENTOR monitoring in ROS2. + +Created on Fri Nov 20 11:35:22 2020 +@author: Adam Binch (abinch@sagarobotics.com) +Converted from ROS1 to ROS2 2025 +@author: Zhuoling Huang + +Example in yaml: + +signal_lambdas: + - expression: "CustomLambda" + file: "CustomLambdaExample" + package: "sentor" + safety_critical: False + process_indices: [] + repeat_exec: False + tags: ["navigation"] + when_published: False + +""" + +def CustomLambda(msg): + """ + Custom lambda that returns True if msg.data equals "t1-r1-c2". + + Args: + msg: A ROS message (expected to have a 'data' attribute) + + Returns: + bool: True if msg.data equals "t1-r1-c2", False otherwise. + """ + return msg.data == "t1-r1-c2" + + +if __name__ == '__main__': + # Test the custom lambda function with a dummy message + class DummyMsg: + def __init__(self, data): + self.data = data + + test_msg1 = DummyMsg("t1-r1-c2") + test_msg2 = DummyMsg("other") + + print("CustomLambda(test_msg1):", CustomLambda(test_msg1)) # Expected: True + print("CustomLambda(test_msg2):", CustomLambda(test_msg2)) # Expected: False \ No newline at end of file diff --git a/sentor/sentor/CustomProcessExample.py b/sentor/sentor/CustomProcessExample.py new file mode 100644 index 0000000..fded55b --- /dev/null +++ b/sentor/sentor/CustomProcessExample.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 +""" +CustomProcessExample.py + +This module defines a custom process class for SENTOR in ROS2. +The custom process prints a message during initialization and when executed. + +Created on Mon Nov 23 16:26:27 2020 +@author: Adam Binch (abinch@sagarobotics.com) +Converted from ROS1 to ROS2 2025 +@author: Zhuoling Huang + +Example in yaml: +execute: + - custom: + verbose: True + name: "CustomProcess" + file: "CustomProcessExample" + package: "sentor" + init_args: + - "Custom process initialisation message" + run_args: + - "Custom process runtime message" + +""" + +class CustomProcess: + def __init__(self, message): + """ + Initialization of the custom process. + + Args: + message (str): A message to print during initialization. + """ + print("CustomProcess initialized with message:", message) + + def run(self, message): + """ + Executes the custom process when the condition is met. + + Args: + message (str): A message provided at runtime. + """ + print("CustomProcess running with message:", message) + +if __name__ == '__main__': + # Test the custom process if this file is run standalone. + cp = CustomProcess("Initialization test") + cp.run("Runtime test") diff --git a/sentor/sentor/MultiMonitor.py b/sentor/sentor/MultiMonitor.py index 5c07deb..81ec60b 100644 --- a/sentor/sentor/MultiMonitor.py +++ b/sentor/sentor/MultiMonitor.py @@ -1,3 +1,12 @@ +#!/usr/bin/env python3 +""" +Created on Fri Dec 6 08:51:15 2019 +@author: Adam Binch (abinch@sagarobotics.com) + +Converted from ROS1 to ROS2 2025 +@author: Zhuoling Huang +""" +##################################################################################### from threading import Event from rclpy.node import Node from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, HistoryPolicy @@ -59,11 +68,11 @@ def callback(self): for expr in monitor.conditions: condition = Monitor() condition.topic = monitor.topic_name - condition.condition = expr + condition.condition = expr # Now 'expr' is the original string, e.g., "CustomLambda" condition.safety_critical = monitor.conditions[expr]["safety_critical"] condition.autonomy_critical = monitor.conditions[expr]["autonomy_critical"] condition.satisfied = self.error_code[count] - condition.tags = monitor.conditions[expr]["tags"] + condition.tags = monitor.conditions[expr].get("tags", []) msg.conditions.append(condition) count += 1 @@ -74,3 +83,251 @@ def stop_monitor(self): def start_monitor(self): self._stop_event.clear() + + +# import rclpy +# from rclpy.node import Node +# from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, HistoryPolicy +# from threading import Event +# from sentor_msgs.msg import Monitor, MonitorArray # Import ROS2 messages + + +# class MultiMonitor(Node): +# def __init__(self, node_name='multi_monitor'): +# """ Initialize MultiMonitor Node. """ +# super().__init__(node_name) + +# # Declare and get the safety publication rate parameter +# self.declare_parameter("safety_pub_rate", 10.0) +# rate = self.get_parameter("safety_pub_rate").value + +# self.topic_monitors = [] +# self._stop_event = Event() +# self.error_code = [] + +# self.monitors_pub = None # Will be set dynamically + +# # Create a timer for periodic execution of the callback +# period = 1.0 / rate +# self.timer = self.create_timer(period, self.callback) + +# def get_default_qos(self): +# """ Provide a fallback default QoS profile. """ +# qos = QoSProfile( +# depth=1, +# reliability=QoSReliabilityPolicy.RELIABLE, +# history=HistoryPolicy.KEEP_LAST, +# durability=QoSDurabilityPolicy.TRANSIENT_LOCAL +# ) +# self.get_logger().warn("[MultiMonitor] No QoS provided; using fallback default QoS.") +# return qos + +# def register_monitors(self, topic_monitor): +# """ Register a new monitor and set up QoS dynamically. """ +# self.get_logger().info(f"[MultiMonitor] Registering monitor for topic: {topic_monitor.topic_name}") + +# # If this is the first topic, set the publisher QoS based on it (or fallback default) +# if not self.topic_monitors: +# qos_profile = topic_monitor.qos_profile or self.get_default_qos() +# self.monitors_pub = self.create_publisher( +# MonitorArray, "/sentor/monitors", qos_profile +# ) + +# self.topic_monitors.append(topic_monitor) + +# def callback(self): +# """ Periodically check conditions and publish monitor status. """ +# if not self._stop_event.is_set() and self.monitors_pub is not None: +# error_code_new = [ +# monitor.conditions[expr]["satisfied"] +# for monitor in self.topic_monitors +# for expr in monitor.conditions +# ] + +# # Publish only if there is a change in error_code +# if error_code_new != self.error_code: +# self.error_code = error_code_new + +# conditions = MonitorArray() +# conditions.header.stamp = self.get_clock().now().to_msg() + +# count = 0 +# for monitor in self.topic_monitors: +# topic_name = monitor.topic_name +# for expr in monitor.conditions: +# condition = Monitor() +# condition.topic = topic_name +# condition.condition = expr +# condition.safety_critical = monitor.conditions[expr]["safety_critical"] +# condition.autonomy_critical = monitor.conditions[expr]["autonomy_critical"] +# condition.satisfied = self.error_code[count] +# condition.tags = monitor.conditions[expr]["tags"] +# conditions.conditions.append(condition) +# count += 1 + +# self.monitors_pub.publish(conditions) + +# def stop_monitor(self): +# """ Stop monitoring. """ +# self._stop_event.set() + +# def start_monitor(self): +# """ Start monitoring. """ +# self._stop_event.clear() + + + + +# import rclpy +# from rclpy.node import Node +# from rclpy.qos import QoSProfile, QoSDurabilityPolicy +# from threading import Event + +# # Assuming these message definitions are available in your ROS2 package. +# from sentor.msg import Monitor, MonitorArray + + +# class MultiMonitor(Node): +# # def __init__(self, node_name='multi_monitor'): +# # super().__init__(node_name) + +# # # Declare and get the safety_pub_rate parameter +# # self.declare_parameter("safety_pub_rate", 10.0) +# # rate = self.get_parameter("safety_pub_rate").value + +# # self.topic_monitors = [] +# # self._stop_event = Event() +# # self.error_code = [] + +# # # Create publisher with transient local durability to mimic latching in ROS1 +# # qos_profile = QoSProfile(depth=1) +# # qos_profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL +# # self.monitors_pub = self.create_publisher(MonitorArray, "/sentor/monitors", qos_profile) + +# # # Create a timer; in ROS2, the timer callback does not receive an event argument. +# # period = 1.0 / rate +# # self.timer = self.create_timer(period, self.callback) + +# # def register_monitors(self, topic_monitor): +# # """ +# # Register a new monitor to be included in the callback processing. +# # """ +# # self.topic_monitors.append(topic_monitor) + +# # def callback(self): +# # if not self._stop_event.is_set(): +# # # Gather the 'satisfied' status from all monitors +# # error_code_new = [ +# # monitor.conditions[expr]["satisfied"] +# # for monitor in self.topic_monitors +# # for expr in monitor.conditions +# # ] + +# # # Publish only if there is a change in error_code +# # if error_code_new != self.error_code: +# # self.error_code = error_code_new + +# # conditions = MonitorArray() +# # # Set the header stamp using ROS2 time +# # conditions.header.stamp = self.get_clock().now().to_msg() + +# # count = 0 +# # for monitor in self.topic_monitors: +# # topic_name = monitor.topic_name +# # for expr in monitor.conditions: +# # condition = Monitor() +# # condition.topic = topic_name +# # condition.condition = expr +# # condition.safety_critical = monitor.conditions[expr]["safety_critical"] +# # condition.autonomy_critical = monitor.conditions[expr]["autonomy_critical"] +# # condition.satisfied = self.error_code[count] +# # condition.tags = monitor.conditions[expr]["tags"] +# # conditions.conditions.append(condition) +# # count += 1 + +# # self.monitors_pub.publish(conditions) +# def __init__(self, node_name='multi_monitor'): +# super().__init__(node_name) + +# # Declare and get the safety_pub_rate parameter +# self.declare_parameter("safety_pub_rate", 10.0) +# rate = self.get_parameter("safety_pub_rate").value + +# self.topic_monitors = [] +# self._stop_event = Event() +# self.error_code = [] + +# # Create publisher with transient local durability to mimic latching in ROS1 +# qos_profile = QoSProfile(depth=1) +# qos_profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL +# self.monitors_pub = self.create_publisher(MonitorArray, "/sentor/monitors", qos_profile) + +# # Create a timer +# period = 1.0 / rate +# self.timer = self.create_timer(period, self.callback) + +# self.get_logger().info("MultiMonitor node started!") # 🔹 Log startup + +# def register_monitors(self, topic_monitor): +# """ Register a new monitor to be included in the callback processing. """ +# self.get_logger().info(f"Registering monitor for topic: {topic_monitor.topic_name}") +# self.topic_monitors.append(topic_monitor) +# self.get_logger().info(f"Total monitors registered: {len(self.topic_monitors)}") # 🔹 Log monitor count + +# def callback(self): +# self.get_logger().info(f"MultiMonitor callback running... Monitors count: {len(self.topic_monitors)}") + +# if not self._stop_event.is_set(): +# error_code_new = [ +# monitor.conditions[expr]["satisfied"] +# for monitor in self.topic_monitors +# for expr in monitor.conditions +# ] + +# self.get_logger().info(f"Previous error_code: {self.error_code}") +# self.get_logger().info(f"New error_code: {error_code_new}") + +# if error_code_new != self.error_code: +# self.get_logger().info("Publishing a new MonitorArray message.") +# self.error_code = error_code_new + +# conditions = MonitorArray() +# conditions.header.stamp = self.get_clock().now().to_msg() + +# count = 0 +# for monitor in self.topic_monitors: +# topic_name = monitor.topic_name +# for expr in monitor.conditions: +# condition = Monitor() +# condition.topic = topic_name +# condition.condition = expr +# condition.safety_critical = monitor.conditions[expr]["safety_critical"] +# condition.autonomy_critical = monitor.conditions[expr]["autonomy_critical"] +# condition.satisfied = self.error_code[count] +# condition.tags = monitor.conditions[expr]["tags"] +# conditions.conditions.append(condition) +# count += 1 + +# self.monitors_pub.publish(conditions) + +# def stop_monitor(self): +# self._stop_event.set() + +# def start_monitor(self): +# self._stop_event.clear() + +# def main(args=None): +# """ Entry point for the node """ +# rclpy.init(args=args) +# node = MultiMonitor() + +# # Keep the node alive +# rclpy.spin(node) + +# # Cleanup after shutdown +# node.destroy_node() +# rclpy.shutdown() + + +# if __name__ == "__main__": +# main() \ No newline at end of file diff --git a/sentor/sentor/NodeMonitor.py b/sentor/sentor/NodeMonitor.py new file mode 100644 index 0000000..25f66db --- /dev/null +++ b/sentor/sentor/NodeMonitor.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 +""" +Created on 20 May 2025 + +@author: Zhuoling Huang +""" +##################################################################################### +import time +import threading +import rclpy +from rclpy.node import Node +from rclpy.executors import SingleThreadedExecutor + +class NodeMonitor(threading.Thread): + """ + Monitors whether a given ROS2 node is alive (and optionally autonomy-alive). + + Spins its own SingleThreadedExecutor so it doesn't interfere with the global one. + """ + def __init__( + self, + target_node_name: str, + timeout: float, + event_callback, + safety_critical: bool = False, + autonomy_critical: bool = False, + poll_rate: float = 1.0, + ): + super().__init__(daemon=True) + self.target = target_node_name + self.timeout = timeout + self.event_cb = event_callback + + # flags for safety vs autonomy + self.safety_critical = safety_critical + self.autonomy_critical = autonomy_critical + + # current states + self.is_alive = False + self.is_autonomy_alive = False + self._last_seen = None + + # build a dedicated node + executor + node_name = f"node_monitor_{self.target.replace('/', '_')}" + self.node = Node(node_name) + self._executor = SingleThreadedExecutor() + self._executor.add_node(self.node) + + # periodic timer to poll graph + self.node.create_timer(1.0 / poll_rate, self._on_timer) + + # control flag for run() + self._stop_event = threading.Event() + + def _on_timer(self): + # check for target in graph + names = [n for (n, ns) in self.node.get_node_names_and_namespaces()] + now = time.time() + if self.target in names: + self._last_seen = now + + alive = (self._last_seen is not None) and (now - self._last_seen < self.timeout) + if alive != self.is_alive: + self.is_alive = alive + lvl = 'error' if (not alive and self.safety_critical) else 'info' + self.event_cb(f"Node {self.target} safety is {'alive' if alive else 'dead'}", lvl) + + auto_alive = alive and self.autonomy_critical + if auto_alive != self.is_autonomy_alive: + self.is_autonomy_alive = auto_alive + lvl = 'error' if (not auto_alive and self.autonomy_critical) else 'info' + self.event_cb(f"Node {self.target} autonomy is {'alive' if auto_alive else 'dead'}", lvl) + + def run(self): + # spin only this node in its own executor + while not self._stop_event.is_set(): + self._executor.spin_once(timeout_sec=0.1) + + def stop_monitor(self): + # stop spinning and clean up + self._stop_event.set() + try: + self.node.destroy_node() + except Exception: + pass diff --git a/sentor/sentor/ROSTopicFilter.py b/sentor/sentor/ROSTopicFilter.py index eed76ca..72bfe5e 100644 --- a/sentor/sentor/ROSTopicFilter.py +++ b/sentor/sentor/ROSTopicFilter.py @@ -30,11 +30,15 @@ def __init__(self, parent_node: Node, topic_name, lambda_fn_str, config, throttl self.unsat_callbacks = [] # Compile lambda function - try: - self.lambda_fn = eval(self.lambda_fn_str) - except Exception as e: - self.node.get_logger().error(f"[ROSTopicFilter] Lambda error: {e}") - self.lambda_fn = None + if isinstance(self.lambda_fn_str, str): + try: + self.lambda_fn = eval(self.lambda_fn_str) + except Exception as e: + self.node.get_logger().error(f"[ROSTopicFilter] Lambda error: {e}") + self.lambda_fn = None + else: + # If it's already a callable (e.g. imported custom lambda), just assign it. + self.lambda_fn = self.lambda_fn_str # Detect message type self.msg_type = self.get_message_type(self.topic_name) diff --git a/sentor/sentor/ROSTopicHz.py b/sentor/sentor/ROSTopicHz.py index cbc0c94..96cc8c4 100644 --- a/sentor/sentor/ROSTopicHz.py +++ b/sentor/sentor/ROSTopicHz.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 """ @author: Francesco Del Duchetto (FDelDuchetto@lincoln.ac.uk) @author: Adam Binch (abinch@sagarobotics.com) @@ -26,7 +26,7 @@ def __init__(self, node, topic_name, window_size=1000, throttle_val=1, stop_even self.last_printed_time = None self.prev_time = None self.times = [] - self.msg_tn = None + self.msg_tn = None # <-- ✅ This was missing self.node.get_logger().info(f"[ROSTopicHz] Initialized for {self.topic_name} with window {self.window_size}") # self._stop_event = Event() # Accept stop_event from outside or create internally @@ -40,6 +40,7 @@ def start_monitoring(self, msg_type, qos_profile): self.subscription = self.node.create_subscription( msg_type, self.topic_name, self.callback_hz, qos_profile ) + # self.node.get_logger().info(f"[HzMonitor] Subscription to {self.topic_name} created") self.times.clear() self.last_msg_time = None self.enabled = True @@ -66,6 +67,7 @@ def stop_monitoring(self): def callback_hz(self, msg): # if self._stop_event and self._stop_event.is_set(): # return + # self.node.get_logger().info(f"[HzMonitor] callback_hz fired on {self.topic_name}") now = self.node.get_clock().now().nanoseconds / 1e9 with self.lock: if self.last_msg_time is not None: @@ -85,11 +87,61 @@ def get_hz(self): stddev = statistics.stdev(self.times) if len(self.times) > 1 else 0.0 return 1.0 / mean, min(self.times), max(self.times), stddev, len(self.times) + # def callback_hz(self, msg): + # # self.node.get_logger().info( + # # f"[HzMonitor] callback_hz triggered! times before: {len(self.times)}" + # # ) + # if self._stop_event.is_set(): + # return + + # now = self.node.get_clock().now().nanoseconds / 1e9 # Time in seconds + + # with self.lock: + # if self.prev_time is None: + # self.prev_time = now + # return + + # delta = now - self.prev_time + # self.prev_time = now + + # self.times.append(delta) + # if len(self.times) > self.window_size: + # self.times.pop(0) + # # self.node.get_logger().info( + # # f"[HzMonitor] callback_hz done. times after: {len(self.times)}" + # # ) + # def callback_hz(self, msg): + # if self._stop_event and self._stop_event.is_set(): + # return + # if not self.subscription: # If the subscription was destroyed, do nothing + # return + # now = self.node.get_clock().now().nanoseconds / 1e9 + # if self.last_msg_time is not None: + # delta = now - self.last_msg_time + # self.times.append(delta) + # if len(self.times) > self.window_size: + # self.times.pop(0) + # self.last_msg_time = now + def callback_hz_throttled(self, msg): self.throttle += 1 if self.throttle % self.throttle_val == 0: self.callback_hz(msg) + # def get_hz(self): + # with self.lock: + # if not self.times or len(self.times) < 2: + # return None + + # n = len(self.times) + # mean = sum(self.times) / n + # rate = 1.0 / mean if mean > 0 else 0.0 + # std_dev = math.sqrt(sum((x - mean) ** 2 for x in self.times) / n) + # max_delta = max(self.times) + # min_delta = min(self.times) + + # return rate, min_delta, max_delta, std_dev, n + def print_hz(self, logger): stats = self.get_hz() if not stats: diff --git a/sentor/sentor/SafetyMonitor.py b/sentor/sentor/SafetyMonitor.py new file mode 100644 index 0000000..3ed7a68 --- /dev/null +++ b/sentor/sentor/SafetyMonitor.py @@ -0,0 +1,128 @@ +# SafetyMonitor.py +#!/usr/bin/env python +""" +Created on Fri Dec 6 08:51:15 2019 + +@author: Adam Binch + +Converted from ROS1 to ROS2 2025 +@author: Zhuoling Huang +""" +import threading +import rclpy +from rclpy.node import Node +from std_msgs.msg import Bool +from example_interfaces.srv import SetBool + +class SafetyMonitor(Node): + def __init__( + self, + topic: str, + event_msg: str, + attr: str, + srv_name: str, + event_cb, + invert: bool = False, + ): + super().__init__('safety_monitor') + + # parameters + self.declare_parameter('safe_operation_timeout', 10.0) + self.declare_parameter('safety_pub_rate', 1.0) + self.declare_parameter('auto_safety_tagging', True) + + self.timeout = self.get_parameter('safe_operation_timeout').value or 0.1 + self.rate = self.get_parameter('safety_pub_rate').value + self.auto_tagging = self.get_parameter('auto_safety_tagging').value + + # internal state + self.attr = attr + self.event_cb = event_cb + self.invert = invert + self.topic_monitors = [] + self.timer = None + self.safe_operation = False + self.safe_msg_sent = False + self.unsafe_msg_sent = False + self._stop_event = threading.Event() + self.event_msg = f"{event_msg}: " + + # publisher + periodic callback + self.safety_pub = self.create_publisher(Bool, topic, 10) + self.create_timer(1.0 / self.rate, self._safety_pub_cb) + + # override service + self.create_service(SetBool, f'/sentor/{srv_name}', self._srv_cb) + + def register_monitor(self, m): + """m can be a TopicMonitor (has .topic_name) or NodeMonitor (has .target).""" + self.topic_monitors.append(m) + # cancel any pending countdown + if self.timer: + self.timer.cancel() + self.timer = None + + def _safety_pub_cb(self): + if self._stop_event.is_set() or not self.topic_monitors: + return + + states = [] + for m in self.topic_monitors: + # first, let each monitor refresh its .is_alive/.is_autonomy_alive + if hasattr(m, '_check_alive_status'): + m._check_alive_status() + + alive = getattr(m, self.attr) + # pick human‐readable name: topic_name or target + name = getattr(m, 'topic_name', None) or getattr(m, 'target', '') + self.get_logger().info(f"[SAFETY] {name}.is_{self.attr} = {alive}") + states.append(alive) + + # arm the “all good” timer if needed + if self.auto_tagging and all(states) and self.timer is None: + self.timer = self.create_timer(self.timeout, self._timer_cb) + + # cancel countdown and mark unsafe if any dead + if not all(states): + if self.timer: + self.timer.cancel() + self.timer = None + self.safe_operation = False + if not self.unsafe_msg_sent: + self.event_cb(self.event_msg + "FALSE", "error") + self.unsafe_msg_sent = True + self.safe_msg_sent = False + + # publish the Bool + msg = Bool() + msg.data = (not self.safe_operation) if self.invert else self.safe_operation + self.safety_pub.publish(msg) + + def _timer_cb(self): + # countdown complete → mark safe + self.safe_operation = True + if not self.safe_msg_sent: + self.event_cb(self.event_msg + "TRUE", "info") + self.safe_msg_sent = True + self.unsafe_msg_sent = False + + # one-shot timer + if self.timer: + self.timer.cancel() + self.timer = None + + def _srv_cb(self, request, response): + # manual override / reset + if self.timer: + self.timer.cancel() + self.timer = None + self.safe_operation = request.data + response.success = True + response.message = f"{self.event_msg}{request.data}" + return response + + def stop_monitor(self): + self._stop_event.set() + + def start_monitor(self): + self._stop_event.clear() \ No newline at end of file diff --git a/sentor/sentor/TopicMonitor.py b/sentor/sentor/TopicMonitor.py index 1e9525f..77c49bd 100644 --- a/sentor/sentor/TopicMonitor.py +++ b/sentor/sentor/TopicMonitor.py @@ -1,15 +1,27 @@ +#!/usr/bin/env python3 +""" +@author: Francesco Del Duchetto (FDelDuchetto@lincoln.ac.uk) +@author: Adam Binch (abinch@sagarobotics.com) + +Converted from ROS1 to ROS2 2025 +@author: Zhuoling Huang +""" +##################################################################################### from threading import Thread, Event, Lock +import time import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy -from rclpy.timer import Timer from sentor.ROSTopicFilter import ROSTopicFilter -from sentor.ROSTopicHz import ROSTopicHz -from sentor.ROSTopicPub import ROSTopicPub -from sentor.Executor import Executor -import time +from sentor.ROSTopicHz import ROSTopicHz +from sentor.ROSTopicPub import ROSTopicPub +from sentor.Executor import Executor class TopicMonitor(Thread): + def debug(self, msg): + if getattr(self, 'enable_internal_logs', False) and hasattr(self, 'node'): + self.node.get_logger().info(msg) + def __init__( self, topic_name, @@ -24,222 +36,178 @@ def __init__( default_notifications, event_callback, thread_num, - enable_internal_logs=True # <-- new optional param + enable_internal_logs=True ): - """ - :param enable_internal_logs: If True, prints extra developer logs (like 'Lambda satisfied' traces). - """ - super().__init__() - - # Basic fields - self.topic_name = topic_name - self.msg_type = msg_type - self.qos_profile = qos_profile - self.rate = rate - self.N = N - self.signal_when_config = signal_when_config + super().__init__(daemon=True) + self.topic_name = topic_name + self.msg_type = msg_type + self.qos_profile = qos_profile + self.rate = rate + self.N = N + self.signal_when_config = signal_when_config self.signal_lambdas_config = signal_lambdas_config - self.processes = processes - self.timeout = timeout if timeout > 0 else 0.1 + self.processes = processes + self.timeout = timeout if timeout>0 else 0.1 self.default_notifications = default_notifications - self._event_callback = event_callback - self.thread_num = thread_num - - # Developer log toggle - self.enable_internal_logs = enable_internal_logs - - # Thread-control + concurrency - self._stop_event = Event() - self._lock = Lock() - - # Condition tracking - self.conditions = {} - self.sat_expr_timer = {} - - # Create the ROS2 Node + self._event_callback = event_callback + self.thread_num = thread_num + self.enable_internal_logs = enable_internal_logs + + # internal state + self._stop_event = Event() + self._lock = Lock() + self.conditions = {} + self.sat_expr_timer= {} + self.is_alive = False + self.is_autonomy_alive = False + + # last‐message timestamp for any‐message callback + self._last_msg_time = time.time() + + # ROS node for this monitor self.node = Node(f"topic_monitor_{thread_num}") - self.debug(f"[TopicMonitor] Created for topic: {self.topic_name}") + self.debug(f"[TopicMonitor] Created for {self.topic_name}") - # QoS fallback + # fallback QoS if self.qos_profile is None: - self.node.get_logger().warn(f"[TopicMonitor] No QoS profile for {self.topic_name}, using fallback QoS") + self.node.get_logger().warn(f"[TopicMonitor] No QoS for {self.topic_name}, using fallback") self.qos_profile = QoSProfile( reliability=ReliabilityPolicy.RELIABLE, history=HistoryPolicy.KEEP_LAST, depth=10 ) - # Optionally set up an Executor if needed - if processes: - self.executor = Executor(processes, event_callback) - else: - self.executor = None + # thread‐safe executor for any “processes” + self.executor = Executor(processes, event_callback) if processes else None - # Parse signal_when config + # --- THIS WAS MISSING in your version! ---------------------------- + # Build self.signal_when_cfg and seed self.conditions for “signal_when” self.process_signal_config() + # ------------------------------------------------------------------ - # Create ROSTopicHz for frequency monitoring - self.hz_monitor = ROSTopicHz(self.node, self.topic_name, 1000, self.N) + # frequency monitoring + self.hz_monitor = ROSTopicHz(self.node, self.topic_name, window_size=1000, throttle_val=self.N) self.hz_monitor.start_monitoring(self.msg_type, self.qos_profile) - # Periodically log frequency + # periodic status checks self.node.create_timer(5.0, self.log_hz_stats) - - # Set up ROSTopicPub if signal_when == "published" - if self.signal_when_cfg.get("signal_when", "").lower() == "published": - self.debug(f"[TopicMonitor] Setting up ROSTopicPub for {self.topic_name}") - self.pub_monitor = ROSTopicPub( + self.node.create_timer(1.0, self.check_not_published) + self.node.create_timer(1.0, self._check_alive_status) + self.node.create_timer(1.0, self._check_autonomy_status) + + # subscribe to update “last_msg_time” + self.node.create_subscription( + self.msg_type, + self.topic_name, + self._on_any_msg, + self.qos_profile + ) + + # “published”‐condition via ROSTopicPub + if self.signal_when_cfg.get("signal_when","").lower() == "published": + self.debug(f"Setting up ROSTopicPub for {self.topic_name}") + pubm = ROSTopicPub( node=self.node, topic_name=self.topic_name, msg_type=self.msg_type, qos_profile=self.qos_profile, - throttle_val=self.N + throttle_val=1 ) - self.pub_monitor.register_published_cb(self.published_cb) - - # Build lambda filters - for i, signal_lambda in enumerate(self.signal_lambdas_config): - config = self.process_lambda_config(signal_lambda) - expr = config["expr"] + pubm.register_published_cb(self.published_cb) - # Track condition - self.conditions[expr] = { + # per‐lambda filters + for lam in self.signal_lambdas_config: + cfg = self.process_lambda_config(lam) + key = cfg["original_expr"] + self.conditions[key] = { "satisfied": False, - "safety_critical": config["safety_critical"], - "autonomy_critical": config["autonomy_critical"], - "tags": config.get("tags", []) + "safety_critical": cfg["safety_critical"], + "autonomy_critical": cfg["autonomy_critical"], + "tags": cfg.get("tags", []), } - - # Create ROSTopicFilter - filter_monitor = ROSTopicFilter( - self.node, self.topic_name, expr, config, throttle_val=self.N, - ) - filter_monitor.register_satisfied_cb(self.lambda_satisfied_cb) - filter_monitor.register_unsatisfied_cb(self.lambda_unsatisfied_cb) - - def debug(self, msg): - """Helper method: only logs if enable_internal_logs=True.""" - if self.enable_internal_logs: - self.node.get_logger().info(msg) + fm = ROSTopicFilter(self.node, self.topic_name, cfg["expr"], cfg, throttle_val=self.N) + fm.register_satisfied_cb(self.lambda_satisfied_cb) + fm.register_unsatisfied_cb(self.lambda_unsatisfied_cb) def process_signal_config(self): - """Parses the 'signal_when_config' to fill up self.signal_when_cfg dict and create conditions.""" + """Initialize self.signal_when_cfg & seed self.conditions for the ‘signal_when’ check.""" + cfg = self.signal_when_config or {} self.signal_when_cfg = { - "signal_when": self.signal_when_config.get("condition", ""), - "timeout": self.signal_when_config.get("timeout", self.timeout), - "safety_critical": self.signal_when_config.get("safety_critical", False), - "autonomy_critical": self.signal_when_config.get("autonomy_critical", False), - "default_notifications": self.signal_when_config.get("default_notifications", self.default_notifications), - "tags": self.signal_when_config.get("tags", []) + "signal_when": cfg.get("condition", ""), + "timeout": max(cfg.get("timeout", self.timeout), 0.1), + "safety_critical": cfg.get("safety_critical", False), + "autonomy_critical": cfg.get("autonomy_critical", False), + "default_notifications": cfg.get("default_notifications", self.default_notifications), + "tags": cfg.get("tags", []), } - if self.signal_when_cfg["timeout"] <= 0: - self.signal_when_cfg["timeout"] = self.timeout - - # If there's a condition, create an entry in self.conditions if self.signal_when_cfg["signal_when"]: - self.conditions[self.signal_when_cfg["signal_when"]] = { - "satisfied": False, - "safety_critical": self.signal_when_cfg["safety_critical"], - "autonomy_critical": self.signal_when_cfg["autonomy_critical"], - "tags": self.signal_when_cfg["tags"] - } - - def process_lambda_config(self, signal_lambda): - """Parses each signal_lambda config to unify structure and fallback timeouts.""" - config = { - "expr": signal_lambda.get("expression", ""), - "timeout": signal_lambda.get("timeout", self.timeout), - "safety_critical": signal_lambda.get("safety_critical", False), - "autonomy_critical": signal_lambda.get("autonomy_critical", False), - "default_notifications": signal_lambda.get("default_notifications", self.default_notifications), - "repeat_exec": signal_lambda.get("repeat_exec", False), - "tags": signal_lambda.get("tags", []) + key = self.signal_when_cfg["signal_when"] + self.conditions[key] = {"satisfied": False, **self.signal_when_cfg} + + def process_lambda_config(self, lam): + return { + "expr": lam["expression"], + "original_expr": lam["expression"], + "timeout": max(lam.get("timeout", self.timeout), 0.1), + "safety_critical": lam.get("safety_critical", False), + "autonomy_critical": lam.get("autonomy_critical", False), + "process_indices": lam.get("process_indices", []), } - if config["timeout"] <= 0: - config["timeout"] = self.timeout - return config - - def lambda_satisfied_cb(self, expr, msg, config): - if expr in self.sat_expr_timer: - return - - def on_timeout(): - self.debug(f"[TopicMonitor] Lambda satisfied: {expr}") - self.conditions[expr]["satisfied"] = True - - if config.get("default_notifications"): - message = f"Lambda '{expr}' satisfied on topic {self.topic_name}" - level = "error" if config.get("safety_critical") else "warn" - if self._event_callback: - self._event_callback(message, level, msg=msg, topic_name=self.topic_name) + def _on_any_msg(self, msg): + self._last_msg_time = time.time() + + def _check_alive_status(self): + stats = self.hz_monitor.get_hz() or [] + rate = stats[0] if stats else 0.0 + rate_ok = rate >= self.rate + # require ALL safety_critical checks + crit = [k for k,v in self.conditions.items() if v.get("safety_critical")] + lamb_ok = all(self.conditions[k]["satisfied"] for k in crit) if crit else True + alive = rate_ok and lamb_ok + self.node.get_logger().info(f"[LIVENESS] {self.topic_name}: rate={rate:.2f}/{self.rate}, lambdas={crit} all={lamb_ok}") + self.is_alive = alive + + def _check_autonomy_status(self): + stats = self.hz_monitor.get_hz() or [] + rate = stats[0] if stats else 0.0 + rate_ok = rate >= self.rate + crit = [k for k,v in self.conditions.items() if v.get("autonomy_critical")] + lamb_ok = all(self.conditions[k]["satisfied"] for k in crit) if crit else True + alive = rate_ok and lamb_ok + self.is_autonomy_alive = alive - if level == "error": - self.node.get_logger().error(message) - else: - self.node.get_logger().warn(message) - - # Here, mimic ROS1 by calling execute() when the condition is met. - if not config.get("repeat_exec", False): - # process_indices should be provided in your YAML for this lambda. - self.execute(msg, config.get("process_indices")) - # Optionally, if repeat_exec is enabled, you could set up a repeat timer. - - with self._lock: - self.sat_expr_timer[expr] = self.node.create_timer( - config["timeout"], lambda: self._on_lambda_timeout(expr, on_timeout) - ) + def published_cb(self, msg): + if "published" in self.conditions: + self.conditions["published"]["satisfied"] = True - def _on_lambda_timeout(self, expr, callback): - """Helper to cancel the timer and then run the provided callback.""" + def lambda_satisfied_cb(self, expr, msg, cfg): with self._lock: - if expr in self.sat_expr_timer: - self.sat_expr_timer[expr].cancel() - del self.sat_expr_timer[expr] - callback() + if not self.conditions[expr]["satisfied"]: + self.conditions[expr]["satisfied"] = True def lambda_unsatisfied_cb(self, expr): - """Called when the lambda condition is no longer satisfied.""" with self._lock: - if expr in self.sat_expr_timer: - self.sat_expr_timer[expr].cancel() - del self.sat_expr_timer[expr] - if expr in self.conditions: self.conditions[expr]["satisfied"] = False - def published_cb(self, msg): - """Called when a 'published' message is received.""" - self.debug(f"[TopicMonitor] Topic {self.topic_name} is published") - if "published" in self.conditions: - self.conditions["published"]["satisfied"] = True + def check_not_published(self): + if self.signal_when_cfg.get("signal_when","")=="not published": + stats = self.hz_monitor.get_hz() + if stats is None and not self.conditions["not published"]["satisfied"]: + self.conditions["not published"]["satisfied"] = True + if self.signal_when_cfg["default_notifications"]: + lvl = "error" if self.signal_when_cfg["safety_critical"] else "warn" + self._event_callback(f"Topic {self.topic_name} not published", lvl) def log_hz_stats(self): - """Timer callback to log frequency stats from ROSTopicHz.""" - if not self.hz_monitor.enabled: - return stats = self.hz_monitor.get_hz() if stats: - rate, min_d, max_d, std_d, n = stats - if self.enable_internal_logs: - self.node.get_logger().info( - f"[HzMonitor] Rate={rate:.2f} Hz | Min={min_d:.3f}s | Max={max_d:.3f}s | Std={std_d:.4f}s | N={n}" - ) - else: - if self.enable_internal_logs: - self.node.get_logger().info("[HzMonitor] No Hz stats yet. Waiting for more data.") - - def execute(self, msg=None, process_indices=None): - """Triggers the executor to run configured processes if available.""" - if self.processes and self.executor: - self.executor.execute(msg, process_indices) + rate, *_ = stats + self.node.get_logger().info(f"[HzMonitor] {self.topic_name} Rate={rate:.2f}Hz") def run(self): - """Background thread spin loop for the TopicMonitor's node.""" - while True: - if not self._stop_event.is_set(): - rclpy.spin_once(self.node, timeout_sec=0.1) - else: - time.sleep(0.1) + while not self._stop_event.is_set(): + rclpy.spin_once(self.node, timeout_sec=0.1) def stop_monitor(self): self._stop_event.set() @@ -247,13 +215,11 @@ def stop_monitor(self): def start_monitor(self): self._stop_event.clear() - self.node.get_logger().info(f"[TopicMonitor] Restarting monitor for {self.topic_name}") self.hz_monitor.start_monitoring(self.msg_type, self.qos_profile) + def kill_monitor(self): + self.stop_monitor() + self.node.destroy_node() + def get_node(self): - """Returns the node so it can be added to an executor externally.""" return self.node - - def event_callback(message, level="info", msg=None, topic_name=None): - print(f"[{level.upper()}] {message}") - diff --git a/sentor/sentor/test_executer.py b/sentor/sentor/test_executer.py deleted file mode 100644 index 3aff9c5..0000000 --- a/sentor/sentor/test_executer.py +++ /dev/null @@ -1,77 +0,0 @@ -#!/usr/bin/env python3 -import rclpy -from rclpy.node import Node -from sentor.Executor import Executor # ✅ Import updated Executor class -from sentor.service_types import get_service_type # ✅ Import service mapping function -import subprocess -import time - -def test_nav_map_services(): - rclpy.init() - node = Executor([], print) - - print("\n🔹 Checking Available Services...") - available_services = subprocess.run(["ros2", "service", "list"], capture_output=True, text=True).stdout - - # ✅ List of navigation and mapping services to test - test_services = [ - "/map_server/get_parameters", - "/map_server/set_parameters", - "/map_server/get_state", - "/map_server/list_parameters", - "/planner_server/get_state", - "/planner_server/change_state" - ] - - for service in test_services: - print(f"\n🔹 Testing service: {service}") - if service in available_services: - try: - service_type = get_service_type(service) - client = node.create_client(service_type, service) - request = service_type.Request() - - # ✅ Auto-fill request data dynamically - if hasattr(request, 'names'): - request.names = ["use_sim_time"] - if hasattr(request, 'state'): - request.state = 1 # Example state change request - - future = client.call_async(request) - rclpy.spin_until_future_complete(node, future) - - if future.done(): - print(f"✅ Service {service} Response: {future.result()}") - else: - print(f"❌ Service {service} call failed!") - - except Exception as e: - print(f"❌ Error calling {service}: {str(e)}") - else: - print(f"❌ Service {service} is not available!") - - # ✅ Test Sleep Function - print("\n🔹 Testing sleep function...") - node.sleep(2) - print("✅ Sleep function test passed.") - - # ✅ Test Logging - print("\n🔹 Testing log function...") - node.event_cb("Test log message", "info") - print("✅ Log function test passed.") - - # ✅ Test Publishing - print("\n🔹 Testing publish function...") - from std_msgs.msg import String - pub = node.create_publisher(String, "/chatter", 10) - time.sleep(1) # Ensure the publisher initializes properly - msg = String() - msg.data = "Hello from test script" - pub.publish(msg) - print("✅ Publish function test passed.") - - node.destroy_node() - rclpy.shutdown() - -if __name__ == "__main__": - test_nav_map_services() From 3753120b5c3b32ed9b3d2e64db7440f39a30dac1 Mon Sep 17 00:00:00 2001 From: Cyano0 <36192489+Cyano0@users.noreply.github.com> Date: Wed, 21 May 2025 13:51:20 +0100 Subject: [PATCH 03/21] Updat README.md --- README.md | 176 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 176 insertions(+) diff --git a/README.md b/README.md index d711678..273fa5c 100644 --- a/README.md +++ b/README.md @@ -2,3 +2,179 @@ Continuously monitor topic messages. Send warnings and execute other processes when certain conditions on the messages are satisfied. See the [wiki](https://github.com/LCAS/sentor/wiki/sentor). +# Sentor Monitoring Configuration + +This document explains how to structure your YAML config file for **Sentor**’s topic- and node-monitoring, and how the two “heartbeats” (`safety/heartbeat` and `warning/heartbeat`) are driven by your *safety-critical* and *autonomy-critical* flags. + +--- + +## Table of Contents +1. [Overview](#overview) +2. [Top-Level Structure](#top-level-structure) +3. [`monitors:` (TopicMonitors)](#monitors-topicmonitors) + * [Common fields](#common-fields) + * [`signal_when`](#signal_when) + * [`signal_lambdas`](#signal_lambdas) + * [Example](#example-topic-monitor) +4. [`node_monitors:` (NodeMonitors)](#node_monitors-nodemonitors) + * [Fields](#fields) + * [Example](#example-node-monitor) +5. [Heartbeats](#heartbeats) + * [Safety heartbeat (`safety/heartbeat`)](#safety-heartbeat-safetyheartbeat) + * [Warning heartbeat (`warning/heartbeat`)](#warning-heartbeat-warningheartbeat) + +--- + +## Overview + +| Component | What it watches | Critical flags in YAML | Feeds into | +|------------------|----------------------------------------------------|--------------------------------------------|------------| +| **TopicMonitor** | Rate + message content of one topic | `safety_critical`, `autonomy_critical` | both beats | +| **NodeMonitor** | Presence of a node on the ROS graph | `safety_critical`, `autonomy_critical` | both beats | +| **SafetyMonitor**| Aggregates monitors and publishes a Boolean topic | — | one beat | + +If **all** linked monitors are healthy for at least `safe_operation_timeout` seconds, the heartbeat publishes **`true`**; otherwise it immediately publishes **`false`**. + +--- + +## Top-Level Structure + +~~~yaml +monitors: # list of topic monitors + - … # one map per topic +node_monitors: # list of node monitors + - … # one map per node +~~~ + +*(Spaces only, no tabs.)* + +--- + +## `monitors:` (TopicMonitors) + +### Common fields + +| Field | Type | Req | Description | +|-------|------|-----|-------------| +| `name` | string | ✔ | Full topic name (`/camera/image_raw`) | +| `message_type` | string | ✔ | Package/type (`sensor_msgs/msg/Image`) | +| `rate` | number | ✔ | Minimum acceptable publish rate (Hz) | +| `N` | int | • | Window size for rate (default `5`) | +| `qos` | map | • | Override QoS (`reliability`, `durability`, `depth`) | +| `timeout` | sec | • | Default lambda / signal timeout (default `10`) | +| `default_notifications` | bool | • | Log flips automatically? (default `true`) | +| `enable_internal_logs` | bool | • | Extra per-topic debug (default `false`) | + +### `signal_when` + +~~~yaml +signal_when: + condition: "published" # or "not published" + timeout: 2.0 # seconds before failure + safety_critical: false + autonomy_critical: true + tags: ["camera", "liveliness"] # optional free-text labels +~~~ + +### `signal_lambdas` + +~~~yaml +signal_lambdas: + - expression: "lambda x: x.height == 480" + timeout: 2.0 + safety_critical: true # affects safety beat + autonomy_critical: false # doesn’t affect warning beat + process_indices: [2] # indexes into `execute` list (optional) + repeat_exec: false # run every time satisfied? + tags: ["resolution"] +~~~ + +### Example Topic Monitor + +~~~yaml +- name: "/front_camera/camera_info" + message_type: "sensor_msgs/msg/CameraInfo" + rate: 8 + N: 5 + qos: + reliability: "reliable" + durability: "volatile" + depth: 5 + signal_when: + condition: "published" + timeout: 2 + safety_critical: false + autonomy_critical: true + signal_lambdas: + - expression: "lambda x: x.height < 480" + timeout: 2 + safety_critical: true + autonomy_critical: false + execute: [] # optional shell / ROS actions + timeout: 10 + default_notifications: false +~~~ + +--- + +## `node_monitors:` (NodeMonitors) + +### Fields + +| Field | Type | Req | Description | +|-------|------|-----|-------------| +| `name` | string | ✔ | Exact node name (`/camera_driver`) | +| `timeout` | sec | ✔ | Max age since last seen | +| `safety_critical` | bool | • | Flip safety beat when missing | +| `autonomy_critical` | bool | • | Flip warning beat when missing | +| `poll_rate` | Hz | • | Graph query rate (default `1.0`) | +| `tags` | list | • | Labels | + +### Example Node Monitor + +~~~yaml +node_monitors: + - name: "/front_camera_camera_controller" + timeout: 2.0 + safety_critical: true + autonomy_critical: false + + - name: "/back_camera_camera_controller" + timeout: 1.0 + safety_critical: false + autonomy_critical: true +~~~ + +--- + +## Heartbeats + +### Safety heartbeat (`safety/heartbeat`) + +* Publishes `std_msgs/Bool` +* **TRUE** when **all** safety-critical monitors are satisfied for `safe_operation_timeout` +* **FALSE** immediately on first safety-critical failure + +### Warning heartbeat (`warning/heartbeat`) + +* Publishes `std_msgs/Bool` +* **TRUE** when all autonomy-critical monitors are satisfied +* **FALSE** on first autonomy-critical failure + (use for degradations that allow limp-home operation) + +--- + +## Quick checklist + +* ✔ Every topic lists `message_type` **and** `rate`. +* ✔ Mark each rule/node as `safety_critical` or `autonomy_critical` (or both). +* ✔ No critical flag ⇒ the rule is informational only. +* ✔ Adjust parameters at launch: + +```bash +ros2 run sentor test_sentor.py \ + --ros-args \ + -p config_file:=/path/to/monitor.yaml \ + -p safety_pub_rate:=1.0 \ + -p safe_operation_timeout:=5.0 + From db297059386ea9ddfb9e64807031810a9082baaf Mon Sep 17 00:00:00 2001 From: Cyano0 <36192489+Cyano0@users.noreply.github.com> Date: Wed, 21 May 2025 15:29:27 +0100 Subject: [PATCH 04/21] Updat README.md --- README.md | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/README.md b/README.md index 273fa5c..0a71072 100644 --- a/README.md +++ b/README.md @@ -177,4 +177,16 @@ ros2 run sentor test_sentor.py \ -p config_file:=/path/to/monitor.yaml \ -p safety_pub_rate:=1.0 \ -p safe_operation_timeout:=5.0 +``` + **Note** + +- /safety/heartbeat toggles **TRUE** only when all safety-critical items pass. + +- /warning/heartbeat toggles **TRUE** when autonomy-critical items pass. + +- Killing a safety-critical publisher flips both beats to FALSE immediately. + +- Killing only an autonomy-critical node flips warning beat but leaves safety unchanged. + +- If desired, run ros2 topic echo /safety/heartbeat and /warning/heartbeat in separate terminals to verify. From ec020a231926b236d06a41a44433f25c9d32e747 Mon Sep 17 00:00:00 2001 From: Cyano0 <36192489+Cyano0@users.noreply.github.com> Date: Mon, 28 Jul 2025 11:27:31 +0100 Subject: [PATCH 05/21] Update sentor/sentor/TopicMonitor.py Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- sentor/sentor/TopicMonitor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sentor/sentor/TopicMonitor.py b/sentor/sentor/TopicMonitor.py index 77c49bd..569277e 100644 --- a/sentor/sentor/TopicMonitor.py +++ b/sentor/sentor/TopicMonitor.py @@ -80,7 +80,7 @@ def __init__( # thread‐safe executor for any “processes” self.executor = Executor(processes, event_callback) if processes else None - # --- THIS WAS MISSING in your version! ---------------------------- + # Build self.signal_when_cfg and seed self.conditions for “signal_when” self.process_signal_config() # ------------------------------------------------------------------ From 761bc724edaf909de4b853e1885ac53c5a78023a Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Mon, 28 Jul 2025 13:33:49 +0000 Subject: [PATCH 06/21] restructure for devcontainer etc --- .devcontainer/Dockerfile | 34 +++++++++ .devcontainer/devcontainer.json | 35 ++++++++++ .devcontainer/post-create.sh | 35 ++++++++++ .github/workflows/dev-container.yml | 29 ++++++++ .github/workflows/ros-ci.yml | 70 +++++++++++++++++++ CMakeLists.txt => src/CMakeLists.txt | 0 {config => src/config}/execute.yaml | 0 {config => src/config}/map.yaml | 0 {config => src/config}/rob_lindsey.yaml | 0 {config => src/config}/test.yaml | 0 {launch => src/launch}/sentor.launch | 0 {launch => src/launch}/topic_mapping.launch | 0 {msg => src/msg}/Monitor.msg | 0 {msg => src/msg}/MonitorArray.msg | 0 {msg => src/msg}/SentorEvent.msg | 0 {msg => src/msg}/TopicMap.msg | 0 {msg => src/msg}/TopicMapArray.msg | 0 package.xml => src/package.xml | 0 {scripts => src/scripts}/sentor_node.py | 0 {scripts => src/scripts}/test.py | 0 .../scripts}/topic_mapping_node.py | 0 setup.py => src/setup.py | 0 src/{ => src}/sentor/CustomLambdaExample.py | 0 src/{ => src}/sentor/CustomProcessExample.py | 0 src/{ => src}/sentor/Executor.py | 0 src/{ => src}/sentor/MultiMonitor.py | 0 src/{ => src}/sentor/ROSTopicFilter.py | 0 src/{ => src}/sentor/ROSTopicHz.py | 0 src/{ => src}/sentor/ROSTopicPub.py | 0 src/{ => src}/sentor/SafetyMonitor.py | 0 src/{ => src}/sentor/TopicMapServer.py | 0 src/{ => src}/sentor/TopicMapper.py | 0 src/{ => src}/sentor/TopicMonitor.py | 0 src/{ => src}/sentor/__init__.py | 0 {srv => src/srv}/GetTopicMaps.srv | 0 35 files changed, 203 insertions(+) create mode 100644 .devcontainer/Dockerfile create mode 100644 .devcontainer/devcontainer.json create mode 100755 .devcontainer/post-create.sh create mode 100644 .github/workflows/dev-container.yml create mode 100644 .github/workflows/ros-ci.yml rename CMakeLists.txt => src/CMakeLists.txt (100%) rename {config => src/config}/execute.yaml (100%) rename {config => src/config}/map.yaml (100%) rename {config => src/config}/rob_lindsey.yaml (100%) rename {config => src/config}/test.yaml (100%) rename {launch => src/launch}/sentor.launch (100%) rename {launch => src/launch}/topic_mapping.launch (100%) rename {msg => src/msg}/Monitor.msg (100%) rename {msg => src/msg}/MonitorArray.msg (100%) rename {msg => src/msg}/SentorEvent.msg (100%) rename {msg => src/msg}/TopicMap.msg (100%) rename {msg => src/msg}/TopicMapArray.msg (100%) rename package.xml => src/package.xml (100%) rename {scripts => src/scripts}/sentor_node.py (100%) rename {scripts => src/scripts}/test.py (100%) rename {scripts => src/scripts}/topic_mapping_node.py (100%) rename setup.py => src/setup.py (100%) rename src/{ => src}/sentor/CustomLambdaExample.py (100%) rename src/{ => src}/sentor/CustomProcessExample.py (100%) rename src/{ => src}/sentor/Executor.py (100%) rename src/{ => src}/sentor/MultiMonitor.py (100%) rename src/{ => src}/sentor/ROSTopicFilter.py (100%) rename src/{ => src}/sentor/ROSTopicHz.py (100%) rename src/{ => src}/sentor/ROSTopicPub.py (100%) rename src/{ => src}/sentor/SafetyMonitor.py (100%) rename src/{ => src}/sentor/TopicMapServer.py (100%) rename src/{ => src}/sentor/TopicMapper.py (100%) rename src/{ => src}/sentor/TopicMonitor.py (100%) rename src/{ => src}/sentor/__init__.py (100%) rename {srv => src/srv}/GetTopicMaps.srv (100%) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 0000000..6e232bc --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,34 @@ +ARG BASE_IMAGE=lcas.lincoln.ac.uk/lcas/ros-docker-images:humble-2 + +FROM ${BASE_IMAGE} as base + +USER root + +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update \ + && apt-get install -qq -y --no-install-recommends \ + git \ + python3-pip \ + python3-rosdep + +# get the source tree and analyse it for its package.xml only +FROM base as sourcefilter +COPY ./src /tmp/src +# remove everything that isn't package.xml +RUN find /tmp/src -type f \! -name "package.xml" -print | xargs rm -rf + +# install all dependencies listed in the package.xml +FROM base as depbuilder +# copy the reduced source tree (only package.xml) from previous stage +COPY --from=sourcefilter /tmp/src /tmp/src +RUN rosdep update --rosdistro ${ROS_DISTRO} && apt-get update +RUN cd /tmp/src && rosdep install --from-paths . --ignore-src -r -y && cd && rm -rf /tmp/src + +FROM depbuilder as final +# add user ros with sudo rights if it doesn't exist + +# add sudo without password +ENV DEBIAN_FRONTEND=noninteractive + +USER ros \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 0000000..f50367b --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,35 @@ +// For format details, see https://aka.ms/devcontainer.json. For config options, see the +// README at: https://github.com/devcontainers/templates/tree/main/src/ubuntu +{ + "name": "L-CAS Humble CUDA-OpenGL Devcontainer", + "build": { + "dockerfile": "./Dockerfile", + "args": { + "BASE_IMAGE": "lcas.lincoln.ac.uk/lcas/ros-docker-images:humble-2" + }, + "context": ".." + }, + + "postStartCommand": "/opt/entrypoint.sh /bin/true; .devcontainer/post-create.sh", + + "remoteUser": "ros", + "updateRemoteUserUID": true, // ensure internal user has the same UID as the host user and update file permissions + "customizations": { + "vscode": { + "extensions": [ + "ms-python.python", + "GitHub.vscode-pull-request-github", + "ms-vscode.cpptools", + "JaehyunShim.vscode-ros2", + "nonanonno.vscode-ros2", + "deitry.colcon-helper", + "github.vscode-github-actions" + ] + } + }, + "hostRequirements": { + "gpu": "optional", + "cpus": 2, + "memory": "8gb" + } +} diff --git a/.devcontainer/post-create.sh b/.devcontainer/post-create.sh new file mode 100755 index 0000000..dc2b09f --- /dev/null +++ b/.devcontainer/post-create.sh @@ -0,0 +1,35 @@ +#!/bin/bash + +set -xe + + +function add_config_if_not_exist { + if ! grep -F -q "$1" $HOME/.bashrc; then + echo "$1" >> $HOME/.bashrc + fi +} + +function add_git_config_if_not_exist { + if ! git config --global --get "$1" > /dev/null; then + git config --global "$1" "$2" + fi +} + +add_config_if_not_exist "source /opt/ros/humble/setup.bash" + +source /opt/ros/humble/setup.bash + +colcon build --symlink-install --continue-on-error || true + +LOCAL_SETUP_FILE=`pwd`/install/setup.bash +add_config_if_not_exist "if [ -r $LOCAL_SETUP_FILE ]; then source $LOCAL_SETUP_FILE; fi" + + +add_git_config_if_not_exist "core.autocrlf" "input" +add_git_config_if_not_exist "core.safecrlf" "warn" +add_git_config_if_not_exist "pull.rebase" "false" +add_git_config_if_not_exist "user.name" "Anonymous L-CAS DevContainer User" +add_git_config_if_not_exist "user.email" "noreply@lcas.lincoln.ac.uk" +add_git_config_if_not_exist "init.defaultBranch" "main" + + diff --git a/.github/workflows/dev-container.yml b/.github/workflows/dev-container.yml new file mode 100644 index 0000000..d0ae6e1 --- /dev/null +++ b/.github/workflows/dev-container.yml @@ -0,0 +1,29 @@ +name: 'devcontainer CI' +on: + workflow_dispatch: + pull_request: + branches: + - main + push: + branches: + - main + +jobs: + build_devcontainer: + runs-on: ubuntu-latest + steps: + - name: Checkout from github + uses: actions/checkout@v3 + - name: extract the github reference + run: echo "BRANCH=${GITHUB_REF##*/}" >> $GITHUB_ENV + - name: "image name from repo name" + id: docker_image_name + run: echo "docker_image=${{ github.repository }}" | tr '[:upper:]' '[:lower:]' |sed 's/[^0-9,a-z,A-Z,=,_,\/]/-/g' >>${GITHUB_OUTPUT} + - name: Build and run dev container task + uses: devcontainers/ci@v0.3 + with: + imageName: devcontainer/${{ steps.docker_image_name.outputs.docker_image }} + configFile: ./.devcontainer/devcontainer.json + push: never + #imageTag: ${{ matrix.config }}-${{ env.BRANCH }} + #runCmd: "bash .devcontainer/run-ci.sh" diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml new file mode 100644 index 0000000..ca82656 --- /dev/null +++ b/.github/workflows/ros-ci.yml @@ -0,0 +1,70 @@ +name: ros CI + +on: + push: + # you may want to configure the branches that this should be run on here. + branches: [ "main" ] + pull_request: + branches: [ "main" ] + +jobs: + test_docker: # On Linux, iterates on all ROS 1 and ROS 2 distributions. + runs-on: ubuntu-latest + strategy: + matrix: + ros_distribution: + # - noetic + - humble + # - iron + + # Define the Docker image(s) associated with each ROS distribution. + # The include syntax allows additional variables to be defined, like + # docker_image in this case. See documentation: + # https://help.github.com/en/actions/reference/workflow-syntax-for-github-actions#example-including-configurations-in-a-matrix-build + # + # Platforms are defined in REP 3 and REP 2000: + # https://ros.org/reps/rep-0003.html + # https://ros.org/reps/rep-2000.html + include: + # Noetic Ninjemys (May 2020 - May 2025) + # - docker_image: ubuntu:focal + # ros_distribution: noetic + # ros_version: 1 + + # Humble Hawksbill (May 2022 - May 2027) + - docker_image: ubuntu:jammy + ros_distribution: humble + ros_version: 2 + + # Iron Irwini (May 2023 - November 2024) + # - docker_image: ubuntu:jammy + # ros_distribution: iron + # ros_version: 2 + + # # Rolling Ridley (No End-Of-Life) + # - docker_image: ubuntu:jammy + # ros_distribution: rolling + # ros_version: 2 + + container: + image: ${{ matrix.docker_image }} + steps: + - uses: actions/checkout@v3 + - name: setup ROS environment + uses: LCAS/setup-ros@master + with: + required-ros-distributions: ${{ matrix.ros_distribution }} + - name: build and test ROS 1 + if: ${{ matrix.ros_version == 1 }} + uses: ros-tooling/action-ros-ci@v0.3 + with: + import-token: ${{ github.token }} + target-ros1-distro: ${{ matrix.ros_distribution }} + skip-tests: true + - name: build and test ROS 2 + if: ${{ matrix.ros_version == 2 }} + uses: ros-tooling/action-ros-ci@v0.3 + with: + import-token: ${{ github.token }} + target-ros2-distro: ${{ matrix.ros_distribution }} + skip-tests: true diff --git a/CMakeLists.txt b/src/CMakeLists.txt similarity index 100% rename from CMakeLists.txt rename to src/CMakeLists.txt diff --git a/config/execute.yaml b/src/config/execute.yaml similarity index 100% rename from config/execute.yaml rename to src/config/execute.yaml diff --git a/config/map.yaml b/src/config/map.yaml similarity index 100% rename from config/map.yaml rename to src/config/map.yaml diff --git a/config/rob_lindsey.yaml b/src/config/rob_lindsey.yaml similarity index 100% rename from config/rob_lindsey.yaml rename to src/config/rob_lindsey.yaml diff --git a/config/test.yaml b/src/config/test.yaml similarity index 100% rename from config/test.yaml rename to src/config/test.yaml diff --git a/launch/sentor.launch b/src/launch/sentor.launch similarity index 100% rename from launch/sentor.launch rename to src/launch/sentor.launch diff --git a/launch/topic_mapping.launch b/src/launch/topic_mapping.launch similarity index 100% rename from launch/topic_mapping.launch rename to src/launch/topic_mapping.launch diff --git a/msg/Monitor.msg b/src/msg/Monitor.msg similarity index 100% rename from msg/Monitor.msg rename to src/msg/Monitor.msg diff --git a/msg/MonitorArray.msg b/src/msg/MonitorArray.msg similarity index 100% rename from msg/MonitorArray.msg rename to src/msg/MonitorArray.msg diff --git a/msg/SentorEvent.msg b/src/msg/SentorEvent.msg similarity index 100% rename from msg/SentorEvent.msg rename to src/msg/SentorEvent.msg diff --git a/msg/TopicMap.msg b/src/msg/TopicMap.msg similarity index 100% rename from msg/TopicMap.msg rename to src/msg/TopicMap.msg diff --git a/msg/TopicMapArray.msg b/src/msg/TopicMapArray.msg similarity index 100% rename from msg/TopicMapArray.msg rename to src/msg/TopicMapArray.msg diff --git a/package.xml b/src/package.xml similarity index 100% rename from package.xml rename to src/package.xml diff --git a/scripts/sentor_node.py b/src/scripts/sentor_node.py similarity index 100% rename from scripts/sentor_node.py rename to src/scripts/sentor_node.py diff --git a/scripts/test.py b/src/scripts/test.py similarity index 100% rename from scripts/test.py rename to src/scripts/test.py diff --git a/scripts/topic_mapping_node.py b/src/scripts/topic_mapping_node.py similarity index 100% rename from scripts/topic_mapping_node.py rename to src/scripts/topic_mapping_node.py diff --git a/setup.py b/src/setup.py similarity index 100% rename from setup.py rename to src/setup.py diff --git a/src/sentor/CustomLambdaExample.py b/src/src/sentor/CustomLambdaExample.py similarity index 100% rename from src/sentor/CustomLambdaExample.py rename to src/src/sentor/CustomLambdaExample.py diff --git a/src/sentor/CustomProcessExample.py b/src/src/sentor/CustomProcessExample.py similarity index 100% rename from src/sentor/CustomProcessExample.py rename to src/src/sentor/CustomProcessExample.py diff --git a/src/sentor/Executor.py b/src/src/sentor/Executor.py similarity index 100% rename from src/sentor/Executor.py rename to src/src/sentor/Executor.py diff --git a/src/sentor/MultiMonitor.py b/src/src/sentor/MultiMonitor.py similarity index 100% rename from src/sentor/MultiMonitor.py rename to src/src/sentor/MultiMonitor.py diff --git a/src/sentor/ROSTopicFilter.py b/src/src/sentor/ROSTopicFilter.py similarity index 100% rename from src/sentor/ROSTopicFilter.py rename to src/src/sentor/ROSTopicFilter.py diff --git a/src/sentor/ROSTopicHz.py b/src/src/sentor/ROSTopicHz.py similarity index 100% rename from src/sentor/ROSTopicHz.py rename to src/src/sentor/ROSTopicHz.py diff --git a/src/sentor/ROSTopicPub.py b/src/src/sentor/ROSTopicPub.py similarity index 100% rename from src/sentor/ROSTopicPub.py rename to src/src/sentor/ROSTopicPub.py diff --git a/src/sentor/SafetyMonitor.py b/src/src/sentor/SafetyMonitor.py similarity index 100% rename from src/sentor/SafetyMonitor.py rename to src/src/sentor/SafetyMonitor.py diff --git a/src/sentor/TopicMapServer.py b/src/src/sentor/TopicMapServer.py similarity index 100% rename from src/sentor/TopicMapServer.py rename to src/src/sentor/TopicMapServer.py diff --git a/src/sentor/TopicMapper.py b/src/src/sentor/TopicMapper.py similarity index 100% rename from src/sentor/TopicMapper.py rename to src/src/sentor/TopicMapper.py diff --git a/src/sentor/TopicMonitor.py b/src/src/sentor/TopicMonitor.py similarity index 100% rename from src/sentor/TopicMonitor.py rename to src/src/sentor/TopicMonitor.py diff --git a/src/sentor/__init__.py b/src/src/sentor/__init__.py similarity index 100% rename from src/sentor/__init__.py rename to src/src/sentor/__init__.py diff --git a/srv/GetTopicMaps.srv b/src/srv/GetTopicMaps.srv similarity index 100% rename from srv/GetTopicMaps.srv rename to src/srv/GetTopicMaps.srv From 6e1cd40b315130a331f2efa0e2a0e3f08d6e2ee0 Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Mon, 28 Jul 2025 13:36:50 +0000 Subject: [PATCH 07/21] Revert "restructure for devcontainer etc" This reverts commit 761bc724edaf909de4b853e1885ac53c5a78023a. --- .devcontainer/Dockerfile | 34 --------- .devcontainer/devcontainer.json | 35 ---------- .devcontainer/post-create.sh | 35 ---------- .github/workflows/dev-container.yml | 29 -------- .github/workflows/ros-ci.yml | 70 ------------------- src/CMakeLists.txt => CMakeLists.txt | 0 {src/config => config}/execute.yaml | 0 {src/config => config}/map.yaml | 0 {src/config => config}/rob_lindsey.yaml | 0 {src/config => config}/test.yaml | 0 {src/launch => launch}/sentor.launch | 0 {src/launch => launch}/topic_mapping.launch | 0 {src/msg => msg}/Monitor.msg | 0 {src/msg => msg}/MonitorArray.msg | 0 {src/msg => msg}/SentorEvent.msg | 0 {src/msg => msg}/TopicMap.msg | 0 {src/msg => msg}/TopicMapArray.msg | 0 src/package.xml => package.xml | 0 {src/scripts => scripts}/sentor_node.py | 0 {src/scripts => scripts}/test.py | 0 .../scripts => scripts}/topic_mapping_node.py | 0 src/setup.py => setup.py | 0 src/{src => }/sentor/CustomLambdaExample.py | 0 src/{src => }/sentor/CustomProcessExample.py | 0 src/{src => }/sentor/Executor.py | 0 src/{src => }/sentor/MultiMonitor.py | 0 src/{src => }/sentor/ROSTopicFilter.py | 0 src/{src => }/sentor/ROSTopicHz.py | 0 src/{src => }/sentor/ROSTopicPub.py | 0 src/{src => }/sentor/SafetyMonitor.py | 0 src/{src => }/sentor/TopicMapServer.py | 0 src/{src => }/sentor/TopicMapper.py | 0 src/{src => }/sentor/TopicMonitor.py | 0 src/{src => }/sentor/__init__.py | 0 {src/srv => srv}/GetTopicMaps.srv | 0 35 files changed, 203 deletions(-) delete mode 100644 .devcontainer/Dockerfile delete mode 100644 .devcontainer/devcontainer.json delete mode 100755 .devcontainer/post-create.sh delete mode 100644 .github/workflows/dev-container.yml delete mode 100644 .github/workflows/ros-ci.yml rename src/CMakeLists.txt => CMakeLists.txt (100%) rename {src/config => config}/execute.yaml (100%) rename {src/config => config}/map.yaml (100%) rename {src/config => config}/rob_lindsey.yaml (100%) rename {src/config => config}/test.yaml (100%) rename {src/launch => launch}/sentor.launch (100%) rename {src/launch => launch}/topic_mapping.launch (100%) rename {src/msg => msg}/Monitor.msg (100%) rename {src/msg => msg}/MonitorArray.msg (100%) rename {src/msg => msg}/SentorEvent.msg (100%) rename {src/msg => msg}/TopicMap.msg (100%) rename {src/msg => msg}/TopicMapArray.msg (100%) rename src/package.xml => package.xml (100%) rename {src/scripts => scripts}/sentor_node.py (100%) rename {src/scripts => scripts}/test.py (100%) rename {src/scripts => scripts}/topic_mapping_node.py (100%) rename src/setup.py => setup.py (100%) rename src/{src => }/sentor/CustomLambdaExample.py (100%) rename src/{src => }/sentor/CustomProcessExample.py (100%) rename src/{src => }/sentor/Executor.py (100%) rename src/{src => }/sentor/MultiMonitor.py (100%) rename src/{src => }/sentor/ROSTopicFilter.py (100%) rename src/{src => }/sentor/ROSTopicHz.py (100%) rename src/{src => }/sentor/ROSTopicPub.py (100%) rename src/{src => }/sentor/SafetyMonitor.py (100%) rename src/{src => }/sentor/TopicMapServer.py (100%) rename src/{src => }/sentor/TopicMapper.py (100%) rename src/{src => }/sentor/TopicMonitor.py (100%) rename src/{src => }/sentor/__init__.py (100%) rename {src/srv => srv}/GetTopicMaps.srv (100%) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile deleted file mode 100644 index 6e232bc..0000000 --- a/.devcontainer/Dockerfile +++ /dev/null @@ -1,34 +0,0 @@ -ARG BASE_IMAGE=lcas.lincoln.ac.uk/lcas/ros-docker-images:humble-2 - -FROM ${BASE_IMAGE} as base - -USER root - -ENV DEBIAN_FRONTEND=noninteractive - -RUN apt-get update \ - && apt-get install -qq -y --no-install-recommends \ - git \ - python3-pip \ - python3-rosdep - -# get the source tree and analyse it for its package.xml only -FROM base as sourcefilter -COPY ./src /tmp/src -# remove everything that isn't package.xml -RUN find /tmp/src -type f \! -name "package.xml" -print | xargs rm -rf - -# install all dependencies listed in the package.xml -FROM base as depbuilder -# copy the reduced source tree (only package.xml) from previous stage -COPY --from=sourcefilter /tmp/src /tmp/src -RUN rosdep update --rosdistro ${ROS_DISTRO} && apt-get update -RUN cd /tmp/src && rosdep install --from-paths . --ignore-src -r -y && cd && rm -rf /tmp/src - -FROM depbuilder as final -# add user ros with sudo rights if it doesn't exist - -# add sudo without password -ENV DEBIAN_FRONTEND=noninteractive - -USER ros \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json deleted file mode 100644 index f50367b..0000000 --- a/.devcontainer/devcontainer.json +++ /dev/null @@ -1,35 +0,0 @@ -// For format details, see https://aka.ms/devcontainer.json. For config options, see the -// README at: https://github.com/devcontainers/templates/tree/main/src/ubuntu -{ - "name": "L-CAS Humble CUDA-OpenGL Devcontainer", - "build": { - "dockerfile": "./Dockerfile", - "args": { - "BASE_IMAGE": "lcas.lincoln.ac.uk/lcas/ros-docker-images:humble-2" - }, - "context": ".." - }, - - "postStartCommand": "/opt/entrypoint.sh /bin/true; .devcontainer/post-create.sh", - - "remoteUser": "ros", - "updateRemoteUserUID": true, // ensure internal user has the same UID as the host user and update file permissions - "customizations": { - "vscode": { - "extensions": [ - "ms-python.python", - "GitHub.vscode-pull-request-github", - "ms-vscode.cpptools", - "JaehyunShim.vscode-ros2", - "nonanonno.vscode-ros2", - "deitry.colcon-helper", - "github.vscode-github-actions" - ] - } - }, - "hostRequirements": { - "gpu": "optional", - "cpus": 2, - "memory": "8gb" - } -} diff --git a/.devcontainer/post-create.sh b/.devcontainer/post-create.sh deleted file mode 100755 index dc2b09f..0000000 --- a/.devcontainer/post-create.sh +++ /dev/null @@ -1,35 +0,0 @@ -#!/bin/bash - -set -xe - - -function add_config_if_not_exist { - if ! grep -F -q "$1" $HOME/.bashrc; then - echo "$1" >> $HOME/.bashrc - fi -} - -function add_git_config_if_not_exist { - if ! git config --global --get "$1" > /dev/null; then - git config --global "$1" "$2" - fi -} - -add_config_if_not_exist "source /opt/ros/humble/setup.bash" - -source /opt/ros/humble/setup.bash - -colcon build --symlink-install --continue-on-error || true - -LOCAL_SETUP_FILE=`pwd`/install/setup.bash -add_config_if_not_exist "if [ -r $LOCAL_SETUP_FILE ]; then source $LOCAL_SETUP_FILE; fi" - - -add_git_config_if_not_exist "core.autocrlf" "input" -add_git_config_if_not_exist "core.safecrlf" "warn" -add_git_config_if_not_exist "pull.rebase" "false" -add_git_config_if_not_exist "user.name" "Anonymous L-CAS DevContainer User" -add_git_config_if_not_exist "user.email" "noreply@lcas.lincoln.ac.uk" -add_git_config_if_not_exist "init.defaultBranch" "main" - - diff --git a/.github/workflows/dev-container.yml b/.github/workflows/dev-container.yml deleted file mode 100644 index d0ae6e1..0000000 --- a/.github/workflows/dev-container.yml +++ /dev/null @@ -1,29 +0,0 @@ -name: 'devcontainer CI' -on: - workflow_dispatch: - pull_request: - branches: - - main - push: - branches: - - main - -jobs: - build_devcontainer: - runs-on: ubuntu-latest - steps: - - name: Checkout from github - uses: actions/checkout@v3 - - name: extract the github reference - run: echo "BRANCH=${GITHUB_REF##*/}" >> $GITHUB_ENV - - name: "image name from repo name" - id: docker_image_name - run: echo "docker_image=${{ github.repository }}" | tr '[:upper:]' '[:lower:]' |sed 's/[^0-9,a-z,A-Z,=,_,\/]/-/g' >>${GITHUB_OUTPUT} - - name: Build and run dev container task - uses: devcontainers/ci@v0.3 - with: - imageName: devcontainer/${{ steps.docker_image_name.outputs.docker_image }} - configFile: ./.devcontainer/devcontainer.json - push: never - #imageTag: ${{ matrix.config }}-${{ env.BRANCH }} - #runCmd: "bash .devcontainer/run-ci.sh" diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml deleted file mode 100644 index ca82656..0000000 --- a/.github/workflows/ros-ci.yml +++ /dev/null @@ -1,70 +0,0 @@ -name: ros CI - -on: - push: - # you may want to configure the branches that this should be run on here. - branches: [ "main" ] - pull_request: - branches: [ "main" ] - -jobs: - test_docker: # On Linux, iterates on all ROS 1 and ROS 2 distributions. - runs-on: ubuntu-latest - strategy: - matrix: - ros_distribution: - # - noetic - - humble - # - iron - - # Define the Docker image(s) associated with each ROS distribution. - # The include syntax allows additional variables to be defined, like - # docker_image in this case. See documentation: - # https://help.github.com/en/actions/reference/workflow-syntax-for-github-actions#example-including-configurations-in-a-matrix-build - # - # Platforms are defined in REP 3 and REP 2000: - # https://ros.org/reps/rep-0003.html - # https://ros.org/reps/rep-2000.html - include: - # Noetic Ninjemys (May 2020 - May 2025) - # - docker_image: ubuntu:focal - # ros_distribution: noetic - # ros_version: 1 - - # Humble Hawksbill (May 2022 - May 2027) - - docker_image: ubuntu:jammy - ros_distribution: humble - ros_version: 2 - - # Iron Irwini (May 2023 - November 2024) - # - docker_image: ubuntu:jammy - # ros_distribution: iron - # ros_version: 2 - - # # Rolling Ridley (No End-Of-Life) - # - docker_image: ubuntu:jammy - # ros_distribution: rolling - # ros_version: 2 - - container: - image: ${{ matrix.docker_image }} - steps: - - uses: actions/checkout@v3 - - name: setup ROS environment - uses: LCAS/setup-ros@master - with: - required-ros-distributions: ${{ matrix.ros_distribution }} - - name: build and test ROS 1 - if: ${{ matrix.ros_version == 1 }} - uses: ros-tooling/action-ros-ci@v0.3 - with: - import-token: ${{ github.token }} - target-ros1-distro: ${{ matrix.ros_distribution }} - skip-tests: true - - name: build and test ROS 2 - if: ${{ matrix.ros_version == 2 }} - uses: ros-tooling/action-ros-ci@v0.3 - with: - import-token: ${{ github.token }} - target-ros2-distro: ${{ matrix.ros_distribution }} - skip-tests: true diff --git a/src/CMakeLists.txt b/CMakeLists.txt similarity index 100% rename from src/CMakeLists.txt rename to CMakeLists.txt diff --git a/src/config/execute.yaml b/config/execute.yaml similarity index 100% rename from src/config/execute.yaml rename to config/execute.yaml diff --git a/src/config/map.yaml b/config/map.yaml similarity index 100% rename from src/config/map.yaml rename to config/map.yaml diff --git a/src/config/rob_lindsey.yaml b/config/rob_lindsey.yaml similarity index 100% rename from src/config/rob_lindsey.yaml rename to config/rob_lindsey.yaml diff --git a/src/config/test.yaml b/config/test.yaml similarity index 100% rename from src/config/test.yaml rename to config/test.yaml diff --git a/src/launch/sentor.launch b/launch/sentor.launch similarity index 100% rename from src/launch/sentor.launch rename to launch/sentor.launch diff --git a/src/launch/topic_mapping.launch b/launch/topic_mapping.launch similarity index 100% rename from src/launch/topic_mapping.launch rename to launch/topic_mapping.launch diff --git a/src/msg/Monitor.msg b/msg/Monitor.msg similarity index 100% rename from src/msg/Monitor.msg rename to msg/Monitor.msg diff --git a/src/msg/MonitorArray.msg b/msg/MonitorArray.msg similarity index 100% rename from src/msg/MonitorArray.msg rename to msg/MonitorArray.msg diff --git a/src/msg/SentorEvent.msg b/msg/SentorEvent.msg similarity index 100% rename from src/msg/SentorEvent.msg rename to msg/SentorEvent.msg diff --git a/src/msg/TopicMap.msg b/msg/TopicMap.msg similarity index 100% rename from src/msg/TopicMap.msg rename to msg/TopicMap.msg diff --git a/src/msg/TopicMapArray.msg b/msg/TopicMapArray.msg similarity index 100% rename from src/msg/TopicMapArray.msg rename to msg/TopicMapArray.msg diff --git a/src/package.xml b/package.xml similarity index 100% rename from src/package.xml rename to package.xml diff --git a/src/scripts/sentor_node.py b/scripts/sentor_node.py similarity index 100% rename from src/scripts/sentor_node.py rename to scripts/sentor_node.py diff --git a/src/scripts/test.py b/scripts/test.py similarity index 100% rename from src/scripts/test.py rename to scripts/test.py diff --git a/src/scripts/topic_mapping_node.py b/scripts/topic_mapping_node.py similarity index 100% rename from src/scripts/topic_mapping_node.py rename to scripts/topic_mapping_node.py diff --git a/src/setup.py b/setup.py similarity index 100% rename from src/setup.py rename to setup.py diff --git a/src/src/sentor/CustomLambdaExample.py b/src/sentor/CustomLambdaExample.py similarity index 100% rename from src/src/sentor/CustomLambdaExample.py rename to src/sentor/CustomLambdaExample.py diff --git a/src/src/sentor/CustomProcessExample.py b/src/sentor/CustomProcessExample.py similarity index 100% rename from src/src/sentor/CustomProcessExample.py rename to src/sentor/CustomProcessExample.py diff --git a/src/src/sentor/Executor.py b/src/sentor/Executor.py similarity index 100% rename from src/src/sentor/Executor.py rename to src/sentor/Executor.py diff --git a/src/src/sentor/MultiMonitor.py b/src/sentor/MultiMonitor.py similarity index 100% rename from src/src/sentor/MultiMonitor.py rename to src/sentor/MultiMonitor.py diff --git a/src/src/sentor/ROSTopicFilter.py b/src/sentor/ROSTopicFilter.py similarity index 100% rename from src/src/sentor/ROSTopicFilter.py rename to src/sentor/ROSTopicFilter.py diff --git a/src/src/sentor/ROSTopicHz.py b/src/sentor/ROSTopicHz.py similarity index 100% rename from src/src/sentor/ROSTopicHz.py rename to src/sentor/ROSTopicHz.py diff --git a/src/src/sentor/ROSTopicPub.py b/src/sentor/ROSTopicPub.py similarity index 100% rename from src/src/sentor/ROSTopicPub.py rename to src/sentor/ROSTopicPub.py diff --git a/src/src/sentor/SafetyMonitor.py b/src/sentor/SafetyMonitor.py similarity index 100% rename from src/src/sentor/SafetyMonitor.py rename to src/sentor/SafetyMonitor.py diff --git a/src/src/sentor/TopicMapServer.py b/src/sentor/TopicMapServer.py similarity index 100% rename from src/src/sentor/TopicMapServer.py rename to src/sentor/TopicMapServer.py diff --git a/src/src/sentor/TopicMapper.py b/src/sentor/TopicMapper.py similarity index 100% rename from src/src/sentor/TopicMapper.py rename to src/sentor/TopicMapper.py diff --git a/src/src/sentor/TopicMonitor.py b/src/sentor/TopicMonitor.py similarity index 100% rename from src/src/sentor/TopicMonitor.py rename to src/sentor/TopicMonitor.py diff --git a/src/src/sentor/__init__.py b/src/sentor/__init__.py similarity index 100% rename from src/src/sentor/__init__.py rename to src/sentor/__init__.py diff --git a/src/srv/GetTopicMaps.srv b/srv/GetTopicMaps.srv similarity index 100% rename from src/srv/GetTopicMaps.srv rename to srv/GetTopicMaps.srv From a56fba141d154c09050d361f3dead0eeed88ba8b Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Mon, 28 Jul 2025 13:42:53 +0000 Subject: [PATCH 08/21] restructure for dev container and CI --- .devcontainer/Dockerfile | 34 +++++++++ .devcontainer/devcontainer.json | 30 ++++++++ .devcontainer/post-create.sh | 34 +++++++++ .github/workflows/dev-container.yml | 31 ++++++++ .github/workflows/ros-ci.yml | 70 ++++++++++++++++++ .gitignore | 52 +++++++++++++ .../__pycache__/Executor.cpython-310.pyc | Bin 13906 -> 0 bytes .../__pycache__/MultiMonitor.cpython-310.pyc | Bin 3217 -> 0 bytes .../__pycache__/service_types.cpython-310.pyc | Bin 5497 -> 0 bytes {sentor => src/sentor}/CMakeLists.txt | 0 .../sentor}/config/test_monitor_config.yaml | 0 {sentor => src/sentor}/package.xml | 0 {sentor => src/sentor}/scripts/sentor_node.py | 0 {sentor => src/sentor}/scripts/test_sentor.py | 0 .../sentor}/sentor/CustomLambdaExample.py | 0 .../sentor}/sentor/CustomProcessExample.py | 0 {sentor => src/sentor}/sentor/Executor.py | 0 {sentor => src/sentor}/sentor/MultiMonitor.py | 0 {sentor => src/sentor}/sentor/NodeMonitor.py | 0 .../sentor}/sentor/ROSTopicFilter.py | 0 {sentor => src/sentor}/sentor/ROSTopicHz.py | 0 {sentor => src/sentor}/sentor/ROSTopicPub.py | 0 .../sentor}/sentor/SafetyMonitor.py | 0 {sentor => src/sentor}/sentor/TopicMonitor.py | 0 {sentor => src/sentor}/sentor/__init__.py | 0 .../sentor}/sentor/service_types.py | 0 {sentor => src/sentor}/setup.cfg | 0 {sentor => src/sentor}/setup.py | 0 .../sentor_msgs}/CMakeLists.txt | 0 .../sentor_msgs}/msg/Monitor.msg | 0 .../sentor_msgs}/msg/MonitorArray.msg | 0 .../sentor_msgs}/msg/SentorEvent.msg | 0 .../sentor_msgs}/msg/TopicMap.msg | 0 .../sentor_msgs}/msg/TopicMapArray.msg | 0 {sentor_msgs => src/sentor_msgs}/package.xml | 0 .../sentor_msgs}/srv/GetTopicMaps.srv | 0 36 files changed, 251 insertions(+) create mode 100644 .devcontainer/Dockerfile create mode 100644 .devcontainer/devcontainer.json create mode 100755 .devcontainer/post-create.sh create mode 100644 .github/workflows/dev-container.yml create mode 100644 .github/workflows/ros-ci.yml create mode 100644 .gitignore delete mode 100644 sentor/sentor/__pycache__/Executor.cpython-310.pyc delete mode 100644 sentor/sentor/__pycache__/MultiMonitor.cpython-310.pyc delete mode 100644 sentor/sentor/__pycache__/service_types.cpython-310.pyc rename {sentor => src/sentor}/CMakeLists.txt (100%) rename {sentor => src/sentor}/config/test_monitor_config.yaml (100%) rename {sentor => src/sentor}/package.xml (100%) rename {sentor => src/sentor}/scripts/sentor_node.py (100%) rename {sentor => src/sentor}/scripts/test_sentor.py (100%) rename {sentor => src/sentor}/sentor/CustomLambdaExample.py (100%) rename {sentor => src/sentor}/sentor/CustomProcessExample.py (100%) rename {sentor => src/sentor}/sentor/Executor.py (100%) rename {sentor => src/sentor}/sentor/MultiMonitor.py (100%) rename {sentor => src/sentor}/sentor/NodeMonitor.py (100%) rename {sentor => src/sentor}/sentor/ROSTopicFilter.py (100%) rename {sentor => src/sentor}/sentor/ROSTopicHz.py (100%) rename {sentor => src/sentor}/sentor/ROSTopicPub.py (100%) rename {sentor => src/sentor}/sentor/SafetyMonitor.py (100%) rename {sentor => src/sentor}/sentor/TopicMonitor.py (100%) rename {sentor => src/sentor}/sentor/__init__.py (100%) rename {sentor => src/sentor}/sentor/service_types.py (100%) rename {sentor => src/sentor}/setup.cfg (100%) rename {sentor => src/sentor}/setup.py (100%) rename {sentor_msgs => src/sentor_msgs}/CMakeLists.txt (100%) rename {sentor_msgs => src/sentor_msgs}/msg/Monitor.msg (100%) rename {sentor_msgs => src/sentor_msgs}/msg/MonitorArray.msg (100%) rename {sentor_msgs => src/sentor_msgs}/msg/SentorEvent.msg (100%) rename {sentor_msgs => src/sentor_msgs}/msg/TopicMap.msg (100%) rename {sentor_msgs => src/sentor_msgs}/msg/TopicMapArray.msg (100%) rename {sentor_msgs => src/sentor_msgs}/package.xml (100%) rename {sentor_msgs => src/sentor_msgs}/srv/GetTopicMaps.srv (100%) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 0000000..4b16ca7 --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,34 @@ +ARG BASE_IMAGE=lcas.lincoln.ac.uk/lcas/ros-docker-images:humble-2 + +FROM ${BASE_IMAGE} AS base + +USER root + +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update \ + && apt-get install -qq -y --no-install-recommends \ + git \ + python3-pip \ + python3-rosdep + +# get the source tree and analyse it for its package.xml only +FROM base AS sourcefilter +COPY ./src /tmp/src +# remove everything that isn't package.xml +RUN find /tmp/src -type f \! -name "package.xml" -print | xargs rm -rf + +# install all dependencies listed in the package.xml +FROM base AS depbuilder +# copy the reduced source tree (only package.xml) from previous stage +COPY --from=sourcefilter /tmp/src /tmp/src +RUN rosdep update --rosdistro ${ROS_DISTRO} && apt-get update +RUN cd /tmp/src && rosdep install --from-paths . --ignore-src -r -y && cd && rm -rf /tmp/src + +FROM depbuilder AS final +# add user ros with sudo rights if it doesn't exist + +# add sudo without password +ENV DEBIAN_FRONTEND=noninteractive + +USER ros \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 0000000..b97a286 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,30 @@ +// For format details, see https://aka.ms/devcontainer.json. For config options, see the +// README at: https://github.com/devcontainers/templates/tree/main/src/ubuntu +{ + "name": "L-CAS Humble CUDA-OpenGL Devcontainer", + "build": { + "dockerfile": "./Dockerfile", + "args": { + "BASE_IMAGE": "lcas.lincoln.ac.uk/lcas/ros-docker-images:humble-2" + }, + "context": ".." + }, + + "postStartCommand": "/opt/entrypoint.sh /bin/true; .devcontainer/post-create.sh", + + "remoteUser": "ros", + "updateRemoteUserUID": true, // ensure internal user has the same UID as the host user and update file permissions + "customizations": { + "vscode": { + "extensions": [ + "ms-python.python", + "GitHub.vscode-pull-request-github", + "ms-vscode.cpptools", + "JaehyunShim.vscode-ros2", + "nonanonno.vscode-ros2", + "deitry.colcon-helper", + "github.vscode-github-actions" + ] + } + } +} diff --git a/.devcontainer/post-create.sh b/.devcontainer/post-create.sh new file mode 100755 index 0000000..56bd180 --- /dev/null +++ b/.devcontainer/post-create.sh @@ -0,0 +1,34 @@ +#!/bin/bash + +set -xe + + +function add_config_if_not_exist { + if ! grep -F -q "$1" $HOME/.bashrc; then + echo "$1" >> $HOME/.bashrc + fi +} + +function add_git_config_if_not_exist { + if ! git config --global --get "$1" > /dev/null; then + git config --global "$1" "$2" + fi +} + +add_config_if_not_exist "source /opt/ros/$ROS_DISTRO/setup.bash" + +source /opt/ros/$ROS_DISTRO/setup.bash + +colcon build --symlink-install --continue-on-error || true + +LOCAL_SETUP_FILE=`pwd`/install/setup.bash +add_config_if_not_exist "if [ -r $LOCAL_SETUP_FILE ]; then source $LOCAL_SETUP_FILE; fi" + + +add_git_config_if_not_exist "core.autocrlf" "input" +add_git_config_if_not_exist "core.safecrlf" "warn" +add_git_config_if_not_exist "pull.rebase" "false" +add_git_config_if_not_exist "user.name" "Anonymous L-CAS DevContainer User" +add_git_config_if_not_exist "user.email" "noreply@lcas.lincoln.ac.uk" +add_git_config_if_not_exist "init.defaultBranch" "main" + diff --git a/.github/workflows/dev-container.yml b/.github/workflows/dev-container.yml new file mode 100644 index 0000000..9a9913c --- /dev/null +++ b/.github/workflows/dev-container.yml @@ -0,0 +1,31 @@ +name: 'devcontainer CI' +on: + workflow_dispatch: + pull_request: + branches: + - ros2-devel + - master + push: + branches: + - ros2-devel + - master + +jobs: + build_devcontainer: + runs-on: ubuntu-latest + steps: + - name: Checkout from github + uses: actions/checkout@v3 + - name: extract the github reference + run: echo "BRANCH=${GITHUB_REF##*/}" >> $GITHUB_ENV + - name: "image name from repo name" + id: docker_image_name + run: echo "docker_image=${{ github.repository }}" | tr '[:upper:]' '[:lower:]' |sed 's/[^0-9,a-z,A-Z,=,_,\/]/-/g' >>${GITHUB_OUTPUT} + - name: Build and run dev container task + uses: devcontainers/ci@v0.3 + with: + imageName: devcontainer/${{ steps.docker_image_name.outputs.docker_image }} + configFile: ./.devcontainer/devcontainer.json + push: never + #imageTag: ${{ matrix.config }}-${{ env.BRANCH }} + #runCmd: "bash .devcontainer/run-ci.sh" diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml new file mode 100644 index 0000000..293066b --- /dev/null +++ b/.github/workflows/ros-ci.yml @@ -0,0 +1,70 @@ +name: ros CI + +on: + push: + # you may want to configure the branches that this should be run on here. + branches: [ "master", "ros2-devel" ] + pull_request: + branches: [ "master", "ros2-devel" ] + +jobs: + test_docker: # On Linux, iterates on all ROS 1 and ROS 2 distributions. + runs-on: ubuntu-latest + strategy: + matrix: + ros_distribution: + # - noetic + - humble + # - iron + + # Define the Docker image(s) associated with each ROS distribution. + # The include syntax allows additional variables to be defined, like + # docker_image in this case. See documentation: + # https://help.github.com/en/actions/reference/workflow-syntax-for-github-actions#example-including-configurations-in-a-matrix-build + # + # Platforms are defined in REP 3 and REP 2000: + # https://ros.org/reps/rep-0003.html + # https://ros.org/reps/rep-2000.html + include: + # Noetic Ninjemys (May 2020 - May 2025) + # - docker_image: ubuntu:focal + # ros_distribution: noetic + # ros_version: 1 + + # Humble Hawksbill (May 2022 - May 2027) + - docker_image: ubuntu:jammy + ros_distribution: humble + ros_version: 2 + + # Iron Irwini (May 2023 - November 2024) + # - docker_image: ubuntu:jammy + # ros_distribution: iron + # ros_version: 2 + + # # Rolling Ridley (No End-Of-Life) + # - docker_image: ubuntu:jammy + # ros_distribution: rolling + # ros_version: 2 + + container: + image: ${{ matrix.docker_image }} + steps: + - uses: actions/checkout@v3 + - name: setup ROS environment + uses: LCAS/setup-ros@master + with: + required-ros-distributions: ${{ matrix.ros_distribution }} + - name: build and test ROS 1 + if: ${{ matrix.ros_version == 1 }} + uses: ros-tooling/action-ros-ci@v0.3 + with: + import-token: ${{ github.token }} + target-ros1-distro: ${{ matrix.ros_distribution }} + skip-tests: true + - name: build and test ROS 2 + if: ${{ matrix.ros_version == 2 }} + uses: ros-tooling/action-ros-ci@v0.3 + with: + import-token: ${{ github.token }} + target-ros2-distro: ${{ matrix.ros_distribution }} + skip-tests: true diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..6dabd73 --- /dev/null +++ b/.gitignore @@ -0,0 +1,52 @@ +devel/ +log/ +build/ +bin/ +lib/ +install/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE diff --git a/sentor/sentor/__pycache__/Executor.cpython-310.pyc b/sentor/sentor/__pycache__/Executor.cpython-310.pyc deleted file mode 100644 index 7d9e147ec8cd9172569c70c747b487548fd640c4..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 13906 zcmbtbU2Gi3ec$if`{aixN}^ z?4BfUj%pIRwbQ~*(xh!rTPXR^ph622=?8+MEfAm&ZD0D3r(*jOpg;k?1OeQ>IE~fc z|3ABTdnYLt&?9zsc6MfVX8zwF?3Bv|4Zr`Hys`QpCpGOq=^^{G@bE0I;HM~rwyX(V z7%hES$K7lh>*lhlQ=8SY*6n4R>vqdo&n@S;?zG(X{BoY_xmIDlxLoAA+bXS>m&;tw zw<_xs%M)BTT9eC@{EeyQX}<4T-o^KsY`qt*b>wmHIZUf7{K&3Ejx!}*ridYx9Y zz3RQxtG8DRAL7up#i|}#iyh(PUV5$(HaqQ;t)|}&@ti-?yU=O|YrdqHOT7z0LpHnA z1<$!9UtVc8Xk7kGUDntAko)ag^25P%aMllcEt<=ErPH{C`{b%0)&e!O7H)L?hC$0n zekSgO6hIa&*oz|6`dVLK1wy=QY#O27H~RV(4zy~}!d==iRv1=|xKOJ#*Sj4V)@pGM zAE}3-teUag>NM)KzRCy4;0I1J}rpc0s@3zfkxZ&{}F(D>#FNi5Ije8NOHiLUf>=v`Q zm&G2j7x#*ITFi<4K*0%dK-`CxNpZh;0QV{3i3f3?76-*cxbG4Vi$`#u5s!*_+;@vZ zqKf;hcuYKw`yO#vJc0XOu^^7%zE3jcp)Z5t=Cqw9cdU#l5_sa7)ul~!nP0pc}hmC9USe?fcm`lcDW zeRDg%qiumL?9(=_z9lRW*x|mmUFe%M)8)Nrv(ff6+KQPr2W`)yt(0lYq3w%kD`(nV zw4Fm+CDWEi+gH#wk!dTSZ4GUcnYJR@B-*AjZ6&nz(KelFE2Hgq(6(#Prfr*|(zo6R z6S-lE$sGe{qFu~w&xk3tt8jNXOD$Xa7M;}|q2a8j7vr*@pk}QN+Krw#->zS1`9$~4 zb~9|&Tg}M#y0X*ogTU*octmO5{8u;Uk9%L;lmLSkTa9|F73aE&#$%_>#2?#1%lEsu zt@(IuwmPe^BmG9Fy%Lu|OP6Z(#^qj9`pJ{@TYf$8W2eyz!p?d$b!Pmd(Zsyh?u1^@ z>k@hTBDSv9WjmfgV{N!vQQeD=_PV}|U63+%0YY}@yUo?u^{;>~YmEzWiQo8A)`C#V zJZ;3T(YoS%vRyxjE#e8Wb;;id;ye%Fy{fV4hKu|n*>!;*)Yg+a@89KiIt0}{If<#H zIddGDru0nI5Opv(-)`4$i`bUnz2HMFA4eekl{)w%wkyC&gQ^`{f!|t@4`W~Q5!zfX z*;QOhD$F!*g6Y<3jaEGfYBgqNa+-!bgo4Hf&!8yj4*eM}O4pdgRnlGk)2|t$qR$x7 zR7%eaDL6)0H648s*Gc-^&rn2X`XK9lkaZwY5c9UyH+FQzw72w4Q@!0bi23apAYyad z+|f4eWR$&61F<`sPH6R=Eh6cI0w}+oyx-A<(>FjUIS`85w^vM&2jejNcD4_gK;Z*h z6hE*xb08aMJJ-(Axa4eVx%am zk-v$to9=a&W>B@O`cJz;@ca3%58v12Y7kAG1ksTQ zPDXk2AM5k6)oibHqFn^~1;W%rk3Dt?G_o30tF|Oi#^v-Y4D;BO{^d9av45ch+AAg> ztp)PROE4_9jcGRTOqYe+EsFvdj$>T1`?;34DlEN07XpD1b z{mVU{=!ketTxbThD>=qewjMrWlX*XvA{O|)Ic;5I7JgQ0ejg|q;+u0qJ*&wkeg zHX9cW##Xa$%KgAxOE@C8W4r;J2yNh_{=Rm@*wVkD1IL_=r-5S@a4e5^`7qbFwg&G- zVbi*9F$NW*U-O=WdV!;A(&9Ee>4&ljRu_2T8Zq;3v!OV5FCd(uMpo-izVfmcTe1@z zi*4=|Tg2ecK@F-j>fNv>{Th(4+XL`I|62I5ek8INfCvlGp7ZTX?atM9c6NZviyk;# zZ?>RBLWHEt$n-hxEmqyQ5cDpj{K%BOcI*V9z=y!NvG8u3lYX~VZ}@TH^?Ix4pOUg8 z0Gl6lVkHvYB%^;9NQ8TyEd$12aA_h+8qIvbUWvPLR2T`a#XRmVoT|Ji_h5hW0*Virz7q&AN7|hz`HWzl1myGbIU4ai9m_1Ur9^6G zIFk{ZXoNx?tjZUuw?_rDm6H4os{1J7$p)J|wPXTF?nQd{8WjURqLAbfY9kIJzeEM& zOqPAn24WZAV;Lyds8f@QB`VHQaR=OyhwP;?Phq#?g0uh z??@IufsBb<1wgIQF}xHDnufEH4%LRh_Y$HqA`WNM*~wZHtpe)(1i2b5S=2o zN5dNjwvGnjW^9>25C`Z6Cd1;@P0%eL8vQ&A%BLCKlCJ>WAQ&Mi3#?ZbZ`cCD4dSv4 zacT8)eW23zps$(}>U7JzPql7$iBncMgtP!c9K3(qV{7Fl>sXHari z1|^Hvi;3_=jalE!q9p4s&Xdduy2LVaC@w=|aNhxiHr>l-6-U2lY z!{TzXlm=VGu|;2D$to|S)9qMK)}S=07~=RV^mq@5UdqTxT7q1sLQ+ApDK7A?S^Ud@ zT0<&+HReBoF5kfwd>#c!N%|u!FBuM`r;?(xkey13QjdymeGFQd$k4*fC@rWn86gE1 z(-UK%tp^{Yh+fZ592 z4l7kb1B_wtBUcBX5$^2X_&UtJH*=dhR<7;SUWd2nvLSSGZ!j7zvKYK+#KyvtvCR{X z3&#A3Rln_D>&j<-M3a(Z!uZ;(7R`?WP1!9AC&(Mp3Z7XQj8f>XFR<1a+gC_#-?nB4 zvl4Fenwzv{m)87_1|1)U{CADZaLMq>;FQ@r&?w0x6x1$k)Vk7NXZ|Vx& zhZ4SU0wz!hmsa!@M?m(D* z+C}Xb|NEPVD}7Csml&7EZv zySdtv#LgJhe0;|}mcp?XDZZruIpkYd@)cUeV0{Yh#(7f#$~biTDdB^IKsy|K;GPAs zo8eFV#lnDh2XX^TkXuxYN@1+-Yt+gn0G*aRisEl@1q&#M;Xr3I9@Yy`L zHe|q79TUo9j7S~D2qPl&PnEilh;2sf?iFTJ6V?Y7DRdVTm5zsQc|`m+5x*KK61D+VE4OK0he8LVkZVl*ZuFG*^1uNTc=dq~CS0qBkW1`3 zq4sbMYLW2BbK%MRu+r0s{g?Hz9ID>5hUT1n8QtU;sqm;ERcrBP?jD2-W*Hh`qYxS* zvOI?QeuC>G63vhZY*_&y|2yqeU{fVl)!ZmCW|wu{+PoeFn)c$)YqC0INnRH|eBPPPnY2=p_$4Fcpw`q9W zz%uyu;M^cjHAI?CIvB*6NxB}iIUa#t@Ne!I?`fM3Yi>iqmFt@r1+j)Q56!J0iV$F> zzB3pJk1#ND0>W%EF&#*lrB03t&T3l9$V9$b;~0XlD1V#px@7X-{p`BkbammDi_2`Gr? zjm7jPu+iwo7JceAjwo3(Bk6F@JTPmF z40mSNXlrp%Ed;`@(~#eyY41vDpTXOo;R;AEG~kwlFT`#4e1Yxu6FZcj+NArweiJ*gKl%N{9(Sc$XEK-CZfMPlFc(8eaH%(o*KrUjIq*cNG zWoB7qY=t+*}cyb?Ya0V6tA~dQap%EDYv77daTzJ|bvPHmRRN>7G zTL3)u5N*cr1MZG{MT~XHfglXO)&Rm~hOo7%ogrR@4I7~ec z7@6@rlo`U9L2!*BT#3H$sSx%#kl{lla>T)dBDGFzM9$Gp_o~Aoz>dC--Nw3LD!(1D zl<+6p4CaEt|~p_F_NbIU)VV$^f72zMoWChpN{yCdk2VBR?bJxMMFdZOt9 z98^rxgf{5WHPqdyaAuFxN+;}Mi z=GzImkRjyaKuX+6>nTMKjDZ;elU3GZlVFhCkCmT}rkU{YG6S6)8~})F`?1}E=d-2! zJH)UTtET)z43mFE#rLTgmG_{~{|}g+#EgftU?N9y!GLo}w|)twCkv)wEF=m1tS*j3 zIbF;(2!{L{9yqdz0Ep0fnk*6{-89=^rSJv8h<)=sT!qC_j>N~gt`+ElX+W(3z4)<<@hP-IHlDO(@JAz7!5%y}3$9>!F2%8f@}3Z>1GClGN5Sfd&Dabkfq>D@#! zXBs(+b4ds-wo&n~sdxkH%6D)H#<^r^&k+0{#)OEF;m<;F0YL`ffdTj6qZ~D>7>C)b zZ2fmMshAP+0g~Y(lxkucx=!NakOc)3$SJ{&WSI3mU6%SVl$HN&N0)n0gOhkizi6YC z&Hm`DcXkj2AiXL{te6ixHq8%vjWxeqLxTI5L3e z-ieJ)fK96rOf_+NtsV@`z_>hOwX^99`eUns)i)P|RfK3vwI)yRIXcD?DkTIPEf7%& zM>*z(=dJJzBd12#Luw138cosHp&x_8?ck)UX0+g)qzwmNbH!8Pr)Id}H3Mv)HjJbf z$orw0#Ta@TwmWeKF#4g{Gq!Bv@-6?Xxg~h4`2OxqO#8}}% ze;(gQXa})I)`HTFeizN-rVnXJ zFpf8!zH<@GN|^8J!9#@5FoEbiZ0ME_i4Zc8`wrr`IG@d2NHJLr1`Cb+E$vne1MVkMPZ%WxOSIg0?cM{PxvMV`wmPTHq-EY9- zn;v)CIv-i{IBBv2(G;8*L9U|~oITZo{8OB`0&Y18nl0pwPmA)Jf*py+icf*?XcOZt@Jn)n0_F(M5pb`20oI8Wrq>j7wFTcs5p%RnHS3E z?Wj3mRKZW*X=8I^gfxrZYlL5S~&LpX51ja;TpuPoVVt(iJq$C^JP;;$GP z#-0Xxa(I#ch$bvtyu7ohq{1BuIq+Vignt<&L_65I#aK?klN8GxIS&lafmZ|T`;z|;uN16_2y-fs| zwZ^dnMicqx=q3MxiU#%Pd^{C|Rd|$QMvJ;gDgr8K@=WOCFYy?clW__~#u0{O8kP>K-Iz@5$FT>@6 zU>Z(;+om)zQi)_0e8eG(7u*OOir%oYgX|NWJVY*CRVSMB)H+Cdj#(MNBtmQX6%-uo z2Jej?yj{Z?u}uOmG7`;Z5vt9XX{>#wgY2K!I`i_GQ}7hP#_6>Y^beu-cH!|2V+WpD zWO6ZCBM8NE>wENRwo;C#i;zE}+88?162Cn>X{18n&-rBuepJiwbn8}@<+FuB=$!uO zpa(3U4jP)N%CRGj#Y41`?__hlh1ey9V4$;--~*$F4r{+h$4YL31fkP+qpejt+T*>_ zS*1fxTs$JW`%|J_3;Y`{)GecMUfc3mZF{uj9oI z9SzMoqeTg*p~69uX2KyuPkia*`K5EOz3RQX^x|<3@F#Us<+gkYwG=dCh6#82HuW6) zKG{7t==%j!DCS(z59qT-R2rZ@)-l2i2{hE1@t*K5n#6&!O7xZnw<0KDi{ogCR|Yr? zHQ(4+fSL~}8BL=%5==o0u+|9#g7KY|dz)=^#(b)=jqX50Io}mP#UQIRiVbj=BrOF> z3QNurFqv2y(Iwb17SFc6A7HAykFJZv?ntO z!Pp!hVn?BJefKdpQUa6s3m`&YjjeM?1Bsm%UOWH%E2q%N7_dQa%4wvAHO_H>5JKD} zfRL`~XpVW{6By^W5M_F30Fv8+Wkc*ZIHY~miy}72v`qE%U_kgb{0?bq6-RPnGir7- zi4G)5s)=L%3hF9;atCXLV*1)O&_sn zMH=|NI@-a6e}G+q|Yx+mlUD);yFNlc=7^zxO_*okcS*Ql`d0EoQ4| zKDzJroQ>uR$wwNP_51(va1I2O{q-*Y-vdm*c~--tDV|>G*vZ`G`2}wY{|?ZD4N1nZ zE&m#mD3<CsdFvuKbr|^>KcxO8p{TOldMCkeH!x&`}cUs7TGfpca>tq~ZqLkZEEu zOSBWzZ<-3y=7?m1r%||M@jvPwbW83&_aXOww}QX@?vy)Sn09B~qHDP}?iO0hZl0g5 zNsH$g5hg^8gdU9Ov^`e#z@pxTDi<27T1XKzXaLGz0`*QObq?>K;xUPmmx}%;gozGX z7pb|FSKAEvYx3{;nQWuDB#_lAI~$xjAG^U?4@rzy+hBJ@u*fw}QtQV-Z>`sx?SCe? zlYFLhRPq&-bomn=2C(9Ntya|u-nm*$bQ(BAUPys$vjxi4m$cWoq#Bgo%PB+gglZb* z$&n@yPpeiWUJm~GMv}G$<(tgCg~ZyFq{R)UNTrbyl{5wXWJtzXL84Vad7zTGirl6# zcDuI@v(acHPKHd>HP%<<59n21v0|9WD$#zBgiI-z0jCP{KftIHrc;prO2tqI;u@h% s?4D3aPaZcdAP`%+>#B^Tv{d#Y*^Hc|ZX(my(teBn3WwYr(mZedAKEe!3IG5A diff --git a/sentor/sentor/__pycache__/MultiMonitor.cpython-310.pyc b/sentor/sentor/__pycache__/MultiMonitor.cpython-310.pyc deleted file mode 100644 index 98c68b5059c24a698d071c1b3f2110e699d15ced..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3217 zcmZ`*&2J+~6|d?qx7%&UnaK>B0IOc@vS2{Yen=n<(J-T#XhE7W+07o1(b9CfDo#7? zZl|g|SzC@cB+^Jod*FaLtdNr9{3rYkbwix?hQt90iCx|+f5aX{TlHM;)z|y{Rh9L6 zK;Zkb{ny@GZxHf#WL7^1m`Bj$5C|iT=A?`NR!#?2*P__Bb9>-)oq^kR4e#XMpwg|F zzMK1lYPUKFx&b905$3VV6=4-_UE19mt1+KduV^>qbrzgj;wL=3B(RPin$`-xf>TMS zMYYo@8E;uS__#bg5an@}bA1>3Pe&p-%JNK&56V1C$1v61f58g{b1TowB2%T%;k@%$ zh-94Nma89{h@X52gp)31q-(M8B{`PnIfrW4w?}c{;=85A%54hM>-X;E54CkvV0|mdJ7q$xewW%G4u%fOS%$?ddHq6^#-qiXwxo|+>Wxo7dE7bt<$mYW z_}QU*MtkRa6pIiNh%wl>S>baM@f3B-I*Xjb@Q^< zgFz=Nj?0#-9m(@!T}A8T0?g75dQ;cVN*NF5DeQqGvXZ@`x*Ese11aP975VpXcTdUz z-(_G^Uc!{!q)fp|k(?iadgEB8yhvbSd@gsTNO$??;Iksh!DH!Z(&HeVg=YS@8jkjG zEE-_oJ~Rm>PeK~nA@wYudNi{6iU4P>y~3k}iA% zoOA8o5cwyAKK^FG@H$Z-1g3m0eu_zy2UwE^oYQ+Ha4n^O9VF#J6@v2lHWYT##Tv)WI|g?;wxvjbT3t?M_YvwQ(qLTEDZ z6m}->ue$s;FT|)QFe%#Y==lt7Wt5hqLOqDqqJW@)Q8C&zxKbGv{nskFA~1JmzU6&523TzVRG+4@E2J*lF=>PJ%}dF z4%E?_s7@Vm$laVTmaxjjUB1a`XDQYmi zwY^eUcFqe^bmA`Vb`$!PUbY+g=Eq_+rjaz5LfH7ug?V7X@ZCqu} z+r{1<5>xnwVD*a~Aiskq9{|~*f#p%h^1i`uAAF^fsrq9#y*hscR+h>Qj3Zz zZeGQA00J_+1A-W~V51!SVH}^0l6*E|E-djw+!tdao+9}Oh_27C&6MYP9Ixp0Hcl~7 zMBK$kt_*?rr13NVzVG_)G;M(+YYsqn+0f8N`5q8O`T&=VegW?zWdZb21sB@ zj1Ncvsv%CZi)|;`FO(QZ!!j#WbX*E^f0;KB;D(ckp6s+d?TR!Xj`6XODWeFy7lv7( zL&o8vFUK*i(7rqwDOR2r5QJx-k9JVJVy3yJoI#Qme?Xnl&JfZR?s2%&1%e#<7stUn zCYlZL9HoIdziqZdvvqYQfY-^&!>OuH;BmwNK4x#NVD_MBL+P3dHN|bzH)n}oz)maL iElWxJd$Y(s#6*|(f%x$7FtuVgZSwno`t^W1-~1Oo6G!L( diff --git a/sentor/sentor/__pycache__/service_types.cpython-310.pyc b/sentor/sentor/__pycache__/service_types.cpython-310.pyc deleted file mode 100644 index 7c3160fc6999bc0b14682729e79fe9614a6deb33..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 5497 zcmb7|S9cr76~}?$5Clnp1i{`Us0(VKsNPAji83wGBxO4j+k2M4l0>w#i|h_b6GwST z@(ui$_|Xq}$`?rQz1K~T6DLlb-ktitvj73oQqHk>@WafVe&^2o2Xd3imI(g6Jn-V` zVh_G4Q~!@4Glq{pf>a?=h}sbu711cAnayN_XpmfR*(e&dt-+4TCebA0BChk?Zk8>g zMYf7onGgwWYlJN+lCn*->3j_NcF`_VA|*RShqg7rmKJH*DLQpNZgUq~SLHQvjpiY*i|g`+xFN^Hm>d`5 zy1diADJR5)oD`FCN=(UVF)e4rjJzdo$yqTgZ;RXVG4Yt36LY$Lm;JcBBkt&Qw|!U6 zi+P>yv7e9&VnL^S?M1mHmgGHgPd+K0luwDL1 zX62RZZD}id?3{3&mE!8UXCa|H*S0M$=%it4`K7HA25jGH^f6WsnQob0Vbi4cvgA6J zqx_xrSw~skikY{34A(}puJs)BFIY79nq`ir?G-Uz|DyHAzQbv=AhozzRQa`9yC4;M zp=QDS+Qtio$)dUHxV|dp;j=}{URknyg_*RVY-!WQ?EIh!L)~;;J7`++imR)Zm*qjr zjHPBQcivQMK`KnIdS+>@n19?X1qoOd+;wHm+oltA-YXU0WK^$JK^LtPYo@bmg&UAY zi}4M!Xq(G+X!3*JdK>n}@r$%$m?#RurAC7UmV_OebM?exK{LuQg@wDbK`WB;ZqcE9 z&{mhB!j5^*rOmbqDD`KZ6&LF?H!P~#fd%8cQiF!XbYo%1K@)XE%W9&{qUy1*I>NCn zTrG)KAa=GZ`9Tt6)Ma~jWXF{& zi+R(wx3aMyHto4`ZqD0-6GB_6CQY?2^(M}{z7_B#cWy4|S%SlC3p2%(uu#OGUvyTp zEkWbWbVhfhPF45(pW!={!ge?{wxOB}` zK_VO_Y!Cm?Dkk6t&m@P#&^RSaYKso?G`VMnLMYo9@W+a_t^GQ0IYHxM$=q~;)RJXe zCD&8q%f4$PxM~(06CCCZ3mtXClln}Vd9plidiUY;Y=d`({6Sl#Lj&RCsl^RveC4A+^&e?hJ3bTm_^+U(2>nu#7or8GqF{q!AXg1wqvBvp zHG)kl2F6tr*sS7Ui)sd2RSTFIQpN57?)A!G6^T4yb-`Pz``XY7iV&L*O1Y4DMBXz>L}p?o%0XzuNag^d%bU zfZC7zL3IE;qz;0I)gkbRIt(6FN5HH)3LaBg@VGh#o>0fZlj;O`N}U9A>J&Jla^Pt- z0*u7Nky zb#P4G0LRrBcvFpo6Y3^7sU~P7HL0f5v^uS3)Gc*fom4~W1U`pUe_`sq$(N$~LaxfKIU7gZa7eDoEjha9bq3YewPJ<2zTDeM zu?!sVs$>~HX;!(nQlhq1t5VRN%U4y)K&m2J<(_cBn#G_A`{j<^cE8*on${}XgnlH8zhx8 z3k9Q46Eb+yhmsizQdUmtQnl$Cn5kcG)?+S1&C2nd3grxSRxy!P%QZ^SvvOxOTahb6 znx%5PHhLs#MhVwH+_|o0joQ^$Oy=@h*S0r<`ZlZ57FzxCfbOnl#wJ$f4b!J4;U>3L z_R_Gmz#%xO3==Xuw^(k|xe{rdo@9DwUf1c$m8wcdb$s|?#U(#L>~y0p?Goy;Qjr7? z-l+_Ps+7`eZVq>t{1LCV!nRi&u~Lz}a#s#9tyK>Wt;!y(x}U0{5`Cdx>#8A&=v>rU z+VxO%hJjrM!`v1?%xeYuXV|um0%hRku`VmyUUGe_9Lp(}iu>_R5@=xB2w_@$MFdf0 znKZDna=a5QbXN2+C(=O_>jZ3Y%9i0k3d>E1bkf3dBj#q^uMDW0!qpW$#C}!Atn`%9 z3(0Fm4EH6Lvq%xK&nx6OLq`@my$?aG_`GB}n1WB5m`&nDsERBaWJ4+Fk4s)cIhQHw&xbP6W6fAVNS>Db)rMRvu(|uO= z+b?(5mr*Q)AwF1d-o5wzJMZlk*UMyY zd!+4N1A!A72{A$wAx>x}(63vsm5?AL32g*=76Aw$?l*iSe>I7m1|I82~dig%QdB^)ChC!8SY2M)b$!dK1+rA`w@ z3G_7b&JxZM&J!*WE)p&gE)%X0t`e>ht`lw$#t7qtn*@3od6R@G!ZcxqaEmZYxD5m? zPn!0+HRXA(ho2*fidq_^mO*PZhj@Bl#Jhv~uRoVtbESpLWdo{GLhzYx9zo=po6D95 zjr8+)c6&wF*z|M0mw!}oZgKq1(%l7|J+0f}MRcmP6{K`f)>+BrK7NEp7~@g)U(VQn zcmw-4=j>m+k^Pg$*z3HB{e#EZ-+43p8*gEM<*n>5Ji-3Vlk88tjs20gvp?_@`#taA zzvF54Ti(fj!@JmPyqo=+_po2_UiK>QW54A6>=%51y}}3C&-oDh86RdZ^F8dRd@uV6 z&#)i!ee6ekf9!|+0Q&(y$iB}HvG4K2EZ|4jcllBF9iC<1=EvB#_;L14eu902pJZR> zr`Xqcj(wGnu&?mb?8|(VeTkomeUYDKU*PB1=lOZ|IevkCmS1F_;g{H_`DOMgeuaIK zUuB=**VxDTb@nlSgME~bv5)X^_F;aLeTYx65AsR=0XD^=5C4n5KfV-?wlQxW-n)}E zf_A!aT*r|9s(;G&HuND22K1vM{7FaJTzN(43Of&AkgBMCt%vO+84T850=Ur}6F*OD z)!y`Z4LWP@h*~$>hdAi5o;PI)qOohjI}dlzS^p-euKFP-K1kce6)V4$M~~aydB`mg zq)7hp`^+d<($y}adm%{BWxL%l1JfgKi6G&a8@0L*xif-9DR0+m&5Q?Y`ysZr%2e(Gn8Ag1UJdVd&8% z6(rQkXly>T!MX6`|s2eQk*(mz!+bgSeLN{BGCL!JBt?k=R9|wJp)!{BR wi(i#)VcoW_ Date: Mon, 28 Jul 2025 13:49:43 +0000 Subject: [PATCH 09/21] Update Dockerfile and devcontainer configuration; change Python shebang to python3 --- .devcontainer/Dockerfile | 2 -- .devcontainer/devcontainer.json | 1 - src/sentor/scripts/sentor_node.py | 2 +- 3 files changed, 1 insertion(+), 4 deletions(-) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 4b16ca7..121fdde 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -30,5 +30,3 @@ FROM depbuilder AS final # add sudo without password ENV DEBIAN_FRONTEND=noninteractive - -USER ros \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index b97a286..cf378d7 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -12,7 +12,6 @@ "postStartCommand": "/opt/entrypoint.sh /bin/true; .devcontainer/post-create.sh", - "remoteUser": "ros", "updateRemoteUserUID": true, // ensure internal user has the same UID as the host user and update file permissions "customizations": { "vscode": { diff --git a/src/sentor/scripts/sentor_node.py b/src/sentor/scripts/sentor_node.py index 487a70e..4db95b1 100755 --- a/src/sentor/scripts/sentor_node.py +++ b/src/sentor/scripts/sentor_node.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import rclpy import yaml import time From df74cfa6fbc74bb00ca4d7950fb5efaf0b5ae7e4 Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Mon, 28 Jul 2025 14:03:11 +0000 Subject: [PATCH 10/21] Update Dockerfile and devcontainer.json; install black and flake8, and rename devcontainer --- .devcontainer/Dockerfile | 3 ++- .devcontainer/devcontainer.json | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 121fdde..1644151 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -26,7 +26,8 @@ RUN rosdep update --rosdistro ${ROS_DISTRO} && apt-get update RUN cd /tmp/src && rosdep install --from-paths . --ignore-src -r -y && cd && rm -rf /tmp/src FROM depbuilder AS final -# add user ros with sudo rights if it doesn't exist + +RUN pip install black flake8 # add sudo without password ENV DEBIAN_FRONTEND=noninteractive diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index cf378d7..560c857 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,7 +1,7 @@ // For format details, see https://aka.ms/devcontainer.json. For config options, see the // README at: https://github.com/devcontainers/templates/tree/main/src/ubuntu { - "name": "L-CAS Humble CUDA-OpenGL Devcontainer", + "name": "L-CAS ROS Humble Devcontainer for Sentor", "build": { "dockerfile": "./Dockerfile", "args": { From 855a8d05726fa52f19bd9a005c02cf5817725f45 Mon Sep 17 00:00:00 2001 From: Cyano0 <36192489+Cyano0@users.noreply.github.com> Date: Mon, 28 Jul 2025 15:23:17 +0100 Subject: [PATCH 11/21] Update package.xml --- src/sentor/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/sentor/package.xml b/src/sentor/package.xml index b078255..2091bc0 100644 --- a/src/sentor/package.xml +++ b/src/sentor/package.xml @@ -12,9 +12,11 @@ rclpy sentor_msgs + control_msgs rclpy sentor_msgs + control_msgs ament_cmake From a5b5dee874ecf36ff7acf0572c2eb15201c5292b Mon Sep 17 00:00:00 2001 From: Cyano0 <36192489+Cyano0@users.noreply.github.com> Date: Mon, 28 Jul 2025 16:38:16 +0100 Subject: [PATCH 12/21] Update SafetyMonitor.py Updates the output information --- src/sentor/sentor/SafetyMonitor.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/sentor/sentor/SafetyMonitor.py b/src/sentor/sentor/SafetyMonitor.py index 3ed7a68..c36e02b 100644 --- a/src/sentor/sentor/SafetyMonitor.py +++ b/src/sentor/sentor/SafetyMonitor.py @@ -75,7 +75,7 @@ def _safety_pub_cb(self): alive = getattr(m, self.attr) # pick human‐readable name: topic_name or target name = getattr(m, 'topic_name', None) or getattr(m, 'target', '') - self.get_logger().info(f"[SAFETY] {name}.is_{self.attr} = {alive}") + self.get_logger().info(f"[SAFETY] {name}.{self.attr} = {alive}") states.append(alive) # arm the “all good” timer if needed @@ -125,4 +125,4 @@ def stop_monitor(self): self._stop_event.set() def start_monitor(self): - self._stop_event.clear() \ No newline at end of file + self._stop_event.clear() From d72af8cd126af80aba135e491fc6b3397b7590e7 Mon Sep 17 00:00:00 2001 From: Cyano0 <36192489+Cyano0@users.noreply.github.com> Date: Mon, 28 Jul 2025 16:43:04 +0100 Subject: [PATCH 13/21] Update Executor.py --- src/sentor/sentor/Executor.py | 62 ++++++++++++++++++++++++----------- 1 file changed, 42 insertions(+), 20 deletions(-) diff --git a/src/sentor/sentor/Executor.py b/src/sentor/sentor/Executor.py index e5c91de..90a6a82 100644 --- a/src/sentor/sentor/Executor.py +++ b/src/sentor/sentor/Executor.py @@ -30,8 +30,8 @@ # Import standard Python libraries import os import numpy as np +import importlib import math -import subprocess import time from threading import Lock import subprocess @@ -139,39 +139,61 @@ def init_call(self, process): self.event_cb(self.init_err_str.format("call", str(e)), "warn") self.processes.append("not_initialized") - def get_topic_type(self, topic_name): - """ Automatically retrieves the topic type using the ROS2 CLI """ + def get_topic_msg_class(self, topic_name): + """Return the *message class* for *topic_name* using rclpy APIs. + + Caches lookups to avoid repeated expensive graph traversals. + """ + # 1. Cached already? -------------------------------------------------- + if topic_name in self.topic_type_cache: + return self.topic_type_cache[topic_name] + + # 2. Query the ROS 2 graph ------------------------------------------- + topic_names_and_types = self.get_topic_names_and_types() + for name, types in topic_names_and_types: + if name == topic_name and types: + ros2_type = types[0] # e.g. "std_msgs/msg/String" + break + else: + raise ValueError(f"Unknown topic type for '{topic_name}'. Is it currently advertised?") + + # 3. Convert ROS2 type → Python import path --------------------------- + # std_msgs/msg/String -> std_msgs.msg.String + parts = ros2_type.split('/') # [pkg, 'msg', MsgName] + if len(parts) != 3 or parts[1] != 'msg': + raise ValueError(f"Unexpected type string '{ros2_type}' for topic '{topic_name}'") + + module_name = f"{parts[0]}.msg" + class_name = parts[2] + try: - result = subprocess.run(["ros2", "topic", "type", topic_name], capture_output=True, text=True) - topic_type = result.stdout.strip() - if topic_type: - return topic_type.replace("/", ".msg.") # Convert ROS2 format to Python import format - else: - raise ValueError(f"Unknown topic type for {topic_name}") - except Exception as e: - raise ValueError(f"Failed to retrieve topic type: {str(e)}") + module = importlib.import_module(module_name) + msg_class = getattr(module, class_name) + except (ImportError, AttributeError) as e: + raise ValueError(f"Cannot import message class for '{ros2_type}': {e}") + + # 4. Cache & return ---------------------------------------------------- + self.topic_type_cache[topic_name] = msg_class + return msg_class def init_publish(self, process): try: topic_name = self.get_param_name(process["publish"]["topic_name"]) - msg_type = self.get_topic_type(topic_name) + msg_class = self.get_topic_msg_class(topic_name) - pub = self.create_publisher(msg_type, topic_name, 10) - - msg = msg_type() + pub = self.create_publisher(msg_class, topic_name, 10) + + msg = msg_class() for arg in process["publish"]["topic_args"]: exec(arg) - d = { + self.processes.append({ "name": "publish", "verbose": self.is_verbose(process["publish"]), "def_msg": (f"Publishing to topic '{topic_name}'", "info", msg), "func": "self.publish(**kwargs)", "kwargs": {"pub": pub, "msg": msg}, - } - - self.processes.append(d) - + }) except Exception as e: self.event_cb(self.init_err_str.format("publish", str(e)), "warn") self.processes.append("not_initialized") From 3553dae98a15251a7560006cbaca6c7f7da6c859 Mon Sep 17 00:00:00 2001 From: Cyano0 <36192489+Cyano0@users.noreply.github.com> Date: Tue, 29 Jul 2025 09:29:15 +0100 Subject: [PATCH 14/21] Update Dockerfile add ros-humble-controller-manager-msgs --- .devcontainer/Dockerfile | 1 + 1 file changed, 1 insertion(+) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 1644151..c3bf050 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -10,6 +10,7 @@ RUN apt-get update \ && apt-get install -qq -y --no-install-recommends \ git \ python3-pip \ + ros-humble-controller-manager-msgs \ python3-rosdep # get the source tree and analyse it for its package.xml only From f9aa32a74c9906488c016c344c84385907e78767 Mon Sep 17 00:00:00 2001 From: Anonymous L-CAS DevContainer User Date: Tue, 29 Jul 2025 12:32:34 +0000 Subject: [PATCH 15/21] Update package.xml with ROS dependencies for sentor --- .devcontainer/Dockerfile | 1 - src/sentor/package.xml | 12 ++++++++++++ src/sentor/sentor/service_types.py | 2 +- 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index c3bf050..1644151 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -10,7 +10,6 @@ RUN apt-get update \ && apt-get install -qq -y --no-install-recommends \ git \ python3-pip \ - ros-humble-controller-manager-msgs \ python3-rosdep # get the source tree and analyse it for its package.xml only diff --git a/src/sentor/package.xml b/src/sentor/package.xml index 2091bc0..0308d81 100644 --- a/src/sentor/package.xml +++ b/src/sentor/package.xml @@ -7,16 +7,28 @@ Your Name MIT + ament_cmake ament_cmake_python + rclpy sentor_msgs control_msgs + rclpy sentor_msgs control_msgs + controller_manager_msgs + example_interfaces + geographic_msgs + logging_demo + map_msgs + pcl_msgs + robot_localization + turtlesim + zed_msgs ament_cmake diff --git a/src/sentor/sentor/service_types.py b/src/sentor/sentor/service_types.py index b6c7445..42213eb 100644 --- a/src/sentor/sentor/service_types.py +++ b/src/sentor/sentor/service_types.py @@ -25,7 +25,7 @@ from tf2_msgs.srv import FrameGraph from turtlesim.srv import Kill, SetPen, Spawn, TeleportAbsolute, TeleportRelative from visualization_msgs.srv import GetInteractiveMarkers -from zed_interfaces.srv import SetPose, SetROI, StartSvoRec +from zed_msgs.srv import SetPose, SetROI, StartSvoRec from rcl_interfaces.srv import GetParameters, SetParameters, ListParameters from lifecycle_msgs.srv import GetState, ChangeState from nav_msgs.srv import GetPlan From d0a8f9fdab97f710ae97d2b8fcb39ebfb6dd045a Mon Sep 17 00:00:00 2001 From: Cyano0 <36192489+Cyano0@users.noreply.github.com> Date: Tue, 29 Jul 2025 14:23:03 +0100 Subject: [PATCH 16/21] Update src/sentor/sentor/ROSTopicHz.py Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/sentor/sentor/ROSTopicHz.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sentor/sentor/ROSTopicHz.py b/src/sentor/sentor/ROSTopicHz.py index 96cc8c4..b6f9e85 100644 --- a/src/sentor/sentor/ROSTopicHz.py +++ b/src/sentor/sentor/ROSTopicHz.py @@ -26,7 +26,7 @@ def __init__(self, node, topic_name, window_size=1000, throttle_val=1, stop_even self.last_printed_time = None self.prev_time = None self.times = [] - self.msg_tn = None # <-- ✅ This was missing + self.msg_tn = None self.node.get_logger().info(f"[ROSTopicHz] Initialized for {self.topic_name} with window {self.window_size}") # self._stop_event = Event() # Accept stop_event from outside or create internally From 724688b6a85a31566e7c163beb458e6e50b267c3 Mon Sep 17 00:00:00 2001 From: Cyano0 <36192489+Cyano0@users.noreply.github.com> Date: Tue, 29 Jul 2025 14:23:19 +0100 Subject: [PATCH 17/21] Update src/sentor/scripts/test_sentor.py Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/sentor/scripts/test_sentor.py | 22 ---------------------- 1 file changed, 22 deletions(-) diff --git a/src/sentor/scripts/test_sentor.py b/src/sentor/scripts/test_sentor.py index bdd1ef8..147c2d5 100755 --- a/src/sentor/scripts/test_sentor.py +++ b/src/sentor/scripts/test_sentor.py @@ -178,28 +178,6 @@ def main(): -# #!/usr/bin/env python3 -# import rclpy -# import yaml -# import time -# import signal -# import os -# from sentor.TopicMonitor import TopicMonitor -# from sentor.MultiMonitor import MultiMonitor -# from sentor.SafetyMonitor import SafetyMonitor -# from std_msgs.msg import String -# from sentor_msgs.msg import SentorEvent -# from std_srvs.srv import Empty -# import importlib -# from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy -# from rclpy.executors import MultiThreadedExecutor - - -# topic_monitors = [] -# event_pub = None -# rich_event_pub = None -# multi_monitor = None - # def __signal_handler(signum, frame): # """ Gracefully stop all monitors on SIGINT. """ # for topic_monitor in topic_monitors: From b4c1ad048cd8f3239badd1098aca38d86a30bba9 Mon Sep 17 00:00:00 2001 From: Anonymous L-CAS DevContainer User Date: Tue, 29 Jul 2025 14:41:04 +0000 Subject: [PATCH 18/21] =?UTF-8?q?Update=20cache=20to=20avoid=20repeating?= =?UTF-8?q?=20expensive=20look=E2=80=91ups?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/sentor/sentor/Executor.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/sentor/sentor/Executor.py b/src/sentor/sentor/Executor.py index 90a6a82..1c010a9 100644 --- a/src/sentor/sentor/Executor.py +++ b/src/sentor/sentor/Executor.py @@ -68,7 +68,8 @@ def __init__(self, config, event_cb): super().__init__('executor_node') self.config = config self.event_cb = event_cb - + self.topic_type_cache: dict[str, type] = {} # NEW: cache to avoid repeating expensive look‑ups + self.init_err_str = "Unable to initialize process of type '{}': {}" self._lock = Lock() self.processes = [] From c8706b60327acc9e2537fe53f1e66f85afe55094 Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Wed, 30 Jul 2025 07:26:46 +0000 Subject: [PATCH 19/21] Add launch files and update package.xml for Sentor monitoring system --- src/sentor/CMakeLists.txt | 7 +++- src/sentor/launch/README.md | 47 +++++++++++++++++++++++++ src/sentor/launch/sentor_launch.py | 55 ++++++++++++++++++++++++++++++ src/sentor/package.xml | 4 +++ 4 files changed, 112 insertions(+), 1 deletion(-) create mode 100644 src/sentor/launch/README.md create mode 100644 src/sentor/launch/sentor_launch.py diff --git a/src/sentor/CMakeLists.txt b/src/sentor/CMakeLists.txt index b3b0fb7..5154016 100644 --- a/src/sentor/CMakeLists.txt +++ b/src/sentor/CMakeLists.txt @@ -18,7 +18,12 @@ install(PROGRAMS # Install config files install(DIRECTORY config - DESTINATION share/${PROJECT_NAME}/config + DESTINATION share/${PROJECT_NAME} +) + +# Install launch files +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} ) ament_package() diff --git a/src/sentor/launch/README.md b/src/sentor/launch/README.md new file mode 100644 index 0000000..60eea2b --- /dev/null +++ b/src/sentor/launch/README.md @@ -0,0 +1,47 @@ +# Sentor Launch Files + +This directory contains launch files for the Sentor monitoring system. + +## Available Launch Files + +### `sentor_launch.py` (Python Launch File) +A Python-based launch file that provides more flexibility and programmatic control. + +**Usage:** +```bash +# Launch with default configuration +ros2 launch sentor sentor_launch.py + +# Launch with custom config file +ros2 launch sentor sentor_launch.py config_file:=/path/to/your/config.yaml + +# Launch with different log level +ros2 launch sentor sentor_launch.py log_level:=debug + +# Launch with both custom config and log level +ros2 launch sentor sentor_launch.py config_file:=/path/to/config.yaml log_level:=warn +``` + +## Launch Arguments + +- `config_file`: Path to the YAML configuration file for sentor monitoring + - Default: `$(find-pkg-share sentor)/config/test_monitor_config.yaml` + - Type: string + +- `log_level`: Log level for the sentor node + - Default: `info` + - Valid values: `debug`, `info`, `warn`, `error`, `fatal` + - Type: string + +## Configuration + +The default configuration file is located at: +``` +src/sentor/config/test_monitor_config.yaml +``` + +After installation, it will be available at: +``` +install/sentor/share/sentor/config/test_monitor_config.yaml +``` + diff --git a/src/sentor/launch/sentor_launch.py b/src/sentor/launch/sentor_launch.py new file mode 100644 index 0000000..7a6fafd --- /dev/null +++ b/src/sentor/launch/sentor_launch.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 +""" +ROS2 Launch file for Sentor monitoring system. + +This launch file starts the sentor_node with the default test_monitor_config.yaml configuration. +""" + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + """Generate the launch description for the sentor monitoring system.""" + + # Declare launch arguments + config_file_arg = DeclareLaunchArgument( + 'config_file', + default_value=PathJoinSubstitution([ + FindPackageShare('sentor'), + 'config', + 'test_monitor_config.yaml' + ]), + description='Path to the YAML configuration file for sentor monitoring' + ) + + log_level_arg = DeclareLaunchArgument( + 'log_level', + default_value='info', + description='Log level for the sentor node (debug, info, warn, error, fatal)' + ) + + # Define the sentor node + sentor_node = Node( + package='sentor', + executable='sentor_node.py', + name='sentor', + output='screen', + parameters=[{ + 'config_file': LaunchConfiguration('config_file') + }], + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], + remappings=[ + # Add any topic remappings here if needed + ] + ) + + return LaunchDescription([ + config_file_arg, + log_level_arg, + sentor_node, + ]) diff --git a/src/sentor/package.xml b/src/sentor/package.xml index 0308d81..b4e6bb2 100644 --- a/src/sentor/package.xml +++ b/src/sentor/package.xml @@ -29,6 +29,10 @@ robot_localization turtlesim zed_msgs + + + launch + launch_ros ament_cmake From 54acaa6d0c84666a907ab9b60ffdf0689da51d41 Mon Sep 17 00:00:00 2001 From: Anonymous L-CAS DevContainer User Date: Wed, 30 Jul 2025 13:21:07 +0000 Subject: [PATCH 20/21] Update sentor_node for proper naming --- src/sentor/scripts/sentor_node.py | 245 ++++++++++++------- src/sentor/scripts/test_sentor.py | 393 ------------------------------ 2 files changed, 160 insertions(+), 478 deletions(-) delete mode 100755 src/sentor/scripts/test_sentor.py diff --git a/src/sentor/scripts/sentor_node.py b/src/sentor/scripts/sentor_node.py index 4db95b1..da3e90e 100755 --- a/src/sentor/scripts/sentor_node.py +++ b/src/sentor/scripts/sentor_node.py @@ -1,102 +1,177 @@ #!/usr/bin/env python3 -import rclpy +""" +Created on 20 May 2025 + +@author: Zhuoling Huang +""" +import os import yaml -import time import signal -import os -from sentor.TopicMonitor import TopicMonitor -from sentor.TopicMonitor import TopicMonitor -from sentor.MultiMonitor import MultiMonitor -from std_msgs.msg import String -from sentor_msgs.msg import SentorEvent + +import rclpy +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor from std_srvs.srv import Empty +from sentor.MultiMonitor import MultiMonitor +from sentor.TopicMonitor import TopicMonitor +from sentor.NodeMonitor import NodeMonitor +from sentor.SafetyMonitor import SafetyMonitor + +# ─── Globals ──────────────────────────────────────────────────────────────── topic_monitors = [] -event_pub = None -rich_event_pub = None +node_monitors = [] +multi_monitor = None + +def shutdown_handler(signum, frame): + for tm in topic_monitors: + tm.stop_monitor() + for nm in node_monitors: + nm.stop_monitor() + if multi_monitor: + multi_monitor.stop_monitor() + rclpy.shutdown() -def __signal_handler(signum, frame): - """ Gracefully stop all monitors on SIGINT. """ - for topic_monitor in topic_monitors: - topic_monitor.kill_monitor() - multi_monitor.stop_monitor() - print("stopped.") - os._exit(signal.SIGTERM) +signal.signal(signal.SIGINT, shutdown_handler) -def stop_monitoring(_): - """ Stop all monitoring activities. """ - for topic_monitor in topic_monitors: - topic_monitor.stop_monitor() - multi_monitor.stop_monitor() +# ─── Services ─────────────────────────────────────────────────────────────── +def start_monitoring(request, _): + for tm in topic_monitors: tm.start_monitor() + for nm in node_monitors: nm.start_monitor() + multi_monitor.start_monitor() return Empty.Response() -def start_monitoring(_): - """ Start monitoring activities. """ - for topic_monitor in topic_monitors: - topic_monitor.start_monitor() - multi_monitor.start_monitor() +def stop_monitoring(request, _): + for tm in topic_monitors: tm.stop_monitor() + for nm in node_monitors: nm.stop_monitor() + multi_monitor.stop_monitor() return Empty.Response() -if __name__ == "__main__": - signal.signal(signal.SIGINT, __signal_handler) - rclpy.init() - node = rclpy.create_node("sentor") - - # 🔹 Load Configuration - config_file = node.declare_parameter("config_file", "").value - topics = [] - - try: - items = [yaml.safe_load(open(item, 'r')) for item in config_file.split(',')] - topics = [item for sublist in items for item in sublist] - except Exception as e: - node.get_logger().error(f"No configuration file provided: {e}") - - stop_srv = node.create_service(Empty, '/sentor/stop_monitor', stop_monitoring) - start_srv = node.create_service(Empty, '/sentor/start_monitor', start_monitoring) - - event_pub = node.create_publisher(String, '/sentor/event', 10) - rich_event_pub = node.create_publisher(SentorEvent, '/sentor/rich_event', 10) - - multi_monitor = MultiMonitor() # 🔹 MultiMonitor no longer creates monitors! - - topic_monitors = [] - print("Monitoring topics:") - for i, topic in enumerate(topics): - if topic.get("include", True) is False: - continue # Skip topics marked as "include: False" - - try: - topic_name = topic["name"] - except KeyError: - node.get_logger().error(f"Topic name not specified for entry: {topic}") - continue - - # Extract parameters with defaults - topic_monitor = TopicMonitor( - topic_name=topic_name, - msg_type=multi_monitor.get_message_type(topic["message_type"]), - qos_profile=multi_monitor.get_qos_profile(topic.get("qos", {})), - rate=topic.get("rate", 0), - N=topic.get("N", 0), - signal_when=topic.get("signal_when", {}), - signal_lambdas=topic.get("signal_lambdas", []), - processes=topic.get("execute", []), - timeout=topic.get("timeout", 0), - default_notifications=topic.get("default_notifications", True), - event_callback=None, - thread_num=i, - ) +def event_callback(msg, level="info", **_): + print(f"[{level.upper()}] {msg}") - topic_monitors.append(topic_monitor) - multi_monitor.register_monitors(topic_monitor) # 🔹 We now pass monitors to MultiMonitor +# ─── Helpers ──────────────────────────────────────────────────────────────── +def get_message_type(type_str): + pkg, msg = type_str.split('/msg/') + module = __import__(f"{pkg}.msg", fromlist=[msg]) + return getattr(module, msg) - time.sleep(1) +def get_qos_profile(qos): + from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy + rel = ReliabilityPolicy.BEST_EFFORT if qos.get("reliability","").lower()=="best_effort" else ReliabilityPolicy.RELIABLE + dur = DurabilityPolicy.TRANSIENT_LOCAL if qos.get("durability","").lower()=="transient_local" else DurabilityPolicy.VOLATILE + hist= HistoryPolicy.KEEP_ALL if qos.get("history","").lower()=="keep_all" else HistoryPolicy.KEEP_LAST + return QoSProfile(reliability=rel, durability=dur, history=hist, depth=qos.get("depth",10)) - # Start monitoring - for topic_monitor in topic_monitors: - topic_monitor.start() +# ─── main() ───────────────────────────────────────────────────────────────── +def main(): + global topic_monitors, node_monitors, multi_monitor - rclpy.spin(node) - node.destroy_node() + rclpy.init() + driver = rclpy.create_node("test_sentor") + + # load config + cfg_path = driver.declare_parameter("config_file","~/config/test_monitor_config.yaml")\ + .get_parameter_value().string_value + cfg_path = os.path.expanduser(cfg_path) + with open(cfg_path,'r') as f: + cfg = yaml.safe_load(f) or {} + + topics_cfg = cfg.get("monitors", []) + nodes_cfg = cfg.get("node_monitors", []) + + # start/stop services + driver.create_service(Empty, "start_monitoring", start_monitoring) + driver.create_service(Empty, "stop_monitoring", stop_monitoring) + + # multi_monitor + multi_monitor = MultiMonitor() + + # 1) TopicMonitors + for idx, t in enumerate(topics_cfg): + # ensure every condition—and every lambda—has a tags list + t.setdefault("signal_when", {}).setdefault("tags", []) + for lam in t.get("signal_lambdas", []): + lam.setdefault("tags", []) + + tm = TopicMonitor( + topic_name = t["name"], + msg_type = get_message_type(t["message_type"]), + qos_profile = get_qos_profile(t.get("qos",{})), + rate = t["rate"], + N = t["N"], + signal_when_config = t.get("signal_when", {}), + signal_lambdas_config = t.get("signal_lambdas", []), + processes = t.get("execute", []), + timeout = t.get("timeout", 0.1), + default_notifications = t.get("default_notifications", True), + event_callback = event_callback, + thread_num = idx, + enable_internal_logs = True, + ) + topic_monitors.append(tm) + multi_monitor.register_monitor(tm) + + # 2) NodeMonitors + for n in nodes_cfg: + nm = NodeMonitor( + target_node_name = n["name"], + timeout = n["timeout"], + event_callback = event_callback, + safety_critical = n.get("safety_critical", False), + autonomy_critical = n.get("autonomy_critical", False), + poll_rate = n.get("poll_rate", 1.0), + ) + node_monitors.append(nm) + + # 3) safety heartbeat (safety_critical) + safety_hb = SafetyMonitor( + topic = 'safety/heartbeat', + event_msg = 'Heartbeat', + attr = 'is_alive', + srv_name = 'heartbeat_override', + event_cb = event_callback, + invert = False, + ) + # register both topic & node monitors + for tm in topic_monitors: + if tm.signal_when_cfg.get("safety_critical") or \ + any(l.get("safety_critical") for l in tm.signal_lambdas_config): + safety_hb.register_monitor(tm) + for nm in node_monitors: + if nm.safety_critical: + safety_hb.register_monitor(nm) + + # 4) warning heartbeat (autonomy_critical) + warning_hb = SafetyMonitor( + topic = 'warning/heartbeat', + event_msg = 'Warning-beat', + attr = 'is_autonomy_alive', + srv_name = 'warning_override', + event_cb = event_callback, + invert = False, + ) + for tm in topic_monitors: + if tm.signal_when_cfg.get("autonomy_critical") or \ + any(l.get("autonomy_critical") for l in tm.signal_lambdas_config): + warning_hb.register_monitor(tm) + for nm in node_monitors: + if nm.autonomy_critical: + warning_hb.register_monitor(nm) + + # 5) spin everything together + executor = MultiThreadedExecutor() + executor.add_node(driver) + executor.add_node(multi_monitor) + executor.add_node(safety_hb) + executor.add_node(warning_hb) + for tm in topic_monitors: + executor.add_node(tm.get_node()) + for nm in node_monitors: + executor.add_node(nm.node) + + executor.spin() rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/sentor/scripts/test_sentor.py b/src/sentor/scripts/test_sentor.py deleted file mode 100755 index 147c2d5..0000000 --- a/src/sentor/scripts/test_sentor.py +++ /dev/null @@ -1,393 +0,0 @@ -#!/usr/bin/env python3 -""" -Created on 20 May 2025 - -@author: Zhuoling Huang -""" -import os -import yaml -import signal - -import rclpy -from rclpy.node import Node -from rclpy.executors import MultiThreadedExecutor -from std_srvs.srv import Empty - -from sentor.MultiMonitor import MultiMonitor -from sentor.TopicMonitor import TopicMonitor -from sentor.NodeMonitor import NodeMonitor -from sentor.SafetyMonitor import SafetyMonitor - -# ─── Globals ──────────────────────────────────────────────────────────────── -topic_monitors = [] -node_monitors = [] -multi_monitor = None - -def shutdown_handler(signum, frame): - for tm in topic_monitors: - tm.stop_monitor() - for nm in node_monitors: - nm.stop_monitor() - if multi_monitor: - multi_monitor.stop_monitor() - rclpy.shutdown() - -signal.signal(signal.SIGINT, shutdown_handler) - -# ─── Services ─────────────────────────────────────────────────────────────── -def start_monitoring(request, _): - for tm in topic_monitors: tm.start_monitor() - for nm in node_monitors: nm.start_monitor() - multi_monitor.start_monitor() - return Empty.Response() - -def stop_monitoring(request, _): - for tm in topic_monitors: tm.stop_monitor() - for nm in node_monitors: nm.stop_monitor() - multi_monitor.stop_monitor() - return Empty.Response() - -def event_callback(msg, level="info", **_): - print(f"[{level.upper()}] {msg}") - -# ─── Helpers ──────────────────────────────────────────────────────────────── -def get_message_type(type_str): - pkg, msg = type_str.split('/msg/') - module = __import__(f"{pkg}.msg", fromlist=[msg]) - return getattr(module, msg) - -def get_qos_profile(qos): - from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy - rel = ReliabilityPolicy.BEST_EFFORT if qos.get("reliability","").lower()=="best_effort" else ReliabilityPolicy.RELIABLE - dur = DurabilityPolicy.TRANSIENT_LOCAL if qos.get("durability","").lower()=="transient_local" else DurabilityPolicy.VOLATILE - hist= HistoryPolicy.KEEP_ALL if qos.get("history","").lower()=="keep_all" else HistoryPolicy.KEEP_LAST - return QoSProfile(reliability=rel, durability=dur, history=hist, depth=qos.get("depth",10)) - -# ─── main() ───────────────────────────────────────────────────────────────── -def main(): - global topic_monitors, node_monitors, multi_monitor - - rclpy.init() - driver = rclpy.create_node("test_sentor") - - # load config - cfg_path = driver.declare_parameter("config_file","~/config/test_monitor_config.yaml")\ - .get_parameter_value().string_value - cfg_path = os.path.expanduser(cfg_path) - with open(cfg_path,'r') as f: - cfg = yaml.safe_load(f) or {} - - topics_cfg = cfg.get("monitors", []) - nodes_cfg = cfg.get("node_monitors", []) - - # start/stop services - driver.create_service(Empty, "start_monitoring", start_monitoring) - driver.create_service(Empty, "stop_monitoring", stop_monitoring) - - # multi_monitor - multi_monitor = MultiMonitor() - - # 1) TopicMonitors - for idx, t in enumerate(topics_cfg): - # ensure every condition—and every lambda—has a tags list - t.setdefault("signal_when", {}).setdefault("tags", []) - for lam in t.get("signal_lambdas", []): - lam.setdefault("tags", []) - - tm = TopicMonitor( - topic_name = t["name"], - msg_type = get_message_type(t["message_type"]), - qos_profile = get_qos_profile(t.get("qos",{})), - rate = t["rate"], - N = t["N"], - signal_when_config = t.get("signal_when", {}), - signal_lambdas_config = t.get("signal_lambdas", []), - processes = t.get("execute", []), - timeout = t.get("timeout", 0.1), - default_notifications = t.get("default_notifications", True), - event_callback = event_callback, - thread_num = idx, - enable_internal_logs = True, - ) - topic_monitors.append(tm) - multi_monitor.register_monitor(tm) - - # 2) NodeMonitors - for n in nodes_cfg: - nm = NodeMonitor( - target_node_name = n["name"], - timeout = n["timeout"], - event_callback = event_callback, - safety_critical = n.get("safety_critical", False), - autonomy_critical = n.get("autonomy_critical", False), - poll_rate = n.get("poll_rate", 1.0), - ) - node_monitors.append(nm) - - # 3) safety heartbeat (safety_critical) - safety_hb = SafetyMonitor( - topic = 'safety/heartbeat', - event_msg = 'Heartbeat', - attr = 'is_alive', - srv_name = 'heartbeat_override', - event_cb = event_callback, - invert = False, - ) - # register both topic & node monitors - for tm in topic_monitors: - if tm.signal_when_cfg.get("safety_critical") or \ - any(l.get("safety_critical") for l in tm.signal_lambdas_config): - safety_hb.register_monitor(tm) - for nm in node_monitors: - if nm.safety_critical: - safety_hb.register_monitor(nm) - - # 4) warning heartbeat (autonomy_critical) - warning_hb = SafetyMonitor( - topic = 'warning/heartbeat', - event_msg = 'Warning-beat', - attr = 'is_autonomy_alive', - srv_name = 'warning_override', - event_cb = event_callback, - invert = False, - ) - for tm in topic_monitors: - if tm.signal_when_cfg.get("autonomy_critical") or \ - any(l.get("autonomy_critical") for l in tm.signal_lambdas_config): - warning_hb.register_monitor(tm) - for nm in node_monitors: - if nm.autonomy_critical: - warning_hb.register_monitor(nm) - - # 5) spin everything together - executor = MultiThreadedExecutor() - executor.add_node(driver) - executor.add_node(multi_monitor) - executor.add_node(safety_hb) - executor.add_node(warning_hb) - for tm in topic_monitors: - executor.add_node(tm.get_node()) - for nm in node_monitors: - executor.add_node(nm.node) - - executor.spin() - rclpy.shutdown() - -if __name__ == "__main__": - main() - - - -# def __signal_handler(signum, frame): -# """ Gracefully stop all monitors on SIGINT. """ -# for topic_monitor in topic_monitors: -# topic_monitor.kill_monitor() -# multi_monitor.stop_monitor() -# print("Stopped monitoring.") -# os._exit(signal.SIGTERM) - -# # def stop_monitoring(_): -# # """ Stop all monitoring activities. """ -# # for topic_monitor in topic_monitors: -# # topic_monitor.stop_monitor() -# # multi_monitor.stop_monitor() -# # return Empty.Response() - -# # def start_monitoring(_): -# # """ Start monitoring activities. """ -# # for topic_monitor in topic_monitors: -# # topic_monitor.start_monitor() -# # multi_monitor.start_monitor() -# # return Empty.Response() - -# def start_monitoring(request, _): -# print("[Service] Received start_monitoring request") -# for topic_monitor in topic_monitors: -# topic_monitor.start_monitor() -# multi_monitor.start_monitor() -# return Empty.Response() - -# def stop_monitoring(request, _): -# print("[Service] Received stop_monitoring request") -# for topic_monitor in topic_monitors: -# topic_monitor.stop_monitor() -# multi_monitor.stop_monitor() -# return Empty.Response() - -# def resolve_qos(topic, multi_monitor): -# return get_qos_profile(topic["qos"]) if "qos" in topic else None - -# def resolve_msg_type(topic, multi_monitor): -# return get_message_type(topic["message_type"]) if "message_type" in topic else None - -# def get_message_type(type_str): -# """Import the actual message type class from a string like 'std_msgs/msg/Int32'.""" -# if not type_str: -# return None -# try: -# package, msg_name = type_str.split('/msg/') -# module = importlib.import_module(f"{package}.msg") -# return getattr(module, msg_name) -# except Exception as e: -# print(f"[ERROR] Failed to import message type '{type_str}': {e}") -# return None -# def get_qos_profile(qos_dict): -# """Convert YAML QoS dict into actual QoSProfile object.""" -# reliability = ReliabilityPolicy.RELIABLE -# durability = DurabilityPolicy.VOLATILE -# history = HistoryPolicy.KEEP_LAST -# depth = 10 - -# if qos_dict.get("reliability", "").lower() == "best_effort": -# reliability = ReliabilityPolicy.BEST_EFFORT -# if qos_dict.get("durability", "").lower() == "transient_local": -# durability = DurabilityPolicy.TRANSIENT_LOCAL -# if qos_dict.get("history", "").lower() == "keep_all": -# history = HistoryPolicy.KEEP_ALL -# if "depth" in qos_dict: -# depth = qos_dict["depth"] - -# return QoSProfile( -# reliability=reliability, -# durability=durability, -# history=history, -# depth=depth -# ) - -# def event_callback(message, level="info", msg=None, nodes=None, topic_name=None): -# prefix = f"[{level.upper()}]" -# print(f"{prefix} {message}") - -# def main(): -# global multi_monitor -# rclpy.init() -# node = rclpy.create_node("test_sentor") - -# node.get_logger().info("Registering start/stop monitoring services...") -# node.create_service(Empty, "start_monitoring", start_monitoring) -# node.create_service(Empty, "stop_monitoring", stop_monitoring) - -# config_file = node.declare_parameter("config_file", "config/test_monitor_config.yaml").value -# topics = [] - -# try: -# items = [yaml.safe_load(open(item, 'r')) for item in config_file.split(',')] -# for item in items: -# if isinstance(item, dict) and "monitors" in item: -# topics.extend(item["monitors"]) -# elif isinstance(item, list): -# topics.extend(item) -# elif isinstance(item, dict): -# topics.append(item) -# except Exception as e: -# node.get_logger().error(f"Error loading config file: {e}") -# rclpy.shutdown() -# return - -# multi_monitor = MultiMonitor() - -# node.get_logger().info("Registering topic monitors:") -# node.get_logger().info(f"Loaded topics from config:\n{topics}") -# for i, topic in enumerate(topics): -# if not isinstance(topic, dict): -# continue -# if topic.get("include", True) is False: -# continue - -# topic_name = topic.get("name") -# if not topic_name: -# continue - -# qos_profile = resolve_qos(topic, multi_monitor) -# msg_type = resolve_msg_type(topic, multi_monitor) - -# node.get_logger().info(f"[Monitor-{i}] Topic: {topic_name}") -# node.get_logger().info(f"[Monitor-{i}] Msg Type: {msg_type}") -# node.get_logger().info(f"[Monitor-{i}] QoS Profile: {qos_profile}") - -# topic_monitor = TopicMonitor( -# topic_name=topic_name, -# msg_type=msg_type, -# qos_profile=qos_profile, -# rate=topic.get("rate", 0), -# N=topic.get("N", 0), -# signal_when_config=topic.get("signal_when", {}), -# signal_lambdas_config=topic.get("signal_lambdas", []), -# processes=topic.get("execute", []), -# timeout=topic.get("timeout", 0), -# default_notifications=topic.get("default_notifications", True), -# event_callback=event_callback, -# thread_num=i, -# enable_internal_logs=topic.get("enable_internal_logs", True) -# ) - -# topic_monitors.append(topic_monitor) -# multi_monitor.register_monitor(topic_monitor) - -# # # Create SafetyMonitor -# # safety_monitor = SafetyMonitor( -# # topic='safety/heartbeat', -# # event_msg='Heartbeat', -# # attr='is_alive', -# # srv_name='heartbeat_override', -# # event_cb=event_callback, -# # invert=False, -# # ) - -# # # Register TopicMonitors with SafetyMonitor -# # for tm in topic_monitors: -# # # signal_when critical OR any lambda critical? -# # sw_crit = tm.signal_when_cfg.get('safety_critical', False) -# # lambdas_crit = any(l.get('safety_critical', False) for l in tm.signal_lambdas_config) -# # if sw_crit or lambdas_crit: -# # safety_monitor.register_monitor(tm) - - -# executor = MultiThreadedExecutor() -# executor.add_node(node) # test_sentor - -# # ────────────────────────────────────────────────────────────────────────────── -# # (1) your existing safety monitor… -# safety_monitor = SafetyMonitor( -# topic='safety/heartbeat', -# event_msg='Heartbeat', -# attr='is_alive', -# srv_name='heartbeat_override', -# event_cb=event_callback, -# invert=False, -# ) -# for tm in topic_monitors: -# sw_crit = tm.signal_when_cfg.get('safety_critical', False) -# lam_crit = any(l.get('safety_critical', False) for l in tm.signal_lambdas_config) -# if sw_crit or lam_crit: -# safety_monitor.register_monitor(tm) -# executor.add_node(safety_monitor) -# # ────────────────────────────────────────────────────────────────────────────── - -# # (2) NEW: warning‐level heartbeat monitor -# warning_monitor = SafetyMonitor( -# topic='warning/heartbeat', -# event_msg='Warning‐beat', -# attr='is_autonomy_alive', -# srv_name='warning_override', -# event_cb=event_callback, -# invert=False, -# ) -# for tm in topic_monitors: -# # pick up anything marked autonomy_critical in signal_when or lambdas -# sw_warn = tm.signal_when_cfg.get('autonomy_critical', False) -# lam_warn = any(l.get('autonomy_critical', False) for l in tm.signal_lambdas_config) -# if sw_warn or lam_warn: -# warning_monitor.register_monitor(tm) -# executor.add_node(warning_monitor) - -# for topic_monitor in topic_monitors: -# executor.add_node(topic_monitor.get_node()) - -# executor.add_node(multi_monitor) -# # executor.add_node(safety_monitor) # ✅ Add SafetyMonitor to executor - -# executor.spin() - -# if __name__ == "__main__": -# main() \ No newline at end of file From 0e8ad06eb7c01ff5ca6ebd76832893b86062ca0e Mon Sep 17 00:00:00 2001 From: Anonymous L-CAS DevContainer User Date: Wed, 30 Jul 2025 13:30:58 +0000 Subject: [PATCH 21/21] delete test_sentor --- src/sentor/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/src/sentor/CMakeLists.txt b/src/sentor/CMakeLists.txt index 5154016..0cd32d3 100644 --- a/src/sentor/CMakeLists.txt +++ b/src/sentor/CMakeLists.txt @@ -12,7 +12,6 @@ ament_python_install_package(${PROJECT_NAME}) # Install scripts install(PROGRAMS scripts/sentor_node.py - scripts/test_sentor.py DESTINATION lib/${PROJECT_NAME} )