diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/.pre-commit-config.yaml b/zebROS_ws/src/ROS-TCP-Endpoint/.pre-commit-config.yaml new file mode 100644 index 000000000..f27883e8a --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/.pre-commit-config.yaml @@ -0,0 +1,12 @@ +repos: +- repo: https://github.com/python/black + rev: 19.3b0 + hooks: + - id: black + args: [--line-length=100] +- repo: https://github.com/pre-commit/pre-commit-hooks + rev: v3.4.0 + hooks: + - id: trailing-whitespace + name: trailing-whitespace-markdown + types: [markdown] \ No newline at end of file diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/.yamato/sonar.yml b/zebROS_ws/src/ROS-TCP-Endpoint/.yamato/sonar.yml new file mode 100644 index 000000000..6ade88609 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/.yamato/sonar.yml @@ -0,0 +1,17 @@ +name: Sonarqube Standard Scan +agent: + type: Unity::metal::macmini + image: package-ci/mac + flavor: m1.mac +variables: + SONARQUBE_PROJECT_KEY: ai-robotics-endpoint-ros +commands: + - curl https://binaries.sonarsource.com/Distribution/sonar-scanner-cli/sonar-scanner-cli-4.6.2.2472-macosx.zip -o sonar-scanner-macosx.zip -L + - unzip sonar-scanner-macosx.zip -d ~/sonar-scanner + - ~/sonar-scanner/sonar-scanner-4.6.2.2472-macosx/bin/sonar-scanner -Dsonar.projectKey=$SONARQUBE_PROJECT_KEY -Dsonar.sources=. -Dsonar.host.url=$SONARQUBE_ENDPOINT_URL_PRD -Dsonar.login=$SONARQUBE_TOKEN_PRD +triggers: + cancel_old_ci: true + expression: | + ((pull_request.target eq "main" OR pull_request.target eq "dev-ros") + AND NOT pull_request.push.changes.all match "**/*.md") OR + (push.branch eq "main" OR push.branch eq "dev-ros") \ No newline at end of file diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/.yamato/yamato-config.yml b/zebROS_ws/src/ROS-TCP-Endpoint/.yamato/yamato-config.yml new file mode 100644 index 000000000..fd3165de3 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/.yamato/yamato-config.yml @@ -0,0 +1,32 @@ +name: Endpoint Unit Tests +agent: + type: Unity::VM + image: robotics/ci-ubuntu20:v0.1.0-795910 + flavor: i1.large +variables: + # Coverage measured as a percentage (out of 100) + COVERAGE_EXPECTED: 30 +commands: + # run unit tests and save test results in /home/bokken/build/output/Unity-Technologies/ROS-TCP-Endpoint + - command: | + source /opt/ros/noetic/setup.bash && echo "ROS_DISTRO == $ROS_DISTRO" + cd .. && mkdir -p catkin_ws/src && cp -r ./ROS-TCP-Endpoint catkin_ws/src + cd catkin_ws && catkin_make && source devel/setup.bash + cd src/ROS-TCP-Endpoint + python3 -m pytest --cov=. --cov-report xml:../../../ROS-TCP-Endpoint/test-results/coverage.xml --cov-report html:../../../ROS-TCP-Endpoint/test-results/coverage.html test/ + # check the test coverage + - command: | + linecoverage=$(head -2 test-results/coverage.xml | grep -Eo 'line-rate="[0-9]+([.][0-9]+)?"' | grep -Eo "[0-9]+([.][0-9]+)?") + echo "Line coverage: $linecoverage" + if (( $(echo "$linecoverage * 100.0 < $COVERAGE_EXPECTED" | bc -l) )); + then echo "ERROR: Code Coverage is under threshold of $COVERAGE_EXPECTED%" && exit 1 + fi +triggers: + cancel_old_ci: true + expression: | + (pull_request.target eq "main" OR push.branch eq "main" OR pull_request.target eq "dev-ros" OR push.branch eq "dev-ros") + AND NOT pull_request.push.changes.all match "**/*.md" +artifacts: + logs: + paths: + - "test-results/**/*" diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/CHANGELOG.md b/zebROS_ws/src/ROS-TCP-Endpoint/CHANGELOG.md new file mode 100644 index 000000000..1f6aa8d80 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/CHANGELOG.md @@ -0,0 +1,100 @@ +# Changelog + +All notable changes to this repository will be documented in this file. + +The format is based on [Keep a Changelog](http://keepachangelog.com/en/1.0.0/) and this project adheres to [Semantic Versioning](http://semver.org/spec/v2.0.0.html). + +## Unreleased + +### Upgrade Notes + +### Known Issues + +### Added + +### Changed + +### Deprecated + +### Removed + +### Fixed + +## [0.7.0] - 2022-02-01 + +### Added + +Added Sonarqube Scanner + +Private ros params + +Send information during hand shaking for ros and package version checks + +Send service response as one queue item + + +## [0.6.0] - 2021-09-30 + +Add the [Close Stale Issues](https://github.com/marketplace/actions/close-stale-issues) action + +### Upgrade Notes + +### Known Issues + +### Added + +Support for queue_size and latch for publishers. (https://github.com/Unity-Technologies/ROS-TCP-Endpoint/issues/82) + +### Changed + +### Deprecated + +### Removed + +### Fixed + +## [0.5.0] - 2021-07-15 + +### Upgrade Notes + +Upgrade the ROS communication to support ROS2 with Unity + +### Known Issues + +### Added + +### Changed + +### Deprecated + +### Removed + +### Fixed + +## [0.4.0] - 2021-05-27 + +Note: the logs only reflects the changes from version 0.3.0 + +### Upgrade Notes + +RosConnection 2.0: maintain a single constant connection from Unity to the Endpoint. This is more efficient than opening one connection per message, and it eliminates a whole bunch of user issues caused by ROS being unable to connect to Unity due to firewalls, proxies, etc. + +### Known Issues + +### Added + +Add a link to the Robotics forum, and add a config.yml to add a link in the Github Issues page + +Add linter, unit tests, and test coverage reporting + +### Changed + +Improving the performance of the read_message in client.py, This is done by receiving the entire message all at once instead of reading 1024 byte chunks and stitching them together as you go. + +### Deprecated + +### Removed + +Remove outdated handshake references + +### Fixed \ No newline at end of file diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/CMakeLists.txt b/zebROS_ws/src/ROS-TCP-Endpoint/CMakeLists.txt new file mode 100644 index 000000000..c095990ea --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ros_tcp_endpoint) + +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs +) + +catkin_python_setup() + +catkin_package(CATKIN_DEPENDS message_runtime ) diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/CODE_OF_CONDUCT.md b/zebROS_ws/src/ROS-TCP-Endpoint/CODE_OF_CONDUCT.md new file mode 100644 index 000000000..a7a4c577b --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/CODE_OF_CONDUCT.md @@ -0,0 +1,74 @@ +# Contributor Covenant Code of Conduct + +## Our Pledge + +In the interest of fostering an open and welcoming environment, we as +contributors and maintainers pledge to making participation in our project and +our community a harassment-free experience for everyone, regardless of age, body +size, disability, ethnicity, gender identity and expression, level of experience, +nationality, personal appearance, race, religion, or sexual identity and +orientation. + +## Our Standards + +Examples of behavior that contributes to creating a positive environment +include: + +* Using welcoming and inclusive language +* Being respectful of differing viewpoints and experiences +* Gracefully accepting constructive criticism +* Focusing on what is best for the community +* Showing empathy towards other community members + +Examples of unacceptable behavior by participants include: + +* The use of sexualized language or imagery and unwelcome sexual attention or + advances +* Trolling, insulting/derogatory comments, and personal or political attacks +* Public or private harassment +* Publishing others' private information, such as a physical or electronic + address, without explicit permission +* Other conduct which could reasonably be considered inappropriate in a + professional setting + +## Our Responsibilities + +Project maintainers are responsible for clarifying the standards of acceptable +behavior and are expected to take appropriate and fair corrective action in +response to any instances of unacceptable behavior. + +Project maintainers have the right and responsibility to remove, edit, or +reject comments, commits, code, wiki edits, issues, and other contributions +that are not aligned to this Code of Conduct, or to ban temporarily or +permanently any contributor for other behaviors that they deem inappropriate, +threatening, offensive, or harmful. + +## Scope + +This Code of Conduct applies both within project spaces and in public spaces +when an individual is representing the project or its community. Examples of +representing a project or community include using an official project e-mail +address, posting via an official social media account, or acting as an appointed +representative at an online or offline event. Representation of a project may be +further defined and clarified by project maintainers. + +## Enforcement + +Instances of abusive, harassing, or otherwise unacceptable behavior may be +reported by contacting the project team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). All +complaints will be reviewed and investigated and will result in a response that +is deemed necessary and appropriate to the circumstances. The project team is +obligated to maintain confidentiality with regard to the reporter of an incident. +Further details of specific enforcement policies may be posted separately. + +Project maintainers who do not follow or enforce the Code of Conduct in good +faith may face temporary or permanent repercussions as determined by other +members of the project's leadership. + +## Attribution + +This Code of Conduct is adapted from the [Contributor Covenant][homepage], +version 1.4, available at +https://www.contributor-covenant.org/version/1/4/code-of-conduct/ + +[homepage]: https://www.contributor-covenant.org \ No newline at end of file diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/CONTRIBUTING.md b/zebROS_ws/src/ROS-TCP-Endpoint/CONTRIBUTING.md new file mode 100644 index 000000000..c0d436050 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/CONTRIBUTING.md @@ -0,0 +1,64 @@ +# Contribution Guidelines + +Thank you for your interest in contributing to Unity Robotics! To facilitate your +contributions, we've outlined a brief set of guidelines to ensure that your extensions +can be easily integrated. + +## Communication + +First, please read through our +[code of conduct](CODE_OF_CONDUCT.md), +as we expect all our contributors to follow it. + +Second, before starting on a project that you intend to contribute to any of our +Unity Robotics packages or tutorials, we **strongly** recommend posting on the repository's +[Issues page](https://github.com/Unity-Technologies/ROS-TCP-Endpoint/issues) and +briefly outlining the changes you plan to make. This will enable us to provide +some context that may be helpful for you. This could range from advice and +feedback on how to optimally perform your changes or reasons for not doing it. + +## Git Branches + +The `main` branch corresponds to the most recent stable version of the project. The `dev` branch +contains changes that are staged to be merged into `main` as the team sees fit. + +When contributing to the project, please make sure that your Pull Request (PR) +does the following: + +- Is up-to-date with and targets the `dev` branch +- Contains a detailed description of the changes performed +- Has corresponding changes to documentation, unit tests and sample environments (if + applicable) +- Contains a summary of the tests performed to validate your changes +- Links to issue numbers that the PR resolves (if any) + + + +## Code style + +All Python code should follow the [PEP 8 style guidelines](https://pep8.org/). + +All C# code should follow the [Microsoft C# Coding Conventions](https://docs.microsoft.com/en-us/dotnet/csharp/programming-guide/inside-a-program/coding-conventions). +Additionally, the [Unity Coding package](https://docs.unity3d.com/Packages/com.unity.coding@0.1/manual/index.html) +can be used to format, encode, and lint your code according to the standard Unity +development conventions. Be aware that these Unity conventions will supersede the +Microsoft C# Coding Conventions where applicable. + +Please note that even if the code you are changing does not adhere to these guidelines, +we expect your submissions to follow these conventions. + +## Contributor License Agreements + +When you open a pull request, you will be asked to acknowledge our Contributor +License Agreement. We allow both individual contributions and contributions made +on behalf of companies. We use an open source tool called CLA assistant. If you +have any questions on our CLA, please +[submit an issue](https://github.com/Unity-Technologies/ROS-TCP-Endpoint/issues) or +email us at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). + +## Contribution review + +Once you have a change ready following the above ground rules, simply make a +pull request in GitHub. \ No newline at end of file diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/LICENSE b/zebROS_ws/src/ROS-TCP-Endpoint/LICENSE new file mode 100644 index 000000000..7507bc9a8 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/LICENSE @@ -0,0 +1,190 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + Copyright 2020 Unity Technologies + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/README.md b/zebROS_ws/src/ROS-TCP-Endpoint/README.md new file mode 100644 index 000000000..57104d426 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/README.md @@ -0,0 +1,35 @@ +# ROS TCP Endpoint + +[![License](https://img.shields.io/badge/license-Apache--2.0-green.svg)](LICENSE.md) +[![Version](https://img.shields.io/github/v/tag/Unity-Technologies/ROS-TCP-Endpoint)](https://github.com/Unity-Technologies/ROS-TCP-Endpoint/releases) +![ROS](https://img.shields.io/badge/ros-melodic,noetic-brightgreen) +![ROS](https://img.shields.io/badge/ros2-foxy,galactic-brightgreen) + +--- + +We're currently working on lots of things! Please take a short moment fill out our [survey](https://unitysoftware.co1.qualtrics.com/jfe/form/SV_0ojVkDVW0nNrHkW) to help us identify what products and packages to build next. + +--- + +## Introduction + +[ROS](https://www.ros.org/) package used to create an endpoint to accept ROS messages sent from a Unity scene. This ROS package works in tandem with the [ROS TCP Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector) Unity package. + +Instructions and examples on how to use this ROS package can be found on the [Unity Robotics Hub](https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/master/tutorials/ros_unity_integration/README.md) repository. + +## Community and Feedback + +The Unity Robotics projects are open-source and we encourage and welcome contributions. +If you wish to contribute, be sure to review our [contribution guidelines](CONTRIBUTING.md) +and [code of conduct](CODE_OF_CONDUCT.md). + +## Support +For questions or discussions about Unity Robotics package installations or how to best set up and integrate your robotics projects, please create a new thread on the [Unity Robotics forum](https://forum.unity.com/forums/robotics.623/) and make sure to include as much detail as possible. + +For feature requests, bugs, or other issues, please file a [GitHub issue](https://github.com/Unity-Technologies/ROS-TCP-Endpoint/issues) using the provided templates and the Robotics team will investigate as soon as possible. + +For any other questions or feedback, connect directly with the +Robotics team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). + +## License +[Apache License 2.0](LICENSE) \ No newline at end of file diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/launch/endpoint.launch b/zebROS_ws/src/ROS-TCP-Endpoint/launch/endpoint.launch new file mode 100644 index 000000000..9ac7b8165 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/launch/endpoint.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/package.xml b/zebROS_ws/src/ROS-TCP-Endpoint/package.xml new file mode 100644 index 000000000..7d435a12a --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/package.xml @@ -0,0 +1,23 @@ + + + ros_tcp_endpoint + 0.7.0 + Acts as the bridge between Unity messages sent via Websocket and ROS messages. + + Unity Robotics + + Apache 2.0 + catkin + rospy + message_generation + rospy + rospy + message_runtime + + + + + + + + diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/requirements.txt b/zebROS_ws/src/ROS-TCP-Endpoint/requirements.txt new file mode 100644 index 000000000..5ad32fbe1 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/requirements.txt @@ -0,0 +1,3 @@ +black +pre-commit +pytest-cov \ No newline at end of file diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/setup.py b/zebROS_ws/src/ROS-TCP-Endpoint/setup.py new file mode 100644 index 000000000..dc361fb9d --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/setup.py @@ -0,0 +1,9 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup(packages=["ros_tcp_endpoint"], package_dir={"": "src"}) + +setup(**setup_args) diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/__init__.py b/zebROS_ws/src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/__init__.py new file mode 100644 index 000000000..381ca8e6c --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/__init__.py @@ -0,0 +1,15 @@ +# Copyright 2020 Unity Technologies +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from .server import TcpServer diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/client.py b/zebROS_ws/src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/client.py new file mode 100644 index 000000000..1c5585a6e --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/client.py @@ -0,0 +1,237 @@ +# Copyright 2020 Unity Technologies +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import struct +import socket +import rospy +from io import BytesIO + +import threading +import json + +from .exceptions import TopicOrServiceNameDoesNotExistError + + +class ClientThread(threading.Thread): + """ + Thread class to read all data from a connection and pass along the data to the + desired source. + """ + + def __init__(self, conn, tcp_server, incoming_ip, incoming_port): + """ + Set class variables + Args: + conn: + tcp_server: server object + incoming_ip: connected from this IP address + incoming_port: connected from this port + """ + self.conn = conn + self.tcp_server = tcp_server + self.incoming_ip = incoming_ip + self.incoming_port = incoming_port + threading.Thread.__init__(self) + + @staticmethod + def recvall(conn, size, flags=0): + """ + Receive exactly bufsize bytes from the socket. + """ + buffer = bytearray(size) + view = memoryview(buffer) + pos = 0 + while pos < size: + read = conn.recv_into(view[pos:], size - pos, flags) + if not read: + raise IOError("No more data available") + pos += read + return bytes(buffer) + + @staticmethod + def read_int32(conn): + """ + Reads four bytes from socket connection and unpacks them to an int + + Returns: int + + """ + raw_bytes = ClientThread.recvall(conn, 4) + num = struct.unpack(" 0 and not data: + self.tcp_server.logerr( + "No data for a message size of {}, breaking!".format(full_message_size) + ) + return + + destination = destination.rstrip("\x00") + return destination, data + + @staticmethod + def serialize_message(destination, message): + """ + Serialize a destination and message class. + + Args: + destination: name of destination + message: message class to serialize + + Returns: + serialized destination and message as a list of bytes + """ + dest_bytes = destination.encode("utf-8") + length = len(dest_bytes) + dest_info = struct.pack(" + names = (str(type(name))).split(".") + module_name = names[0][8:] + class_name = names[-1].split("_")[-1][:-2] + return "{}/{}".format(module_name, class_name) + except (IndexError, AttributeError, ImportError) as e: + self.tcp_server.logerr("Failed to resolve message name: {}".format(e)) + return None + + +class SysCommand_Log: + def __init__(self): + self.text = "" + + +class SysCommand_Service: + def __init__(self): + self.srv_id = 0 + + +class SysCommand_TopicsResponse: + def __init__(self): + self.topics = [] + self.types = [] + + +class SysCommand_Handshake: + def __init__(self, metadata): + self.version = "v0.7.0" + self.metadata = json.dumps(metadata.__dict__) + + +class SysCommand_Handshake_Metadata: + def __init__(self): + self.protocol = "ROS1" diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/thread_pauser.py b/zebROS_ws/src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/thread_pauser.py new file mode 100644 index 000000000..8aaae2101 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/thread_pauser.py @@ -0,0 +1,16 @@ +import threading + + +class ThreadPauser: + def __init__(self): + self.condition = threading.Condition() + self.result = None + + def sleep_until_resumed(self): + with self.condition: + self.condition.wait() + + def resume_with_result(self, result): + self.result = result + with self.condition: + self.condition.notify() diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/unity_service.py b/zebROS_ws/src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/unity_service.py new file mode 100644 index 000000000..da54c0ea1 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/unity_service.py @@ -0,0 +1,63 @@ +# Copyright 2020 Unity Technologies +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rospy +import socket +import re + +from .communication import RosReceiver +from .client import ClientThread + + +class UnityService(RosReceiver): + """ + Class to register a ROS service that's implemented in Unity. + """ + + def __init__(self, topic, service_class, tcp_server, queue_size=10): + """ + + Args: + topic: Topic name to publish messages to + service_class: The message class in catkin workspace + queue_size: Max number of entries to maintain in an outgoing queue + """ + strippedTopic = re.sub("[^A-Za-z0-9_]+", "", topic) + self.node_name = "{}_service".format(strippedTopic) + + self.topic = topic + self.service_class = service_class + self.tcp_server = tcp_server + self.queue_size = queue_size + + self.service = rospy.Service(self.topic, self.service_class, self.send) + + def send(self, request): + """ + Connect to TCP endpoint on client, pass along message and get reply + Args: + data: message data to send outside of ROS network + + Returns: + The response message + """ + return self.tcp_server.send_unity_service(self.topic, self.service_class, request) + + def unregister(self): + """ + + Returns: + + """ + self.service.shutdown() diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/vis_oculus_msg.py b/zebROS_ws/src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/vis_oculus_msg.py new file mode 100755 index 000000000..0b5120625 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/vis_oculus_msg.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 + +import rospy +import math +import tf +from unity_robotics_demo_msgs.msg import QuestPoseStamped, UnityColor +import angles +from std_srvs.srv import Empty, EmptyResponse # Standard empty service +import tf2_ros +from geometry_msgs.msg import TransformStamped, Transform, Quaternion +from tf.transformations import quaternion_from_euler, euler_from_quaternion +import tf2_geometry_msgs +import geometry_msgs + +from wpimath import geometry as wpigeo + + +class OculusInterface: + def __init__(self): + rospy.init_node("oculus_interface", anonymous=True) + + # Yaw offset for calibration + self.yaw_offset: float = 0.0 + self.reset_postion: wpigeo.Pose2d = wpigeo.Pose2d(0, 0, 0) + + self.quest_euler_angles: list[float, float, float] = [0.0, 0.0, 0.0] # instantly converted to radians + self.quest_position: list[float, float, float] = [0.0, 0.0, 0.0] + self.quest_timestamp: float = 0 + # Whether the gyro direction is reversed (set this in your launch/config) + #self.gyro_reversed = rospy.get_param("~gyro_reversed", False) + + # Subscriber to the Oculus data topic + self.quest_topic = "/pos_rot" + quest_sub = rospy.Subscriber(self.quest_topic, QuestPoseStamped, self.oculus_data_callback) + rospy.wait_for_message(self.quest_topic, QuestPoseStamped) + # Publisher for the pose + self.reset_quest_pose = rospy.Publisher("/color", UnityColor, queue_size=1) + self.tf_broadcaster = tf.TransformBroadcaster() + + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) + # Service to zero the yaw + rospy.Service("zero_quest_odom", Empty, self.zero_position) + rospy.Service("zero_quest_yaw", Empty, self.__zero_heading) + + self.static_broadcaster = tf2_ros.StaticTransformBroadcaster() + self.static_transform_initialized = False + rospy.Timer(rospy.Duration(0.02), self.publish_pose) + + # zeroes the quests heading (should also zero robot heading to match) + def __zero_heading(self): + self.yaw_offset = self.quest_euler_angles[1] + + # zeros quest odom to by subtracing out current pose + def __reset_quest_odom(self): + self.reset_postion = self.__get_oculus_pose() + rospy.logerr(f"Reset positon {self.reset_postion}") + self.static_transform_initialized = True + + def publish_pose(self, event): + """Timer callback to publish the Oculus pose.""" + if not self.static_transform_initialized: + self.__reset_quest_odom() + final_pose: wpigeo.Pose2d = self.__get_oculus_pose() - self.reset_postion + # Create a TransformStamped message + transform_stamped = geometry_msgs.msg.TransformStamped() + + # Set the header + transform_stamped.header.stamp = rospy.Time.now() # current time + transform_stamped.header.frame_id = "quest_hardware_offset" # reference frame (could be any frame) + transform_stamped.child_frame_id = "oculus_pose" # the child frame + + # Set the translation (position) + transform_stamped.transform.translation.x = final_pose.X() + transform_stamped.transform.translation.y = final_pose.Y() + transform_stamped.transform.translation.z = 0.0 # Assuming 2D pose, set Z to 0 + + # Set the rotation (orientation) from Euler angles (yaw is the only component needed) + quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, final_pose.rotation().radians()) + transform_stamped.transform.rotation.x = quaternion[0] + transform_stamped.transform.rotation.y = quaternion[1] + transform_stamped.transform.rotation.z = quaternion[2] + transform_stamped.transform.rotation.w = quaternion[3] + + # Create a TransformBroadcaster and send the transform + tf_broadcaster = tf2_ros.TransformBroadcaster() + tf_broadcaster.sendTransform(transform_stamped) + + def zero_position(self, req): + """Service callback to zero the yaw AND call recenterPlayer""" + self.reset_quest_pose.publish(UnityColor(0,0,0,0)) + rospy.sleep(0.5) + rospy.wait_for_message(self.quest_topic, QuestPoseStamped) + self.__reset_quest_odom() + rospy.loginfo("Yaw zeroed. New offset: %f", self.yaw_offset) + return EmptyResponse() + + def oculus_data_callback(self, msg): + """Callback to handle incoming Oculus data messages.""" + self.quest_euler_angles = list(map(math.radians, [msg.eul_x, msg.eul_y, msg.eul_z])) + self.quest_position = [msg.pos_x, msg.pos_y, msg.pos_z] + #rospy.loginfo(f"Received Oculus Data: {msg}") + + def get_oculus_yaw(self): + """Return the yaw Euler angle with the offset subtracted.""" + return self.quest_euler_angles[1] - self.yaw_offset + + def get_heading(self): + """Return the robot's heading in degrees, between -pi, pi""" + yaw = self.get_oculus_yaw() + return angles.normalize_angle(yaw) + + def get_oculus_position(self) -> wpigeo.Translation2d: + return wpigeo.Translation2d(self.quest_position[2], -self.quest_position[0]) + + def __get_oculus_pose(self) -> wpigeo.Pose2d: + return wpigeo.Pose2d(self.get_oculus_position(), wpigeo.Rotation2d(self.get_heading())) # Use Pose2D for simplified 2D representation + +def main(): + oculus_interface = OculusInterface() + rospy.loginfo("Oculus Interface Node Started.") + + rate = rospy.Rate(100) # 10 Hz loop + while not rospy.is_shutdown(): + #pose = oculus_interface.get_oculus_pose() + #rospy.loginfo(f"Oculus Pose: {pose}") + rate.sleep() + +if __name__ == "__main__": + main() diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/test/__init__.py b/zebROS_ws/src/ROS-TCP-Endpoint/test/__init__.py new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/test/__init__.py @@ -0,0 +1 @@ + diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/test/test_client.py b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_client.py new file mode 100644 index 000000000..23a3c6c22 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_client.py @@ -0,0 +1,51 @@ +from unittest import mock +from unittest.mock import Mock +from ros_tcp_endpoint.client import ClientThread +from ros_tcp_endpoint.server import TcpServer +import sys +import threading + + +def test_client_thread_initialization(): + tcp_server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + mock_conn = mock.Mock() + client_thread = ClientThread(mock_conn, tcp_server, "127.0.0.1", 10000) + assert client_thread.tcp_server.node_name == "test-tcp-server" + assert client_thread.incoming_ip == "127.0.0.1" + assert client_thread.incoming_port == 10000 + + +def test_recvall_should_read_bytes_exact_size(): + mock_conn = mock.Mock() + mock_conn.recv_into.return_value = 1 + result = ClientThread.recvall(mock_conn, 8) + assert result == b"\x00\x00\x00\x00\x00\x00\x00\x00" + + +@mock.patch.object(ClientThread, "recvall", return_value=b"\x01\x00\x00\x00") +def test_read_int32_should_parse_int(mock_recvall): + mock_conn = mock.Mock() + result = ClientThread.read_int32(mock_conn) + mock_recvall.assert_called_once + assert result == 1 + + +@mock.patch.object(ClientThread, "read_int32", return_value=4) +@mock.patch.object(ClientThread, "recvall", return_value=b"Hello world!") +def test_read_string_should_decode(mock_recvall, mock_read_int): + mock_conn = mock.Mock() + result = ClientThread.read_string(mock_conn) + mock_recvall.assert_called_once + mock_read_int.assert_called_once + assert result == "Hello world!" + + +@mock.patch.object(ClientThread, "read_string", return_value="__srv") +@mock.patch.object(ClientThread, "read_int32", return_value=4) +@mock.patch.object(ClientThread, "recvall", return_value=b"Hello world!") +def test_read_message_return_destination_data(mock_recvall, mock_read_int, mock_read_str): + tcp_server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + mock_conn = mock.Mock() + client_thread = ClientThread(mock_conn, tcp_server, "127.0.0.1", 10000) + result = client_thread.read_message(mock_conn) + assert result == ("__srv", b"Hello world!") diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/test/test_publisher.py b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_publisher.py new file mode 100644 index 000000000..feecb79c0 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_publisher.py @@ -0,0 +1,20 @@ +from unittest import mock +from ros_tcp_endpoint.publisher import RosPublisher +import rospy + + +@mock.patch.object(rospy, "Publisher") +def test_publisher_send(mock_ros): + mock_tcp_server = mock.Mock() + publisher = RosPublisher("color", mock.Mock(), mock_tcp_server) + publisher.send("test data") + publisher.msg.deserialize.assert_called_once() + publisher.pub.publish.assert_called_once() + + +@mock.patch.object(rospy, "Publisher") +def test_publisher_unregister(mock_ros): + mock_tcp_server = mock.Mock() + publisher = RosPublisher("color", mock.Mock(), mock_tcp_server) + publisher.unregister() + publisher.pub.unregister.assert_called_once() diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/test/test_ros_service.py b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_ros_service.py new file mode 100644 index 000000000..50e9ff18c --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_ros_service.py @@ -0,0 +1,18 @@ +from unittest import mock +from ros_tcp_endpoint.service import RosService +import rospy + + +@mock.patch.object(rospy, "ServiceProxy") +def test_subscriber_send(mock_ros): + ros_service = RosService("color", mock.Mock()) + service_response = ros_service.send("test data") + ros_service.srv_class.deserialize.assert_called_once() + assert service_response is not None + + +@mock.patch.object(rospy, "ServiceProxy") +def test_subscriber_unregister(mock_ros): + ros_service = RosService("color", mock.Mock()) + ros_service.unregister() + ros_service.srv.close.assert_called_once() diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/test/test_server.py b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_server.py new file mode 100644 index 000000000..d86ce7875 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_server.py @@ -0,0 +1,174 @@ +from unittest import mock +from ros_tcp_endpoint import TcpServer +from ros_tcp_endpoint.server import SysCommands +import importlib +import rospy +import sys +import ros_tcp_endpoint + + +@mock.patch("socket.socket") +@mock.patch("ros_tcp_endpoint.server.rospy") +def test_server_constructor(mock_ros, mock_socket): + mock_ros.get_param = mock.Mock(return_value="127.0.0.1") + server = TcpServer("test-tcp-server") + assert server.node_name == "test-tcp-server" + assert server.tcp_ip == "127.0.0.1" + assert server.buffer_size == 1024 + assert server.connections == 10 + + +def test_start_server(): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + assert server.tcp_ip == "127.0.0.1" + assert server.tcp_port == 10000 + assert server.connections == 10 + server.start() + + +def test_unity_service_empty_topic_should_return_none(): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + system_cmds = SysCommands(server) + result = system_cmds.unity_service("", "test message") + assert result is None + + +def test_unity_service_resolve_message_name_failure(): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + system_cmds = SysCommands(server) + result = system_cmds.unity_service("get_pos", "unresolvable message") + assert result is None + + +@mock.patch.object(rospy, "Service") +@mock.patch.object( + SysCommands, "resolve_message_name", return_value="unity_interfaces.msg/RosUnitySrvMessage" +) +def test_unity_service_resolve_news_service(mock_resolve_message, mock_ros_service): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + assert server.ros_services_table == {} + system_cmds = SysCommands(server) + result = system_cmds.unity_service("get_pos", "unity_interfaces.msg/RosUnitySrvMessage") + mock_ros_service.assert_called_once + assert result is None + + +@mock.patch.object(rospy, "Service") +@mock.patch.object( + SysCommands, "resolve_message_name", return_value="unity_interfaces.msg/RosUnitySrvMessage" +) +def test_unity_service_resolve_existing_service(mock_resolve_message, mock_ros_service): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + server.ros_services = {"get_pos": mock.Mock()} + system_cmds = SysCommands(server) + result = system_cmds.unity_service("get_pos", "unity_interfaces.msg/RosUnitySrvMessage") + mock_ros_service.assert_called_once + assert result is None + + +@mock.patch.object(sys, "modules", return_value="unity_interfaces.msg") +@mock.patch.object(importlib, "import_module") +def test_resolve_message_name(mock_import_module, mock_sys_modules): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + msg_name = "unity_interfaces.msg/UnityColor.msg" + result = SysCommands(server).resolve_message_name(msg_name) + mock_import_module.assert_called_once + mock_sys_modules.assert_called_once + assert result is not None + + +@mock.patch.object(rospy, "Publisher") +def test_publish_add_new_topic(mock_ros_publisher): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + result = SysCommands(server).publish("object_pos_topic", "std_msgs/Bool") + assert server.publishers_table != {} + mock_ros_publisher.assert_called_once + + +@mock.patch.object(rospy, "Publisher") +def test_publish_existing_topic(mock_ros_publisher): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + server.publishers_table = {"object_pos_topic": mock.Mock()} + result = SysCommands(server).publish("object_pos_topic", "std_msgs/Bool") + assert server.publishers_table["object_pos_topic"] is not None + mock_ros_publisher.assert_called_once + + +def test_publish_empty_topic_should_return_none(): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + result = SysCommands(server).publish("", "pos") + assert result is None + assert server.publishers_table == {} + + +def test_publish_empty_message_should_return_none(): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + result = SysCommands(server).publish("test-topic", "") + assert result is None + assert server.publishers_table == {} + + +@mock.patch.object(rospy, "Subscriber") +@mock.patch.object(SysCommands, "resolve_message_name", return_value="unity_interfaces.msg/Pos") +def test_subscribe_to_new_topic(mock_resolve_msg, mock_ros_subscriber): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + result = SysCommands(server).subscribe("object_pos_topic", "pos") + assert server.subscribers_table != {} + mock_ros_subscriber.assert_called_once + + +@mock.patch.object(rospy, "Subscriber") +@mock.patch.object(SysCommands, "resolve_message_name", return_value="unity_interfaces.msg/Pos") +def test_subscribe_to_existing_topic(mock_resolve_msg, mock_ros_subscriber): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + server.subscribers_table = {"object_pos_topic": mock.Mock()} + result = SysCommands(server).subscribe("object_pos_topic", "pos") + assert server.subscribers_table["object_pos_topic"] is not None + mock_ros_subscriber.assert_called_once + + +def test_subscribe_to_empty_topic_should_return_none(): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + result = SysCommands(server).subscribe("", "pos") + assert result is None + assert server.subscribers_table == {} + + +def test_subscribe_to_empty_message_should_return_none(): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + result = SysCommands(server).subscribe("test-topic", "") + assert result is None + assert server.subscribers_table == {} + + +@mock.patch.object(rospy, "ServiceProxy") +@mock.patch.object(SysCommands, "resolve_message_name") +def test_ros_service_new_topic(mock_resolve_msg, mock_ros_service): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + result = SysCommands(server).ros_service("object_pos_topic", "pos") + assert server.ros_services_table != {} + mock_ros_service.assert_called_once + + +@mock.patch.object(rospy, "ServiceProxy") +@mock.patch.object(SysCommands, "resolve_message_name") +def test_ros_service_existing_topic(mock_resolve_msg, mock_ros_service): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + server.ros_services_table = {"object_pos_topic": mock.Mock()} + result = SysCommands(server).ros_service("object_pos_topic", "pos") + assert server.ros_services_table["object_pos_topic"] is not None + mock_ros_service.assert_called_once + + +def test_ros_service_empty_topic_should_return_none(): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + result = SysCommands(server).ros_service("", "pos") + assert result is None + assert server.ros_services_table == {} + + +def test_ros_service_empty_message_should_return_none(): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + result = SysCommands(server).ros_service("test-topic", "") + assert result is None + assert server.ros_services_table == {} diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/test/test_subscriber.py b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_subscriber.py new file mode 100644 index 000000000..2a7f13eff --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_subscriber.py @@ -0,0 +1,21 @@ +from unittest import mock +from ros_tcp_endpoint.subscriber import RosSubscriber +import rospy + + +@mock.patch.object(rospy, "Subscriber") +def test_subscriber_send(mock_ros): + mock_tcp_server = mock.Mock() + subscriber = RosSubscriber("color", mock.Mock(), mock_tcp_server) + assert subscriber.node_name == "color_RosSubscriber" + subscriber.send("test data") + mock_tcp_server.send_unity_message.assert_called_once() + + +@mock.patch.object(rospy, "Subscriber") +def test_subscriber_unregister(mock_ros): + mock_tcp_server = mock.Mock() + subscriber = RosSubscriber("color", mock.Mock(), mock_tcp_server) + assert subscriber.node_name == "color_RosSubscriber" + subscriber.unregister() + subscriber.sub.unregister.assert_called_once() diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/test/test_tcp_sender.py b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_tcp_sender.py new file mode 100644 index 000000000..93efd13d0 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_tcp_sender.py @@ -0,0 +1,64 @@ +import queue +import socket +from unittest import mock +from ros_tcp_endpoint import TcpServer +import ros_tcp_endpoint + + +@mock.patch("ros_tcp_endpoint.tcp_sender.rospy") +def test_tcp_sender_constructor(mock_ros): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + tcp_sender = ros_tcp_endpoint.tcp_sender.UnityTcpSender(server) + assert tcp_sender.sender_id == 1 + assert tcp_sender.time_between_halt_checks == 5 + assert tcp_sender.queue == None + assert tcp_sender.next_srv_id == 1001 + assert tcp_sender.srv_lock != None + assert tcp_sender.services_waiting == {} + + +# @mock.patch("socket.socket") +# @mock.patch.object(ros_tcp_endpoint.client.ClientThread, "serialize_message") +# def test_send_unity_error_should_send_msg(mock_serialize_msg, mock_socket): +# sender = ros_tcp_endpoint.tcp_sender.UnityTcpSender() +# sender.queue = queue.Queue() +# sender.send_unity_error("Test error") +# mock_serialize_msg.assert_called_once() + + +@mock.patch.object(ros_tcp_endpoint.client.ClientThread, "serialize_message") +def test_send_message_should_serialize_message(mock_serialize_msg): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + sender = ros_tcp_endpoint.tcp_sender.UnityTcpSender(server) + sender.queue = queue.Queue() + sender.send_unity_message("test topic", "test msg") + mock_serialize_msg.assert_called_once() + + +# @mock.patch.object(ros_tcp_endpoint.thread_pauser.ThreadPauser, "sleep_until_resumed") +# @mock.patch.object(ros_tcp_endpoint.client.ClientThread, "serialize_message") +# def test_send_unity_service_should_serialize_ros_unity_srv(mock_serialize_msg, mock_thread_pauser): +# sender = ros_tcp_endpoint.tcp_sender.UnityTcpSender() +# sender.send_unity_service_request(mock.Mock(), mock.Mock(), mock.Mock()) +# mock_serialize_msg.assert_called_once() +# # TODO: Test the scenario when the queue is not None +# assert sender.queue == None + + +@mock.patch("ros_tcp_endpoint.thread_pauser.ThreadPauser") +def test_send_unity_service_response_should_resume(mock_thread_pauser_class): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + sender = ros_tcp_endpoint.tcp_sender.UnityTcpSender(server) + thread_pauser = mock_thread_pauser_class() + sender.services_waiting = {"moveit": thread_pauser} + sender.send_unity_service_response("moveit", "test data") + thread_pauser.resume_with_result.assert_called_once() + + +def test_start_sender_should_increase_sender_id(): + server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) + sender = ros_tcp_endpoint.tcp_sender.UnityTcpSender(server) + init_sender_id = 1 + assert sender.sender_id == init_sender_id + sender.start_sender(mock.Mock(), mock.Mock()) + assert sender.sender_id == init_sender_id + 1 diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/test/test_thread_pauser.py b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_thread_pauser.py new file mode 100644 index 000000000..38d786e27 --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_thread_pauser.py @@ -0,0 +1,17 @@ +import threading +from unittest import mock +from ros_tcp_endpoint.thread_pauser import ThreadPauser + + +@mock.patch.object(threading.Condition, "wait") +def test_sleep_until_resumed(mock_thread_wait): + thread_pauser = ThreadPauser() + thread_pauser.sleep_until_resumed() + mock_thread_wait.assert_called_once + + +@mock.patch.object(threading.Condition, "notify") +def test_resume(mock_thread_notify): + thread_pauser = ThreadPauser() + thread_pauser.resume_with_result(mock.Mock) + mock_thread_notify.assert_called_once diff --git a/zebROS_ws/src/ROS-TCP-Endpoint/test/test_unity_service.py b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_unity_service.py new file mode 100644 index 000000000..6b847bf8e --- /dev/null +++ b/zebROS_ws/src/ROS-TCP-Endpoint/test/test_unity_service.py @@ -0,0 +1,21 @@ +from unittest import mock +from ros_tcp_endpoint.unity_service import UnityService +import rospy + + +@mock.patch.object(rospy, "Service") +def test_unity_service_send(mock_ros_service): + mock_tcp_server = mock.Mock() + unity_service = UnityService("color", mock.Mock(), mock_tcp_server) + assert unity_service.node_name == "color_service" + unity_service.send("test data") + mock_tcp_server.send_unity_service.assert_called_once() + + +@mock.patch.object(rospy, "Service") +def test_unity_service_unregister(mock_ros_service): + mock_tcp_server = mock.Mock() + unity_service = UnityService("color", mock.Mock(), mock_tcp_server) + assert unity_service.node_name == "color_service" + unity_service.unregister() + unity_service.service.shutdown.assert_called_once() diff --git a/zebROS_ws/src/controller_node/launch/9000_jetson.launch b/zebROS_ws/src/controller_node/launch/9000_jetson.launch index e172ebfc4..13474cfc8 100644 --- a/zebROS_ws/src/controller_node/launch/9000_jetson.launch +++ b/zebROS_ws/src/controller_node/launch/9000_jetson.launch @@ -42,11 +42,14 @@ fl_cancoder_controller br_cancoder_controller fr_cancoder_controller - swerve_drive_controller - swerve_drive_state_controller + swerve_angle_phoenix6_controller cancoder_state_controller robot_code_ready_controller" /> + + + + + diff --git a/zebROS_ws/src/ros_packages/unity_robotics_demo/scripts/color_publisher.py b/zebROS_ws/src/ros_packages/unity_robotics_demo/scripts/color_publisher.py new file mode 100644 index 000000000..28dc4e296 --- /dev/null +++ b/zebROS_ws/src/ros_packages/unity_robotics_demo/scripts/color_publisher.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python + +import random +import rospy +import rosgraph +import time +from unity_robotics_demo_msgs.msg import UnityColor + + +TOPIC_NAME = 'color' +NODE_NAME = 'color_publisher' + + +def post_color(): + pub = rospy.Publisher(TOPIC_NAME, UnityColor, queue_size=10) + rospy.init_node(NODE_NAME, anonymous=True) + + r = random.randint(0, 255) + g = random.randint(0, 255) + b = random.randint(0, 255) + color = UnityColor(r, g, b, 1) + + wait_for_connections(pub, TOPIC_NAME) + pub.publish(color) + + time.sleep(0.1) + + +def wait_for_connections(pub, topic): + ros_master = rosgraph.Master('/rostopic') + topic = rosgraph.names.script_resolve_name('rostopic', topic) + num_subs = 0 + for sub in ros_master.getSystemState()[1]: + if sub[0] == topic: + num_subs+=1 + + for i in range(10): + if pub.get_num_connections() == num_subs: + return + time.sleep(0.1) + raise RuntimeError("failed to get publisher") + + +if __name__ == '__main__': + try: + post_color() + except rospy.ROSInterruptException: + pass diff --git a/zebROS_ws/src/ros_packages/unity_robotics_demo/scripts/position_service.py b/zebROS_ws/src/ros_packages/unity_robotics_demo/scripts/position_service.py new file mode 100644 index 000000000..d2e5ef2a7 --- /dev/null +++ b/zebROS_ws/src/ros_packages/unity_robotics_demo/scripts/position_service.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python + +from __future__ import print_function + +import random +import rospy + +from unity_robotics_demo_msgs.srv import PositionService, PositionServiceResponse + + +def new_position(req): + print("Request: \n{}".format(req.input)) + req.input.pos_x = random.uniform(-4.0, 4.0) + req.input.pos_z = random.uniform(-4.0, 4.0) + + return PositionServiceResponse(req.input) + + +def translate_position_server(): + rospy.init_node('position_server') + s = rospy.Service('pos_srv', PositionService, new_position) + print("Ready to move cubes!") + rospy.spin() + + +if __name__ == "__main__": + translate_position_server() diff --git a/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/CMakeLists.txt b/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/CMakeLists.txt new file mode 100644 index 000000000..7950be735 --- /dev/null +++ b/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 2.8.3) +project(unity_robotics_demo_msgs) + +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + geometry_msgs + message_generation +) + +add_message_files(FILES + PosRot.msg + UnityColor.msg + QuestPoseStamped.msg +) +add_service_files(FILES + ObjectPoseService.srv + PositionService.srv +) + +generate_messages( + DEPENDENCIES + geometry_msgs + std_msgs +) + +catkin_package(CATKIN_DEPENDS + message_runtime) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_robotics_demo.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/msg/PosRot.msg b/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/msg/PosRot.msg new file mode 100644 index 000000000..43abb1f55 --- /dev/null +++ b/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/msg/PosRot.msg @@ -0,0 +1,7 @@ +float32 pos_x +float32 pos_y +float32 pos_z +float32 rot_x +float32 rot_y +float32 rot_z +float32 rot_w \ No newline at end of file diff --git a/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/msg/QuestPoseStamped.msg b/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/msg/QuestPoseStamped.msg new file mode 100644 index 000000000..61358075f --- /dev/null +++ b/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/msg/QuestPoseStamped.msg @@ -0,0 +1,12 @@ +int32 frame_count +float64 timestamp +float32 pos_x +float32 pos_y +float32 pos_z +float32 rot_x +float32 rot_y +float32 rot_z +float32 rot_w +float32 eul_x +float32 eul_y +float32 eul_z \ No newline at end of file diff --git a/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/msg/UnityColor.msg b/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/msg/UnityColor.msg new file mode 100644 index 000000000..a77ff60b1 --- /dev/null +++ b/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/msg/UnityColor.msg @@ -0,0 +1,4 @@ +int32 r +int32 g +int32 b +int32 a diff --git a/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/package.xml b/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/package.xml new file mode 100644 index 000000000..39ab862fb --- /dev/null +++ b/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/package.xml @@ -0,0 +1,28 @@ + + + unity_robotics_demo_msgs + 0.0.1 + Messages for the unity_robotics_demo package (ROS1 version) + + Unity Robotics + + + Apache 2.0 + + catkin + message_generation + std_msgs + rospy + std_msgs + message_runtime + rospy + std_msgs + ros_tcp_endpoint + + + + + + + + diff --git a/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/srv/ObjectPoseService.srv b/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/srv/ObjectPoseService.srv new file mode 100644 index 000000000..83d3c5b9c --- /dev/null +++ b/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/srv/ObjectPoseService.srv @@ -0,0 +1,3 @@ +string object_name +--- +geometry_msgs/Pose object_pose diff --git a/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/srv/PositionService.srv b/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/srv/PositionService.srv new file mode 100644 index 000000000..e780c56e8 --- /dev/null +++ b/zebROS_ws/src/ros_packages/unity_robotics_demo_msgs/srv/PositionService.srv @@ -0,0 +1,3 @@ +PosRot input +--- +PosRot output \ No newline at end of file