diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 00000000..c53b622d --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,37 @@ +--- +name: Bug report +about: Create a report to help us improve +title: '' +labels: '' +assignees: '' + +--- + +**Describe the bug** +A clear and concise description of what the bug is. + +**To Reproduce** +Steps to reproduce the behavior: +1. Go to '...' +2. Click on '....' +3. Scroll down to '....' +4. See error + +**Console logs / stack traces** +Please wrap in [triple backticks (```)](https://help.github.com/en/articles/creating-and-highlighting-code-blocks) to make it easier to read. + +**Expected behavior** +A clear and concise description of what you expected to happen. + +**Screenshots** +If applicable, add screenshots or videos to help explain your problem. + +**Environment (please complete the following information, where applicable):** +- Unity Version: [e.g. Unity 2020.2.0f1] +- Unity machine OS + version: [e.g. Windows 10] +- ROS machine OS + version: [e.g. Ubuntu 18.04, ROS Noetic] +- ROS–Unity communication: [e.g. Docker] +- Branch or version: [e.g. v0.2.0] + +**Additional context** +Add any other context about the problem here. diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 00000000..bbcbbe7d --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,20 @@ +--- +name: Feature request +about: Suggest an idea for this project +title: '' +labels: '' +assignees: '' + +--- + +**Is your feature request related to a problem? Please describe.** +A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] + +**Describe the solution you'd like** +A clear and concise description of what you want to happen. + +**Describe alternatives you've considered** +A clear and concise description of any alternative solutions or features you've considered. + +**Additional context** +Add any other context or screenshots about the feature request here. diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md new file mode 100644 index 00000000..6c88fdec --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -0,0 +1,34 @@ +## Proposed change(s) + +Describe the changes made in this PR. + +### Useful links (GitHub issues, JIRA tickets, forum threads, etc.) + +Provide any relevant links here. + +### Types of change(s) + +- [ ] Bug fix +- [ ] New feature +- [ ] Code refactor +- [ ] Documentation update +- [ ] Other (please describe) + +## Testing and Verification + +Please describe the tests that you ran to verify your changes. Please also provide instructions, ROS packages, and Unity project files as appropriate so we can reproduce the test environment. + +### Test Configuration: +- Unity Version: [e.g. Unity 2020.2.0f1] +- Unity machine OS + version: [e.g. Windows 10] +- ROS machine OS + version: [e.g. Ubuntu 18.04, ROS Noetic] +- ROS–Unity communication: [e.g. Docker] + +## Checklist +- [ ] Ensured this PR is up-to-date with the `dev` branch +- [ ] Created this PR to target the `dev` branch +- [ ] Followed the style guidelines as described in the [Contribution Guidelines](../CONTRIBUTING.md) +- [ ] Added tests that prove my fix is effective or that my feature works +- [ ] Updated the documentation as appropriate + +## Other comments \ No newline at end of file diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md new file mode 100644 index 00000000..a7a4c577 --- /dev/null +++ b/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/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..de34ac98 --- /dev/null +++ b/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/Unity-Robotics-Hub/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/Unity-Robotics-Hub/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/LICENSE b/LICENSE index d9a10c0d..54541744 100644 --- a/LICENSE +++ b/LICENSE @@ -174,3 +174,17 @@ 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. \ No newline at end of file diff --git a/README.md b/README.md index ac1c7328..4b76a093 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,8 @@ # Unity Robotics Hub +[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) + This is a central repository for tools, tutorials, resources, and documentation for robotic simulation in Unity. > The contents of this repository are in active development. Its features and API are subject to significant change as development progresses. @@ -48,22 +50,33 @@ Use articulation bodies to easily prototype industrial designs with realistic mo - [Unity Industrial Simulation](https://unity.com/products/unity-simulation) - [Unity Computer Vision](https://unity.com/computer-vision) -## Coming Soon - New Physics Features in Unity! +## New Physics Features in Unity +### New Features +- **Contact Modification API** This API will allow users to define custom contact reactions, such as ignoring subsets of contact points, in order to help simulate holes, slippery surfaces, soft contacts, and more. It is available in Unity versions **2021.2a12+**. [Read more about the new Contact Modification API](https://forum.unity.com/threads/experimental-contacts-modification-api.924809/). +- **Collision detection modes exposed for ArticulationBody: discrete, sweep-based CCD, and speculative CCD**. New continuous collision detection (CCD) modes will ensure that fast-moving objects collide with objects, instead of tunneling or passing through those objects, which can happen in the default “discrete” mode. This API is available in Unity versions **2020.3.5f1+**. [Read more about continuous collision detection](https://docs.unity3d.com/2020.3/Documentation/ScriptReference/ArticulationBody-collisionDetectionMode.html). + +### Coming Soon Here’s a peek into what our Physics Team is hard at work on… -- **Contact Modification API**. This API will allow users to define custom contact reactions, such as ignoring subsets of contact points, in order to help simulate holes, slippery surfaces, soft contacts, and more. [Read more about the new Contact Modification API](https://forum.unity.com/threads/experimental-contacts-modification-api.924809/). - **Wheel Collider shapes**. This feature will allow the user to specify the shape of the collider to be used for collision detection. Currently the collider shape is fixed to a cylinder, and collision detection is performed by casting a ray from the center of the cylinder. Custom shapes will improve the accuracy of simulating wheels over rough terrains, holes, etc. [Read more about Wheel Collider](https://docs.unity3d.com/Manual/class-WheelCollider.html). -- **Collision detection modes exposed for ArticulationBody: discrete, sweep-based ccd, and speculative ccd**. New continuous collision detection (ccd) modes will ensure that fast-moving objects collide with objects, instead of tunneling or passing through those objects, which can happen in the default “discrete” mode. [Read more about continuous collision detection](https://docs.unity3d.com/Manual/ContinuousCollisionDetection.html). - **Force/Torque Sensor API**. This API will allow users to get the force and torque acting on an articulation body (useful for simulating a force/torque sensor!), as well as to get the motor torque applied by an articulation drive. - **Query primitives**. These simple, GameObject-less shapes allow for collision detection without requiring simulation (i.e., without calling Physics.Simulate). This feature will allow users to initialize objects in feasible locations, and can also be used for motion planning. ## FAQs [FAQs](faq.md) +## 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 general questions, feedback, or feature requests, connect directly with the Robotics team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). +For general questions, feedback, or feature requests, connect directly with the +Robotics team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). -For bugs or other issues, please file a GitHub issue and the Robotics team will investigate the issue as soon as possible. +For bugs or other issues, please file a GitHub issue and the Robotics team will +investigate the issue as soon as possible. ## License -[Apache License 2.0](LICENSE) \ No newline at end of file +[Apache License 2.0](LICENSE) diff --git a/tutorials/pick_and_place/2_ros_tcp.md b/tutorials/pick_and_place/2_ros_tcp.md index 56d8d5f4..360a012d 100644 --- a/tutorials/pick_and_place/2_ros_tcp.md +++ b/tutorials/pick_and_place/2_ros_tcp.md @@ -55,7 +55,7 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n ![](img/2_robottraj.png) - - One new C# script should populate the `Assets/RosMessages/Moveit/msg` directory: RobotTrajectory. + - One new C# script should populate the `Assets/RosMessages/Moveit/msg` directory: MRobotTrajectory. This name is the same as the message you built, with an "M" prefix (for message). 1. Next, the custom message scripts for this tutorial will need to be generated. @@ -63,7 +63,7 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n ![](img/2_msg.png) - - Two new C# scripts should populate the `Assets/RosMessages/NiryoMoveit/msg` directory: NiryoMoveitJoints and NiryoTrajectory. NiryoMoveitJoints describes a value for each joint in the Niryo arm as well as poses for the target object and target goal. NiryoTrajectory describes a list of RobotTrajectory values, which will hold the calculated trajectories for the pick-and-place task. + - Two new C# scripts should populate the `Assets/RosMessages/NiryoMoveit/msg` directory: MNiryoMoveitJoints and MNiryoTrajectory. MNiryoMoveitJoints describes a value for each joint in the Niryo arm as well as poses for the target object and target goal. MNiryoTrajectory describes a list of RobotTrajectory values, which will hold the calculated trajectories for the pick-and-place task. > MessageGeneration generates a C# class from a ROS msg file with protections for use of C# reserved keywords and conversion to C# datatypes. Learn more about [ROS Messages](https://wiki.ros.org/Messages). @@ -73,7 +73,7 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n ![](img/2_srv.png) - - Two new C# scripts should populate the `Assets/RosMessages/NiryoMoveit/srv` directory: MoverServiceRequest and MoverServiceResponse. These files describe the expected input and output formats for the service requests and responses when calculating trajectories. + - Two new C# scripts should populate the `Assets/RosMessages/NiryoMoveit/srv` directory: MMoverServiceRequest and MMoverServiceResponse. These files describe the expected input and output formats for the service requests and responses when calculating trajectories. > MessageGeneration generates two C# classes, a request and response, from a ROS srv file with protections for use of C# reserved keywords and conversion to C# datatypes. Learn more about [ROS Services](https://wiki.ros.org/Services). @@ -86,7 +86,7 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n ```csharp public void Publish() { - NiryoMoveitJoints sourceDestinationMessage = new NiryoMoveitJoints(); + MNiryoMoveitJoints sourceDestinationMessage = new MNiryoMoveitJoints(); sourceDestinationMessage.joint_00 = jointArticulationBodies[0].xDrive.target; sourceDestinationMessage.joint_01 = jointArticulationBodies[1].xDrive.target; @@ -96,7 +96,7 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n sourceDestinationMessage.joint_05 = jointArticulationBodies[5].xDrive.target; // Pick Pose - sourceDestinationMessage.pick_pose = new RosMessageTypes.Geometry.Pose + sourceDestinationMessage.pick_pose = new MPose { position = target.transform.position.To(), // The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping. @@ -104,7 +104,7 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n }; // Place Pose - sourceDestinationMessage.place_pose = new RosMessageTypes.Geometry.Pose + sourceDestinationMessage.place_pose = new MPose { position = targetPlacement.transform.position.To(), orientation = pickOrientation.To() diff --git a/tutorials/pick_and_place/3_pick_and_place.md b/tutorials/pick_and_place/3_pick_and_place.md index f6fd9498..5d525a8f 100644 --- a/tutorials/pick_and_place/3_pick_and_place.md +++ b/tutorials/pick_and_place/3_pick_and_place.md @@ -31,11 +31,11 @@ Steps covered in this tutorial includes invoking a motion planning service in RO ```csharp public void PublishJoints() { - MoverServiceRequest request = new MoverServiceRequest(); + MMoverServiceRequest request = new MMoverServiceRequest(); request.joints_input = CurrentJointConfig(); // Pick Pose - request.pick_pose = new RosMessageTypes.Geometry.Pose + request.pick_pose = new MPose { position = (target.transform.position + pickPoseOffset).To(), // The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping. @@ -43,16 +43,16 @@ Steps covered in this tutorial includes invoking a motion planning service in RO }; // Place Pose - request.place_pose = new RosMessageTypes.Geometry.Pose + request.place_pose = new MPose { position = (targetPlacement.transform.position + pickPoseOffset).To(), orientation = pickOrientation.To() }; - ros.SendServiceMessage(rosServiceName, request, TrajectoryResponse); + ros.SendServiceMessage(rosServiceName, request, TrajectoryResponse); } - void TrajectoryResponse(MoverServiceResponse response) + void TrajectoryResponse(MMoverServiceResponse response) { if (response.trajectories != null) { @@ -71,7 +71,7 @@ Steps covered in this tutorial includes invoking a motion planning service in RO > The `response.trajectories` are received in the `TrajectoryResponse()` callback, as defined in the `ros.SendServiceMessage` parameters. These trajectories are passed to `ExecuteTrajectories()` and executed as a [coroutine](https://docs.unity3d.com/Manual/Coroutines.html): ```csharp - private IEnumerator ExecuteTrajectories(MoverServiceResponse response) + private IEnumerator ExecuteTrajectories(MMoverServiceResponse response) { if (response.trajectories != null) { diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs index 8878ce80..0b4a591b 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs +++ b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs @@ -75,8 +75,10 @@ void Awake() { EditorApplication.LockReloadAssemblies(); SetupScene(); - SetupRos(); - CreateTrajectoryPlannerPubliser(); + AddRosMessages(); + ImportRobot(); + CreateRosConnection(); + CreateTrajectoryPlannerPublisher(); } void Update() @@ -104,31 +106,18 @@ void OnApplicationQuit() private void SetupScene() { // Instantiate the table, target, and target placement - GameObject table = InstantiatePrefab(tableName); - GameObject target = InstantiatePrefab(targetName); - GameObject targetPlacement = InstantiatePrefab(targetPlacementName); + InstantiatePrefab(tableName); + InstantiatePrefab(targetName); + InstantiatePrefab(targetPlacementName); // Adjust main camera GameObject camera = GameObject.Find(cameraName); camera.transform.position = cameraPosition; camera.transform.rotation = cameraRotation; - - // Import Niryo One with URDF importer - string urdfFilepath = Path.Combine(Application.dataPath, urdfRelativeFilepath); - ImportRobot(urdfFilepath, ImportSettings.convexDecomposer.unity); - - // Adjust robot parameters - Controller controller = GameObject.Find(niryoOneName).GetComponent(); - controller.stiffness = controllerStiffness; - controller.damping = controllerDamping; - controller.forceLimit = controllerForceLimit; - controller.speed = controllerSpeed; - controller.acceleration = controllerAcceleration; - GameObject.Find(baseLinkName).GetComponent().immovable = true; } // Tutorial Part 2 without the Publisher object - private void SetupRos() + private void AddRosMessages() { if (generateRosMessages) { @@ -152,19 +141,9 @@ private void SetupRos() scripts.AddRange(Directory.GetFiles(rosMessagesDirectory, scriptPattern, SearchOption.AllDirectories)); scripts.AddRange(Directory.GetFiles(externalScriptsDirectory)); RecompileScripts(scripts.ToArray()); - - // Create RosConnect - GameObject rosConnect = new GameObject(rosConnectName); - rosConnection = rosConnect.AddComponent(); - rosConnection.rosIPAddress = hostIP; - rosConnection.rosPort = hostPort; - rosConnection.overrideUnityIP = overrideUnityIP; - rosConnection.unityPort = unityPort; - rosConnection.awaitDataMaxRetries = awaitDataMaxRetries; - rosConnection.awaitDataSleepSeconds = awaitDataSleepSeconds; } - private void CreateTrajectoryPlannerPubliser() + private void CreateTrajectoryPlannerPublisher() { GameObject publisher = new GameObject(publisherName); dynamic planner = publisher.AddComponent(assembly.GetType(trajectoryPlannerType)); @@ -182,15 +161,39 @@ private GameObject InstantiatePrefab(string name) return gameObject; } - private void ImportRobot(string urdfFilepath, ImportSettings.convexDecomposer convexDecomposer) + private void CreateRosConnection() { - // Import Niryo One by URDF Importer - ImportSettings settings = new ImportSettings + // Create RosConnect + GameObject rosConnect = new GameObject(rosConnectName); + rosConnection = rosConnect.AddComponent(); + rosConnection.rosIPAddress = hostIP; + rosConnection.rosPort = hostPort; + rosConnection.overrideUnityIP = overrideUnityIP; + rosConnection.unityPort = unityPort; + rosConnection.awaitDataMaxRetries = awaitDataMaxRetries; + rosConnection.awaitDataSleepSeconds = awaitDataSleepSeconds; + } + + private void ImportRobot() + { + ImportSettings urdfImportSettings = new ImportSettings { choosenAxis = ImportSettings.axisType.yAxis, - convexMethod = convexDecomposer, + convexMethod = ImportSettings.convexDecomposer.unity }; - UrdfRobotExtensions.Create(urdfFilepath, settings); + // Import Niryo One with URDF importer + string urdfFilepath = Path.Combine(Application.dataPath, urdfRelativeFilepath); + // Create is a coroutine that would usually run only in EditMode, so we need to force its execution here + var robotImporter = UrdfRobotExtensions.Create(urdfFilepath, urdfImportSettings, false); + while (robotImporter.MoveNext()) {} + // Adjust robot parameters + Controller controller = GameObject.Find(niryoOneName).GetComponent(); + controller.stiffness = controllerStiffness; + controller.damping = controllerDamping; + controller.forceLimit = controllerForceLimit; + controller.speed = controllerSpeed; + controller.acceleration = controllerAcceleration; + GameObject.Find(baseLinkName).GetComponent().immovable = true; } // Credit to https://www.codeproject.com/Tips/715891/Compiling-Csharp-Code-at-Runtime diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json index c2a1a3db..c0a03b65 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json +++ b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json @@ -4,8 +4,9 @@ "com.unity.ide.rider": "2.0.7", "com.unity.ide.visualstudio": "2.0.3", "com.unity.ide.vscode": "1.2.2", - "com.unity.robotics.ros-tcp-connector": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#v0.2.0", - "com.unity.robotics.urdf-importer": "https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer#v0.2.0", + "com.unity.robotics.ros-tcp-connector": + "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#v0.3.0", + "com.unity.robotics.urdf-importer": "https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer#v0.3.0", "com.unity.test-framework": "1.1.18", "com.unity.textmeshpro": "3.0.1", "com.unity.timeline": "1.4.3", diff --git a/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint b/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint index c56f5586..a5e0fd1c 160000 --- a/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint +++ b/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint @@ -1 +1 @@ -Subproject commit c56f558687c2467f1e9782b58d12393c9bc47bc7 +Subproject commit a5e0fd1ce1a711f557fe67e73657c74f92482fbe diff --git a/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs b/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs index f0aa675c..c5e0a6b2 100644 --- a/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs +++ b/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs @@ -2,6 +2,7 @@ using UnityEngine; using Unity.Robotics.ROSTCPConnector; using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using RosMessageTypes.Geometry; public class SourceDestinationPublisher : MonoBehaviour { @@ -51,7 +52,7 @@ void Start() public void Publish() { - NiryoMoveitJoints sourceDestinationMessage = new NiryoMoveitJoints(); + MNiryoMoveitJoints sourceDestinationMessage = new MNiryoMoveitJoints(); sourceDestinationMessage.joint_00 = jointArticulationBodies[0].xDrive.target; sourceDestinationMessage.joint_01 = jointArticulationBodies[1].xDrive.target; @@ -61,14 +62,14 @@ public void Publish() sourceDestinationMessage.joint_05 = jointArticulationBodies[5].xDrive.target; // Pick Pose - sourceDestinationMessage.pick_pose = new RosMessageTypes.Geometry.Pose + sourceDestinationMessage.pick_pose = new MPose { position = target.transform.position.To(), orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To() }; // Place Pose - sourceDestinationMessage.place_pose = new RosMessageTypes.Geometry.Pose + sourceDestinationMessage.place_pose = new MPose { position = targetPlacement.transform.position.To(), orientation = pickOrientation.To() diff --git a/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs b/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs index f14015a1..ead3e41b 100644 --- a/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs +++ b/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs @@ -4,7 +4,7 @@ using UnityEngine; using Unity.Robotics.ROSTCPConnector; using Unity.Robotics.ROSTCPConnector.ROSGeometry; - +using RosMessageTypes.Geometry; public class TrajectoryPlanner : MonoBehaviour { @@ -78,9 +78,9 @@ private void OpenGripper() /// Get the current values of the robot's joint angles. /// /// NiryoMoveitJoints - NiryoMoveitJoints CurrentJointConfig() + MNiryoMoveitJoints CurrentJointConfig() { - NiryoMoveitJoints joints = new NiryoMoveitJoints(); + MNiryoMoveitJoints joints = new MNiryoMoveitJoints(); joints.joint_00 = jointArticulationBodies[0].xDrive.target; joints.joint_01 = jointArticulationBodies[1].xDrive.target; @@ -101,11 +101,11 @@ NiryoMoveitJoints CurrentJointConfig() /// public void PublishJoints() { - MoverServiceRequest request = new MoverServiceRequest(); + MMoverServiceRequest request = new MMoverServiceRequest(); request.joints_input = CurrentJointConfig(); // Pick Pose - request.pick_pose = new RosMessageTypes.Geometry.Pose + request.pick_pose = new MPose { position = (target.transform.position + pickPoseOffset).To(), // The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping. @@ -113,16 +113,16 @@ public void PublishJoints() }; // Place Pose - request.place_pose = new RosMessageTypes.Geometry.Pose + request.place_pose = new MPose { position = (targetPlacement.transform.position + pickPoseOffset).To(), orientation = pickOrientation.To() }; - ros.SendServiceMessage(rosServiceName, request, TrajectoryResponse); + ros.SendServiceMessage(rosServiceName, request, TrajectoryResponse); } - void TrajectoryResponse(MoverServiceResponse response) + void TrajectoryResponse(MMoverServiceResponse response) { if (response.trajectories.Length > 0) { @@ -149,7 +149,7 @@ void TrajectoryResponse(MoverServiceResponse response) /// /// MoverServiceResponse received from niryo_moveit mover service running in ROS /// - private IEnumerator ExecuteTrajectories(MoverServiceResponse response) + private IEnumerator ExecuteTrajectories(MMoverServiceResponse response) { if (response.trajectories != null) { diff --git a/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs b/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs index c322b356..8a118002 100644 --- a/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs +++ b/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs @@ -1,12 +1,12 @@ using System.Collections; using System.Linq; -using ROSGeometry; +using RosMessageTypes.Geometry; using RosMessageTypes.Moveit; using RosMessageTypes.NiryoMoveit; using RosMessageTypes.NiryoOne; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; using UnityEngine; -using Pose = RosMessageTypes.Geometry.Pose; -using RosQuaternion = RosMessageTypes.Geometry.Quaternion; public class RealSimPickAndPlace : MonoBehaviour { @@ -81,7 +81,7 @@ private void OpenGripper() /// public void PublishJoints() { - MoverServiceRequest request = new MoverServiceRequest + MMoverServiceRequest request = new MMoverServiceRequest { joints_input = { @@ -92,13 +92,13 @@ public void PublishJoints() joint_04 = jointArticulationBodies[4].xDrive.target, joint_05 = jointArticulationBodies[5].xDrive.target }, - pick_pose = new Pose + pick_pose = new MPose { position = (target.transform.position + PICK_POSE_OFFSET).To(), // The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping. orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To() }, - place_pose = new Pose + place_pose = new MPose { position = (targetPlacement.transform.position + PICK_POSE_OFFSET).To(), orientation = pickOrientation.To() @@ -146,7 +146,7 @@ void Awake() void Start() { - ros.Subscribe(rosRobotCommandsTopicName, ExecuteRobotCommands); + ros.Subscribe(rosRobotCommandsTopicName, ExecuteRobotCommands); } /// @@ -155,7 +155,7 @@ void Start() /// executed in a coroutine. /// /// RobotMoveActionGoal of trajectory or gripper commands - void ExecuteRobotCommands(RobotMoveActionGoal robotAction) + void ExecuteRobotCommands(MRobotMoveActionGoal robotAction) { if (robotAction.goal.cmd.cmd_type == TRAJECTORY_COMMAND_EXECUTION) { @@ -184,7 +184,7 @@ void ExecuteRobotCommands(RobotMoveActionGoal robotAction) /// /// /// The array of poses for the robot to execute - private IEnumerator ExecuteTrajectories(RobotTrajectory trajectories) + private IEnumerator ExecuteTrajectories(MRobotTrajectory trajectories) { // For every robot pose in trajectory plan foreach (var point in trajectories.joint_trajectory.points) diff --git a/tutorials/quick_setup.md b/tutorials/quick_setup.md index e7678516..c240031c 100644 --- a/tutorials/quick_setup.md +++ b/tutorials/quick_setup.md @@ -12,9 +12,9 @@ This page provides brief instructions on installing the Unity Robotics packages. ![](../images/packman.png) 1. Enter the git URL for the desired package. - 1. For the [ROS-TCP-Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector), enter `https://github.com/Unity-Technologies/ROS-TCP-Connector.git`. - 1. For the [URDF-Importer](https://github.com/Unity-Technologies/URDF-Importer), enter `https://github.com/Unity-Technologies/URDF-Importer.git`. + 1. For the [ROS-TCP-Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector), enter `https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#v0.2.0`. + 1. For the [URDF-Importer](https://github.com/Unity-Technologies/URDF-Importer), enter `https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer#v0.2.0`. 1. Click `Add`. -To install from a local clone of the repository, see [installing a local package](https://docs.unity3d.com/Manual/upm-ui-local.html) in the Unity manual. \ No newline at end of file +To install from a local clone of the repository, see [installing a local package](https://docs.unity3d.com/Manual/upm-ui-local.html) in the Unity manual. diff --git a/tutorials/ros_packages/robotics_demo/scripts/object_pose_client.py b/tutorials/ros_packages/robotics_demo/scripts/object_pose_client.py new file mode 100755 index 00000000..3ebe8b07 --- /dev/null +++ b/tutorials/ros_packages/robotics_demo/scripts/object_pose_client.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python + +from __future__ import print_function + +import random +import rospy +import sys + +from robotics_demo.srv import ObjectPoseService, ObjectPoseServiceResponse + + +def get_object_pose_client(name): + rospy.wait_for_service('obj_pose_srv') + try: + get_obj_pose = rospy.ServiceProxy('obj_pose_srv', ObjectPoseService) + obj_pose_resp = get_obj_pose(name) + return obj_pose_resp.object_pose + except rospy.ServiceException as e: + print("Service call failed: %s"%e) + + +def usage(): + return "%s [object_name]"%sys.argv[0] + +if __name__ == "__main__": + if len(sys.argv) == 2: + name = str(sys.argv[1]) + else: + print(usage()) + sys.exit(1) + print("Requesting pose for %s"%(name)) + print("Pose for %s: %s"%(name, get_object_pose_client(name))) diff --git a/tutorials/ros_packages/robotics_demo/scripts/position_service.py b/tutorials/ros_packages/robotics_demo/scripts/position_service.py index d9ce9a9f..1a0e11a5 100755 --- a/tutorials/ros_packages/robotics_demo/scripts/position_service.py +++ b/tutorials/ros_packages/robotics_demo/scripts/position_service.py @@ -18,7 +18,7 @@ def new_position(req): def translate_position_server(): rospy.init_node('position_server') - s = rospy.Service('position_service', PositionService, new_position) + s = rospy.Service('pos_srv', PositionService, new_position) print("Ready to move cubes!") rospy.spin() diff --git a/tutorials/ros_packages/robotics_demo/scripts/server_endpoint.py b/tutorials/ros_packages/robotics_demo/scripts/server_endpoint.py index 1fcb647e..ababf68c 100644 --- a/tutorials/ros_packages/robotics_demo/scripts/server_endpoint.py +++ b/tutorials/ros_packages/robotics_demo/scripts/server_endpoint.py @@ -2,9 +2,9 @@ import rospy -from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService +from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService, UnityService from robotics_demo.msg import PosRot, UnityColor -from robotics_demo.srv import PositionService +from robotics_demo.srv import PositionService, ObjectPoseService def main(): ros_node_name = rospy.get_param("/TCP_NODE_NAME", 'TCPServer') @@ -14,9 +14,10 @@ def main(): rospy.init_node(ros_node_name, anonymous=True) tcp_server.start({ - 'pos_srv': RosService('position_service', PositionService), 'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10), - 'color': RosSubscriber('color', UnityColor, tcp_server) + 'color': RosSubscriber('color', UnityColor, tcp_server), + 'pos_srv': RosService('pos_srv', PositionService), + 'obj_pose_srv': UnityService('obj_pose_srv', ObjectPoseService, tcp_server), }) rospy.spin() diff --git a/tutorials/ros_packages/robotics_demo/srv/ObjectPoseService.srv b/tutorials/ros_packages/robotics_demo/srv/ObjectPoseService.srv new file mode 100644 index 00000000..83d3c5b9 --- /dev/null +++ b/tutorials/ros_packages/robotics_demo/srv/ObjectPoseService.srv @@ -0,0 +1,3 @@ +string object_name +--- +geometry_msgs/Pose object_pose diff --git a/tutorials/ros_unity_integration/README.md b/tutorials/ros_unity_integration/README.md index b29b7279..2f985cef 100644 --- a/tutorials/ros_unity_integration/README.md +++ b/tutorials/ros_unity_integration/README.md @@ -1,5 +1,8 @@ # ROS–Unity Integration +## Editor Version +Most of our projects are developed/tested on Unity Editor version 2020.2.0f9 or later. We expect that anything 2020.2+ should be compatible, and will be actively working to ensure this for future versions. 2020.1.X and below are unlikely to work well, if at all, and we strongly encourage updating to a more recent version to support the bleeding edge updates to Unity's physics and simulation components that we are leveraging. + ## ROS–Unity Communication ![](images/unity_ros.png) @@ -14,6 +17,7 @@ The `ROSConnection` plugin (also from [ROS TCP Connector](https://github.com/Uni - [ROS–Unity Integration: Publisher](publisher.md) - Adding a Publisher to a Unity Scene - [ROS–Unity Integration: Subscriber](subscriber.md) - Adding a Subscriber to a Unity Scene - [ROS–Unity Integration: Service](service.md) - Adding a Service call to a Unity Scene +- [ROS–Unity Integration: UnityService](unity_service.md) - Adding a Service that runs in a Unity Scene - [ROS–Unity Integration: Server Endpoint](server_endpoint.md) - How to write a Server Endpoint ## Example Unity Scripts @@ -23,8 +27,12 @@ Example scripts implemented in tutorials: - `unity_scripts/RosPublisherExample.cs` - Publishes the position of a GameObject every 0.5 seconds. +- `unity_scripts/RosSubscriberExample.cs` + - Subscribes to a topic that accepts color messages and uses them to change the color of a GameObject in the Unity scene. + - `unity_scripts/RosServiceExample.cs` - Returns a destination position for a GameObject to move towards each time the service is called. -- `unity_scripts/RosSubscriberExample.cs` - - Subscribes to a topic that accepts color messages and uses them to change the color of a GameObject in the Unity scene. \ No newline at end of file +- `unity_scripts/RosUnityServiceExample.cs` + - Runs a service in the Unity scene that takes a GameObject's name and responds with the Pose of that object. + diff --git a/tutorials/ros_unity_integration/images/generate_messages_1.png b/tutorials/ros_unity_integration/images/generate_messages_1.png index 5b58923b..96b0c13a 100644 Binary files a/tutorials/ros_unity_integration/images/generate_messages_1.png and b/tutorials/ros_unity_integration/images/generate_messages_1.png differ diff --git a/tutorials/ros_unity_integration/images/generate_messages_2.png b/tutorials/ros_unity_integration/images/generate_messages_2.png index 07aa88ca..d6de9ae1 100644 Binary files a/tutorials/ros_unity_integration/images/generate_messages_2.png and b/tutorials/ros_unity_integration/images/generate_messages_2.png differ diff --git a/tutorials/ros_unity_integration/publisher.md b/tutorials/ros_unity_integration/publisher.md index b50b3e47..f736af41 100644 --- a/tutorials/ros_unity_integration/publisher.md +++ b/tutorials/ros_unity_integration/publisher.md @@ -71,7 +71,7 @@ public class RosPublisherExample : MonoBehaviour { cube.transform.rotation = Random.rotation; - PosRot cubePos = new PosRot( + MPosRot cubePos = new MPosRot( cube.transform.position.x, cube.transform.position.y, cube.transform.position.z, diff --git a/tutorials/ros_unity_integration/server_endpoint.md b/tutorials/ros_unity_integration/server_endpoint.md index 25fa8345..aa65fdc6 100644 --- a/tutorials/ros_unity_integration/server_endpoint.md +++ b/tutorials/ros_unity_integration/server_endpoint.md @@ -14,9 +14,9 @@ The following is an example of a server endpoint Python script that: import rospy -from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService +from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService, UnityService from robotics_demo.msg import PosRot, UnityColor -from robotics_demo.srv import PositionService +from robotics_demo.srv import PositionService, ObjectPoseService def main(): ros_node_name = rospy.get_param("/TCP_NODE_NAME", 'TCPServer') @@ -26,9 +26,10 @@ def main(): rospy.init_node(ros_node_name, anonymous=True) tcp_server.start({ - 'pos_srv': RosService('position_service', PositionService), 'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10), - 'color': RosSubscriber('color', UnityColor, tcp_server) + 'color': RosSubscriber('color', UnityColor, tcp_server), + 'pos_srv': RosService('pos_srv', PositionService), + 'obj_pose_srv': UnityService('obj_pose_srv', ObjectPoseService, tcp_server), }) rospy.spin() @@ -41,9 +42,9 @@ if __name__ == "__main__": ## Import Statements for Services and Messages ```python -from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService +from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService, UnityService from robotics_demo.msg import PosRot, UnityColor -from robotics_demo.srv import PositionService +from robotics_demo.srv import PositionService, ObjectPoseService ``` ## Creating the Server @@ -68,9 +69,10 @@ The `ros_node_name` argument is required and the `buffer_size` and `connections` ```python tcp_server.start({ - 'pos_srv': RosService('position_service', PositionService), 'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10), - 'color': RosSubscriber('color', UnityColor, tcp_server) + 'color': RosSubscriber('color', UnityColor, tcp_server), + 'pos_srv': RosService('pos_srv', PositionService), + 'obj_pose_srv': UnityService('obj_pose_srv', ObjectPoseService, tcp_server), }) rospy.spin() @@ -104,7 +106,7 @@ A ROS Service is similar to a RosPublisher, in that a Unity component sends a Re - Service name - ROS Service class generated from running `catkin_make` command -`RosService('position_service', PositionService)` +`RosService('pos_srv', PositionService)` ## Unity Service @@ -114,7 +116,7 @@ A Unity Service is similar to a RosSubscriber, in that a Unity component receive - ROS Service class generated from running `catkin_make` command - The tcp server that will connect to Unity -`UnityService('unity_service', PositionService, tcp_server)` +`UnityService('obj_pose_srv', ObjectPoseService, tcp_server)` ## Parameters diff --git a/tutorials/ros_unity_integration/service.md b/tutorials/ros_unity_integration/service.md index 865c8864..e98d36d7 100644 --- a/tutorials/ros_unity_integration/service.md +++ b/tutorials/ros_unity_integration/service.md @@ -29,7 +29,7 @@ Once the server_endpoint has started, it will print something similar to `[INFO] ## Setting Up Unity Scene - Generate the C# code for `PositionService`'s messages by going to `Robotics` -> `Generate ROS Messages...` -- Set the input file path to `PATH/TO/Unity-Robotics-Hub/tutorials/ros_packages/robotics_demo`, expand the robotics_demo folder and click `Build 1 srv`. +- Set the input file path to `PATH/TO/Unity-Robotics-Hub/tutorials/ros_packages/robotics_demo`, expand the robotics_demo folder and click `Build 2 srvs`. ![](images/generate_messages_2.png) @@ -74,7 +74,7 @@ public class RosServiceExample : MonoBehaviour { Debug.Log("Destination reached."); - PosRot cubePos = new PosRot( + MPosRot cubePos = new MPosRot( cube.transform.position.x, cube.transform.position.y, cube.transform.position.z, @@ -84,15 +84,15 @@ public class RosServiceExample : MonoBehaviour cube.transform.rotation.w ); - PositionServiceRequest positionServiceRequest = new PositionServiceRequest(cubePos); + MPositionServiceRequest positionServiceRequest = new MPositionServiceRequest(cubePos); // Send message to ROS and return the response - ros.SendServiceMessage(serviceName, positionServiceRequest, Callback_Destination); + ros.SendServiceMessage(serviceName, positionServiceRequest, Callback_Destination); awaitingResponseUntilTimestamp = Time.time+1.0f; // don't send again for 1 second, or until we receive a response } } - void Callback_Destination(PositionServiceResponse response) + void Callback_Destination(MPositionServiceResponse response) { awaitingResponseUntilTimestamp = -1; destination = new Vector3(response.output.pos_x, response.output.pos_y, response.output.pos_z); diff --git a/tutorials/ros_unity_integration/subscriber.md b/tutorials/ros_unity_integration/subscriber.md index 70f5e250..ac5164e9 100644 --- a/tutorials/ros_unity_integration/subscriber.md +++ b/tutorials/ros_unity_integration/subscriber.md @@ -34,7 +34,7 @@ Once the server_endpoint has started, it will print something similar to `[INFO] ```csharp using UnityEngine; using Unity.Robotics.ROSTCPConnector; -using RosColor = RosMessageTypes.RoboticsDemo.UnityColor; +using RosColor = RosMessageTypes.RoboticsDemo.MUnityColor; public class RosSubscriberExample : MonoBehaviour { diff --git a/tutorials/ros_unity_integration/unity_scripts/RosPublisherExample.cs b/tutorials/ros_unity_integration/unity_scripts/RosPublisherExample.cs index fdbdd393..f91bf2a0 100644 --- a/tutorials/ros_unity_integration/unity_scripts/RosPublisherExample.cs +++ b/tutorials/ros_unity_integration/unity_scripts/RosPublisherExample.cs @@ -32,7 +32,7 @@ private void Update() { cube.transform.rotation = Random.rotation; - PosRot cubePos = new PosRot( + MPosRot cubePos = new MPosRot( cube.transform.position.x, cube.transform.position.y, cube.transform.position.z, diff --git a/tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs b/tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs index 3091e231..3a715a0b 100644 --- a/tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs +++ b/tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs @@ -33,7 +33,7 @@ private void Update() { Debug.Log("Destination reached."); - PosRot cubePos = new PosRot( + MPosRot cubePos = new MPosRot( cube.transform.position.x, cube.transform.position.y, cube.transform.position.z, @@ -43,15 +43,15 @@ private void Update() cube.transform.rotation.w ); - PositionServiceRequest positionServiceRequest = new PositionServiceRequest(cubePos); + MPositionServiceRequest positionServiceRequest = new MPositionServiceRequest(cubePos); // Send message to ROS and return the response - ros.SendServiceMessage(serviceName, positionServiceRequest, Callback_Destination); + ros.SendServiceMessage(serviceName, positionServiceRequest, Callback_Destination); awaitingResponseUntilTimestamp = Time.time+1.0f; // don't send again for 1 second, or until we receive a response } } - void Callback_Destination(PositionServiceResponse response) + void Callback_Destination(MPositionServiceResponse response) { awaitingResponseUntilTimestamp = -1; destination = new Vector3(response.output.pos_x, response.output.pos_y, response.output.pos_z); diff --git a/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs b/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs index 7040b625..6b3a69a6 100644 --- a/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs +++ b/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs @@ -1,6 +1,6 @@ using UnityEngine; using Unity.Robotics.ROSTCPConnector; -using RosColor = RosMessageTypes.RoboticsDemo.UnityColor; +using RosColor = RosMessageTypes.RoboticsDemo.MUnityColor; public class RosSubscriberExample : MonoBehaviour { diff --git a/tutorials/ros_unity_integration/unity_scripts/RosUnityServiceExample.cs b/tutorials/ros_unity_integration/unity_scripts/RosUnityServiceExample.cs new file mode 100644 index 00000000..c9d3f50d --- /dev/null +++ b/tutorials/ros_unity_integration/unity_scripts/RosUnityServiceExample.cs @@ -0,0 +1,43 @@ +using RosMessageTypes.RoboticsDemo; +using UnityEngine; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; + +/// +/// Example demonstration of implementing a UnityService that receives a Request message from another ROS node and sends a Response back +/// +public class RosUnityServiceExample : MonoBehaviour +{ + [SerializeField] + string m_ServiceName = "obj_pose_srv"; + + void Start() + { + // register the service with ROS + ROSConnection.instance.ImplementService(m_ServiceName, GetObjectPose); + } + + /// + /// Callback to respond to the request + /// + /// service request containing the object name + /// service response containing the object pose (or 0 if object not found) + private MObjectPoseServiceResponse GetObjectPose(MObjectPoseServiceRequest request) + { + // process the service request + Debug.Log("Received request for object: " + request.object_name); + + // prepare a response + MObjectPoseServiceResponse objectPoseResponse = new MObjectPoseServiceResponse(); + // Find a game object with the requested name + GameObject gameObject = GameObject.Find(request.object_name); + if (gameObject) + { + // Fill-in the response with the object pose converted from Unity coordinate to ROS coordinate system + objectPoseResponse.object_pose.position = gameObject.transform.position.To(); + objectPoseResponse.object_pose.orientation = gameObject.transform.rotation.To(); + } + + return objectPoseResponse; + } +} diff --git a/tutorials/ros_unity_integration/unity_service.md b/tutorials/ros_unity_integration/unity_service.md new file mode 100644 index 00000000..366656e5 --- /dev/null +++ b/tutorials/ros_unity_integration/unity_service.md @@ -0,0 +1,126 @@ +# ROS–Unity Integration: UnityService + +Create a simple Unity scene which create a [Service](http://wiki.ros.org/Services) in Unity that takes a request with a GameObject's name and responds with the GameObject's pose (position and orientation) in the ROS coordinate system. + +## Setting Up ROS + +(Skip to [Setting Up the Unity Scene](unity_service.md#setting-up-the-unity-scene) if you already did the [ROS–Unity Integration Publisher](publisher.md) or [Subscriber](subscriber.md) tutorials.) + +- Copy the `tutorials/ros_packages/robotics_demo` folder of this repo into the `src` folder in your Catkin workspace. + +- Follow the [ROS–Unity Initial Setup](setup.md) guide. + +- Open a new terminal window, navigate to your ROS workspace, and run the following commands: + + ```bash + source devel/setup.bash + rosrun robotics_demo server_endpoint.py + ``` + +Once the server_endpoint has started, it will print something similar to `[INFO] [1603488341.950794]: Starting server on 192.168.50.149:10000`. + +## Setting Up the Unity Scene +- Generate the C# code for `ObjectPoseService`'s messages by going to `Robotics` -> `Generate ROS Messages...` + - Set the input file path to `PATH/TO/Unity-Robotics-Hub/tutorials/ros_packages/robotics_demo`, expand the robotics_demo folder and click `Build 2 srvs` (Note that you may skip this step if you have already done it in the previous tutorial). + + ![](images/generate_messages_2.png) + + - The generated files will be saved in the default directory `Assets/RosMessages/RoboticsDemo/srv`. + +- Create a new C# script and name it `RosUnityServiceExample.cs` +- Paste the following code into `RosUnityServiceExample.cs` + - **Note:** This script can be found at `tutorials/ros_unity_integration/unity_scripts`. + +```csharp +using RosMessageTypes.RoboticsDemo; +using UnityEngine; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; + +/// +/// Example demonstration of implementing a UnityService that receives a Request message from another ROS node and sends a Response back +/// +public class RosUnityServiceExample : MonoBehaviour +{ + [SerializeField] + string m_ServiceName = "obj_pose_srv"; + + void Start() + { + // register the service with ROS + ROSConnection.instance.ImplementService(m_ServiceName, GetObjectPose); + } + + /// + /// Callback to respond to the request + /// + /// service request containing the object name + /// service response containing the object pose (or 0 if object not found) + private MObjectPoseServiceResponse GetObjectPose(MObjectPoseServiceRequest request) + { + // process the service request + Debug.Log("Received request for object: " + request.object_name); + + // prepare a response + MObjectPoseServiceResponse objectPoseResponse = new MObjectPoseServiceResponse(); + // Find a game object with the requested name + GameObject gameObject = GameObject.Find(request.object_name); + if (gameObject) + { + // Fill-in the response with the object pose converted from Unity coordinate to ROS coordinate system + objectPoseResponse.object_pose.position = gameObject.transform.position.To(); + objectPoseResponse.object_pose.orientation = gameObject.transform.rotation.To(); + } + + return objectPoseResponse; + } +} +``` + +- From the main menu bar, open `Robotics/ROS Settings`, and change the `ROS IP Address` variable to the ROS IP. +- Create an empty GameObject and name it `UnityService`. +- Attach the `RosUnityServiceExample` script to the `UnityService` GameObject. +- Pressing play in the Editor should start running as a ROS node, waiting to accept ObjectPose requests. Once a connection to ROS has been established, a message will be printed on the ROS terminal similar to `ROS-Unity Handshake received, will connect to 192.168.50.130:5005`. + + +## Start the Client +- On your ROS system, open a new terminal window, navigate to your ROS workspace, and run the following commands: + + ```bash + source devel/setup.bash + rosrun robotics_demo object_pose_client.py Cube + ``` +- This wil print an output similar to the following with the current pose information of the game object (note that the coordinates are converted to the ROS coordinate system in our Unity Service): + + ```bash + Requesting pose for Cube + Pose for Cube: + position: + x: 0.0 + y: -1.0 + z: 0.20000000298023224 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: -1.0 + ``` +You may replace `Cube` with the name of any other GameObject currently present in the Unity hierarchy. + +- Alternatively you may also call the ROS service using `rosservice call`: + + ```bash + rosservice call /obj_pose_srv Cube + ``` + ```bash + object_pose: + position: + x: 0.0 + y: -1.0 + z: 0.20000000298023224 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: -1.0 + ``` \ No newline at end of file diff --git a/tutorials/urdf_importer/urdf_tutorial.md b/tutorials/urdf_importer/urdf_tutorial.md index 2e7550e6..966610f5 100644 --- a/tutorials/urdf_importer/urdf_tutorial.md +++ b/tutorials/urdf_importer/urdf_tutorial.md @@ -19,8 +19,6 @@ - Click `Import` ## Using the Controller -- By default a robot imported using the URDF Importer is loaded without any constraints or controller. - - A controller is pre-built in Unity URDF Importer to help showcase the movement of robots in Unity. - To add the controller to an imported robot click the `Enable` button in the Inspector window in front of the `Controller Script` option. This will add a Controller Script, FKrobot and Joint Control at runtime. - To prevent the joints from slipping set the `Stiffness` and `Damping` to `100,000` and `10,000` respectively. - To be able to apply forces to the joints set the `Force Limit` to `10,000`.