From 57da882b60e6793bfa87bcb0a01d7e1f846dd77b Mon Sep 17 00:00:00 2001 From: Kayman Date: Thu, 1 Jun 2017 14:09:16 +0900 Subject: [PATCH 01/59] Repository split from ROBOTIS-OP3 --- .gitignore | 11 +++++++++++ LICENSE | 26 ++++++++++++++++++++++++++ 2 files changed, 37 insertions(+) create mode 100644 .gitignore create mode 100644 LICENSE diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..36a5c0a --- /dev/null +++ b/.gitignore @@ -0,0 +1,11 @@ +build +devel +bin +lib +msg_gen +srv_gen +qtcreator-build +*~ +*.backup +*.user +*.autosave diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..5298325 --- /dev/null +++ b/LICENSE @@ -0,0 +1,26 @@ +Software License Agreement (BSD License) + +Copyright (c) 2014, ROBOTIS Inc. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of ROBOTIS nor the names of its contributors may be + used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY ROBOTIS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL ROBOTIS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; +OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. From c5dfc8f5b91b8dd32b80e5f50b9d2ce01d80f75a Mon Sep 17 00:00:00 2001 From: Kayman Date: Thu, 26 Oct 2017 19:51:28 +0900 Subject: [PATCH 02/59] Changed license from BSD to Apache2.0 --- LICENSE | 227 +++++++++++++++++++++++++++++++++++++++++++++++++------- 1 file changed, 201 insertions(+), 26 deletions(-) diff --git a/LICENSE b/LICENSE index 5298325..c0ee812 100644 --- a/LICENSE +++ b/LICENSE @@ -1,26 +1,201 @@ -Software License Agreement (BSD License) - -Copyright (c) 2014, ROBOTIS Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of ROBOTIS nor the names of its contributors may be - used to endorse or promote products derived from this software - without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY ROBOTIS "AS IS" AND ANY EXPRESS OR IMPLIED -WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF -MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. -IN NO EVENT SHALL ROBOTIS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, -PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; -OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR -OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF -ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + 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 + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "{}" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright {yyyy} {name of copyright owner} + + 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 87186d3d687211fea53699ecbdff0a9c442c21eb Mon Sep 17 00:00:00 2001 From: Pyo Date: Fri, 30 Mar 2018 22:18:17 +0900 Subject: [PATCH 03/59] refacoring to release --- README.md | 34 ++++++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 0000000..2387548 --- /dev/null +++ b/README.md @@ -0,0 +1,34 @@ +## ROBOTIS OP3 + + +## ROS Packages for ROBOTIS OP3 Demo +|Version|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic| +|:---:|:---:|:---:| +|[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-OP3-Demo.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-OP3-Demo)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-OP3-Demo.svg?branch=kinetic-devel)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-OP3-Demo)|-| + +## ROBOTIS e-Manual for ROBOTIS OP3 +- [ROBOTIS e-Manual for ROBOTIS OP3](http://emanual.robotis.com/docs/en/platform/op3/introduction/) + +## Wiki for robotis_op3_demo Packages +- http://wiki.ros.org/robotis_op3_demo (metapackage) +- http://wiki.ros.org/ball_detector +- http://wiki.ros.org/op3_demo + +## Open Source related to ROBOTIS OP3 +- [robotis_op3](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3) +- [robotis_op3_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-msgs) +- [robotis_op3_common](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Common) +- [robotis_op3_tools](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Tools) +- [robotis_op3_demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) +- [robotis_framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) +- [robotis_controller_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs) +- [robotis_utility](https://github.com/ROBOTIS-GIT/ROBOTIS-Utility) +- [robotis_math](https://github.com/ROBOTIS-GIT/ROBOTIS-Math) +- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) +- [OpenCR-Hardware](https://github.com/ROBOTIS-GIT/OpenCR-Hardware) +- [OpenCR](https://github.com/ROBOTIS-GIT/OpenCR) + +## Documents and Videos related to ROBOTIS OP3 +- [ROBOTIS e-Manual for ROBOTIS OP3](http://emanual.robotis.com/docs/en/platform/op3/introduction/) +- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) +- [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) From b039bfb29883c884282de72ef283cdfd00aafa0f Mon Sep 17 00:00:00 2001 From: Pyo Date: Mon, 2 Apr 2018 08:25:00 +0900 Subject: [PATCH 04/59] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 2387548..51875b9 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -## ROBOTIS OP3 +# ROBOTIS OP3 ## ROS Packages for ROBOTIS OP3 Demo From a55826444bb9adfa14692548b4dd3bbe6aa7cd96 Mon Sep 17 00:00:00 2001 From: Kayman Date: Mon, 16 Apr 2018 17:47:15 +0900 Subject: [PATCH 05/59] merged with master added op3_bringup package --- README.md | 34 ++++++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 0000000..51875b9 --- /dev/null +++ b/README.md @@ -0,0 +1,34 @@ +# ROBOTIS OP3 + + +## ROS Packages for ROBOTIS OP3 Demo +|Version|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic| +|:---:|:---:|:---:| +|[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-OP3-Demo.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-OP3-Demo)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-OP3-Demo.svg?branch=kinetic-devel)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-OP3-Demo)|-| + +## ROBOTIS e-Manual for ROBOTIS OP3 +- [ROBOTIS e-Manual for ROBOTIS OP3](http://emanual.robotis.com/docs/en/platform/op3/introduction/) + +## Wiki for robotis_op3_demo Packages +- http://wiki.ros.org/robotis_op3_demo (metapackage) +- http://wiki.ros.org/ball_detector +- http://wiki.ros.org/op3_demo + +## Open Source related to ROBOTIS OP3 +- [robotis_op3](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3) +- [robotis_op3_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-msgs) +- [robotis_op3_common](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Common) +- [robotis_op3_tools](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Tools) +- [robotis_op3_demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) +- [robotis_framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) +- [robotis_controller_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs) +- [robotis_utility](https://github.com/ROBOTIS-GIT/ROBOTIS-Utility) +- [robotis_math](https://github.com/ROBOTIS-GIT/ROBOTIS-Math) +- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) +- [OpenCR-Hardware](https://github.com/ROBOTIS-GIT/OpenCR-Hardware) +- [OpenCR](https://github.com/ROBOTIS-GIT/OpenCR) + +## Documents and Videos related to ROBOTIS OP3 +- [ROBOTIS e-Manual for ROBOTIS OP3](http://emanual.robotis.com/docs/en/platform/op3/introduction/) +- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) +- [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) From 8b7ff2e9068ffeb7ee42ef4dfef411d6d239cc7d Mon Sep 17 00:00:00 2001 From: Pyo Date: Thu, 19 Apr 2018 21:37:13 +0900 Subject: [PATCH 06/59] updated the CHANGELOG and version to release binary packages --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 51875b9..cd34832 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,8 @@ ## Wiki for robotis_op3_demo Packages - http://wiki.ros.org/robotis_op3_demo (metapackage) -- http://wiki.ros.org/ball_detector +- http://wiki.ros.org/op3_ball_detector +- http://wiki.ros.org/op3_bringup - http://wiki.ros.org/op3_demo ## Open Source related to ROBOTIS OP3 @@ -30,5 +31,6 @@ ## Documents and Videos related to ROBOTIS OP3 - [ROBOTIS e-Manual for ROBOTIS OP3](http://emanual.robotis.com/docs/en/platform/op3/introduction/) +- [ROBOTIS e-Manual for ROBOTIS THORMANG3](http://emanual.robotis.com/docs/en/platform/thormang3/introduction/) - [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) - [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) From 1970d4a5a3eee493719c4e4938c6ed104e2883b8 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Sun, 15 May 2022 16:39:50 -0400 Subject: [PATCH 07/59] Create cmake.yml --- .github/workflows/cmake.yml | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 .github/workflows/cmake.yml diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml new file mode 100644 index 0000000..97f7fe7 --- /dev/null +++ b/.github/workflows/cmake.yml @@ -0,0 +1,37 @@ +name: CMake + +on: + push: + branches: [ noetic ] + pull_request: + branches: [ noetic ] + +env: + # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) + BUILD_TYPE: Release + +jobs: + build: + # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. + # You can convert this to a matrix build if you need cross-platform coverage. + # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + + - name: Configure CMake + # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. + # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} + + - name: Build + # Build your program with the given configuration + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + - name: Test + working-directory: ${{github.workspace}}/build + # Execute tests defined by the CMake configuration. + # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail + run: ctest -C ${{env.BUILD_TYPE}} + From d743555e5e5fad68a70225369dd4b8e8372bf85e Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Sun, 15 May 2022 16:40:14 -0400 Subject: [PATCH 08/59] Create docker-publish.yml --- .github/workflows/docker-publish.yml | 93 ++++++++++++++++++++++++++++ 1 file changed, 93 insertions(+) create mode 100644 .github/workflows/docker-publish.yml diff --git a/.github/workflows/docker-publish.yml b/.github/workflows/docker-publish.yml new file mode 100644 index 0000000..7591dd1 --- /dev/null +++ b/.github/workflows/docker-publish.yml @@ -0,0 +1,93 @@ +name: Docker + +# This workflow uses actions that are not certified by GitHub. +# They are provided by a third-party and are governed by +# separate terms of service, privacy policy, and support +# documentation. + +on: + schedule: + - cron: '21 21 * * *' + push: + branches: [ noetic ] + # Publish semver tags as releases. + tags: [ 'v*.*.*' ] + pull_request: + branches: [ noetic ] + +env: + # Use docker.io for Docker Hub if empty + REGISTRY: ghcr.io + # github.repository as / + IMAGE_NAME: ${{ github.repository }} + + +jobs: + build: + + runs-on: ubuntu-latest + permissions: + contents: read + packages: write + # This is used to complete the identity challenge + # with sigstore/fulcio when running outside of PRs. + id-token: write + + steps: + - name: Checkout repository + uses: actions/checkout@v3 + + # Install the cosign tool except on PR + # https://github.com/sigstore/cosign-installer + - name: Install cosign + if: github.event_name != 'pull_request' + uses: sigstore/cosign-installer@d6a3abf1bdea83574e28d40543793018b6035605 + with: + cosign-release: 'v1.7.1' + + + # Workaround: https://github.com/docker/build-push-action/issues/461 + - name: Setup Docker buildx + uses: docker/setup-buildx-action@79abd3f86f79a9d68a23c75a09a9a85889262adf + + # Login against a Docker registry except on PR + # https://github.com/docker/login-action + - name: Log into registry ${{ env.REGISTRY }} + if: github.event_name != 'pull_request' + uses: docker/login-action@28218f9b04b4f3f62068d7b6ce6ca5b26e35336c + with: + registry: ${{ env.REGISTRY }} + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + # Extract metadata (tags, labels) for Docker + # https://github.com/docker/metadata-action + - name: Extract Docker metadata + id: meta + uses: docker/metadata-action@98669ae865ea3cffbcbaa878cf57c20bbf1c6c38 + with: + images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} + + # Build and push Docker image with Buildx (don't push on PR) + # https://github.com/docker/build-push-action + - name: Build and push Docker image + id: build-and-push + uses: docker/build-push-action@ac9327eae2b366085ac7f6a2d02df8aa8ead720a + with: + context: . + push: ${{ github.event_name != 'pull_request' }} + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} + + # Sign the resulting Docker image digest except on PRs. + # This will only write to the public Rekor transparency log when the Docker + # repository is public to avoid leaking data. If you would like to publish + # transparency data even for private images, pass --force to cosign below. + # https://github.com/sigstore/cosign + - name: Sign the published Docker image + if: ${{ github.event_name != 'pull_request' }} + env: + COSIGN_EXPERIMENTAL: "true" + # This step uses the identity token to provision an ephemeral certificate + # against the sigstore community Fulcio instance. + run: cosign sign ${{ steps.meta.outputs.tags }}@${{ steps.build-and-push.outputs.digest }} From 9424c96adf400e15ed56cad64e2be5109fc58032 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 14 Jun 2022 03:35:01 -0400 Subject: [PATCH 09/59] Create docker-image.yml --- .github/workflows/docker-image.yml | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 .github/workflows/docker-image.yml diff --git a/.github/workflows/docker-image.yml b/.github/workflows/docker-image.yml new file mode 100644 index 0000000..acdd5fa --- /dev/null +++ b/.github/workflows/docker-image.yml @@ -0,0 +1,18 @@ +name: Docker Image CI + +on: + push: + branches: [ "noetic" ] + pull_request: + branches: [ "noetic" ] + +jobs: + + build: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - name: Build the Docker image + run: docker build . --file Dockerfile --tag my-image-name:$(date +%s) From d029ca900701d587984b1022f7591d1c92b9890e Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 4 Aug 2022 05:17:59 -0400 Subject: [PATCH 10/59] Update cmake.yml --- .github/workflows/cmake.yml | 82 +++++++++++++++++++++++++++---------- 1 file changed, 61 insertions(+), 21 deletions(-) diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index 97f7fe7..9bf6f0a 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -2,36 +2,76 @@ name: CMake on: push: - branches: [ noetic ] + branches: [ master ] pull_request: - branches: [ noetic ] + branches: [ master ] env: - # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) BUILD_TYPE: Release jobs: build: - # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. - # You can convert this to a matrix build if you need cross-platform coverage. - # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix runs-on: ubuntu-latest - steps: - uses: actions/checkout@v3 - - name: Configure CMake - # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. - # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type - run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} - - - name: Build - # Build your program with the given configuration - run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} - - - name: Test - working-directory: ${{github.workspace}}/build - # Execute tests defined by the CMake configuration. - # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail - run: ctest -C ${{env.BUILD_TYPE}} + - name: Configure CMake 1 + run: cmake -E make_directory ${{runner.workspace}}/op3_ball_detector/build + - name: Configure CMake 2 + run: cmake -E make_directory ${{runner.workspace}}/op3_bringup/build + + - name: Configure CMake 2 + run: cmake -E make_directory ${{runner.workspace}}/op3_demo/build + + - name: Configure CMake 4 + run: cmake -E make_directory ${{runner.workspace}}/op3_read_write_demo/build + + - name: Configure CMake 5 + run: cmake -E make_directory ${{runner.workspace}}/robotis_op3_demo/build + + + + - name: Install dependencies + shell: bash + run: sudo apt-get update ; sudo apt-get install Date: Wed, 27 Sep 2023 09:51:05 -0400 Subject: [PATCH 11/59] latest pushes --- .github/workflows/cmake.yml | 78 +++++------------ docker/Dockerfile | 73 ++++++++++++++++ docker/gpu.Dockerfile | 90 ++++++++++++++++++++ requirements/requirements.txt | 39 +++++++++ requirements/ros_repository_requirements.txt | 9 ++ requirements/ros_requirements.txt | 15 ++++ requirements/system_requirements.txt | 29 +++++++ 7 files changed, 274 insertions(+), 59 deletions(-) create mode 100644 docker/Dockerfile create mode 100644 docker/gpu.Dockerfile create mode 100644 requirements/requirements.txt create mode 100644 requirements/ros_repository_requirements.txt create mode 100644 requirements/ros_requirements.txt create mode 100644 requirements/system_requirements.txt diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index 9bf6f0a..97f7fe7 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -2,76 +2,36 @@ name: CMake on: push: - branches: [ master ] + branches: [ noetic ] pull_request: - branches: [ master ] + branches: [ noetic ] env: + # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) BUILD_TYPE: Release jobs: build: + # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. + # You can convert this to a matrix build if you need cross-platform coverage. + # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix runs-on: ubuntu-latest + steps: - uses: actions/checkout@v3 - - name: Configure CMake 1 - run: cmake -E make_directory ${{runner.workspace}}/op3_ball_detector/build - - - name: Configure CMake 2 - run: cmake -E make_directory ${{runner.workspace}}/op3_bringup/build - - - name: Configure CMake 2 - run: cmake -E make_directory ${{runner.workspace}}/op3_demo/build - - - name: Configure CMake 4 - run: cmake -E make_directory ${{runner.workspace}}/op3_read_write_demo/build - - - name: Configure CMake 5 - run: cmake -E make_directory ${{runner.workspace}}/robotis_op3_demo/build - - - - - name: Install dependencies - shell: bash - run: sudo apt-get update ; sudo apt-get install > ~/.bashrc +RUN source ~/.bashrc + +RUN cd $CATKIN_WS \ + && rosdep init \ + && rosdep update \ + && rosdep update --rosdistro noetic \ + && rosdep fix-permissions \ + && rosdep install -y --from-paths . --ignore-src --rosdistro noetic + +# Always source catkin_setup.sh when launching bash +RUN echo "source /usr/local/bin/catkin_setup.sh" >> /root/.bashrc +COPY catkin_setup.sh /usr/local/bin/catkin_setup.sh +RUN chmod +x /usr/local/bin/catkin_setup.sh + +ENTRYPOINT ["/usr/local/bin/catkin_setup.sh"] +CMD ["bash"] diff --git a/docker/gpu.Dockerfile b/docker/gpu.Dockerfile new file mode 100644 index 0000000..df15614 --- /dev/null +++ b/docker/gpu.Dockerfile @@ -0,0 +1,90 @@ +ARG ROS_ARCHITECTURE_VERSION=latest + +FROM ubuntu:20.04 as base_build +FROM nvidia/cuda:11.2.1-base-ubuntu20.04 + +ENV DEBIAN_FRONTEND noninteractive +ENV PYTHON_VERSION="3.8" +ENV CUDNN_VERSION=8.1.0.77 +ENV TF_TENSORRT_VERSION=7.2.2 +ENV CUDA=11.2 +ENV LD_LIBRARY_PATH /usr/local/cuda/extras/CUPTI/lib64:$LD_LIBRARY_PATH + +ARG ROS_ARCHITECTURE_VERSION_GIT_BRANCH=master +ARG ROS_ARCHITECTURE_VERSION_GIT_COMMIT=HEAD + +LABEL maintainer=ronaldsonbellande@gmail.com +LABEL ROS_architecture_github_branchtag=${ROS_ARCHITECTURE_VERSION_GIT_BRANCH} +LABEL ROS_architecture_github_commit=${ROS_ARCHITECTURE_VERSION_GIT_COMMIT} + +# Ubuntu setup +RUN apt-get update -y +RUN apt-get upgrade -y + +# RUN workspace and sourcing +WORKDIR ./ +COPY requirements.txt . +COPY system_requirements.txt . +COPY ros_requirements.txt . +COPY ros_repository_requirements.txt . + +# Install dependencies for system +RUN apt-get update && apt-get install -y --no-install-recommends > ~/.bashrc +RUN source ~/.bashrc + +RUN cd $CATKIN_WS \ + && rosdep init \ + && rosdep update \ + && rosdep update --rosdistro noetic \ + && rosdep fix-permissions \ + && rosdep install -y --from-paths . --ignore-src --rosdistro noetic + +# Always source catkin_setup.sh when launching bash +RUN echo "source /usr/local/bin/catkin_setup.sh" >> /root/.bashrc +COPY catkin_setup.sh /usr/local/bin/catkin_setup.sh +RUN chmod +x /usr/local/bin/catkin_setup.sh + +ENTRYPOINT ["/usr/local/bin/catkin_setup.sh"] +CMD ["bash"] + +RUN apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/3bf863cc.pub && \ + apt-get update && apt-get install -y --no-install-recommends \ + cuda-nvrtc-${CUDA/./-} \ + libcudnn8=${CUDNN_VERSION}-1+cuda${CUDA} \ + -r cuda_requirements.txt + +# We don't install libnvinfer-dev since we don't need to build against TensorRT +RUN apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/machine-learning/repos/ubuntu2004/x86_64/7fa2af80.pub && \ + echo "deb https://developer.download.nvidia.com/compute/machine-learning/repos/ubuntu2004/x86_64 /" > /etc/apt/sources.list.d/tensorRT.list && \ + apt-get update && \ + apt-get install -y --no-install-recommends libnvinfer7=${TF_TENSORRT_VERSION}-1+cuda11.0 \ + libnvinfer-plugin7=${TF_TENSORRT_VERSION}-1+cuda11.0 \ + && apt-get clean && \ + rm -rf /var/lib/apt/lists/*; + diff --git a/requirements/requirements.txt b/requirements/requirements.txt new file mode 100644 index 0000000..d08c640 --- /dev/null +++ b/requirements/requirements.txt @@ -0,0 +1,39 @@ +# Recomandation for library to install in python + +setuptools +pandas +scipy +sklearn +future +grpcio +h5py +requests +opencv-python +python-math +random2 +pytest-warnings +os.path2 +pydicom +glob2 +pytest-shutil +DateTime +zipfile36 +urllib3 +python-time +trimesh +librosa +gym +matplotlib +image-slicer +nvidia-ml-py3 +imgaug +tqdm +rosdep + +protobuf==3.15.2 +numpy==1.19.2 +numba==0.55.2 +imageio==2.9.0 +pillow==7.2.0 +tensorflow==2.7.0 +keras==2.7.0 diff --git a/requirements/ros_repository_requirements.txt b/requirements/ros_repository_requirements.txt new file mode 100644 index 0000000..09c4578 --- /dev/null +++ b/requirements/ros_repository_requirements.txt @@ -0,0 +1,9 @@ +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-msgs +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3 +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Tools +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Framework-msgs +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Framework +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Common +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Utility +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Demo +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Math diff --git a/requirements/ros_requirements.txt b/requirements/ros_requirements.txt new file mode 100644 index 0000000..7b80df2 --- /dev/null +++ b/requirements/ros_requirements.txt @@ -0,0 +1,15 @@ +# Recomandation for dependencies for ros system + +ros-noetic-desktop-full +build-essential +ros-noetic-catkin +python-rosdep +python-rosinstall +python-rosinstall-generator +python-wstool +python-catkin-tools +ros-noetic-pcl-ros +ros-noetic-flexbe-behavior-engine +ros-noetic-moveit +ros-noetic-gazebo-ros-pkgs +ros-noetic-gazebo-ros-control diff --git a/requirements/system_requirements.txt b/requirements/system_requirements.txt new file mode 100644 index 0000000..46eea93 --- /dev/null +++ b/requirements/system_requirements.txt @@ -0,0 +1,29 @@ +# Recomandation for dependencies for linux system + +software-properties-common +python3.8 +neovim +apt-utils +automake +build-essential +ca-certificates +pycurl +git +python3-pip +libcurl3-dev +libfreetype6-dev +libpng-dev +libtool +libzmq3-dev +mlocate +openjdk-8-jdk +openjdk-8-jre-headless +pkg-config +python-dev +software-properties-common +swig +unzip +wget +zip +zlib1g-dev +python3-distutils From 6eb7d23127a942139b87f62c120dc1aedd55f054 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Wed, 27 Sep 2023 16:28:56 -0400 Subject: [PATCH 12/59] latest pushes --- README.md | 45 ++++++-------------- requirements/ros_repository_requirements.txt | 10 ++--- 2 files changed, 19 insertions(+), 36 deletions(-) diff --git a/README.md b/README.md index cd34832..a6a7663 100644 --- a/README.md +++ b/README.md @@ -1,36 +1,19 @@ -# ROBOTIS OP3 - +# ROS/ROS2 Humanoid Robot Demos -## ROS Packages for ROBOTIS OP3 Demo -|Version|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic| -|:---:|:---:|:---:| -|[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-OP3-Demo.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-OP3-Demo)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-OP3-Demo.svg?branch=kinetic-devel)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-OP3-Demo)|-| +-------------------------------------------------------------------------------------------------------- +Updated Version [humanoid_robot_module](https://github.com/Robotics-Sensors/humanoid_robot_demos) readme. -## ROBOTIS e-Manual for ROBOTIS OP3 -- [ROBOTIS e-Manual for ROBOTIS OP3](http://emanual.robotis.com/docs/en/platform/op3/introduction/) +-------------------------------------------------------------------------------------------------------- +## Important +The repository has diverged, as the old commits and codes are under the previous License and +the new commits and codes are under New License -## Wiki for robotis_op3_demo Packages -- http://wiki.ros.org/robotis_op3_demo (metapackage) -- http://wiki.ros.org/op3_ball_detector -- http://wiki.ros.org/op3_bringup -- http://wiki.ros.org/op3_demo +-------------------------------------------------------------------------------------------------------- +Latest versions and Maintainer is on organization https://github.com/Robotics-Sensors -## Open Source related to ROBOTIS OP3 -- [robotis_op3](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3) -- [robotis_op3_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-msgs) -- [robotis_op3_common](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Common) -- [robotis_op3_tools](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Tools) -- [robotis_op3_demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) -- [robotis_framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) -- [robotis_controller_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs) -- [robotis_utility](https://github.com/ROBOTIS-GIT/ROBOTIS-Utility) -- [robotis_math](https://github.com/ROBOTIS-GIT/ROBOTIS-Math) -- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) -- [OpenCR-Hardware](https://github.com/ROBOTIS-GIT/OpenCR-Hardware) -- [OpenCR](https://github.com/ROBOTIS-GIT/OpenCR) -## Documents and Videos related to ROBOTIS OP3 -- [ROBOTIS e-Manual for ROBOTIS OP3](http://emanual.robotis.com/docs/en/platform/op3/introduction/) -- [ROBOTIS e-Manual for ROBOTIS THORMANG3](http://emanual.robotis.com/docs/en/platform/thormang3/introduction/) -- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) -- [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) +### Maintainer +* Ronaldson Bellande + +## License +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0), see [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_demos/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_demos/blob/main/LICENSE) for more information. diff --git a/requirements/ros_repository_requirements.txt b/requirements/ros_repository_requirements.txt index 09c4578..e3c338f 100644 --- a/requirements/ros_repository_requirements.txt +++ b/requirements/ros_repository_requirements.txt @@ -1,9 +1,9 @@ -git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-msgs -git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3 -git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Tools +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-HUMANOID_ROBOT-msgs +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-HUMANOID_ROBOT +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-HUMANOID_ROBOT-Tools git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Framework-msgs git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Framework -git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Common +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-HUMANOID_ROBOT-Common git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Utility -git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Demo +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-HUMANOID_ROBOT-Demo git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Math From 8d0709c92142e8675488d8b3f5bcfab1fb11e4b0 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 28 Sep 2023 10:57:43 -0400 Subject: [PATCH 13/59] latest pushes --- .gitignore | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/.gitignore b/.gitignore index 36a5c0a..a78ffcf 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,13 @@ qtcreator-build *.backup *.user *.autosave + +# Scripts +init_setup.sh +repository_recal.sh +push.sh +publish.sh +ros_publish.sh +prerelease_test.sh +fix_errors.sh +replace_add_index.sh From 09299a2facec15b69d171fc479c1942b424bac45 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 28 Sep 2023 11:08:48 -0400 Subject: [PATCH 14/59] latest pushes --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index a6a7663..20728e6 100644 --- a/README.md +++ b/README.md @@ -12,6 +12,7 @@ the new commits and codes are under New License Latest versions and Maintainer is on organization https://github.com/Robotics-Sensors + ### Maintainer * Ronaldson Bellande From 9571269e17e1fecc683c9c5689413b3ece9d208d Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 28 Sep 2023 11:18:08 -0400 Subject: [PATCH 15/59] latest pushes --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 20728e6..f32c3fa 100644 --- a/README.md +++ b/README.md @@ -3,6 +3,8 @@ -------------------------------------------------------------------------------------------------------- Updated Version [humanoid_robot_module](https://github.com/Robotics-Sensors/humanoid_robot_demos) readme. +Old Version/Previous Used for Different Context [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) readme. + -------------------------------------------------------------------------------------------------------- ## Important The repository has diverged, as the old commits and codes are under the previous License and @@ -12,7 +14,6 @@ the new commits and codes are under New License Latest versions and Maintainer is on organization https://github.com/Robotics-Sensors - ### Maintainer * Ronaldson Bellande From 7342ae9209f930f11f6bc7b0830ee8cd18057dd6 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Sat, 30 Sep 2023 16:51:44 -0400 Subject: [PATCH 16/59] latest pushes --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index f32c3fa..7dcaab3 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,11 @@ the new commits and codes are under New License Latest versions and Maintainer is on organization https://github.com/Robotics-Sensors +# USE CASE +-------------------------------------------------------------------------------------------------------- +* Every repository within our organization is a valuable resource that can be utilized for educational purposes by individuals who actively contribute to the repository or choose to become sponsors of the organization. For those who wish to use our services conversationally, the acquisition of a license and subscription from our company website is mandatory. This ensures that the services are appropriately compensated for their use. Additionally, for the 90% of services designated as private, acquiring a license and subscription can be facilitated through our company website. We encourage interested parties to visit our website to explore and procure the necessary licenses and subscriptions for the diverse range of services and products we offer. Your support and commitment enable us to maintain and enhance the quality of our offerings, contributing to a thriving collaborative environment. +-------------------------------------------------------------------------------------------------------- + ### Maintainer * Ronaldson Bellande From 9357a069935b065b06d475e7923c4e34c6984e7a Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 3 Oct 2023 08:20:12 -0400 Subject: [PATCH 17/59] Update LICENSE --- LICENSE | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/LICENSE b/LICENSE index c0ee812..ffeeac4 100644 --- a/LICENSE +++ b/LICENSE @@ -186,7 +186,7 @@ same "printed page" as the copyright notice for easier identification within third-party archives. - Copyright {yyyy} {name of copyright owner} + Copyright 2023 Ronaldson Bellande Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. From dbbde3d4234148a5afc7e8b00d2b697e5521d26c Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 11 Oct 2023 11:46:17 -0400 Subject: [PATCH 18/59] Create jekyll-gh-pages.yml --- .github/workflows/jekyll-gh-pages.yml | 51 +++++++++++++++++++++++++++ 1 file changed, 51 insertions(+) create mode 100644 .github/workflows/jekyll-gh-pages.yml diff --git a/.github/workflows/jekyll-gh-pages.yml b/.github/workflows/jekyll-gh-pages.yml new file mode 100644 index 0000000..559bddf --- /dev/null +++ b/.github/workflows/jekyll-gh-pages.yml @@ -0,0 +1,51 @@ +# Sample workflow for building and deploying a Jekyll site to GitHub Pages +name: Deploy Jekyll with GitHub Pages dependencies preinstalled + +on: + # Runs on pushes targeting the default branch + push: + branches: ["main"] + + # Allows you to run this workflow manually from the Actions tab + workflow_dispatch: + +# Sets permissions of the GITHUB_TOKEN to allow deployment to GitHub Pages +permissions: + contents: read + pages: write + id-token: write + +# Allow only one concurrent deployment, skipping runs queued between the run in-progress and latest queued. +# However, do NOT cancel in-progress runs as we want to allow these production deployments to complete. +concurrency: + group: "pages" + cancel-in-progress: false + +jobs: + # Build job + build: + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Setup Pages + uses: actions/configure-pages@v3 + - name: Build with Jekyll + uses: actions/jekyll-build-pages@v1 + with: + source: ./ + destination: ./_site + - name: Upload artifact + uses: actions/upload-pages-artifact@v2 + + # Deployment job + deploy: + environment: + name: github-pages + url: ${{ steps.deployment.outputs.page_url }} + runs-on: ubuntu-latest + needs: build + steps: + - name: Deploy to GitHub Pages + id: deployment + uses: actions/deploy-pages@v2 From 97e2d931309cc7d9c87a68d1e07cf69992743fa1 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 12 Oct 2023 17:57:00 -0400 Subject: [PATCH 19/59] Update README.md --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 7dcaab3..1d61e63 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,9 @@ # ROS/ROS2 Humanoid Robot Demos +-------------------------------------------------------------------------------------------------------- +# Website +https://robotics-sensors.github.io/humanoid_robot_demos + -------------------------------------------------------------------------------------------------------- Updated Version [humanoid_robot_module](https://github.com/Robotics-Sensors/humanoid_robot_demos) readme. From f314dcde2946425c6c724d1b181dab9f8d34dd08 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 24 Oct 2023 08:25:30 -0400 Subject: [PATCH 20/59] Update README.md --- README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 1d61e63..b382302 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,10 @@ # ROS/ROS2 Humanoid Robot Demos +[![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) +[![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) +[![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) -------------------------------------------------------------------------------------------------------- -# Website +# Repository Website https://robotics-sensors.github.io/humanoid_robot_demos -------------------------------------------------------------------------------------------------------- From 35caa7ec6d3976bb12c33bec8d91228b8c0b4ac1 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 2 Nov 2023 08:22:51 -0400 Subject: [PATCH 21/59] Create dependabot.yml --- .github/dependabot.yml | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 .github/dependabot.yml diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000..ac6621f --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,11 @@ +# To get started with Dependabot version updates, you'll need to specify which +# package ecosystems to update and where the package manifests are located. +# Please see the documentation for all configuration options: +# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates + +version: 2 +updates: + - package-ecosystem: "" # See documentation for possible values + directory: "/" # Location of package manifests + schedule: + interval: "weekly" From ea23e79ab2c615fb84c9024d23298c0b8095d7d5 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 2 Nov 2023 08:22:59 -0400 Subject: [PATCH 22/59] Create codeql.yml --- .github/workflows/codeql.yml | 82 ++++++++++++++++++++++++++++++++++++ 1 file changed, 82 insertions(+) create mode 100644 .github/workflows/codeql.yml diff --git a/.github/workflows/codeql.yml b/.github/workflows/codeql.yml new file mode 100644 index 0000000..7fceeec --- /dev/null +++ b/.github/workflows/codeql.yml @@ -0,0 +1,82 @@ +# For most projects, this workflow file will not need changing; you simply need +# to commit it to your repository. +# +# You may wish to alter this file to override the set of languages analyzed, +# or to provide custom queries or build logic. +# +# ******** NOTE ******** +# We have attempted to detect the languages in your repository. Please check +# the `language` matrix defined below to confirm you have the correct set of +# supported CodeQL languages. +# +name: "CodeQL" + +on: + push: + branches: [ "main" ] + pull_request: + # The branches below must be a subset of the branches above + branches: [ "main" ] + schedule: + - cron: '17 17 * * 4' + +jobs: + analyze: + name: Analyze + # Runner size impacts CodeQL analysis time. To learn more, please see: + # - https://gh.io/recommended-hardware-resources-for-running-codeql + # - https://gh.io/supported-runners-and-hardware-resources + # - https://gh.io/using-larger-runners + # Consider using larger runners for possible analysis time improvements. + runs-on: ${{ (matrix.language == 'swift' && 'macos-latest') || 'ubuntu-latest' }} + timeout-minutes: ${{ (matrix.language == 'swift' && 120) || 360 }} + permissions: + actions: read + contents: read + security-events: write + + strategy: + fail-fast: false + matrix: + language: [ 'c-cpp', 'python' ] + # CodeQL supports [ 'c-cpp', 'csharp', 'go', 'java-kotlin', 'javascript-typescript', 'python', 'ruby', 'swift' ] + # Use only 'java-kotlin' to analyze code written in Java, Kotlin or both + # Use only 'javascript-typescript' to analyze code written in JavaScript, TypeScript or both + # Learn more about CodeQL language support at https://aka.ms/codeql-docs/language-support + + steps: + - name: Checkout repository + uses: actions/checkout@v3 + + # Initializes the CodeQL tools for scanning. + - name: Initialize CodeQL + uses: github/codeql-action/init@v2 + with: + languages: ${{ matrix.language }} + # If you wish to specify custom queries, you can do so here or in a config file. + # By default, queries listed here will override any specified in a config file. + # Prefix the list here with "+" to use these queries and those in the config file. + + # For more details on CodeQL's query packs, refer to: https://docs.github.com/en/code-security/code-scanning/automatically-scanning-your-code-for-vulnerabilities-and-errors/configuring-code-scanning#using-queries-in-ql-packs + # queries: security-extended,security-and-quality + + + # Autobuild attempts to build any compiled languages (C/C++, C#, Go, Java, or Swift). + # If this step fails, then you should remove it and run the build manually (see below) + - name: Autobuild + uses: github/codeql-action/autobuild@v2 + + # ℹ️ Command-line programs to run using the OS shell. + # 📚 See https://docs.github.com/en/actions/using-workflows/workflow-syntax-for-github-actions#jobsjob_idstepsrun + + # If the Autobuild fails above, remove it and uncomment the following three lines. + # modify them (or add more) to build your code if your project, please refer to the EXAMPLE below for guidance. + + # - run: | + # echo "Run, Build Application using script" + # ./location_of_script_within_repo/buildscript.sh + + - name: Perform CodeQL Analysis + uses: github/codeql-action/analyze@v2 + with: + category: "/language:${{matrix.language}}" From b204d0a1fd497ae4d97a5d0f8b1962b1dae51672 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Fri, 3 Nov 2023 13:54:46 -0400 Subject: [PATCH 23/59] Update README.md --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index b382302..3df430f 100644 --- a/README.md +++ b/README.md @@ -9,9 +9,11 @@ https://robotics-sensors.github.io/humanoid_robot_demos -------------------------------------------------------------------------------------------------------- Updated Version [humanoid_robot_module](https://github.com/Robotics-Sensors/humanoid_robot_demos) readme. - Old Version/Previous Used for Different Context [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) readme. +# Contact +If you are interested in accessing the complete version of this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. + -------------------------------------------------------------------------------------------------------- ## Important The repository has diverged, as the old commits and codes are under the previous License and From ce38659d483a94630efc95e6c75b7b32cec80f00 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Sat, 4 Nov 2023 10:41:26 -0400 Subject: [PATCH 24/59] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 3df430f..b41ae45 100644 --- a/README.md +++ b/README.md @@ -12,7 +12,7 @@ Updated Version [humanoid_robot_module](https://github.com/Robotics-Sensors/huma Old Version/Previous Used for Different Context [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) readme. # Contact -If you are interested in accessing the complete version of this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. +Depeding on the repository, If you are interested in accessing the complete version or other repository related to this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. -------------------------------------------------------------------------------------------------------- ## Important From 8252a16455e4bde66f25ed106de62a5924c3cca1 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Sun, 5 Nov 2023 01:14:42 -0500 Subject: [PATCH 25/59] Update README.md --- README.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/README.md b/README.md index b41ae45..c99b32e 100644 --- a/README.md +++ b/README.md @@ -3,6 +3,20 @@ [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) [![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) + +# Stats +[![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_demos.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_demos/stargazers) +[![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_demos.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_demos/network) +[![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_demos.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_demos/watchers) + +[![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_demos/issues) +[![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_demos/pulls) +[![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_demos/blob/main/LICENSE) + +[![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_demos/commits) +[![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_demos/traffic)](https://github.com/Robotics-Sensors/humanoid_robot_demos) + + -------------------------------------------------------------------------------------------------------- # Repository Website https://robotics-sensors.github.io/humanoid_robot_demos From 66d713e6a0b655979c2474c53731226855461f65 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Sun, 5 Nov 2023 01:15:17 -0500 Subject: [PATCH 26/59] Update README.md --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index c99b32e..619e927 100644 --- a/README.md +++ b/README.md @@ -25,6 +25,9 @@ https://robotics-sensors.github.io/humanoid_robot_demos Updated Version [humanoid_robot_module](https://github.com/Robotics-Sensors/humanoid_robot_demos) readme. Old Version/Previous Used for Different Context [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) readme. +# Release +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_demos/releases/) + # Contact Depeding on the repository, If you are interested in accessing the complete version or other repository related to this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. From e5610ce9386196e4f524a3a5d8025e42c6cc0ba9 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Mon, 13 Nov 2023 17:56:15 -0500 Subject: [PATCH 27/59] latest pushes --- README.md | 24 +- .../CHANGELOG.rst | 33 ++ .../CMakeLists.txt | 19 + .../package.xml | 60 +++ .../rviz/humanoid_robot_bringup.rviz | 357 ++++++++++++++++++ .../CMakeLists.txt | 57 +++ .../package.xml | 53 +++ 7 files changed, 591 insertions(+), 12 deletions(-) create mode 100644 humanoid_robot_intelligence_control_system_bringup/CHANGELOG.rst create mode 100644 humanoid_robot_intelligence_control_system_bringup/CMakeLists.txt create mode 100644 humanoid_robot_intelligence_control_system_bringup/package.xml create mode 100644 humanoid_robot_intelligence_control_system_bringup/rviz/humanoid_robot_bringup.rviz create mode 100644 humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt create mode 100644 humanoid_robot_intelligence_control_system_read_write_demo/package.xml diff --git a/README.md b/README.md index 619e927..9066ff0 100644 --- a/README.md +++ b/README.md @@ -5,28 +5,28 @@ [![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) # Stats -[![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_demos.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_demos/stargazers) -[![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_demos.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_demos/network) -[![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_demos.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_demos/watchers) +[![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/stargazers) +[![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/network) +[![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/watchers) -[![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_demos/issues) -[![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_demos/pulls) -[![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_demos/blob/main/LICENSE) +[![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/issues) +[![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/pulls) +[![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE) -[![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_demos/commits) -[![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_demos/traffic)](https://github.com/Robotics-Sensors/humanoid_robot_demos) +[![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/commits) +[![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/traffic)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos) -------------------------------------------------------------------------------------------------------- # Repository Website -https://robotics-sensors.github.io/humanoid_robot_demos +https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_demos -------------------------------------------------------------------------------------------------------- -Updated Version [humanoid_robot_module](https://github.com/Robotics-Sensors/humanoid_robot_demos) readme. +Updated Version [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos) readme. Old Version/Previous Used for Different Context [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) readme. # Release -[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_demos/releases/) +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/releases/) # Contact Depeding on the repository, If you are interested in accessing the complete version or other repository related to this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. @@ -49,4 +49,4 @@ Latest versions and Maintainer is on organization https://github.com/Robotics-Se * Ronaldson Bellande ## License -This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0), see [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_demos/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_demos/blob/main/LICENSE) for more information. +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0), see [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE) for more information. diff --git a/humanoid_robot_intelligence_control_system_bringup/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_bringup/CHANGELOG.rst new file mode 100644 index 0000000..878e980 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_bringup/CHANGELOG.rst @@ -0,0 +1,33 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package humanoid_robot_intelligence_control_system_bringup +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.2 (2023-10-03) +------------------ +* Make it compatible for ROS1/ROS2 +* Fix bugs +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.3.1 (2023-09-27) +------------------ +* Starting from this point it under a new license +* Fix errors and Issues +* Rename Repository for a completely different purpose +* Make it compatible with ROS/ROS2 +* Upgrade version of all builds and make it more compatible +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.3.0 (2021-05-05) +------------------ +* Update package.xml and CMakeList.txt for noetic branch +* Contributors: Ronaldson Bellande + +0.1.0 (2018-04-19) +------------------ +* first release for ROS Kinetic +* updated CMakeLists.txt and package.xml of humanoid_robot_intelligence_control_system_bringup +* changed rviz config file +* refacoring to release +* Contributors: Kayman, Pyo diff --git a/humanoid_robot_intelligence_control_system_bringup/CMakeLists.txt b/humanoid_robot_intelligence_control_system_bringup/CMakeLists.txt new file mode 100644 index 0000000..b0f7a88 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_bringup/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.8) +project(humanoid_robot_intelligence_control_system_bringup) + + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED) +else() + find_package(ament_cmake REQUIRED) +endif() + + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_package() +endif() + +install( + DIRECTORY launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/humanoid_robot_intelligence_control_system_bringup/package.xml b/humanoid_robot_intelligence_control_system_bringup/package.xml new file mode 100644 index 0000000..2d44949 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_bringup/package.xml @@ -0,0 +1,60 @@ + + + humanoid_robot_intelligence_control_system_bringup + 0.3.2 + + This package is a demo for first time users. + There is an example in the demo where you can run and visualize the robot. + + Apache 2.0 + Ronaldson Bellande + + + catkin + + humanoid_robot_intelligence_control_system_manager + humanoid_robot_intelligence_control_system_description + usb_cam + joint_state_publisher + robot_state_publisher + rviz + + humanoid_robot_intelligence_control_system_manager + humanoid_robot_intelligence_control_system_description + usb_cam + joint_state_publisher + robot_state_publisher + rviz + + humanoid_robot_intelligence_control_system_manager + humanoid_robot_intelligence_control_system_description + usb_cam + joint_state_publisher + robot_state_publisher + rviz + + + ament_cmake + + humanoid_robot_intelligence_control_system_manager + humanoid_robot_intelligence_control_system_description + usb_cam + joint_state_publisher + robot_state_publisher + rviz + + humanoid_robot_intelligence_control_system_manager + humanoid_robot_intelligence_control_system_description + usb_cam + joint_state_publisher + robot_state_publisher + rviz + + humanoid_robot_intelligence_control_system_manager + humanoid_robot_intelligence_control_system_description + usb_cam + joint_state_publisher + robot_state_publisher + rviz + + diff --git a/humanoid_robot_intelligence_control_system_bringup/rviz/humanoid_robot_bringup.rviz b/humanoid_robot_intelligence_control_system_bringup/rviz/humanoid_robot_bringup.rviz new file mode 100644 index 0000000..3ba310a --- /dev/null +++ b/humanoid_robot_intelligence_control_system_bringup/rviz/humanoid_robot_bringup.rviz @@ -0,0 +1,357 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + Splitter Ratio: 0.5 + Tree Height: 352 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Image +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + body_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + cam_gazebo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cam_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_ank_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_ank_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_el_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_hip_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_hip_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_hip_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_knee_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_sho_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_sho_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_ank_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_ank_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_el_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_hip_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_hip_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_hip_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_knee_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_sho_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_sho_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/Image + Enabled: true + Image Topic: /usb_cam_node/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + body_link: + Value: true + cam_gazebo_link: + Value: true + cam_link: + Value: true + head_pan_link: + Value: true + head_tilt_link: + Value: true + l_ank_pitch_link: + Value: true + l_ank_roll_link: + Value: true + l_el_link: + Value: true + l_hip_pitch_link: + Value: true + l_hip_roll_link: + Value: true + l_hip_yaw_link: + Value: true + l_knee_link: + Value: true + l_sho_pitch_link: + Value: true + l_sho_roll_link: + Value: true + r_ank_pitch_link: + Value: true + r_ank_roll_link: + Value: true + r_el_link: + Value: true + r_hip_pitch_link: + Value: true + r_hip_roll_link: + Value: true + r_hip_yaw_link: + Value: true + r_knee_link: + Value: true + r_sho_pitch_link: + Value: true + r_sho_roll_link: + Value: true + world: + Value: true + Marker Scale: 0.200000003 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: false + Tree: + world: + body_link: + head_pan_link: + head_tilt_link: + cam_gazebo_link: + {} + cam_link: + {} + l_hip_yaw_link: + l_hip_roll_link: + l_hip_pitch_link: + l_knee_link: + l_ank_pitch_link: + l_ank_roll_link: + {} + l_sho_pitch_link: + l_sho_roll_link: + l_el_link: + {} + r_hip_yaw_link: + r_hip_roll_link: + r_hip_pitch_link: + r_knee_link: + r_ank_pitch_link: + r_ank_roll_link: + {} + r_sho_pitch_link: + r_sho_roll_link: + r_el_link: + {} + Update Interval: 0 + Value: true + - Alpha: 0.200000003 + Class: rviz_plugin_tutorials/Imu + Color: 204; 51; 204 + Enabled: true + History Length: 1 + Name: Imu + Topic: /humanoid_robot_intelligence_control_system/open_cr/imu + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: body_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.32116389 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.275397927 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.68858385 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1023 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000023300000358fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000001f1000000df00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000023a000001610000001800ffffff000000010000010f00000358fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004300000358000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000022400fffffffb0000000800540069006d00650100000000000004500000000000000000000005470000035800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1920 + X: 0 + Y: 0 diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt b/humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt new file mode 100644 index 0000000..da225ca --- /dev/null +++ b/humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.8) +project(humanoid_robot_intelligence_control_system_read_write_demo) + + +if($ENV{ROS_VERSION} EQUAL 1) + find_package( + catkin REQUIRED COMPONENTS + humanoid_robot_intelligence_control_system_controller_msgs + roscpp + sensor_msgs + std_msgs + ) +else() + find_package(ament_cmake REQUIRED) +endif() + + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_package( + INCLUDE_DIRS + CATKIN_DEPENDS + roscpp + humanoid_robot_intelligence_control_system_controller_msgs + roscpp + sensor_msgs + std_msgs + ) +endif() + + +include_directories(${catkin_INCLUDE_DIRS}) + +add_executable( + read_write + src/read_write.cpp +) + +add_dependencies( + read_write + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +target_link_libraries( + read_write + ${catkin_LIBRARIES} +) + +install( + TARGETS read_write + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/package.xml b/humanoid_robot_intelligence_control_system_read_write_demo/package.xml new file mode 100644 index 0000000..0efd54a --- /dev/null +++ b/humanoid_robot_intelligence_control_system_read_write_demo/package.xml @@ -0,0 +1,53 @@ + + + humanoid_robot_intelligence_control_system_read_write_demo + 0.3.2 + + The humanoid_robot_intelligence_control_system_read_write_demo package + + Apache 2.0 + Ronaldson Bellande + + + catkin + + roscpp + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + humanoid_robot_intelligence_control_system_manager + + roscpp + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + humanoid_robot_intelligence_control_system_manager + + roscpp + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + humanoid_robot_intelligence_control_system_manager + + + ament_cmake + + roscpp + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + humanoid_robot_intelligence_control_system_manager + + roscpp + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + humanoid_robot_intelligence_control_system_manager + + roscpp + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + humanoid_robot_intelligence_control_system_manager + + From 8840ad3071fcb8956996691e43cfbb17267615a5 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 30 Nov 2023 19:59:29 -0500 Subject: [PATCH 28/59] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 9066ff0..b8a908e 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# ROS/ROS2 Humanoid Robot Demos +# ROS/ROS2 Humanoid Robot Intelligence Control System Demos [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) From 462b4d6a1c5958e9d3ee8540d4af72c3a2a38957 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 30 Nov 2023 21:24:59 -0500 Subject: [PATCH 29/59] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index b8a908e..f9291c2 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# ROS/ROS2 Humanoid Robot Intelligence Control System Demos +# ROS/ROS2 {BR&SRI} Humanoid Robot Intelligence Control System Demos [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) From 7a65e0bc6e716a0d11b2bfbe4f8b61158ee48dc7 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 6 Dec 2023 02:56:03 -0500 Subject: [PATCH 30/59] Update README.md --- README.md | 75 +++++++++++++++++++++++++++++++------------------------ 1 file changed, 42 insertions(+), 33 deletions(-) diff --git a/README.md b/README.md index f9291c2..2e84110 100644 --- a/README.md +++ b/README.md @@ -1,52 +1,61 @@ -# ROS/ROS2 {BR&SRI} Humanoid Robot Intelligence Control System Demos +# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) [![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) -# Stats -[![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/stargazers) -[![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/network) -[![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/watchers) +## 🤖 Explore Humanoid Robot Intelligence with Us! -[![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/issues) -[![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/pulls) -[![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE) +Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. -[![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/commits) -[![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/traffic)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos) +### 🚀 Key Repository Stats +- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Starred by the community +- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Forked by enthusiasts +- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Watched by curious minds --------------------------------------------------------------------------------------------------------- -# Repository Website -https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_demos +- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Actively addressing issues +- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Welcoming contributions +- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Licensed under Apache 2.0 --------------------------------------------------------------------------------------------------------- -Updated Version [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos) readme. -Old Version/Previous Used for Different Context [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) readme. +- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Latest updates +- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/traffic) Engaging with the community + +--- + +## 🌐 Repository Website + +Explore our [repository website](https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_demos) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. + +--- + +### 🔄 Updates and Versions + +- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos) +- **Old Version/Previous Used for Different Context:** [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) + +--- + +# 🎉 Latest Release -# Release [![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/releases/) -# Contact -Depeding on the repository, If you are interested in accessing the complete version or other repository related to this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. +--- --------------------------------------------------------------------------------------------------------- -## Important -The repository has diverged, as the old commits and codes are under the previous License and -the new commits and codes are under New License +## 📢 Important Announcement --------------------------------------------------------------------------------------------------------- -Latest versions and Maintainer is on organization https://github.com/Robotics-Sensors +The repository has recently undergone significant updates. Older commits and codes are covered under the previous license, while new commits and codes fall under the new license. +--- -# USE CASE --------------------------------------------------------------------------------------------------------- -* Every repository within our organization is a valuable resource that can be utilized for educational purposes by individuals who actively contribute to the repository or choose to become sponsors of the organization. For those who wish to use our services conversationally, the acquisition of a license and subscription from our company website is mandatory. This ensures that the services are appropriately compensated for their use. Additionally, for the 90% of services designated as private, acquiring a license and subscription can be facilitated through our company website. We encourage interested parties to visit our website to explore and procure the necessary licenses and subscriptions for the diverse range of services and products we offer. Your support and commitment enable us to maintain and enhance the quality of our offerings, contributing to a thriving collaborative environment. --------------------------------------------------------------------------------------------------------- +🚀 For the latest versions and updates, visit our organization: [Robotics-Sensors](https://github.com/Robotics-Sensors). -### Maintainer -* Ronaldson Bellande +### 🧑‍💼 Maintainer -## License -This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0), see [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE) for more information. +Meet our dedicated maintainer, **Ronaldson Bellande**. + +--- + +## 📄 License + +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE). From 1978a09574f165aa1668936683591563d4396cfc Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 8 Feb 2024 02:23:50 -0500 Subject: [PATCH 31/59] Update README.md --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 2e84110..c750338 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,10 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. +## 💻 Functionality To Switch from ROS to ROS2 Checkout The Below Repository + +[![Toggle ROS Versions](https://img.shields.io/badge/Toggle%20ROS%20Versions-Explore%20ROS%20and%20ROS2%20migration-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ros_extension/) + ### 🚀 Key Repository Stats - ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Starred by the community From ba1bfc2d0d0322c5f5f214b6c14367a16b7780f9 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Mon, 29 Apr 2024 15:55:06 -0400 Subject: [PATCH 32/59] latest pushes --- .../package.xml | 28 +++++++++++-- .../package.xml | 40 +++++++++++++++---- 2 files changed, 56 insertions(+), 12 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_bringup/package.xml b/humanoid_robot_intelligence_control_system_bringup/package.xml index 2d44949..a145af4 100644 --- a/humanoid_robot_intelligence_control_system_bringup/package.xml +++ b/humanoid_robot_intelligence_control_system_bringup/package.xml @@ -1,3 +1,19 @@ + + humanoid_robot_intelligence_control_system_bringup @@ -19,8 +35,10 @@ robot_state_publisher rviz - humanoid_robot_intelligence_control_system_manager - humanoid_robot_intelligence_control_system_description + + humanoid_robot_intelligence_control_system_manager + + humanoid_robot_intelligence_control_system_description usb_cam joint_state_publisher robot_state_publisher @@ -43,8 +61,10 @@ robot_state_publisher rviz - humanoid_robot_intelligence_control_system_manager - humanoid_robot_intelligence_control_system_description + + humanoid_robot_intelligence_control_system_manager + + humanoid_robot_intelligence_control_system_description usb_cam joint_state_publisher robot_state_publisher diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/package.xml b/humanoid_robot_intelligence_control_system_read_write_demo/package.xml index 0efd54a..5092833 100644 --- a/humanoid_robot_intelligence_control_system_read_write_demo/package.xml +++ b/humanoid_robot_intelligence_control_system_read_write_demo/package.xml @@ -1,3 +1,19 @@ + + humanoid_robot_intelligence_control_system_read_write_demo @@ -14,19 +30,23 @@ roscpp std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs humanoid_robot_intelligence_control_system_manager roscpp std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs - humanoid_robot_intelligence_control_system_manager + + humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_manager roscpp std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs humanoid_robot_intelligence_control_system_manager @@ -35,19 +55,23 @@ roscpp std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs humanoid_robot_intelligence_control_system_manager roscpp std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs - humanoid_robot_intelligence_control_system_manager + + humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_manager roscpp std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs humanoid_robot_intelligence_control_system_manager From 3e8509c8f8d37967e30ee8a9883c2418a923e371 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Mon, 29 Apr 2024 16:04:57 -0400 Subject: [PATCH 33/59] latest pushes --- .../CMakeLists.txt | 14 ++++++++++++++ .../CMakeLists.txt | 14 ++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/humanoid_robot_intelligence_control_system_bringup/CMakeLists.txt b/humanoid_robot_intelligence_control_system_bringup/CMakeLists.txt index b0f7a88..3e75bd5 100644 --- a/humanoid_robot_intelligence_control_system_bringup/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_bringup/CMakeLists.txt @@ -1,3 +1,17 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + cmake_minimum_required(VERSION 3.8) project(humanoid_robot_intelligence_control_system_bringup) diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt b/humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt index da225ca..5daccda 100644 --- a/humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt @@ -1,3 +1,17 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + cmake_minimum_required(VERSION 3.8) project(humanoid_robot_intelligence_control_system_read_write_demo) From a8e84ace18a95120886c39f9fe82100c0ddd72ca Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 7 May 2024 00:55:55 -0400 Subject: [PATCH 34/59] latest pushes --- .../package.xml | 4 ++-- .../package.xml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_bringup/package.xml b/humanoid_robot_intelligence_control_system_bringup/package.xml index a145af4..e652f1b 100644 --- a/humanoid_robot_intelligence_control_system_bringup/package.xml +++ b/humanoid_robot_intelligence_control_system_bringup/package.xml @@ -1,5 +1,6 @@ + - humanoid_robot_intelligence_control_system_bringup 0.3.2 diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/package.xml b/humanoid_robot_intelligence_control_system_read_write_demo/package.xml index 5092833..3bcd921 100644 --- a/humanoid_robot_intelligence_control_system_read_write_demo/package.xml +++ b/humanoid_robot_intelligence_control_system_read_write_demo/package.xml @@ -1,5 +1,6 @@ + - humanoid_robot_intelligence_control_system_read_write_demo 0.3.2 From 60408edb4af5c748768e97034fc69ef2359f9faf Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 7 May 2024 17:13:55 -0400 Subject: [PATCH 35/59] Update README.md --- README.md | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index c750338..8126025 100644 --- a/README.md +++ b/README.md @@ -8,10 +8,18 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. +## 📢 Work with Bellande Algorithms and Models through Bellande's API! + +[![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20API-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/Web-ROS-API) + +--- + ## 💻 Functionality To Switch from ROS to ROS2 Checkout The Below Repository [![Toggle ROS Versions](https://img.shields.io/badge/Toggle%20ROS%20Versions-Explore%20ROS%20and%20ROS2%20migration-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ros_extension/) +--- + ### 🚀 Key Repository Stats - ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Starred by the community @@ -56,7 +64,7 @@ The repository has recently undergone significant updates. Older commits and cod ### 🧑‍💼 Maintainer -Meet our dedicated maintainer, **Ronaldson Bellande**. +Meet our dedicated author & maintainer, **Ronaldson Bellande**. --- From 57e1e86dee63701e5f803f54746baaeffa5e580d Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 8 May 2024 23:44:59 -0400 Subject: [PATCH 36/59] Update README.md --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index 8126025..754cb24 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,12 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos reposit --- +## 🧑‍💼 Work with Bellande Models through Bellande Framework! + +[![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Models%20through%20Bellande's%20Framework-Bellande%20Framework-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ROS-MODELS) + +--- + ## 💻 Functionality To Switch from ROS to ROS2 Checkout The Below Repository [![Toggle ROS Versions](https://img.shields.io/badge/Toggle%20ROS%20Versions-Explore%20ROS%20and%20ROS2%20migration-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ros_extension/) From 02636bb472307d8f070e25743caa793d04ef5afa Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 4 Jun 2024 21:39:56 -0400 Subject: [PATCH 37/59] Update README.md --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index 754cb24..a5487a1 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,11 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. +## 🔄 Run and Experiment with the build Bellande's Humanoid Robot + +[![Bellande's Humanoid](https://img.shields.io/badge/Bellande's-Humanoid%20Robot%20Package-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/bellande_humanoid_robotics_package) + + ## 📢 Work with Bellande Algorithms and Models through Bellande's API! [![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20API-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/Web-ROS-API) From 00f77b6452c9c4f02802815f3259198bbdc8ca76 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 11 Jun 2024 01:09:33 -0400 Subject: [PATCH 38/59] latest pushes --- README.md | 21 +-------------------- 1 file changed, 1 insertion(+), 20 deletions(-) diff --git a/README.md b/README.md index a5487a1..c750338 100644 --- a/README.md +++ b/README.md @@ -8,29 +8,10 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. -## 🔄 Run and Experiment with the build Bellande's Humanoid Robot - -[![Bellande's Humanoid](https://img.shields.io/badge/Bellande's-Humanoid%20Robot%20Package-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/bellande_humanoid_robotics_package) - - -## 📢 Work with Bellande Algorithms and Models through Bellande's API! - -[![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20API-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/Web-ROS-API) - ---- - -## 🧑‍💼 Work with Bellande Models through Bellande Framework! - -[![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Models%20through%20Bellande's%20Framework-Bellande%20Framework-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ROS-MODELS) - ---- - ## 💻 Functionality To Switch from ROS to ROS2 Checkout The Below Repository [![Toggle ROS Versions](https://img.shields.io/badge/Toggle%20ROS%20Versions-Explore%20ROS%20and%20ROS2%20migration-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ros_extension/) ---- - ### 🚀 Key Repository Stats - ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Starred by the community @@ -75,7 +56,7 @@ The repository has recently undergone significant updates. Older commits and cod ### 🧑‍💼 Maintainer -Meet our dedicated author & maintainer, **Ronaldson Bellande**. +Meet our dedicated maintainer, **Ronaldson Bellande**. --- From 1262a6438f9cba8cc4de558900bf59cc79fcc5e5 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 12 Jun 2024 00:07:49 -0400 Subject: [PATCH 39/59] Update README.md Signed-off-by: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index c750338..3ee5208 100644 --- a/README.md +++ b/README.md @@ -29,20 +29,20 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos reposit ## 🌐 Repository Website -Explore our [repository website](https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_demos) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. +Explore our [repository website](https://robotics-sensors.github.io/bellande_humanoid_robot_intelligence_control_system_demos) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. --- ### 🔄 Updates and Versions -- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos) +- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_demos) - **Old Version/Previous Used for Different Context:** [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) --- # 🎉 Latest Release -[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/releases/) +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_demos/releases/) --- From 24942e39cbb5a13d68d6f8d6adf77d3604294d80 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Sun, 16 Jun 2024 14:33:55 -0400 Subject: [PATCH 40/59] latest pushes --- .../CMakeLists.txt | 54 +++--- .../package.xml | 12 +- .../setup.py | 11 ++ .../src/read_write.py | 168 ++++++++++++++++++ 4 files changed, 214 insertions(+), 31 deletions(-) create mode 100644 humanoid_robot_intelligence_control_system_read_write_demo/setup.py create mode 100644 humanoid_robot_intelligence_control_system_read_write_demo/src/read_write.py diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt b/humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt index 5daccda..582d2fa 100644 --- a/humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt @@ -25,7 +25,13 @@ if($ENV{ROS_VERSION} EQUAL 1) std_msgs ) else() - find_package(ament_cmake REQUIRED) + find_package( + ament_cmake REQUIRED COMPONENTS + humanoid_robot_intelligence_control_system_controller_msgs + roscpp + sensor_msgs + std_msgs + ) endif() @@ -41,31 +47,23 @@ if($ENV{ROS_VERSION} EQUAL 1) ) endif() +# Install Python scripts for both ROS 1 +if($ENV{ROS_VERSION} EQUAL 1) + catkin_install_python( + PROGRAMS + src/read_write.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) +endif() -include_directories(${catkin_INCLUDE_DIRS}) - -add_executable( - read_write - src/read_write.cpp -) - -add_dependencies( - read_write - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) - -target_link_libraries( - read_write - ${catkin_LIBRARIES} -) - -install( - TARGETS read_write - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install( - DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +# Install Python scripts, configuration files, and launch files +if($ENV{ROS_VERSION} EQUAL "1") + install(PROGRAMS src/read_write.py DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) + install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) +else() + install(PROGRAMS src/read_write.py DESTINATION lib/${PROJECT_NAME}) + install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config) + install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch) + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/package.xml b/humanoid_robot_intelligence_control_system_read_write_demo/package.xml index 3bcd921..77cae1d 100644 --- a/humanoid_robot_intelligence_control_system_read_write_demo/package.xml +++ b/humanoid_robot_intelligence_control_system_read_write_demo/package.xml @@ -28,6 +28,7 @@ the License. catkin roscpp + rospy std_msgs sensor_msgs @@ -35,6 +36,7 @@ the License. humanoid_robot_intelligence_control_system_manager roscpp + rospy std_msgs sensor_msgs @@ -43,6 +45,7 @@ the License. humanoid_robot_intelligence_control_system_manager roscpp + rospy std_msgs sensor_msgs @@ -52,14 +55,16 @@ the License. ament_cmake - roscpp + rclcpp + rclpy std_msgs sensor_msgs humanoid_robot_intelligence_control_system_controller_msgs humanoid_robot_intelligence_control_system_manager - roscpp + rclcpp + rclpy std_msgs sensor_msgs @@ -67,7 +72,8 @@ the License. humanoid_robot_intelligence_control_system_manager - roscpp + rclcpp + rclpy std_msgs sensor_msgs diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/setup.py b/humanoid_robot_intelligence_control_system_read_write_demo/setup.py new file mode 100644 index 0000000..723c5aa --- /dev/null +++ b/humanoid_robot_intelligence_control_system_read_write_demo/setup.py @@ -0,0 +1,11 @@ +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/read_write.py'], + packages=['humanoid_robot_intelligence_control_system_read_write_demo'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/src/read_write.py b/humanoid_robot_intelligence_control_system_read_write_demo/src/read_write.py new file mode 100644 index 0000000..808805f --- /dev/null +++ b/humanoid_robot_intelligence_control_system_read_write_demo/src/read_write.py @@ -0,0 +1,168 @@ +#!/usr/bin/env python +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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 +from sensor_msgs.msg import JointState +from std_msgs.msg import String +from humanoid_robot_intelligence_control_system_controller_msgs.srv import SetModule +from humanoid_robot_intelligence_control_system_controller_msgs.msg import SyncWriteItem + +def button_handler_callback(msg): + global control_module + if msg.data == "mode": + control_module = ControlModule.Framework + rospy.loginfo("Button : mode | Framework") + ready_to_demo() + elif msg.data == "start": + control_module = ControlModule.DirectControlModule + rospy.loginfo("Button : start | Direct control module") + ready_to_demo() + elif msg.data == "user": + torque_on_all() + control_module = ControlModule.None + +def joint_states_callback(msg): + if control_module == ControlModule.None: + return + + write_msg = JointState() + write_msg.header = msg.header + + for ix in range(len(msg.name)): + joint_name = msg.name[ix] + joint_position = msg.position[ix] + + if joint_name == "r_sho_pitch": + write_msg.name.append("r_sho_pitch") + write_msg.position.append(joint_position) + write_msg.name.append("l_sho_pitch") + write_msg.position.append(-joint_position) + if joint_name == "r_sho_roll": + write_msg.name.append("r_sho_roll") + write_msg.position.append(joint_position) + write_msg.name.append("l_sho_roll") + write_msg.position.append(-joint_position) + if joint_name == "r_el": + write_msg.name.append("r_el") + write_msg.position.append(joint_position) + write_msg.name.append("l_el") + write_msg.position.append(-joint_position) + + if control_module == ControlModule.Framework: + write_joint_pub.publish(write_msg) + elif control_module == ControlModule.DirectControlModule: + write_joint_pub2.publish(write_msg) + +def ready_to_demo(): + rospy.loginfo("Start Read-Write Demo") + set_led(0x04) + torque_on_all() + rospy.loginfo("Torque on All joints") + go_init_pose() + rospy.loginfo("Go Init pose") + rospy.sleep(4.0) + set_led(control_module) + if control_module == ControlModule.Framework: + set_module("none") + rospy.loginfo("Change module to none") + elif control_module == ControlModule.DirectControlModule: + set_module("direct_control_module") + rospy.loginfo("Change module to direct_control_module") + else: + return + torque_off("right") + rospy.loginfo("Torque off") + +def go_init_pose(): + init_msg = String() + init_msg.data = "ini_pose" + init_pose_pub.publish(init_msg) + +def set_led(led): + syncwrite_msg = SyncWriteItem() + syncwrite_msg.item_name = "LED" + syncwrite_msg.joint_name.append("open-cr") + syncwrite_msg.value.append(led) + sync_write_pub.publish(syncwrite_msg) + +def check_manager_running(manager_name): + node_list = rospy.get_published_topics() + for node in node_list: + if node[0] == manager_name: + return True + rospy.logerr("Can't find humanoid_robot_intelligence_control_system_manager") + return False + +def set_module(module_name): + try: + set_module_srv = rospy.ServiceProxy('/humanoid_robot_intelligence_control_system/set_present_ctrl_modules', SetModule) + set_module_srv(module_name) + except rospy.ServiceException as e: + rospy.logerr("Failed to set module: %s" % e) + +def torque_on_all(): + check_msg = String() + check_msg.data = "check" + dxl_torque_pub.publish(check_msg) + +def torque_off(body_side): + syncwrite_msg = SyncWriteItem() + torque_value = 0 + syncwrite_msg.item_name = "torque_enable" + if body_side == "right": + syncwrite_msg.joint_name.extend(["r_sho_pitch", "r_sho_roll", "r_el"]) + syncwrite_msg.value.extend([torque_value] * 3) + elif body_side == "left": + syncwrite_msg.joint_name.extend(["l_sho_pitch", "l_sho_roll", "l_el"]) + syncwrite_msg.value.extend([torque_value] * 3) + sync_write_pub.publish(syncwrite_msg) + +class ControlModule: + None = 0 + DirectControlModule = 1 + Framework = 2 + +if __name__ == '__main__': + rospy.init_node('read_write', anonymous=True) + + init_pose_pub = rospy.Publisher('/humanoid_robot_intelligence_control_system/base/ini_pose', String, queue_size=10) + sync_write_pub = rospy.Publisher('/humanoid_robot_intelligence_control_system/sync_write_item', SyncWriteItem, queue_size=10) + dxl_torque_pub = rospy.Publisher('/humanoid_robot_intelligence_control_system/dxl_torque', String, queue_size=10) + write_joint_pub = rospy.Publisher('/humanoid_robot_intelligence_control_system/set_joint_states', JointState, queue_size=10) + write_joint_pub2 = rospy.Publisher('/humanoid_robot_intelligence_control_system/direct_control/set_joint_states', JointState, queue_size=10) + + read_joint_sub = rospy.Subscriber('/humanoid_robot_intelligence_control_system/present_joint_states', JointState, joint_states_callback) + button_sub = rospy.Subscriber('/humanoid_robot_intelligence_control_system/open_cr/button', String, button_handler_callback) + + control_module = ControlModule.None + demo_ready = False + SPIN_RATE = 30 + DEBUG_PRINT = False + + loop_rate = rospy.Rate(SPIN_RATE) + + manager_name = "/humanoid_robot_intelligence_control_system_manager" + while not rospy.is_shutdown(): + rospy.sleep(1.0) + if check_manager_running(manager_name): + rospy.loginfo("Succeed to connect") + break + rospy.logwarn("Waiting for humanoid_robot_intelligence_control_system manager") + + ready_to_demo() + + while not rospy.is_shutdown(): + rospy.spin() + loop_rate.sleep() From 71e197d3533e8e93db69c446d1521df506962e15 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Mon, 22 Jul 2024 16:58:01 -0400 Subject: [PATCH 41/59] latest pushes --- .github/workflows/clone.yml | 90 +++++++++++++ git_scripts/.gitignore | 3 + ...elligence_control_system_bringup.launch.py | 76 +++++++++++ ...rol_system_bringup_visualization.launch.py | 85 ++++++++++++ ...intelligence_control_system_bringup.launch | 15 +++ ...ontrol_system_bringup_visualization.launch | 22 ++++ ...igence_control_system_read_write.launch.py | 121 ++++++++++++++++++ ...elligence_control_system_read_write.launch | 34 +++++ .../setup.py | 15 +++ 9 files changed, 461 insertions(+) create mode 100644 .github/workflows/clone.yml create mode 100644 git_scripts/.gitignore create mode 100644 humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup.launch.py create mode 100644 humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup_visualization.launch.py create mode 100644 humanoid_robot_intelligence_control_system_bringup/launch/ros1/humanoid_robot_intelligence_control_system_bringup.launch create mode 100644 humanoid_robot_intelligence_control_system_bringup/launch/ros1/humanoid_robot_intelligence_control_system_bringup_visualization.launch create mode 100644 humanoid_robot_intelligence_control_system_read_write_demo/launch/humanoid_robot_intelligence_control_system_read_write.launch.py create mode 100644 humanoid_robot_intelligence_control_system_read_write_demo/launch/ros1/humanoid_robot_intelligence_control_system_read_write.launch diff --git a/.github/workflows/clone.yml b/.github/workflows/clone.yml new file mode 100644 index 0000000..074054b --- /dev/null +++ b/.github/workflows/clone.yml @@ -0,0 +1,90 @@ +name: GitHub Clone Count Update Everyday + +on: + schedule: + - cron: "0 */24 * * *" + workflow_dispatch: + +jobs: + build: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v2 + + - name: gh login + run: echo "${{ secrets.SECRET_TOKEN }}" | gh auth login --with-token + + - name: parse latest clone count + run: | + curl --user "${{ github.actor }}:${{ secrets.SECRET_TOKEN }}" \ + -H "Accept: application/vnd.github.v3+json" \ + https://api.github.com/repos/${{ github.repository }}/traffic/clones \ + > clone.json + + - name: create gist and download previous count + id: set_id + run: | + if gh secret list | grep -q "GIST_ID" + then + echo "GIST_ID found" + echo "GIST=${{ secrets.GIST_ID }}" >> $GITHUB_OUTPUT + curl https://gist.githubusercontent.com/${{ github.actor }}/${{ secrets.GIST_ID }}/raw/clone.json > clone_before.json + if cat clone_before.json | grep '404: Not Found'; then + echo "GIST_ID not valid anymore. Creating another gist..." + gist_id=$(gh gist create clone.json | awk -F / '{print $NF}') + echo $gist_id | gh secret set GIST_ID + echo "GIST=$gist_id" >> $GITHUB_OUTPUT + cp clone.json clone_before.json + git rm --ignore-unmatch CLONE.md + fi + else + echo "GIST_ID not found. Creating a gist..." + gist_id=$(gh gist create clone.json | awk -F / '{print $NF}') + echo $gist_id | gh secret set GIST_ID + echo "GIST=$gist_id" >> $GITHUB_OUTPUT + cp clone.json clone_before.json + fi + + - name: update clone.json + run: | + curl https://raw.githubusercontent.com/MShawon/github-clone-count-badge/master/main.py > main.py + python3 main.py + + - name: Update gist with latest count + run: | + content=$(sed -e 's/\\/\\\\/g' -e 's/\t/\\t/g' -e 's/\"/\\"/g' -e 's/\r//g' "clone.json" | sed -E ':a;N;$!ba;s/\r{0,1}\n/\\n/g') + echo '{"description": "${{ github.repository }} clone statistics", "files": {"clone.json": {"content": "'"$content"'"}}}' > post_clone.json + curl -s -X PATCH \ + --user "${{ github.actor }}:${{ secrets.SECRET_TOKEN }}" \ + -H "Content-Type: application/json" \ + -d @post_clone.json https://api.github.com/gists/${{ steps.set_id.outputs.GIST }} > /dev/null 2>&1 + + if [ ! -f CLONE.md ]; then + shields="https://img.shields.io/badge/dynamic/json?color=success&label=Clone&query=count&url=" + url="https://gist.githubusercontent.com/${{ github.actor }}/${{ steps.set_id.outputs.GIST }}/raw/clone.json" + repo="https://github.com/MShawon/github-clone-count-badge" + echo ''> CLONE.md + echo ' + **Markdown** + + ```markdown' >> CLONE.md + echo "[![GitHub Clones]($shields$url&logo=github)]($repo)" >> CLONE.md + echo ' + ``` + + **HTML** + ```html' >> CLONE.md + echo "GitHub Clones" >> CLONE.md + echo '```' >> CLONE.md + + git add CLONE.md + git config --global user.name "GitHub Action" + git config --global user.email "action@github.com" + git commit -m "create clone count badge" + fi + + - name: Push + uses: ad-m/github-push-action@master + with: + github_token: ${{ secrets.GITHUB_TOKEN }} diff --git a/git_scripts/.gitignore b/git_scripts/.gitignore new file mode 100644 index 0000000..e5a7a9c --- /dev/null +++ b/git_scripts/.gitignore @@ -0,0 +1,3 @@ +fix_errors.sh +push.sh +repository_recal.sh diff --git a/humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup.launch.py b/humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup.launch.py new file mode 100644 index 0000000..9b89968 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup.launch.py @@ -0,0 +1,76 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch commandi + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_bringup", "humanoid_robot_intelligence_control_system_bringup.launch"] + args + + # Execute the launch command + subprocess.call(roslaunch_command) + + +def ros2_launch_description(): + # Create a list to hold all nodes to be launched + nodes_to_launch = [] + + # Add the HUMANOID_ROBOT Manager launch file + nodes_to_launch.append(IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + '$(find humanoid_robot_intelligence_control_system_manager)/launch/', + 'humanoid_robot_intelligence_control_system_manager.launch.py' + ]) + )) + + # Add the UVC camera node + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='usb_cam_node', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 1280, + 'image_height': 720, + 'framerate': 30, + 'camera_frame_id': 'cam_link', + 'camera_name': 'camera' + }] + )) + + # Return the LaunchDescription containing all nodes + return LaunchDescription(nodes_to_launch) + + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup_visualization.launch.py b/humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup_visualization.launch.py new file mode 100644 index 0000000..ac4898f --- /dev/null +++ b/humanoid_robot_intelligence_control_system_bringup/launch/humanoid_robot_intelligence_control_system_bringup_visualization.launch.py @@ -0,0 +1,85 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription, ExecuteProcess +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import FindExecutable, Command + + + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_bringup", "humanoid_robot_intelligence_control_system_bringup_visualization.launch"] + args + + # Execute the launch command + subprocess.call(roslaunch_command) + + +def ros2_launch_description(): + # Create a list to hold all nodes to be launched + nodes_to_launch = [] + + # Add robot description + nodes_to_launch.append(ExecuteProcess( + cmd=[FindExecutable(name='xacro'), '$(find humanoid_robot_intelligence_control_system_description)/urdf/humanoid_robot_intelligence_control_system.urdf.xacro'], + output='screen' + )) + + # Add joint state publisher + nodes_to_launch.append(Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[{'use_gui': True}], + remappings=[('/robot/joint_states', '/humanoid_robot_intelligence_control_system/present_joint_states')] + )) + + # Add robot state publisher + nodes_to_launch.append(Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + remappings=[('/joint_states', '/humanoid_robot_intelligence_control_system/present_joint_states')] + )) + + # Add rviz + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', '$(find humanoid_robot_intelligence_control_system_bringup)/rviz/humanoid_robot_intelligence_control_system_bringup.rviz'] + )) + + # Return the LaunchDescription containing all nodes + return LaunchDescription(nodes_to_launch) + + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_bringup/launch/ros1/humanoid_robot_intelligence_control_system_bringup.launch b/humanoid_robot_intelligence_control_system_bringup/launch/ros1/humanoid_robot_intelligence_control_system_bringup.launch new file mode 100644 index 0000000..d9f4f02 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_bringup/launch/ros1/humanoid_robot_intelligence_control_system_bringup.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_bringup/launch/ros1/humanoid_robot_intelligence_control_system_bringup_visualization.launch b/humanoid_robot_intelligence_control_system_bringup/launch/ros1/humanoid_robot_intelligence_control_system_bringup_visualization.launch new file mode 100644 index 0000000..7c8a537 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_bringup/launch/ros1/humanoid_robot_intelligence_control_system_bringup_visualization.launch @@ -0,0 +1,22 @@ + + + + + + + + + [/humanoid_robot_intelligence_control_system/present_joint_states] + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/launch/humanoid_robot_intelligence_control_system_read_write.launch.py b/humanoid_robot_intelligence_control_system_read_write_demo/launch/humanoid_robot_intelligence_control_system_read_write.launch.py new file mode 100644 index 0000000..dbdd3ba --- /dev/null +++ b/humanoid_robot_intelligence_control_system_read_write_demo/launch/humanoid_robot_intelligence_control_system_read_write.launch.py @@ -0,0 +1,121 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_read_write_demo", "humanoid_robot_intelligence_control_system_read_write.launch"] + args + + # Add parameters + roslaunch_command.extend([ + "gazebo:=false", + "gazebo_robot_name:=humanoid_robot_intelligence_control_system", + "offset_file_path:=$(find humanoid_robot_intelligence_control_system_manager)/config/offset.yaml", + "robot_file_path:=$(find humanoid_robot_intelligence_control_system_manager)/config/HUMANOID_ROBOT.robot", + "init_file_path:=$(find humanoid_robot_intelligence_control_system_manager)/config/dxl_init_HUMANOID_ROBOT.yaml", + "device_name:=/dev/ttyUSB0", + "/humanoid_robot_intelligence_control_system/direct_control/default_moving_time:=0.04", + "/humanoid_robot_intelligence_control_system/direct_control/default_moving_angle:=90" + ]) + + # Add nodes + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_manager", "humanoid_robot_intelligence_control_system_manager", "angle_unit:=30", + "humanoid_robot_intelligence_control_system_localization", "humanoid_robot_intelligence_control_system_localization", + "humanoid_robot_intelligence_control_system_read_write_demo", "read_write" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + + +def ros2_launch_description(): + # Declare launch arguments + gazebo_arg = DeclareLaunchArgument('gazebo', default_value='false') + gazebo_robot_name_arg = DeclareLaunchArgument('gazebo_robot_name', default_value='humanoid_robot_intelligence_control_system') + offset_file_path_arg = DeclareLaunchArgument('offset_file_path', default_value='$(find humanoid_robot_intelligence_control_system_manager)/config/offset.yaml') + robot_file_path_arg = DeclareLaunchArgument('robot_file_path', default_value='$(find humanoid_robot_intelligence_control_system_manager)/config/HUMANOID_ROBOT.robot') + init_file_path_arg = DeclareLaunchArgument('init_file_path', default_value='$(find humanoid_robot_intelligence_control_system_manager)/config/dxl_init_HUMANOID_ROBOT.yaml') + device_name_arg = DeclareLaunchArgument('device_name', default_value='/dev/ttyUSB0') + + # Create a list to hold all nodes to be launched + nodes_to_launch = [] + + # Add HUMANOID_ROBOT Manager node + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_manager', + executable='humanoid_robot_intelligence_control_system_manager', + name='humanoid_robot_intelligence_control_system_manager', + output='screen', + parameters=[ + {'angle_unit': 30}, + {'gazebo': LaunchConfiguration('gazebo')}, + {'gazebo_robot_name': LaunchConfiguration('gazebo_robot_name')}, + {'offset_file_path': LaunchConfiguration('offset_file_path')}, + {'robot_file_path': LaunchConfiguration('robot_file_path')}, + {'init_file_path': LaunchConfiguration('init_file_path')}, + {'device_name': LaunchConfiguration('device_name')}, + {'/humanoid_robot_intelligence_control_system/direct_control/default_moving_time': 0.04}, + {'/humanoid_robot_intelligence_control_system/direct_control/default_moving_angle': 90} + ] + )) + + # Add HUMANOID_ROBOT Localization node + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_localization', + executable='humanoid_robot_intelligence_control_system_localization', + name='humanoid_robot_intelligence_control_system_localization', + output='screen' + )) + + # Add HUMANOID_ROBOT Read-Write demo node + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_read_write_demo', + executable='read_write', + name='humanoid_robot_intelligence_control_system_read_write', + output='screen' + )) + + # Return the LaunchDescription containing all nodes and arguments + return LaunchDescription([ + gazebo_arg, + gazebo_robot_name_arg, + offset_file_path_arg, + robot_file_path_arg, + init_file_path_arg, + device_name_arg + ] + nodes_to_launch) + + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/launch/ros1/humanoid_robot_intelligence_control_system_read_write.launch b/humanoid_robot_intelligence_control_system_read_write_demo/launch/ros1/humanoid_robot_intelligence_control_system_read_write.launch new file mode 100644 index 0000000..8bf520d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_read_write_demo/launch/ros1/humanoid_robot_intelligence_control_system_read_write.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/setup.py b/humanoid_robot_intelligence_control_system_read_write_demo/setup.py index 723c5aa..317c74c 100644 --- a/humanoid_robot_intelligence_control_system_read_write_demo/setup.py +++ b/humanoid_robot_intelligence_control_system_read_write_demo/setup.py @@ -1,3 +1,18 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup From b73e3a23aec19876a131dfe8a6b22f87bc96dd32 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Mon, 22 Jul 2024 20:46:28 -0400 Subject: [PATCH 42/59] latest pushes --- .../CMakeLists.txt | 71 +++++++++++ .../launch/face_detector_processor.launch.py | 108 +++++++++++++++++ .../ros1/face_detector_processor.launch | 41 +++++++ .../package.xml | 61 ++++++++++ .../setup.py | 26 ++++ .../src/face_detection_processor.py | 111 ++++++++++++++++++ 6 files changed, 418 insertions(+) create mode 100644 humanoid_robot_intelligence_control_system_face_detector/CMakeLists.txt create mode 100644 humanoid_robot_intelligence_control_system_face_detector/launch/face_detector_processor.launch.py create mode 100644 humanoid_robot_intelligence_control_system_face_detector/launch/ros1/face_detector_processor.launch create mode 100644 humanoid_robot_intelligence_control_system_face_detector/package.xml create mode 100644 humanoid_robot_intelligence_control_system_face_detector/setup.py create mode 100644 humanoid_robot_intelligence_control_system_face_detector/src/face_detection_processor.py diff --git a/humanoid_robot_intelligence_control_system_face_detector/CMakeLists.txt b/humanoid_robot_intelligence_control_system_face_detector/CMakeLists.txt new file mode 100644 index 0000000..2eb131f --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/CMakeLists.txt @@ -0,0 +1,71 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_face_detector) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + + catkin_package( + CATKIN_DEPENDS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(cv_bridge REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/face_detection_processor.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/face_detection_processor.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_face_detector/launch/face_detector_processor.launch.py b/humanoid_robot_intelligence_control_system_face_detector/launch/face_detector_processor.launch.py new file mode 100644 index 0000000..4a6456b --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/launch/face_detector_processor.launch.py @@ -0,0 +1,108 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_face_detector", "face_detector_processor.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_face_detection.py", "name:=face_detection_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_ball_detector", "face_detection_processor.py", "name:=face_detection_processor_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='ros_web_api_bellande_2d_computer_vision', + executable='bellande_2d_computer_vision_face_detection.py', + name='face_detection_node', + output='screen', + remappings=[('camera/image_raw', '/usb_cam/image_raw')] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_face_detector', + executable='face_detection_processor.py', + name='face_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/face_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_face_detector/launch/ros1/face_detector_processor.launch b/humanoid_robot_intelligence_control_system_face_detector/launch/ros1/face_detector_processor.launch new file mode 100644 index 0000000..6c4507d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/launch/ros1/face_detector_processor.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_face_detector/package.xml b/humanoid_robot_intelligence_control_system_face_detector/package.xml new file mode 100644 index 0000000..9e60a19 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/package.xml @@ -0,0 +1,61 @@ + + + + + humanoid_robot_intelligence_control_system_face_detector + 0.0.1 + + This Package is for Object Detection, detecting faces like tools, or utilities + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + dynamic_reconfigure + message_generation + message_runtime + + + rclpy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + rosidl_default_generators + rosidl_default_runtime + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_face_detector/setup.py b/humanoid_robot_intelligence_control_system_face_detector/setup.py new file mode 100644 index 0000000..f391975 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/face_detection_processor.py'], + packages=['humanoid_robot_intelligence_control_system_face_detector'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_face_detector/src/face_detection_processor.py b/humanoid_robot_intelligence_control_system_face_detector/src/face_detection_processor.py new file mode 100644 index 0000000..5484d2d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/src/face_detection_processor.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import rospy +import cv2 +import numpy as np +from sensor_msgs.msg import Image, CameraInfo +from std_msgs.msg import Bool, String +from cv_bridge import CvBridge +import yaml + +class ObjectDetectionProcessor: + def __init__(self): + rospy.init_node('face_detection_processor') + self.bridge = CvBridge() + self.enable = True + self.new_image_flag = False + self.load_params() + self.setup_ros() + + def load_params(self): + param_path = rospy.get_param('~yaml_path', '') + if param_path: + with open(param_path, 'r') as file: + self.params = yaml.safe_load(file) + else: + self.set_default_params() + + def set_default_params(self): + self.params = { + 'debug': False, + 'ellipse_size': 2, + # Add other default parameters as needed + } + + def setup_ros(self): + self.image_pub = rospy.Publisher('image_out', Image, queue_size=10) + self.camera_info_pub = rospy.Publisher('camera_info', CameraInfo, queue_size=10) + + rospy.Subscriber('enable', Bool, self.enable_callback) + rospy.Subscriber('image_in', Image, self.image_callback) + rospy.Subscriber('cameraInfo_in', CameraInfo, self.camera_info_callback) + rospy.Subscriber('face_detection_result', String, self.face_detection_callback) + + def enable_callback(self, msg): + self.enable = msg.data + + def image_callback(self, msg): + if not self.enable: + return + self.cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8") + self.new_image_flag = True + self.image_header = msg.header + + def camera_info_callback(self, msg): + if not self.enable: + return + self.camera_info_msg = msg + + def face_detection_callback(self, msg): + if not self.enable or not hasattr(self, 'cv_image'): + return + + faces = eval(msg.data) # Assuming the data is a string representation of a list + self.process_detected_faces(faces) + + def process_detected_faces(self, faces): + output_image = self.cv_image.copy() + for obj in faces: + x, y, w, h = obj['bbox'] + cv2.rectangle(output_image, (int(x), int(y)), (int(x+w), int(y+h)), (0, 255, 0), 2) + cv2.putText(output_image, f"{obj['label']}: {obj['confidence']:.2f}", + (int(x), int(y-10)), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2) + + self.publish_image(output_image) + + def publish_image(self, image): + img_msg = self.bridge.cv2_to_imgmsg(image, encoding="bgr8") + img_msg.header = self.image_header + self.image_pub.publish(img_msg) + if hasattr(self, 'camera_info_msg'): + self.camera_info_pub.publish(self.camera_info_msg) + + def run(self): + rate = rospy.Rate(30) # 30 Hz + while not rospy.is_shutdown(): + if self.new_image_flag: + # The processing is done in face_detection_callback + self.new_image_flag = False + rate.sleep() + +if __name__ == '__main__': + try: + processor = ObjectDetectionProcessor() + processor.run() + except rospy.ROSInterruptException: + pass From 8fb6ddc86e6a10a32c6217f847deefba7492c0a5 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 23 Jul 2024 01:28:20 -0400 Subject: [PATCH 43/59] latest pushes --- .../CMakeLists.txt | 77 +++++++ .../launch/face_follower.launch.py | 114 +++++++++ .../launch/face_tracker.launch.py | 112 +++++++++ .../launch/object_follower.launch.py | 108 +++++++++ .../launch/object_tracker.launch.py | 108 +++++++++ .../launch/ros1/face_follower.launch | 41 ++++ .../launch/ros1/face_tracker.launch | 41 ++++ .../launch/ros1/object_follower.launch | 41 ++++ .../launch/ros1/object_tracker.launch | 41 ++++ .../package.xml | 61 +++++ .../setup.py | 26 +++ .../src/face_follower.py | 173 ++++++++++++++ .../src/face_tracker.py | 179 ++++++++++++++ .../src/object_follower.py | 218 ++++++++++++++++++ .../src/object_tracker.py | 194 ++++++++++++++++ 15 files changed, 1534 insertions(+) create mode 100644 humanoid_robot_intelligence_control_system_configure/CMakeLists.txt create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/ros1/face_follower.launch create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/ros1/face_tracker.launch create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/ros1/object_follower.launch create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/ros1/object_tracker.launch create mode 100644 humanoid_robot_intelligence_control_system_configure/package.xml create mode 100644 humanoid_robot_intelligence_control_system_configure/setup.py create mode 100644 humanoid_robot_intelligence_control_system_configure/src/face_follower.py create mode 100644 humanoid_robot_intelligence_control_system_configure/src/face_tracker.py create mode 100644 humanoid_robot_intelligence_control_system_configure/src/object_follower.py create mode 100644 humanoid_robot_intelligence_control_system_configure/src/object_tracker.py diff --git a/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt b/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt new file mode 100644 index 0000000..0277ccd --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt @@ -0,0 +1,77 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_object_detector) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + + catkin_package( + CATKIN_DEPENDS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(cv_bridge REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/face_tracker.py + src/face_follower.py + src/object_tracker.py + src/object_follower.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/face_tracker.py + src/face_follower.py + src/object_tracker.py + src/object_follower.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py new file mode 100644 index 0000000..051f56a --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py @@ -0,0 +1,114 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +#!/usr/bin/env python3 + +#!/usr/bin/env python3 + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_object_detector", "object_follow.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_object_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_object_detector", "object_follower.py", "name:=object_follower_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find humanoid_robot_intelligence_control_system_object_detector)/rviz/object_follow_visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_detector', + executable='object_detection_processor.py', + name='object_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find humanoid_robot_intelligence_control_system_object_detector)/config/object_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_detector', + executable='object_follower.py', + name='object_follower_node', + output='screen', + parameters=[{ + 'p_gain': 0.4, + 'i_gain': 0.0, + 'd_gain': 0.0 + }] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find humanoid_robot_intelligence_control_system_object_detector)/rviz/object_follow_visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py new file mode 100644 index 0000000..91a9896 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py @@ -0,0 +1,112 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +#!/usr/bin/env python3 + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_tracker.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_object_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_configure", "object_tracker.py", "name:=object_tracker_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find humanoid_robot_intelligence_control_system_object_detector)/rviz/object_tracking_visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_detector', + executable='object_detection_processor.py', + name='object_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find humanoid_robot_intelligence_control_system_object_detector)/config/object_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_detector', + executable='object_tracker.py', + name='object_tracker_node', + output='screen', + parameters=[{ + 'p_gain': 0.4, + 'i_gain': 0.0, + 'd_gain': 0.0 + }] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find humanoid_robot_intelligence_control_system_object_detector)/rviz/object_tracking_visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py new file mode 100644 index 0000000..e61d87a --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py @@ -0,0 +1,108 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_object_detector", "object_detector_processor.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_object_detection.py", "name:=object_detection_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_ball_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='ros_web_api_bellande_2d_computer_vision', + executable='bellande_2d_computer_vision_object_detection.py', + name='object_detection_node', + output='screen', + remappings=[('camera/image_raw', '/usb_cam/image_raw')] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_detector', + executable='object_detection_processor.py', + name='object_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py new file mode 100644 index 0000000..e61d87a --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py @@ -0,0 +1,108 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_object_detector", "object_detector_processor.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_object_detection.py", "name:=object_detection_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_ball_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='ros_web_api_bellande_2d_computer_vision', + executable='bellande_2d_computer_vision_object_detection.py', + name='object_detection_node', + output='screen', + remappings=[('camera/image_raw', '/usb_cam/image_raw')] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_detector', + executable='object_detection_processor.py', + name='object_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_follower.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_follower.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_follower.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_tracker.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_tracker.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_tracker.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_follower.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_follower.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_follower.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_tracker.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_tracker.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_tracker.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/package.xml b/humanoid_robot_intelligence_control_system_configure/package.xml new file mode 100644 index 0000000..740f1f7 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/package.xml @@ -0,0 +1,61 @@ + + + + + humanoid_robot_intelligence_control_system_configure + 0.0.1 + + This Package is for Configuration of Tracker and Follower related to it + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + dynamic_reconfigure + message_generation + message_runtime + + + rclpy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + rosidl_default_generators + rosidl_default_runtime + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_configure/setup.py b/humanoid_robot_intelligence_control_system_configure/setup.py new file mode 100644 index 0000000..690d947 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/face_tracker.py', "src/face_follower.py", "src/object_tracker.py", "src/object_follower.py"], + packages=['humanoid_robot_intelligence_control_system_configure'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py new file mode 100644 index 0000000..0f11ade --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py @@ -0,0 +1,173 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import rospy +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String, Bool +from geometry_msgs.msg import Point +from humanoid_robot_intelligence_control_system_walking_module_msgs.msg import WalkingParam +from humanoid_robot_intelligence_control_system_walking_module_msgs.srv import GetWalkingParam + +class FaceFollower: + def __init__(self): + rospy.init_node('face_follower') + + self.FOV_WIDTH = 35.2 * math.pi / 180 + self.FOV_HEIGHT = 21.6 * math.pi / 180 + self.count_not_found = 0 + self.count_to_approach = 0 + self.on_following = False + self.approach_face_position = "NotFound" + self.CAMERA_HEIGHT = 0.46 + self.NOT_FOUND_THRESHOLD = 50 + self.MAX_FB_STEP = 40.0 * 0.001 + self.MAX_RL_TURN = 15.0 * math.pi / 180 + self.IN_PLACE_FB_STEP = -3.0 * 0.001 + self.MIN_FB_STEP = 5.0 * 0.001 + self.MIN_RL_TURN = 5.0 * math.pi / 180 + self.UNIT_FB_STEP = 1.0 * 0.001 + self.UNIT_RL_TURN = 0.5 * math.pi / 180 + self.SPOT_FB_OFFSET = 0.0 * 0.001 + self.SPOT_RL_OFFSET = 0.0 * 0.001 + self.SPOT_ANGLE_OFFSET = 0.0 + self.hip_pitch_offset = 7.0 + self.current_pan = -10 + self.current_tilt = -10 + self.current_x_move = 0.005 + self.current_r_angle = 0 + self.curr_period_time = 0.6 + self.accum_period_time = 0.0 + self.DEBUG_PRINT = False + + self.current_joint_states_sub = rospy.Subscriber( + "/humanoid_robot_intelligence_control_system/goal_joint_states", JointState, self.current_joint_states_callback) + self.set_walking_command_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/command", String, queue_size=1) + self.set_walking_param_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/set_params", WalkingParam, queue_size=1) + self.get_walking_param_client = rospy.ServiceProxy( + "/humanoid_robot_intelligence_control_system/walking/get_params", GetWalkingParam) + + self.face_position_sub = rospy.Subscriber( + "/face_detector/face_position", Point, self.face_position_callback) + + self.prev_time = rospy.Time.now() + self.current_walking_param = WalkingParam() + + def start_following(self): + self.on_following = True + rospy.loginfo("Start Face following") + self.set_walking_command("start") + result = self.get_walking_param() + if result: + self.hip_pitch_offset = self.current_walking_param.hip_pitch_offset + self.curr_period_time = self.current_walking_param.period_time + else: + self.hip_pitch_offset = 7.0 * math.pi / 180 + self.curr_period_time = 0.6 + + def stop_following(self): + self.on_following = False + self.count_to_approach = 0 + rospy.loginfo("Stop Face following") + self.set_walking_command("stop") + + def current_joint_states_callback(self, msg): + for i, name in enumerate(msg.name): + if name == "head_pan": + self.current_pan = msg.position[i] + elif name == "head_tilt": + self.current_tilt = msg.position[i] + + def face_position_callback(self, msg): + if self.on_following: + self.process_following(msg.x, msg.y, msg.z) + + def calc_footstep(self, target_distance, target_angle, delta_time): + next_movement = self.current_x_move + target_distance = max(0, target_distance) + fb_goal = min(target_distance * 0.1, self.MAX_FB_STEP) + self.accum_period_time += delta_time + if self.accum_period_time > (self.curr_period_time / 4): + self.accum_period_time = 0.0 + if (target_distance * 0.1 / 2) < self.current_x_move: + next_movement -= self.UNIT_FB_STEP + else: + next_movement += self.UNIT_FB_STEP + fb_goal = min(next_movement, fb_goal) + fb_move = max(fb_goal, self.MIN_FB_STEP) + + rl_goal = 0.0 + if abs(target_angle) * 180 / math.pi > 5.0: + rl_offset = abs(target_angle) * 0.2 + rl_goal = min(rl_offset, self.MAX_RL_TURN) + rl_goal = max(rl_goal, self.MIN_RL_TURN) + rl_angle = min(abs(self.current_r_angle) + self.UNIT_RL_TURN, rl_goal) + if target_angle < 0: + rl_angle *= -1 + else: + rl_angle = 0 + + return fb_move, rl_angle + + def process_following(self, x_angle, y_angle, face_size): + curr_time = rospy.Time.now() + delta_time = (curr_time - self.prev_time).to_sec() + self.prev_time = curr_time + + self.count_not_found = 0 + + if self.current_tilt == -10 and self.current_pan == -10: + rospy.logerr("Failed to get current angle of head joints.") + self.set_walking_command("stop") + self.on_following = False + self.approach_face_position = "NotFound" + return False + + self.approach_face_position = "OutOfRange" + + distance_to_face = self.CAMERA_HEIGHT * math.tan(math.pi * 0.5 + self.current_tilt - self.hip_pitch_offset - face_size) + distance_to_face = abs(distance_to_face) + + distance_to_approach = 0.5 # Adjust this value as needed + + if (distance_to_face < distance_to_approach) and (abs(x_angle) < 25.0): + self.count_to_approach += 1 + if self.count_to_approach > 20: + self.set_walking_command("stop") + self.on_following = False + self.approach_face_position = "OnLeft" if x_angle > 0 else "OnRight" + return True + elif self.count_to_approach > 15: + self.set_walking_param(self.IN_PLACE_FB_STEP, 0, 0) + return False + else: + self.count_to_approach = 0 + + distance_to_walk = distance_to_face - distance_to_approach + fb_move, rl_angle = self.calc_footstep(distance_to_walk, self.current_pan, delta_time) + self.set_walking_param(fb_move, 0, rl_angle) + return False + + def set_walking_command(self, command): + if command == "start": + self.get_walking_param() + self.set_walking_param(self.IN_PLACE_FB_STEP, 0, 0, True) + msg = String() + msg.data = command + self.set_walking_command_pub. diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py new file mode 100644 index 0000000..046f5b0 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + + +import rospy +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String +from geometry_msgs.msg import Point + +class FaceTracker: + def __init__(self): + rospy.init_node('face_tracker') + + self.FOV_WIDTH = 35.2 * math.pi / 180 + self.FOV_HEIGHT = 21.6 * math.pi / 180 + self.NOT_FOUND_THRESHOLD = 50 + self.WAITING_THRESHOLD = 5 + self.use_head_scan = True + self.count_not_found = 0 + self.on_tracking = False + self.current_face_pan = 0 + self.current_face_tilt = 0 + self.x_error_sum = 0 + self.y_error_sum = 0 + self.tracking_status = "NotFound" + self.DEBUG_PRINT = False + + self.p_gain = rospy.get_param('~p_gain', 0.4) + self.i_gain = rospy.get_param('~i_gain', 0.0) + self.d_gain = rospy.get_param('~d_gain', 0.0) + + rospy.loginfo(f"Face tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}") + + self.head_joint_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=1) + self.head_scan_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=1) + + self.face_position_sub = rospy.Subscriber( + "/face_detector/face_position", Point, self.face_position_callback) + self.face_tracking_command_sub = rospy.Subscriber( + "/face_tracker/command", String, self.face_tracker_command_callback) + + self.face_position = Point() + self.prev_time = rospy.Time.now() + + def face_position_callback(self, msg): + self.face_position = msg + + def face_tracker_command_callback(self, msg): + if msg.data == "start": + self.start_tracking() + elif msg.data == "stop": + self.stop_tracking() + elif msg.data == "toggle_start": + if not self.on_tracking: + self.start_tracking() + else: + self.stop_tracking() + + def start_tracking(self): + self.on_tracking = True + rospy.loginfo("Start Face tracking") + + def stop_tracking(self): + self.go_init() + self.on_tracking = False + rospy.loginfo("Stop Face tracking") + self.current_face_pan = 0 + self.current_face_tilt = 0 + self.x_error_sum = 0 + self.y_error_sum = 0 + + def process_tracking(self): + if not self.on_tracking: + self.face_position.z = 0 + self.count_not_found = 0 + return "NotFound" + + if self.face_position.z <= 0: + self.count_not_found += 1 + if self.count_not_found < self.WAITING_THRESHOLD: + tracking_status = "Waiting" + elif self.count_not_found > self.NOT_FOUND_THRESHOLD: + self.scan_face() + self.count_not_found = 0 + tracking_status = "NotFound" + else: + tracking_status = "NotFound" + else: + self.count_not_found = 0 + tracking_status = "Found" + + if tracking_status != "Found": + self.tracking_status = tracking_status + return tracking_status + + x_error = -math.atan(self.face_position.x * math.tan(self.FOV_WIDTH)) + y_error = -math.atan(self.face_position.y * math.tan(self.FOV_HEIGHT)) + + curr_time = rospy.Time.now() + delta_time = (curr_time - self.prev_time).to_sec() + self.prev_time = curr_time + + x_error_diff = (x_error - self.current_face_pan) / delta_time + y_error_diff = (y_error - self.current_face_tilt) / delta_time + self.x_error_sum += x_error + self.y_error_sum += y_error + x_error_target = x_error * self.p_gain + x_error_diff * self.d_gain + self.x_error_sum * self.i_gain + y_error_target = y_error * self.p_gain + y_error_diff * self.d_gain + self.y_error_sum * self.i_gain + + if self.DEBUG_PRINT: + rospy.loginfo(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}") + rospy.loginfo(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}") + rospy.loginfo(f"Error sum: {self.x_error_sum * 180 / math.pi} | {self.y_error_sum * 180 / math.pi}") + rospy.loginfo(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}") + + self.publish_head_joint(x_error_target, y_error_target) + + self.current_face_pan = x_error + self.current_face_tilt = y_error + + self.tracking_status = tracking_status + return tracking_status + + def publish_head_joint(self, pan, tilt): + min_angle = 1 * math.pi / 180 + if abs(pan) < min_angle and abs(tilt) < min_angle: + return + + head_angle_msg = JointState() + head_angle_msg.name = ["head_pan", "head_tilt"] + head_angle_msg.position = [pan, tilt] + + self.head_joint_pub.publish(head_angle_msg) + + def go_init(self): + head_angle_msg = JointState() + head_angle_msg.name = ["head_pan", "head_tilt"] + head_angle_msg.position = [0.0, 0.0] + + self.head_joint_pub.publish(head_angle_msg) + + def scan_face(self): + if not self.use_head_scan: + return + + scan_msg = String() + scan_msg.data = "scan" + + self.head_scan_pub.publish(scan_msg) + + def run(self): + rate = rospy.Rate(30) # 30Hz + while not rospy.is_shutdown(): + self.process_tracking() + rate.sleep() + +if __name__ == '__main__': + try: + tracker = FaceTracker() + tracker.run() + except rospy.ROSInterruptException: + pass diff --git a/humanoid_robot_intelligence_control_system_configure/src/object_follower.py b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py new file mode 100644 index 0000000..87c320a --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py @@ -0,0 +1,218 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import rospy +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String +from humanoid_robot_intelligence_control_system_walking_module_msgs.msg import WalkingParam +from humanoid_robot_intelligence_control_system_walking_module_msgs.srv import GetWalkingParam +from geometry_msgs.msg import Point + + +class ObjectFollower: + def __init__(self): + rospy.init_node('object_follower') + + self.FOV_WIDTH = 35.2 * math.pi / 180 + self.FOV_HEIGHT = 21.6 * math.pi / 180 + self.count_not_found = 0 + self.count_to_approach = 0 + self.on_tracking = False + self.approach_object_position = "NotFound" + self.CAMERA_HEIGHT = 0.46 + self.NOT_FOUND_THRESHOLD = 50 + self.MAX_FB_STEP = 40.0 * 0.001 + self.MAX_RL_TURN = 15.0 * math.pi / 180 + self.IN_PLACE_FB_STEP = -3.0 * 0.001 + self.MIN_FB_STEP = 5.0 * 0.001 + self.MIN_RL_TURN = 5.0 * math.pi / 180 + self.UNIT_FB_STEP = 1.0 * 0.001 + self.UNIT_RL_TURN = 0.5 * math.pi / 180 + self.SPOT_FB_OFFSET = 0.0 * 0.001 + self.SPOT_RL_OFFSET = 0.0 * 0.001 + self.SPOT_ANGLE_OFFSET = 0.0 + self.hip_pitch_offset = 7.0 + self.current_pan = -10 + self.current_tilt = -10 + self.current_x_move = 0.005 + self.current_r_angle = 0 + self.curr_period_time = 0.6 + self.accum_period_time = 0.0 + self.DEBUG_PRINT = False + + self.current_joint_states_sub = rospy.Subscriber( + "/humanoid_robot_intelligence_control_system/goal_joint_states", JointState, self.current_joint_states_callback) + self.set_walking_command_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/command", String, queue_size=1) + self.set_walking_param_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/set_params", WalkingParam, queue_size=1) + self.get_walking_param_client = rospy.ServiceProxy( + "/humanoid_robot_intelligence_control_system/walking/get_params", GetWalkingParam) + + # Subscribe to object detection results + self.object_detection_sub = rospy.Subscriber( + "/object_detection_result", Point, self.object_detection_callback) + + self.prev_time = rospy.Time.now() + self.current_walking_param = WalkingParam() + + def start_following(self): + self.on_tracking = True + rospy.loginfo("Start Object following") + self.set_walking_command("start") + result = self.get_walking_param() + if result: + self.hip_pitch_offset = self.current_walking_param.hip_pitch_offset + self.curr_period_time = self.current_walking_param.period_time + else: + self.hip_pitch_offset = 7.0 * math.pi / 180 + self.curr_period_time = 0.6 + + def stop_following(self): + self.on_tracking = False + self.count_to_approach = 0 + rospy.loginfo("Stop Object following") + self.set_walking_command("stop") + + def current_joint_states_callback(self, msg): + for i, name in enumerate(msg.name): + if name == "head_pan": + self.current_pan = msg.position[i] + elif name == "head_tilt": + self.current_tilt = msg.position[i] + + def calc_footstep(self, target_distance, target_angle, delta_time): + next_movement = self.current_x_move + target_distance = max(0, target_distance) + fb_goal = min(target_distance * 0.1, self.MAX_FB_STEP) + self.accum_period_time += delta_time + if self.accum_period_time > (self.curr_period_time / 4): + self.accum_period_time = 0.0 + if (target_distance * 0.1 / 2) < self.current_x_move: + next_movement -= self.UNIT_FB_STEP + else: + next_movement += self.UNIT_FB_STEP + fb_goal = min(next_movement, fb_goal) + fb_move = max(fb_goal, self.MIN_FB_STEP) + + rl_goal = 0.0 + if abs(target_angle) * 180 / math.pi > 5.0: + rl_offset = abs(target_angle) * 0.2 + rl_goal = min(rl_offset, self.MAX_RL_TURN) + rl_goal = max(rl_goal, self.MIN_RL_TURN) + rl_angle = min(abs(self.current_r_angle) + self.UNIT_RL_TURN, rl_goal) + if target_angle < 0: + rl_angle *= -1 + else: + rl_angle = 0 + + return fb_move, rl_angle + + def process_following(self, x_angle, y_angle, object_size): + curr_time = rospy.Time.now() + delta_time = (curr_time - self.prev_time).to_sec() + self.prev_time = curr_time + + self.count_not_found = 0 + + if self.current_tilt == -10 and self.current_pan == -10: + rospy.logerr("Failed to get current angle of head joints.") + self.set_walking_command("stop") + self.on_tracking = False + self.approach_object_position = "NotFound" + return False + + self.approach_object_position = "OutOfRange" + + distance_to_object = self.CAMERA_HEIGHT * math.tan(math.pi * 0.5 + self.current_tilt - self.hip_pitch_offset - object_size) + distance_to_object = abs(distance_to_object) + + distance_to_approach = 0.22 + + if (distance_to_object < distance_to_approach) and (abs(x_angle) < 25.0): + self.count_to_approach += 1 + if self.count_to_approach > 20: + self.set_walking_command("stop") + self.on_tracking = False + self.approach_object_position = "OnLeft" if x_angle > 0 else "OnRight" + return True + elif self.count_to_approach > 15: + self.set_walking_param(self.IN_PLACE_FB_STEP, 0, 0) + return False + else: + self.count_to_approach = 0 + + distance_to_walk = distance_to_object - distance_to_approach + fb_move, rl_angle = self.calc_footstep(distance_to_walk, self.current_pan, delta_time) + self.set_walking_param(fb_move, 0, rl_angle) + return False + + def decide_object_position(self, x_angle, y_angle): + if self.current_tilt == -10 and self.current_pan == -10: + self.approach_object_position = "NotFound" + return + object_x_angle = self.current_pan + x_angle + self.approach_object_position = "OnLeft" if object_x_angle > 0 else "OnRight" + + def wait_following(self): + self.count_not_found += 1 + if self.count_not_found > self.NOT_FOUND_THRESHOLD * 0.5: + self.set_walking_param(0.0, 0.0, 0.0) + + def set_walking_command(self, command): + if command == "start": + self.get_walking_param() + self.set_walking_param(self.IN_PLACE_FB_STEP, 0, 0, True) + msg = String() + msg.data = command + self.set_walking_command_pub.publish(msg) + + def set_walking_param(self, x_move, y_move, rotation_angle, balance=False): + self.current_walking_param.balance_enable = balance + self.current_walking_param.x_move_amplitude = x_move + self.SPOT_FB_OFFSET + self.current_walking_param.y_move_amplitude = y_move + self.SPOT_RL_OFFSET + self.current_walking_param.angle_move_amplitude = rotation_angle + self.SPOT_ANGLE_OFFSET + self.set_walking_param_pub.publish(self.current_walking_param) + self.current_x_move = x_move + self.current_r_angle = rotation_angle + + def get_walking_param(self): + try: + response = self.get_walking_param_client() + self.current_walking_param = response.parameters + return True + except rospy.ServiceException as e: + rospy.logerr("Failed to get walking parameters: %s" % e) + return False + + def object_detection_callback(self, msg): + if self.on_tracking: + x_angle = msg.x + y_angle = msg.y + object_size = msg.z + self.process_following(x_angle, y_angle, object_size) + else: + self.wait_following() + +if __name__ == '__main__': + try: + follower = ObjectFollower() + follower.start_following() + rospy.spin() + except rospy.ROSInterruptException: + pass diff --git a/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py b/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py new file mode 100644 index 0000000..2d54fba --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py @@ -0,0 +1,194 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import rospy +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String, Float64MultiArray +from humanoid_robot_intelligence_control_system_ball_detector.msg import CircleSetStamped +from geometry_msgs.msg import Point + +class ObjectTracker: + def __init__(self): + rospy.init_node('object_tracker') + + self.FOV_WIDTH = 35.2 * math.pi / 180 + self.FOV_HEIGHT = 21.6 * math.pi / 180 + self.NOT_FOUND_THRESHOLD = 50 + self.WAITING_THRESHOLD = 5 + self.use_head_scan = True + self.count_not_found = 0 + self.on_tracking = False + self.current_object_pan = 0 + self.current_object_tilt = 0 + self.x_error_sum = 0 + self.y_error_sum = 0 + self.current_object_bottom = 0 + self.tracking_status = "NotFound" + self.DEBUG_PRINT = False + + param_nh = rospy.get_param("~") + self.p_gain = param_nh.get("p_gain", 0.4) + self.i_gain = param_nh.get("i_gain", 0.0) + self.d_gain = param_nh.get("d_gain", 0.0) + + rospy.loginfo(f"Object tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}") + + self.head_joint_offset_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", JointState, queue_size=0) + self.head_joint_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=0) + self.head_scan_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=0) + + self.object_position_sub = rospy.Subscriber( + "/object_detector_node/object_position", Point, self.object_position_callback) + self.object_tracking_command_sub = rospy.Subscriber( + "/object_tracker/command", String, self.object_tracker_command_callback) + + self.object_position = Point() + self.prev_time = rospy.Time.now() + + def object_position_callback(self, msg): + self.object_position = msg + + def object_tracker_command_callback(self, msg): + if msg.data == "start": + self.start_tracking() + elif msg.data == "stop": + self.stop_tracking() + elif msg.data == "toggle_start": + if not self.on_tracking: + self.start_tracking() + else: + self.stop_tracking() + + def start_tracking(self): + self.on_tracking = True + rospy.loginfo("Start Object tracking") if self.DEBUG_PRINT else None + + def stop_tracking(self): + self.go_init() + self.on_tracking = False + rospy.loginfo("Stop Object tracking") if self.DEBUG_PRINT else None + self.current_object_pan = 0 + self.current_object_tilt = 0 + self.x_error_sum = 0 + self.y_error_sum = 0 + + def set_using_head_scan(self, use_scan): + self.use_head_scan = use_scan + + def process_tracking(self): + if not self.on_tracking: + self.object_position.z = 0 + self.count_not_found = 0 + return "NotFound" + + if self.object_position.z <= 0: + self.count_not_found += 1 + if self.count_not_found < self.WAITING_THRESHOLD: + if self.tracking_status in ["Found", "Waiting"]: + tracking_status = "Waiting" + else: + tracking_status = "NotFound" + elif self.count_not_found > self.NOT_FOUND_THRESHOLD: + self.scan_object() + self.count_not_found = 0 + tracking_status = "NotFound" + else: + tracking_status = "NotFound" + else: + self.count_not_found = 0 + tracking_status = "Found" + + if tracking_status != "Found": + self.tracking_status = tracking_status + self.current_object_pan = 0 + self.current_object_tilt = 0 + self.x_error_sum = 0 + self.y_error_sum = 0 + return tracking_status + + x_error = -math.atan(self.object_position.x * math.tan(self.FOV_WIDTH)) + y_error = -math.atan(self.object_position.y * math.tan(self.FOV_HEIGHT)) + object_size = self.object_position.z + + curr_time = rospy.Time.now() + delta_time = (curr_time - self.prev_time).to_sec() + self.prev_time = curr_time + + x_error_diff = (x_error - self.current_object_pan) / delta_time + y_error_diff = (y_error - self.current_object_tilt) / delta_time + self.x_error_sum += x_error + self.y_error_sum += y_error + x_error_target = x_error * self.p_gain + x_error_diff * self.d_gain + self.x_error_sum * self.i_gain + y_error_target = y_error * self.p_gain + y_error_diff * self.d_gain + self.y_error_sum * self.i_gain + + if self.DEBUG_PRINT: + rospy.loginfo(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}") + rospy.loginfo(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}") + rospy.loginfo(f"Error sum: {self.x_error_sum * 180 / math.pi} | {self.y_error_sum * 180 / math.pi}") + rospy.loginfo(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}") + + self.publish_head_joint(x_error_target, y_error_target) + + self.current_object_pan = x_error + self.current_object_tilt = y_error + self.current_object_bottom = object_size + + self.object_position.z = 0 + + self.tracking_status = tracking_status + return tracking_status + + def publish_head_joint(self, pan, tilt): + min_angle = 1 * math.pi / 180 + if abs(pan) < min_angle and abs(tilt) < min_angle: + return + + head_angle_msg = JointState() + head_angle_msg.name = ["head_pan", "head_tilt"] + head_angle_msg.position = [pan, tilt] + + self.head_joint_offset_pub.publish(head_angle_msg) + + def go_init(self): + head_angle_msg = JointState() + head_angle_msg.name = ["head_pan", "head_tilt"] + head_angle_msg.position = [0.0, 0.0] + + self.head_joint_pub.publish(head_angle_msg) + + def scan_object(self): + if not self.use_head_scan: + return + + scan_msg = String() + scan_msg.data = "scan" + + self.head_scan_pub.publish(scan_msg) + +if __name__ == '__main__': + try: + tracker = ObjectTracker() + rate = rospy.Rate(30) + while not rospy.is_shutdown(): + tracker.process_tracking() + rate.sleep() + except rospy.ROSInterruptException: + pass From b2271c014a38f8180a45942a934c94254622c887 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 23 Jul 2024 12:52:27 -0400 Subject: [PATCH 44/59] latest pushes --- .../launch/face_follower.launch.py | 28 ++-- .../launch/object_follower.launch.py | 30 ++-- .../launch/object_tracker.launch.py | 32 ++-- .../src/face_follower.py | 143 +++++++++++++++--- 4 files changed, 168 insertions(+), 65 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py index 051f56a..917e490 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py @@ -13,9 +13,6 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . -#!/usr/bin/env python3 - -#!/usr/bin/env python3 import os import sys @@ -28,7 +25,7 @@ def ros1_launch_description(): args = sys.argv[1:] # Construct the ROS 1 launch command - roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_object_detector", "object_follow.launch"] + args + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_follow.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", @@ -41,21 +38,22 @@ def ros1_launch_description(): ]) roslaunch_command.extend([ - "humanoid_robot_intelligence_control_system_object_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + "humanoid_robot_intelligence_control_system_face_detector", "face_detection_processor.py", "name:=face_detection_processor_node" ]) roslaunch_command.extend([ - "humanoid_robot_intelligence_control_system_object_detector", "object_follower.py", "name:=object_follower_node" + "humanoid_robot_intelligence_control_system_configure", "face_follower.py", "name:=face_follower_node" ]) roslaunch_command.extend([ "rviz", "rviz", "name:=rviz", - "args:=-d $(find humanoid_robot_intelligence_control_system_object_detector)/rviz/object_follow_visualization.rviz" + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" ]) # Execute the launch command subprocess.call(roslaunch_command) + def ros2_launch_description(): nodes_to_launch = [] @@ -75,17 +73,17 @@ def ros2_launch_description(): )) nodes_to_launch.append(Node( - package='humanoid_robot_intelligence_control_system_object_detector', - executable='object_detection_processor.py', - name='object_detection_processor_node', + package='humanoid_robot_intelligence_control_system_face_detector', + executable='face_detection_processor.py', + name='face_detection_processor_node', output='screen', - parameters=[{'yaml_path': '$(find humanoid_robot_intelligence_control_system_object_detector)/config/object_detection_params.yaml'}] + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/face_detection_params.yaml'}] )) nodes_to_launch.append(Node( - package='humanoid_robot_intelligence_control_system_object_detector', - executable='object_follower.py', - name='object_follower_node', + package='humanoid_robot_intelligence_control_system_configure', + executable='face_follower.py', + name='face_follower_node', output='screen', parameters=[{ 'p_gain': 0.4, @@ -98,7 +96,7 @@ def ros2_launch_description(): package='rviz2', executable='rviz2', name='rviz', - arguments=['-d', '$(find humanoid_robot_intelligence_control_system_object_detector)/rviz/object_follow_visualization.rviz'] + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] )) return LaunchDescription(nodes_to_launch) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py index e61d87a..dba557d 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py @@ -13,20 +13,19 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . + import os import sys import subprocess from launch import LaunchDescription from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration def ros1_launch_description(): # Get command-line arguments args = sys.argv[1:] # Construct the ROS 1 launch command - roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_object_detector", "object_detector_processor.launch"] + args + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_follow.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", @@ -39,11 +38,11 @@ def ros1_launch_description(): ]) roslaunch_command.extend([ - "ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_object_detection.py", "name:=object_detection_node" + "humanoid_robot_intelligence_control_system_object_detector", "object_detection_processor.py", "name:=object_detection_processor_node" ]) roslaunch_command.extend([ - "humanoid_robot_intelligence_control_system_ball_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + "humanoid_robot_intelligence_control_system_configure", "object_follower.py", "name:=object_follower_node" ]) roslaunch_command.extend([ @@ -54,6 +53,7 @@ def ros1_launch_description(): # Execute the launch command subprocess.call(roslaunch_command) + def ros2_launch_description(): nodes_to_launch = [] @@ -72,14 +72,6 @@ def ros2_launch_description(): }] )) - nodes_to_launch.append(Node( - package='ros_web_api_bellande_2d_computer_vision', - executable='bellande_2d_computer_vision_object_detection.py', - name='object_detection_node', - output='screen', - remappings=[('camera/image_raw', '/usb_cam/image_raw')] - )) - nodes_to_launch.append(Node( package='humanoid_robot_intelligence_control_system_object_detector', executable='object_detection_processor.py', @@ -88,6 +80,18 @@ def ros2_launch_description(): parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml'}] )) + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_configure', + executable='object_follower.py', + name='object_follower_node', + output='screen', + parameters=[{ + 'p_gain': 0.4, + 'i_gain': 0.0, + 'd_gain': 0.0 + }] + )) + nodes_to_launch.append(Node( package='rviz2', executable='rviz2', diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py index e61d87a..9e0beef 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py @@ -13,20 +13,19 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . + import os import sys import subprocess from launch import LaunchDescription from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration def ros1_launch_description(): # Get command-line arguments args = sys.argv[1:] # Construct the ROS 1 launch command - roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_object_detector", "object_detector_processor.launch"] + args + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_follow.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", @@ -39,11 +38,11 @@ def ros1_launch_description(): ]) roslaunch_command.extend([ - "ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_object_detection.py", "name:=object_detection_node" + "humanoid_robot_intelligence_control_system_object_tracker", "object_detection_processor.py", "name:=object_detection_processor_node" ]) roslaunch_command.extend([ - "humanoid_robot_intelligence_control_system_ball_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + "humanoid_robot_intelligence_control_system_configure", "object_follower.py", "name:=object_follower_node" ]) roslaunch_command.extend([ @@ -54,6 +53,7 @@ def ros1_launch_description(): # Execute the launch command subprocess.call(roslaunch_command) + def ros2_launch_description(): nodes_to_launch = [] @@ -73,21 +73,25 @@ def ros2_launch_description(): )) nodes_to_launch.append(Node( - package='ros_web_api_bellande_2d_computer_vision', - executable='bellande_2d_computer_vision_object_detection.py', - name='object_detection_node', - output='screen', - remappings=[('camera/image_raw', '/usb_cam/image_raw')] - )) - - nodes_to_launch.append(Node( - package='humanoid_robot_intelligence_control_system_object_detector', + package='humanoid_robot_intelligence_control_system_object_tracker', executable='object_detection_processor.py', name='object_detection_processor_node', output='screen', parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml'}] )) + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_configure', + executable='object_follower.py', + name='object_follower_node', + output='screen', + parameters=[{ + 'p_gain': 0.4, + 'i_gain': 0.0, + 'd_gain': 0.0 + }] + )) + nodes_to_launch.append(Node( package='rviz2', executable='rviz2', diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py index 0f11ade..3f923e4 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py +++ b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py @@ -15,18 +15,32 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . -import rospy +import os +import sys import math +from std_msgs.msg import String from sensor_msgs.msg import JointState -from std_msgs.msg import String, Bool from geometry_msgs.msg import Point from humanoid_robot_intelligence_control_system_walking_module_msgs.msg import WalkingParam from humanoid_robot_intelligence_control_system_walking_module_msgs.srv import GetWalkingParam class FaceFollower: - def __init__(self): - rospy.init_node('face_follower') - + def __init__(self, ros_version): + self.ros_version = ros_version + if self.ros_version == '1': + rospy.init_node('face_follower') + self.get_param = rospy.get_param + self.logger = rospy + else: + rclpy.init() + self.node = rclpy.create_node('face_follower') + self.get_param = self.node.get_parameter + self.logger = self.node.get_logger() + + self.initialize_parameters() + self.setup_ros_communication() + + def initialize_parameters(self): self.FOV_WIDTH = 35.2 * math.pi / 180 self.FOV_HEIGHT = 21.6 * math.pi / 180 self.count_not_found = 0 @@ -54,24 +68,37 @@ class FaceFollower: self.accum_period_time = 0.0 self.DEBUG_PRINT = False - self.current_joint_states_sub = rospy.Subscriber( - "/humanoid_robot_intelligence_control_system/goal_joint_states", JointState, self.current_joint_states_callback) - self.set_walking_command_pub = rospy.Publisher( - "/humanoid_robot_intelligence_control_system/walking/command", String, queue_size=1) - self.set_walking_param_pub = rospy.Publisher( - "/humanoid_robot_intelligence_control_system/walking/set_params", WalkingParam, queue_size=1) - self.get_walking_param_client = rospy.ServiceProxy( - "/humanoid_robot_intelligence_control_system/walking/get_params", GetWalkingParam) + def setup_ros_communication(self): + if self.ros_version == '1': + self.current_joint_states_sub = rospy.Subscriber( + "/humanoid_robot_intelligence_control_system/goal_joint_states", JointState, self.current_joint_states_callback) + self.set_walking_command_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/command", String, queue_size=1) + self.set_walking_param_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/set_params", WalkingParam, queue_size=1) + self.get_walking_param_client = rospy.ServiceProxy( + "/humanoid_robot_intelligence_control_system/walking/get_params", GetWalkingParam) + self.face_position_sub = rospy.Subscriber( + "/face_detector/face_position", Point, self.face_position_callback) + self.prev_time = rospy.Time.now() + else: + self.current_joint_states_sub = self.node.create_subscription( + JointState, "/humanoid_robot_intelligence_control_system/goal_joint_states", self.current_joint_states_callback, 10) + self.set_walking_command_pub = self.node.create_publisher( + String, "/humanoid_robot_intelligence_control_system/walking/command", 1) + self.set_walking_param_pub = self.node.create_publisher( + WalkingParam, "/humanoid_robot_intelligence_control_system/walking/set_params", 1) + self.get_walking_param_client = self.node.create_client( + GetWalkingParam, "/humanoid_robot_intelligence_control_system/walking/get_params") + self.face_position_sub = self.node.create_subscription( + Point, "/face_detector/face_position", self.face_position_callback, 10) + self.prev_time = self.node.get_clock().now() - self.face_position_sub = rospy.Subscriber( - "/face_detector/face_position", Point, self.face_position_callback) - - self.prev_time = rospy.Time.now() self.current_walking_param = WalkingParam() def start_following(self): self.on_following = True - rospy.loginfo("Start Face following") + self.logger.info("Start Face following") self.set_walking_command("start") result = self.get_walking_param() if result: @@ -84,7 +111,7 @@ class FaceFollower: def stop_following(self): self.on_following = False self.count_to_approach = 0 - rospy.loginfo("Stop Face following") + self.logger.info("Stop Face following") self.set_walking_command("stop") def current_joint_states_callback(self, msg): @@ -126,14 +153,18 @@ class FaceFollower: return fb_move, rl_angle def process_following(self, x_angle, y_angle, face_size): - curr_time = rospy.Time.now() - delta_time = (curr_time - self.prev_time).to_sec() + if self.ros_version == '1': + curr_time = rospy.Time.now() + delta_time = (curr_time - self.prev_time).to_sec() + else: + curr_time = self.node.get_clock().now() + delta_time = (curr_time - self.prev_time).nanoseconds / 1e9 self.prev_time = curr_time self.count_not_found = 0 if self.current_tilt == -10 and self.current_pan == -10: - rospy.logerr("Failed to get current angle of head joints.") + self.logger.error("Failed to get current angle of head joints.") self.set_walking_command("stop") self.on_following = False self.approach_face_position = "NotFound" @@ -170,4 +201,70 @@ class FaceFollower: self.set_walking_param(self.IN_PLACE_FB_STEP, 0, 0, True) msg = String() msg.data = command - self.set_walking_command_pub. + self.set_walking_command_pub.publish(msg) + + def set_walking_param(self, x_move, y_move, rotation_angle, balance=False): + self.current_walking_param.balance_enable = balance + self.current_walking_param.x_move_amplitude = x_move + self.SPOT_FB_OFFSET + self.current_walking_param.y_move_amplitude = y_move + self.SPOT_RL_OFFSET + self.current_walking_param.angle_move_amplitude = rotation_angle + self.SPOT_ANGLE_OFFSET + self.set_walking_param_pub.publish(self.current_walking_param) + self.current_x_move = x_move + self.current_r_angle = rotation_angle + + def get_walking_param(self): + if self.ros_version == '1': + try: + response = self.get_walking_param_client() + self.current_walking_param = response.parameters + return True + except rospy.ServiceException as e: + self.logger.error("Failed to get walking parameters: %s" % e) + return False + else: + future = self.get_walking_param_client.call_async(GetWalkingParam.Request()) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is not None: + self.current_walking_param = future.result().parameters + return True + else: + self.logger.error('Service call failed %r' % (future.exception(),)) + return False + + def run(self): + if self.ros_version == '1': + rate = rospy.Rate(30) # 30Hz + while not rospy.is_shutdown(): + if self.on_following: + self.process_following(self.current_pan, self.current_tilt, 0.1) + rate.sleep() + else: + while rclpy.ok(): + if self.on_following: + self.process_following(self.current_pan, self.current_tilt, 0.1) + rclpy.spin_once(self.node, timeout_sec=0.03) + +def main(ros_version): + try: + follower = FaceFollower(ros_version) + follower.start_following() + follower.run() + except Exception as e: + if ros_version == '1': + rospy.logerr(f"An error occurred: {e}") + else: + rclpy.shutdown() + print(f"An error occurred: {e}") + +if __name__ == '__main__': + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + import rospy + elif ros_version == "2": + import rclpy + from rclpy.node import Node + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) + + main(ros_version) From d69617ee5a58ed20f525527e9e4b509705764353 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 23 Jul 2024 13:50:25 -0400 Subject: [PATCH 45/59] latest pushes --- .../launch/face_tracker.launch.py | 26 +++++++++---------- .../launch/object_tracker.launch.py | 6 ++--- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py index 91a9896..6d4beeb 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py @@ -13,7 +13,6 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . -#!/usr/bin/env python3 import os import sys @@ -26,7 +25,7 @@ def ros1_launch_description(): args = sys.argv[1:] # Construct the ROS 1 launch command - roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_tracker.launch"] + args + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_follow.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", @@ -39,21 +38,22 @@ def ros1_launch_description(): ]) roslaunch_command.extend([ - "humanoid_robot_intelligence_control_system_object_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + "humanoid_robot_intelligence_control_system_face_tracker", "face_detection_processor.py", "name:=face_detection_processor_node" ]) roslaunch_command.extend([ - "humanoid_robot_intelligence_control_system_configure", "object_tracker.py", "name:=object_tracker_node" + "humanoid_robot_intelligence_control_system_configure", "face_tracker.py", "name:=face_tracker_node" ]) roslaunch_command.extend([ "rviz", "rviz", "name:=rviz", - "args:=-d $(find humanoid_robot_intelligence_control_system_object_detector)/rviz/object_tracking_visualization.rviz" + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" ]) # Execute the launch command subprocess.call(roslaunch_command) + def ros2_launch_description(): nodes_to_launch = [] @@ -73,17 +73,17 @@ def ros2_launch_description(): )) nodes_to_launch.append(Node( - package='humanoid_robot_intelligence_control_system_object_detector', - executable='object_detection_processor.py', - name='object_detection_processor_node', + package='humanoid_robot_intelligence_control_system_face_tracker', + executable='face_detection_processor.py', + name='face_detection_processor_node', output='screen', - parameters=[{'yaml_path': '$(find humanoid_robot_intelligence_control_system_object_detector)/config/object_detection_params.yaml'}] + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/face_detection_params.yaml'}] )) nodes_to_launch.append(Node( - package='humanoid_robot_intelligence_control_system_object_detector', - executable='object_tracker.py', - name='object_tracker_node', + package='humanoid_robot_intelligence_control_system_configure', + executable='face_tracker.py', + name='face_tracker_node', output='screen', parameters=[{ 'p_gain': 0.4, @@ -96,7 +96,7 @@ def ros2_launch_description(): package='rviz2', executable='rviz2', name='rviz', - arguments=['-d', '$(find humanoid_robot_intelligence_control_system_object_detector)/rviz/object_tracking_visualization.rviz'] + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] )) return LaunchDescription(nodes_to_launch) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py index 9e0beef..892e0cc 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py @@ -42,7 +42,7 @@ def ros1_launch_description(): ]) roslaunch_command.extend([ - "humanoid_robot_intelligence_control_system_configure", "object_follower.py", "name:=object_follower_node" + "humanoid_robot_intelligence_control_system_configure", "object_tracker.py", "name:=object_tracker_node" ]) roslaunch_command.extend([ @@ -82,8 +82,8 @@ def ros2_launch_description(): nodes_to_launch.append(Node( package='humanoid_robot_intelligence_control_system_configure', - executable='object_follower.py', - name='object_follower_node', + executable='object_tracker.py', + name='object_tracker_node', output='screen', parameters=[{ 'p_gain': 0.4, From 4a9833b8f1ec12869cd11eb169e859f98dfdeacf Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 23 Jul 2024 14:06:35 -0400 Subject: [PATCH 46/59] latest pushes --- .../launch/face_tracker.launch.py | 2 +- .../launch/object_follower.launch.py | 2 +- .../launch/object_tracker.launch.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py index 6d4beeb..d7d94dc 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py @@ -25,7 +25,7 @@ def ros1_launch_description(): args = sys.argv[1:] # Construct the ROS 1 launch command - roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_follow.launch"] + args + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_tracker.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py index dba557d..2788682 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py @@ -25,7 +25,7 @@ def ros1_launch_description(): args = sys.argv[1:] # Construct the ROS 1 launch command - roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_follow.launch"] + args + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_follower.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py index 892e0cc..49307ae 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py @@ -25,7 +25,7 @@ def ros1_launch_description(): args = sys.argv[1:] # Construct the ROS 1 launch command - roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_follow.launch"] + args + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_tracker.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", From e4f3617b86fc78d94dc077b7e5bfb9141c6d4a3d Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 23 Jul 2024 14:19:45 -0400 Subject: [PATCH 47/59] latest pushes --- README.md | 33 ++++++++----------- .../launch/face_follower.launch.py | 2 +- 2 files changed, 14 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index 3ee5208..8da2494 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos +# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Architecture [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) @@ -6,7 +6,7 @@ ## 🤖 Explore Humanoid Robot Intelligence with Us! -Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. +Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System architecture repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. ## 💻 Functionality To Switch from ROS to ROS2 Checkout The Below Repository @@ -14,35 +14,28 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos reposit ### 🚀 Key Repository Stats -- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Starred by the community -- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Forked by enthusiasts -- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Watched by curious minds +- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg?style=social) Starred by the community +- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg?style=social) Forked by enthusiasts +- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg?style=social) Watched by curious minds -- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Actively addressing issues -- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Welcoming contributions -- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Licensed under Apache 2.0 +- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Actively addressing issues +- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Welcoming contributions +- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Licensed under Apache 2.0 -- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Latest updates -- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/traffic) Engaging with the community +- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Latest updates +- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture/traffic) Engaging with the community --- ## 🌐 Repository Website -Explore our [repository website](https://robotics-sensors.github.io/bellande_humanoid_robot_intelligence_control_system_demos) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. - ---- - -### 🔄 Updates and Versions - -- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_demos) -- **Old Version/Previous Used for Different Context:** [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) +Explore our [repository website](https://robotics-sensors.github.io/bellande_humanoid_robot_intelligence_control_system_architecture) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. --- # 🎉 Latest Release -[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_demos/releases/) +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture/releases/) --- @@ -62,4 +55,4 @@ Meet our dedicated maintainer, **Ronaldson Bellande**. ## 📄 License -This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE). +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture/blob/main/LICENSE). diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py index 917e490..44a7599 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py @@ -25,7 +25,7 @@ def ros1_launch_description(): args = sys.argv[1:] # Construct the ROS 1 launch command - roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_follow.launch"] + args + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_follower.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", From dce855f5153b32f60269b30e68e5221407f91bb1 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 23 Jul 2024 14:22:49 -0400 Subject: [PATCH 48/59] Update README.md --- README.md | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/README.md b/README.md index 8da2494..2f508a1 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,17 @@ [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) [![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) +## Mobile and ROS Control System +- [![Bellande's Internal Sensor Mobile iOS API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Mobile%20iOS%20API-Bellande%20API-blue?style=for-the-badge&logo=swift&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_mobile_ios_api) +- [![Bellande's External Sensor Mobile iOS API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Mobile%20iOS%20API-Bellande%20API-blue?style=for-the-badge&logo=swift&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_mobile_ios_api) +- [![Bellande's Functionality Mobile iOS API](https://img.shields.io/badge/Bellande's%20Functionality%20Mobile%20iOS%20API-Bellande%20API-blue?style=for-the-badge&logo=swift&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_mobile_ios_api) +- [![Bellande's Internal Sensor Mobile Android API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Mobile%20Android%20API-Bellande%20API-blue?style=for-the-badge&logo=android&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_mobile_android_api) +- [![Bellande's External Sensor Mobile Android API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Mobile%20Android%20API-Bellande%20API-blue?style=for-the-badge&logo=android&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_mobile_android_api) +- [![Bellande's Functionality Mobile Android API](https://img.shields.io/badge/Bellande's%20Functionality%20Mobile%20Android%20API-Bellande%20API-blue?style=for-the-badge&logo=android&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_mobile_android_api) +- [![Bellande's Internal Sensor Web API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_web_api) +- [![Bellande's External Sensor Web API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_web_api) +- [![Bellande's Functionality Web API](https://img.shields.io/badge/Bellande's%20Functionality%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_web_api) + ## 🤖 Explore Humanoid Robot Intelligence with Us! Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System architecture repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. From 34ceaf0e1395866290a8e477339913a14c547fcd Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 23 Jul 2024 14:26:19 -0400 Subject: [PATCH 49/59] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 2f508a1..b3b5c8e 100644 --- a/README.md +++ b/README.md @@ -58,7 +58,7 @@ The repository has recently undergone significant updates. Older commits and cod 🚀 For the latest versions and updates, visit our organization: [Robotics-Sensors](https://github.com/Robotics-Sensors). -### 🧑‍💼 Maintainer +### 🧑‍💼 Maintainer & Author Meet our dedicated maintainer, **Ronaldson Bellande**. From acd84ed2d995aed78e330cd1265a116bcf38db09 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 23 Jul 2024 14:37:10 -0400 Subject: [PATCH 50/59] Update README.md --- README.md | 6 ------ 1 file changed, 6 deletions(-) diff --git a/README.md b/README.md index b3b5c8e..c214325 100644 --- a/README.md +++ b/README.md @@ -50,12 +50,6 @@ Explore our [repository website](https://robotics-sensors.github.io/bellande_hum --- -## 📢 Important Announcement - -The repository has recently undergone significant updates. Older commits and codes are covered under the previous license, while new commits and codes fall under the new license. - ---- - 🚀 For the latest versions and updates, visit our organization: [Robotics-Sensors](https://github.com/Robotics-Sensors). ### 🧑‍💼 Maintainer & Author From 04676b74b1af27e5bc9ac10b6cd3fe1a7b694bb4 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 23 Jul 2024 14:38:20 -0400 Subject: [PATCH 51/59] Update README.md --- README.md | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index c214325..412d3e9 100644 --- a/README.md +++ b/README.md @@ -25,16 +25,16 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System architecture ### 🚀 Key Repository Stats -- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg?style=social) Starred by the community -- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg?style=social) Forked by enthusiasts -- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg?style=social) Watched by curious minds +- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg?style=social) Starred by the community +- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg?style=social) Forked by enthusiasts +- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg?style=social) Watched by curious minds -- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Actively addressing issues -- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Welcoming contributions -- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Licensed under Apache 2.0 +- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Actively addressing issues +- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Welcoming contributions +- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Licensed under Apache 2.0 -- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Latest updates -- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture/traffic) Engaging with the community +- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Latest updates +- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture/traffic) Engaging with the community --- From 6774280085d7d527420f82065b70e2eb91ea25b2 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 23 Jul 2024 19:44:16 -0400 Subject: [PATCH 52/59] latest pushes --- .../src/face_follower.py | 26 +++- .../src/face_tracker.py | 132 +++++++++++++----- 2 files changed, 115 insertions(+), 43 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py index 3f923e4..40fd0da 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py +++ b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py @@ -24,6 +24,7 @@ from geometry_msgs.msg import Point from humanoid_robot_intelligence_control_system_walking_module_msgs.msg import WalkingParam from humanoid_robot_intelligence_control_system_walking_module_msgs.srv import GetWalkingParam + class FaceFollower: def __init__(self, ros_version): self.ros_version = ros_version @@ -40,6 +41,7 @@ class FaceFollower: self.initialize_parameters() self.setup_ros_communication() + def initialize_parameters(self): self.FOV_WIDTH = 35.2 * math.pi / 180 self.FOV_HEIGHT = 21.6 * math.pi / 180 @@ -233,7 +235,7 @@ class FaceFollower: def run(self): if self.ros_version == '1': - rate = rospy.Rate(30) # 30Hz + rate = rospy.Rate(30) while not rospy.is_shutdown(): if self.on_following: self.process_following(self.current_pan, self.current_tilt, 0.1) @@ -244,6 +246,7 @@ class FaceFollower: self.process_following(self.current_pan, self.current_tilt, 0.1) rclpy.spin_once(self.node, timeout_sec=0.03) + def main(ros_version): try: follower = FaceFollower(ros_version) @@ -256,15 +259,26 @@ def main(ros_version): rclpy.shutdown() print(f"An error occurred: {e}") + + if __name__ == '__main__': ros_version = os.getenv("ROS_VERSION") if ros_version == "1": - import rospy + try: + import rospy + main(ros_version) + except rospy.ROSInterruptException: + print("Error in ROS1 main") + elif ros_version == "2": - import rclpy - from rclpy.node import Node + try: + import rclpy + from rclpy.node import Node + main(ros_version) + except KeyboardInterrupt: + rclpy.shutdown() + print("Error in ROS1 main") + else: print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") sys.exit(1) - - main(ros_version) diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py index 046f5b0..138d645 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py +++ b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py @@ -15,17 +15,31 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . - -import rospy +import os +import sys import math from sensor_msgs.msg import JointState from std_msgs.msg import String from geometry_msgs.msg import Point class FaceTracker: - def __init__(self): - rospy.init_node('face_tracker') - + def __init__(self, ros_version): + self.ros_version = ros_version + if self.ros_version == '1': + rospy.init_node('face_tracker') + self.get_param = rospy.get_param + self.logger = rospy + else: + rclpy.init() + self.node = Node('face_tracker') + self.get_param = self.node.get_parameter + self.logger = self.node.get_logger() + + self.initialize_parameters() + self.setup_ros_communication() + + + def initialize_parameters(self): self.FOV_WIDTH = 35.2 * math.pi / 180 self.FOV_HEIGHT = 21.6 * math.pi / 180 self.NOT_FOUND_THRESHOLD = 50 @@ -39,25 +53,36 @@ class FaceTracker: self.y_error_sum = 0 self.tracking_status = "NotFound" self.DEBUG_PRINT = False + + self.p_gain = self.get_param('p_gain', 0.4) + self.i_gain = self.get_param('i_gain', 0.0) + self.d_gain = self.get_param('d_gain', 0.0) - self.p_gain = rospy.get_param('~p_gain', 0.4) - self.i_gain = rospy.get_param('~i_gain', 0.0) - self.d_gain = rospy.get_param('~d_gain', 0.0) + self.logger.info(f"Face tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}") - rospy.loginfo(f"Face tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}") - - self.head_joint_pub = rospy.Publisher( - "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=1) - self.head_scan_pub = rospy.Publisher( - "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=1) - - self.face_position_sub = rospy.Subscriber( - "/face_detector/face_position", Point, self.face_position_callback) - self.face_tracking_command_sub = rospy.Subscriber( - "/face_tracker/command", String, self.face_tracker_command_callback) + def setup_ros_communication(self): + if self.ros_version == '1': + self.head_joint_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=1) + self.head_scan_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=1) + self.face_position_sub = rospy.Subscriber( + "/face_detector/face_position", Point, self.face_position_callback) + self.face_tracking_command_sub = rospy.Subscriber( + "/face_tracker/command", String, self.face_tracker_command_callback) + self.prev_time = rospy.Time.now() + else: + self.head_joint_pub = self.node.create_publisher( + JointState, "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 1) + self.head_scan_pub = self.node.create_publisher( + String, "/humanoid_robot_intelligence_control_system/head_control/scan_command", 1) + self.face_position_sub = self.node.create_subscription( + Point, "/face_detector/face_position", self.face_position_callback, 10) + self.face_tracking_command_sub = self.node.create_subscription( + String, "/face_tracker/command", self.face_tracker_command_callback, 10) + self.prev_time = self.node.get_clock().now() self.face_position = Point() - self.prev_time = rospy.Time.now() def face_position_callback(self, msg): self.face_position = msg @@ -75,12 +100,12 @@ class FaceTracker: def start_tracking(self): self.on_tracking = True - rospy.loginfo("Start Face tracking") + self.logger.info("Start Face tracking") def stop_tracking(self): self.go_init() self.on_tracking = False - rospy.loginfo("Stop Face tracking") + self.logger.info("Stop Face tracking") self.current_face_pan = 0 self.current_face_tilt = 0 self.x_error_sum = 0 @@ -113,8 +138,12 @@ class FaceTracker: x_error = -math.atan(self.face_position.x * math.tan(self.FOV_WIDTH)) y_error = -math.atan(self.face_position.y * math.tan(self.FOV_HEIGHT)) - curr_time = rospy.Time.now() - delta_time = (curr_time - self.prev_time).to_sec() + if self.ros_version == '1': + curr_time = rospy.Time.now() + delta_time = (curr_time - self.prev_time).to_sec() + else: + curr_time = self.node.get_clock().now() + delta_time = (curr_time - self.prev_time).nanoseconds / 1e9 self.prev_time = curr_time x_error_diff = (x_error - self.current_face_pan) / delta_time @@ -125,10 +154,10 @@ class FaceTracker: y_error_target = y_error * self.p_gain + y_error_diff * self.d_gain + self.y_error_sum * self.i_gain if self.DEBUG_PRINT: - rospy.loginfo(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}") - rospy.loginfo(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}") - rospy.loginfo(f"Error sum: {self.x_error_sum * 180 / math.pi} | {self.y_error_sum * 180 / math.pi}") - rospy.loginfo(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}") + self.logger.info(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}") + self.logger.info(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}") + self.logger.info(f"Error sum: {self.x_error_sum * 180 / math.pi} | {self.y_error_sum * 180 / math.pi}") + self.logger.info(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}") self.publish_head_joint(x_error_target, y_error_target) @@ -166,14 +195,43 @@ class FaceTracker: self.head_scan_pub.publish(scan_msg) def run(self): - rate = rospy.Rate(30) # 30Hz - while not rospy.is_shutdown(): - self.process_tracking() - rate.sleep() + if self.ros_version == '1': + rate = rospy.Rate(30) # 30Hz + while not rospy.is_shutdown(): + self.process_tracking() + rate.sleep() + else: + while rclpy.ok(): + self.process_tracking() + rclpy.spin_once(self.node, timeout_sec=0.03) # Approximately 30Hz + +def main(ros_version): + try: + tracker = FaceTracker(ros_version) + tracker.run() + except Exception as e: + if ros_version == '1': + rospy.logerr(f"An error occurred: {e}") + else: + rclpy.shutdown() + print(f"An error occurred: {e}") if __name__ == '__main__': - try: - tracker = FaceTracker() - tracker.run() - except rospy.ROSInterruptException: - pass + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + try: + import rospy + main(ros_version) + except rospy.ROSInterruptException: + print("Error in ROS1 main") + elif ros_version == "2": + try: + import rclpy + from rclpy.node import Node + main(ros_version) + except KeyboardInterrupt: + rclpy.shutdown() + print("Error in ROS2 main") + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) From 9f051b74d66f8c8da8421af626da3ac6a76112ab Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 23 Jul 2024 22:34:35 -0400 Subject: [PATCH 53/59] latest pushes --- .../src/face_follower.py | 205 +++++-------- .../src/object_follower.py | 269 ++++++++++-------- 2 files changed, 221 insertions(+), 253 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py index 40fd0da..e6d4b4f 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py +++ b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py @@ -18,16 +18,24 @@ import os import sys import math -from std_msgs.msg import String from sensor_msgs.msg import JointState +from std_msgs.msg import String from geometry_msgs.msg import Point from humanoid_robot_intelligence_control_system_walking_module_msgs.msg import WalkingParam from humanoid_robot_intelligence_control_system_walking_module_msgs.srv import GetWalkingParam +from humanoid_robot_intelligence_control_system_detection.follower import calculate_distance_to_face, calculate_footstep, calculate_delta_time +from humanoid_robot_intelligence_control_system_detection.follower_config import FollowerConfig, FollowerInitializeConfig class FaceFollower: def __init__(self, ros_version): self.ros_version = ros_version + self.initialize_config = FollowerInitializeConfig() + self.config = FollowerConfig() + self.initialize_ros() + self.setup_ros_communication() + + def initialize_ros(self): if self.ros_version == '1': rospy.init_node('face_follower') self.get_param = rospy.get_param @@ -37,38 +45,9 @@ class FaceFollower: self.node = rclpy.create_node('face_follower') self.get_param = self.node.get_parameter self.logger = self.node.get_logger() - - self.initialize_parameters() - self.setup_ros_communication() - - - def initialize_parameters(self): - self.FOV_WIDTH = 35.2 * math.pi / 180 - self.FOV_HEIGHT = 21.6 * math.pi / 180 - self.count_not_found = 0 - self.count_to_approach = 0 - self.on_following = False - self.approach_face_position = "NotFound" - self.CAMERA_HEIGHT = 0.46 - self.NOT_FOUND_THRESHOLD = 50 - self.MAX_FB_STEP = 40.0 * 0.001 - self.MAX_RL_TURN = 15.0 * math.pi / 180 - self.IN_PLACE_FB_STEP = -3.0 * 0.001 - self.MIN_FB_STEP = 5.0 * 0.001 - self.MIN_RL_TURN = 5.0 * math.pi / 180 - self.UNIT_FB_STEP = 1.0 * 0.001 - self.UNIT_RL_TURN = 0.5 * math.pi / 180 - self.SPOT_FB_OFFSET = 0.0 * 0.001 - self.SPOT_RL_OFFSET = 0.0 * 0.001 - self.SPOT_ANGLE_OFFSET = 0.0 - self.hip_pitch_offset = 7.0 - self.current_pan = -10 - self.current_tilt = -10 - self.current_x_move = 0.005 - self.current_r_angle = 0 - self.curr_period_time = 0.6 - self.accum_period_time = 0.0 - self.DEBUG_PRINT = False + + self.config.update_from_params(self.get_param) + self.initialize_config.update_from_params(self.get_param) def setup_ros_communication(self): if self.ros_version == '1': @@ -80,8 +59,8 @@ class FaceFollower: "/humanoid_robot_intelligence_control_system/walking/set_params", WalkingParam, queue_size=1) self.get_walking_param_client = rospy.ServiceProxy( "/humanoid_robot_intelligence_control_system/walking/get_params", GetWalkingParam) - self.face_position_sub = rospy.Subscriber( - "/face_detector/face_position", Point, self.face_position_callback) + self.face_detection_sub = rospy.Subscriber( + "/face_detection_result", Point, self.face_detection_callback) self.prev_time = rospy.Time.now() else: self.current_joint_states_sub = self.node.create_subscription( @@ -92,127 +71,103 @@ class FaceFollower: WalkingParam, "/humanoid_robot_intelligence_control_system/walking/set_params", 1) self.get_walking_param_client = self.node.create_client( GetWalkingParam, "/humanoid_robot_intelligence_control_system/walking/get_params") - self.face_position_sub = self.node.create_subscription( - Point, "/face_detector/face_position", self.face_position_callback, 10) + self.face_detection_sub = self.node.create_subscription( + Point, "/face_detection_result", self.face_detection_callback, 10) self.prev_time = self.node.get_clock().now() - self.current_walking_param = WalkingParam() - def start_following(self): - self.on_following = True - self.logger.info("Start Face following") + self.initialize_config.on_tracking = True + self.logger.info("Start face following") self.set_walking_command("start") result = self.get_walking_param() if result: - self.hip_pitch_offset = self.current_walking_param.hip_pitch_offset - self.curr_period_time = self.current_walking_param.period_time + self.config.hip_pitch_offset = self.current_walking_param.hip_pitch_offset + self.config.curr_period_time = self.current_walking_param.period_time else: - self.hip_pitch_offset = 7.0 * math.pi / 180 - self.curr_period_time = 0.6 + self.config.hip_pitch_offset = 7.0 * math.pi / 180 + self.config.curr_period_time = 0.6 def stop_following(self): - self.on_following = False - self.count_to_approach = 0 - self.logger.info("Stop Face following") + self.initialize_config.on_tracking = False + self.initialize_config.count_to_approach = 0 + self.logger.info("Stop face following") self.set_walking_command("stop") def current_joint_states_callback(self, msg): for i, name in enumerate(msg.name): if name == "head_pan": - self.current_pan = msg.position[i] + self.initialize_config.current_pan = msg.position[i] elif name == "head_tilt": - self.current_tilt = msg.position[i] - - def face_position_callback(self, msg): - if self.on_following: - self.process_following(msg.x, msg.y, msg.z) - - def calc_footstep(self, target_distance, target_angle, delta_time): - next_movement = self.current_x_move - target_distance = max(0, target_distance) - fb_goal = min(target_distance * 0.1, self.MAX_FB_STEP) - self.accum_period_time += delta_time - if self.accum_period_time > (self.curr_period_time / 4): - self.accum_period_time = 0.0 - if (target_distance * 0.1 / 2) < self.current_x_move: - next_movement -= self.UNIT_FB_STEP - else: - next_movement += self.UNIT_FB_STEP - fb_goal = min(next_movement, fb_goal) - fb_move = max(fb_goal, self.MIN_FB_STEP) - - rl_goal = 0.0 - if abs(target_angle) * 180 / math.pi > 5.0: - rl_offset = abs(target_angle) * 0.2 - rl_goal = min(rl_offset, self.MAX_RL_TURN) - rl_goal = max(rl_goal, self.MIN_RL_TURN) - rl_angle = min(abs(self.current_r_angle) + self.UNIT_RL_TURN, rl_goal) - if target_angle < 0: - rl_angle *= -1 - else: - rl_angle = 0 - - return fb_move, rl_angle + self.initialize_config.current_tilt = msg.position[i] def process_following(self, x_angle, y_angle, face_size): - if self.ros_version == '1': - curr_time = rospy.Time.now() - delta_time = (curr_time - self.prev_time).to_sec() - else: - curr_time = self.node.get_clock().now() - delta_time = (curr_time - self.prev_time).nanoseconds / 1e9 + curr_time = rospy.Time.now() if self.ros_version == '1' else self.node.get_clock().now() + delta_time = calculate_delta_time(curr_time, self.prev_time) self.prev_time = curr_time - self.count_not_found = 0 + self.initialize_config.count_not_found = 0 - if self.current_tilt == -10 and self.current_pan == -10: + if self.initialize_config.current_tilt == -10 and self.initialize_config.current_pan == -10: self.logger.error("Failed to get current angle of head joints.") self.set_walking_command("stop") - self.on_following = False - self.approach_face_position = "NotFound" + self.initialize_config.on_tracking = False + self.initialize_config.approach_face_position = "NotFound" return False - self.approach_face_position = "OutOfRange" + self.initialize_config.approach_face_position = "OutOfRange" - distance_to_face = self.CAMERA_HEIGHT * math.tan(math.pi * 0.5 + self.current_tilt - self.hip_pitch_offset - face_size) - distance_to_face = abs(distance_to_face) + distance_to_face = calculate_distance_to_face( + self.config.CAMERA_HEIGHT, self.initialize_config.current_tilt, self.config.hip_pitch_offset, face_size) - distance_to_approach = 0.5 # Adjust this value as needed + distance_to_approach = 0.22 if (distance_to_face < distance_to_approach) and (abs(x_angle) < 25.0): - self.count_to_approach += 1 - if self.count_to_approach > 20: + self.initialize_config.count_to_approach += 1 + if self.initialize_config.count_to_approach > 20: self.set_walking_command("stop") - self.on_following = False - self.approach_face_position = "OnLeft" if x_angle > 0 else "OnRight" + self.initialize_config.on_tracking = False + self.initialize_config.approach_face_position = "OnLeft" if x_angle > 0 else "OnRight" return True - elif self.count_to_approach > 15: - self.set_walking_param(self.IN_PLACE_FB_STEP, 0, 0) + elif self.initialize_config.count_to_approach > 15: + self.set_walking_param(self.config.IN_PLACE_FB_STEP, 0, 0) return False else: - self.count_to_approach = 0 + self.initialize_config.count_to_approach = 0 distance_to_walk = distance_to_face - distance_to_approach - fb_move, rl_angle = self.calc_footstep(distance_to_walk, self.current_pan, delta_time) + fb_move, rl_angle = calculate_footstep( + self.initialize_config.current_x_move, distance_to_walk, self.initialize_config.current_r_angle, self.initialize_config.current_pan, delta_time, self.config) self.set_walking_param(fb_move, 0, rl_angle) return False + def decide_face_position(self, x_angle, y_angle): + if self.initialize_config.current_tilt == -10 and self.initialize_config.current_pan == -10: + self.initialize_config.approach_face_position = "NotFound" + return + face_x_angle = self.initialize_config.current_pan + x_angle + self.initialize_config.approach_face_position = "OnLeft" if face_x_angle > 0 else "OnRight" + + def wait_following(self): + self.initialize_config.count_not_found += 1 + if self.initialize_config.count_not_found > self.config.NOT_FOUND_THRESHOLD * 0.5: + self.set_walking_param(0.0, 0.0, 0.0) + def set_walking_command(self, command): if command == "start": self.get_walking_param() - self.set_walking_param(self.IN_PLACE_FB_STEP, 0, 0, True) + self.set_walking_param(self.config.IN_PLACE_FB_STEP, 0, 0, True) msg = String() msg.data = command self.set_walking_command_pub.publish(msg) def set_walking_param(self, x_move, y_move, rotation_angle, balance=False): self.current_walking_param.balance_enable = balance - self.current_walking_param.x_move_amplitude = x_move + self.SPOT_FB_OFFSET - self.current_walking_param.y_move_amplitude = y_move + self.SPOT_RL_OFFSET - self.current_walking_param.angle_move_amplitude = rotation_angle + self.SPOT_ANGLE_OFFSET + self.current_walking_param.x_move_amplitude = x_move + self.config.SPOT_FB_OFFSET + self.current_walking_param.y_move_amplitude = y_move + self.config.SPOT_RL_OFFSET + self.current_walking_param.angle_move_amplitude = rotation_angle + self.config.SPOT_ANGLE_OFFSET self.set_walking_param_pub.publish(self.current_walking_param) - self.current_x_move = x_move - self.current_r_angle = rotation_angle + self.initialize_config.current_x_move = x_move + self.initialize_config.current_r_angle = rotation_angle def get_walking_param(self): if self.ros_version == '1': @@ -233,19 +188,20 @@ class FaceFollower: self.logger.error('Service call failed %r' % (future.exception(),)) return False + def face_detection_callback(self, msg): + if self.initialize_config.on_tracking: + x_angle = msg.x + y_angle = msg.y + face_size = msg.z + self.process_following(x_angle, y_angle, face_size) + else: + self.wait_following() + def run(self): if self.ros_version == '1': - rate = rospy.Rate(30) - while not rospy.is_shutdown(): - if self.on_following: - self.process_following(self.current_pan, self.current_tilt, 0.1) - rate.sleep() + rospy.spin() else: - while rclpy.ok(): - if self.on_following: - self.process_following(self.current_pan, self.current_tilt, 0.1) - rclpy.spin_once(self.node, timeout_sec=0.03) - + rclpy.spin(self.node) def main(ros_version): try: @@ -259,26 +215,21 @@ def main(ros_version): rclpy.shutdown() print(f"An error occurred: {e}") - - if __name__ == '__main__': ros_version = os.getenv("ROS_VERSION") if ros_version == "1": + import rospy try: - import rospy main(ros_version) except rospy.ROSInterruptException: print("Error in ROS1 main") - elif ros_version == "2": + import rclpy try: - import rclpy - from rclpy.node import Node main(ros_version) except KeyboardInterrupt: rclpy.shutdown() - print("Error in ROS1 main") - + print("Error in ROS2 main") else: print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/src/object_follower.py b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py index 87c320a..5377ce5 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/object_follower.py +++ b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py @@ -15,193 +15,181 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . -import rospy +import os +import sys import math from sensor_msgs.msg import JointState from std_msgs.msg import String +from geometry_msgs.msg import Point from humanoid_robot_intelligence_control_system_walking_module_msgs.msg import WalkingParam from humanoid_robot_intelligence_control_system_walking_module_msgs.srv import GetWalkingParam -from geometry_msgs.msg import Point +from humanoid_robot_intelligence_control_system_detection.follower import calculate_distance_to_object, calculate_footstep, calculate_delta_time +from humanoid_robot_intelligence_control_system_detection.follower_config import FollowerConfig, FollowerInitializeConfig class ObjectFollower: - def __init__(self): - rospy.init_node('object_follower') + def __init__(self, ros_version): + self.ros_version = ros_version + self.initialize_config = FollowerInitializeConfig() + self.config = FollowerConfig() + self.initialize_ros() + self.setup_ros_communication() + + def initialize_ros(self): + if self.ros_version == '1': + rospy.init_node('object_follower') + self.get_param = rospy.get_param + self.logger = rospy + else: + rclpy.init() + self.node = rclpy.create_node('object_follower') + self.get_param = self.node.get_parameter + self.logger = self.node.get_logger() - self.FOV_WIDTH = 35.2 * math.pi / 180 - self.FOV_HEIGHT = 21.6 * math.pi / 180 - self.count_not_found = 0 - self.count_to_approach = 0 - self.on_tracking = False - self.approach_object_position = "NotFound" - self.CAMERA_HEIGHT = 0.46 - self.NOT_FOUND_THRESHOLD = 50 - self.MAX_FB_STEP = 40.0 * 0.001 - self.MAX_RL_TURN = 15.0 * math.pi / 180 - self.IN_PLACE_FB_STEP = -3.0 * 0.001 - self.MIN_FB_STEP = 5.0 * 0.001 - self.MIN_RL_TURN = 5.0 * math.pi / 180 - self.UNIT_FB_STEP = 1.0 * 0.001 - self.UNIT_RL_TURN = 0.5 * math.pi / 180 - self.SPOT_FB_OFFSET = 0.0 * 0.001 - self.SPOT_RL_OFFSET = 0.0 * 0.001 - self.SPOT_ANGLE_OFFSET = 0.0 - self.hip_pitch_offset = 7.0 - self.current_pan = -10 - self.current_tilt = -10 - self.current_x_move = 0.005 - self.current_r_angle = 0 - self.curr_period_time = 0.6 - self.accum_period_time = 0.0 - self.DEBUG_PRINT = False + self.config.update_from_params(self.get_param) + self.initialize_config.update_from_params(self.get_param) - self.current_joint_states_sub = rospy.Subscriber( - "/humanoid_robot_intelligence_control_system/goal_joint_states", JointState, self.current_joint_states_callback) - self.set_walking_command_pub = rospy.Publisher( - "/humanoid_robot_intelligence_control_system/walking/command", String, queue_size=1) - self.set_walking_param_pub = rospy.Publisher( - "/humanoid_robot_intelligence_control_system/walking/set_params", WalkingParam, queue_size=1) - self.get_walking_param_client = rospy.ServiceProxy( - "/humanoid_robot_intelligence_control_system/walking/get_params", GetWalkingParam) - - # Subscribe to object detection results - self.object_detection_sub = rospy.Subscriber( - "/object_detection_result", Point, self.object_detection_callback) - - self.prev_time = rospy.Time.now() - self.current_walking_param = WalkingParam() + def setup_ros_communication(self): + if self.ros_version == '1': + self.current_joint_states_sub = rospy.Subscriber( + "/humanoid_robot_intelligence_control_system/goal_joint_states", JointState, self.current_joint_states_callback) + self.set_walking_command_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/command", String, queue_size=1) + self.set_walking_param_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/set_params", WalkingParam, queue_size=1) + self.get_walking_param_client = rospy.ServiceProxy( + "/humanoid_robot_intelligence_control_system/walking/get_params", GetWalkingParam) + self.object_detection_sub = rospy.Subscriber( + "/object_detection_result", Point, self.object_detection_callback) + self.prev_time = rospy.Time.now() + else: + self.current_joint_states_sub = self.node.create_subscription( + JointState, "/humanoid_robot_intelligence_control_system/goal_joint_states", self.current_joint_states_callback, 10) + self.set_walking_command_pub = self.node.create_publisher( + String, "/humanoid_robot_intelligence_control_system/walking/command", 1) + self.set_walking_param_pub = self.node.create_publisher( + WalkingParam, "/humanoid_robot_intelligence_control_system/walking/set_params", 1) + self.get_walking_param_client = self.node.create_client( + GetWalkingParam, "/humanoid_robot_intelligence_control_system/walking/get_params") + self.object_detection_sub = self.node.create_subscription( + Point, "/object_detection_result", self.object_detection_callback, 10) + self.prev_time = self.node.get_clock().now() def start_following(self): - self.on_tracking = True - rospy.loginfo("Start Object following") + self.initialize_config.on_tracking = True + self.logger.info("Start Object following") self.set_walking_command("start") result = self.get_walking_param() if result: - self.hip_pitch_offset = self.current_walking_param.hip_pitch_offset - self.curr_period_time = self.current_walking_param.period_time + self.config.hip_pitch_offset = self.current_walking_param.hip_pitch_offset + self.config.curr_period_time = self.current_walking_param.period_time else: - self.hip_pitch_offset = 7.0 * math.pi / 180 - self.curr_period_time = 0.6 + self.config.hip_pitch_offset = 7.0 * math.pi / 180 + self.config.curr_period_time = 0.6 def stop_following(self): - self.on_tracking = False - self.count_to_approach = 0 - rospy.loginfo("Stop Object following") + self.initialize_config.on_tracking = False + self.initialize_config.count_to_approach = 0 + self.logger.info("Stop Object following") self.set_walking_command("stop") def current_joint_states_callback(self, msg): for i, name in enumerate(msg.name): if name == "head_pan": - self.current_pan = msg.position[i] + self.initialize_config.current_pan = msg.position[i] elif name == "head_tilt": - self.current_tilt = msg.position[i] - - def calc_footstep(self, target_distance, target_angle, delta_time): - next_movement = self.current_x_move - target_distance = max(0, target_distance) - fb_goal = min(target_distance * 0.1, self.MAX_FB_STEP) - self.accum_period_time += delta_time - if self.accum_period_time > (self.curr_period_time / 4): - self.accum_period_time = 0.0 - if (target_distance * 0.1 / 2) < self.current_x_move: - next_movement -= self.UNIT_FB_STEP - else: - next_movement += self.UNIT_FB_STEP - fb_goal = min(next_movement, fb_goal) - fb_move = max(fb_goal, self.MIN_FB_STEP) - - rl_goal = 0.0 - if abs(target_angle) * 180 / math.pi > 5.0: - rl_offset = abs(target_angle) * 0.2 - rl_goal = min(rl_offset, self.MAX_RL_TURN) - rl_goal = max(rl_goal, self.MIN_RL_TURN) - rl_angle = min(abs(self.current_r_angle) + self.UNIT_RL_TURN, rl_goal) - if target_angle < 0: - rl_angle *= -1 - else: - rl_angle = 0 - - return fb_move, rl_angle + self.initialize_config.current_tilt = msg.position[i] def process_following(self, x_angle, y_angle, object_size): - curr_time = rospy.Time.now() - delta_time = (curr_time - self.prev_time).to_sec() + curr_time = rospy.Time.now() if self.ros_version == '1' else self.node.get_clock().now() + delta_time = calculate_delta_time(curr_time, self.prev_time) self.prev_time = curr_time - self.count_not_found = 0 + self.initialize_config.count_not_found = 0 - if self.current_tilt == -10 and self.current_pan == -10: - rospy.logerr("Failed to get current angle of head joints.") + if self.initialize_config.current_tilt == -10 and self.initialize_config.current_pan == -10: + self.logger.error("Failed to get current angle of head joints.") self.set_walking_command("stop") - self.on_tracking = False - self.approach_object_position = "NotFound" + self.initialize_config.on_tracking = False + self.initialize_config.approach_object_position = "NotFound" return False - self.approach_object_position = "OutOfRange" + self.initialize_config.approach_object_position = "OutOfRange" - distance_to_object = self.CAMERA_HEIGHT * math.tan(math.pi * 0.5 + self.current_tilt - self.hip_pitch_offset - object_size) - distance_to_object = abs(distance_to_object) + distance_to_object = calculate_distance_to_object( + self.config.CAMERA_HEIGHT, self.initialize_config.current_tilt, self.config.hip_pitch_offset, object_size) distance_to_approach = 0.22 if (distance_to_object < distance_to_approach) and (abs(x_angle) < 25.0): - self.count_to_approach += 1 - if self.count_to_approach > 20: + self.initialize_config.count_to_approach += 1 + if self.initialize_config.count_to_approach > 20: self.set_walking_command("stop") - self.on_tracking = False - self.approach_object_position = "OnLeft" if x_angle > 0 else "OnRight" + self.initialize_config.on_tracking = False + self.initialize_config.approach_object_position = "OnLeft" if x_angle > 0 else "OnRight" return True - elif self.count_to_approach > 15: - self.set_walking_param(self.IN_PLACE_FB_STEP, 0, 0) + elif self.initialize_config.count_to_approach > 15: + self.set_walking_param(self.config.IN_PLACE_FB_STEP, 0, 0) return False else: - self.count_to_approach = 0 + self.initialize_config.count_to_approach = 0 distance_to_walk = distance_to_object - distance_to_approach - fb_move, rl_angle = self.calc_footstep(distance_to_walk, self.current_pan, delta_time) + fb_move, rl_angle = calculate_footstep( + self.initialize_config.current_x_move, distance_to_walk, self.initialize_config.current_r_angle, self.initialize_config.current_pan, delta_time, self.config) self.set_walking_param(fb_move, 0, rl_angle) return False def decide_object_position(self, x_angle, y_angle): - if self.current_tilt == -10 and self.current_pan == -10: - self.approach_object_position = "NotFound" + if self.initialize_config.current_tilt == -10 and self.initialize_config.current_pan == -10: + self.initialize_config.approach_object_position = "NotFound" return - object_x_angle = self.current_pan + x_angle - self.approach_object_position = "OnLeft" if object_x_angle > 0 else "OnRight" + object_x_angle = self.initialize_config.current_pan + x_angle + self.initialize_config.approach_object_position = "OnLeft" if object_x_angle > 0 else "OnRight" def wait_following(self): - self.count_not_found += 1 - if self.count_not_found > self.NOT_FOUND_THRESHOLD * 0.5: + self.initialize_config.count_not_found += 1 + if self.initialize_config.count_not_found > self.config.NOT_FOUND_THRESHOLD * 0.5: self.set_walking_param(0.0, 0.0, 0.0) def set_walking_command(self, command): if command == "start": self.get_walking_param() - self.set_walking_param(self.IN_PLACE_FB_STEP, 0, 0, True) + self.set_walking_param(self.config.IN_PLACE_FB_STEP, 0, 0, True) msg = String() msg.data = command self.set_walking_command_pub.publish(msg) def set_walking_param(self, x_move, y_move, rotation_angle, balance=False): self.current_walking_param.balance_enable = balance - self.current_walking_param.x_move_amplitude = x_move + self.SPOT_FB_OFFSET - self.current_walking_param.y_move_amplitude = y_move + self.SPOT_RL_OFFSET - self.current_walking_param.angle_move_amplitude = rotation_angle + self.SPOT_ANGLE_OFFSET + self.current_walking_param.x_move_amplitude = x_move + self.config.SPOT_FB_OFFSET + self.current_walking_param.y_move_amplitude = y_move + self.config.SPOT_RL_OFFSET + self.current_walking_param.angle_move_amplitude = rotation_angle + self.config.SPOT_ANGLE_OFFSET self.set_walking_param_pub.publish(self.current_walking_param) - self.current_x_move = x_move - self.current_r_angle = rotation_angle + self.initialize_config.current_x_move = x_move + self.initialize_config.current_r_angle = rotation_angle def get_walking_param(self): - try: - response = self.get_walking_param_client() - self.current_walking_param = response.parameters - return True - except rospy.ServiceException as e: - rospy.logerr("Failed to get walking parameters: %s" % e) - return False + if self.ros_version == '1': + try: + response = self.get_walking_param_client() + self.current_walking_param = response.parameters + return True + except rospy.ServiceException as e: + self.logger.error("Failed to get walking parameters: %s" % e) + return False + else: + future = self.get_walking_param_client.call_async(GetWalkingParam.Request()) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is not None: + self.current_walking_param = future.result().parameters + return True + else: + self.logger.error('Service call failed %r' % (future.exception(),)) + return False def object_detection_callback(self, msg): - if self.on_tracking: + if self.initialize_config.on_tracking: x_angle = msg.x y_angle = msg.y object_size = msg.z @@ -209,10 +197,39 @@ class ObjectFollower: else: self.wait_following() -if __name__ == '__main__': + def run(self): + if self.ros_version == '1': + rospy.spin() + else: + rclpy.spin(self.node) + +def main(ros_version): try: - follower = ObjectFollower() + follower = ObjectFollower(ros_version) follower.start_following() - rospy.spin() - except rospy.ROSInterruptException: - pass + follower.run() + except Exception as e: + if ros_version == '1': + rospy.logerr(f"An error occurred: {e}") + else: + rclpy.shutdown() + print(f"An error occurred: {e}") + +if __name__ == '__main__': + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + import rospy + try: + main(ros_version) + except rospy.ROSInterruptException: + print("Error in ROS1 main") + elif ros_version == "2": + import rclpy + try: + main(ros_version) + except KeyboardInterrupt: + rclpy.shutdown() + print("Error in ROS2 main") + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) From 7e61eae008dc936f954772c26ee959c02ad4a1b2 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Wed, 24 Jul 2024 00:05:59 -0400 Subject: [PATCH 54/59] latest pushes --- .../src/face_follower.py | 5 +- .../src/face_tracker.py | 157 +++++------- .../src/object_follower.py | 5 +- .../src/object_tracker.py | 233 ++++++++++-------- 4 files changed, 200 insertions(+), 200 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py index e6d4b4f..f12b5a4 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py +++ b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py @@ -24,9 +24,10 @@ from geometry_msgs.msg import Point from humanoid_robot_intelligence_control_system_walking_module_msgs.msg import WalkingParam from humanoid_robot_intelligence_control_system_walking_module_msgs.srv import GetWalkingParam -from humanoid_robot_intelligence_control_system_detection.follower import calculate_distance_to_face, calculate_footstep, calculate_delta_time +from humanoid_robot_intelligence_control_system_detection.follower import calculate_distance_to, calculate_footstep, calculate_delta_time from humanoid_robot_intelligence_control_system_detection.follower_config import FollowerConfig, FollowerInitializeConfig + class FaceFollower: def __init__(self, ros_version): self.ros_version = ros_version @@ -116,7 +117,7 @@ class FaceFollower: self.initialize_config.approach_face_position = "OutOfRange" - distance_to_face = calculate_distance_to_face( + distance_to_face = calculate_distance_to( self.config.CAMERA_HEIGHT, self.initialize_config.current_tilt, self.config.hip_pitch_offset, face_size) distance_to_approach = 0.22 diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py index 138d645..a7776ab 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py +++ b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py @@ -15,31 +15,17 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . -import os -import sys +import rospy import math from sensor_msgs.msg import JointState -from std_msgs.msg import String +from std_msgs.msg import String, Float64MultiArray +from humanoid_robot_intelligence_control_system_ball_detector.msg import CircleSetStamped from geometry_msgs.msg import Point class FaceTracker: - def __init__(self, ros_version): - self.ros_version = ros_version - if self.ros_version == '1': - rospy.init_node('face_tracker') - self.get_param = rospy.get_param - self.logger = rospy - else: - rclpy.init() - self.node = Node('face_tracker') - self.get_param = self.node.get_parameter - self.logger = self.node.get_logger() - - self.initialize_parameters() - self.setup_ros_communication() - - - def initialize_parameters(self): + def __init__(self): + rospy.init_node('face_tracker') + self.FOV_WIDTH = 35.2 * math.pi / 180 self.FOV_HEIGHT = 21.6 * math.pi / 180 self.NOT_FOUND_THRESHOLD = 50 @@ -51,38 +37,31 @@ class FaceTracker: self.current_face_tilt = 0 self.x_error_sum = 0 self.y_error_sum = 0 + self.current_face_bottom = 0 self.tracking_status = "NotFound" self.DEBUG_PRINT = False - - self.p_gain = self.get_param('p_gain', 0.4) - self.i_gain = self.get_param('i_gain', 0.0) - self.d_gain = self.get_param('d_gain', 0.0) - self.logger.info(f"Face tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}") + param_nh = rospy.get_param("~") + self.p_gain = param_nh.get("p_gain", 0.4) + self.i_gain = param_nh.get("i_gain", 0.0) + self.d_gain = param_nh.get("d_gain", 0.0) - def setup_ros_communication(self): - if self.ros_version == '1': - self.head_joint_pub = rospy.Publisher( - "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=1) - self.head_scan_pub = rospy.Publisher( - "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=1) - self.face_position_sub = rospy.Subscriber( - "/face_detector/face_position", Point, self.face_position_callback) - self.face_tracking_command_sub = rospy.Subscriber( - "/face_tracker/command", String, self.face_tracker_command_callback) - self.prev_time = rospy.Time.now() - else: - self.head_joint_pub = self.node.create_publisher( - JointState, "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 1) - self.head_scan_pub = self.node.create_publisher( - String, "/humanoid_robot_intelligence_control_system/head_control/scan_command", 1) - self.face_position_sub = self.node.create_subscription( - Point, "/face_detector/face_position", self.face_position_callback, 10) - self.face_tracking_command_sub = self.node.create_subscription( - String, "/face_tracker/command", self.face_tracker_command_callback, 10) - self.prev_time = self.node.get_clock().now() + rospy.loginfo(f"face tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}") + + self.head_joint_offset_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", JointState, queue_size=0) + self.head_joint_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=0) + self.head_scan_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=0) + + self.face_position_sub = rospy.Subscriber( + "/face_detector_node/face_position", Point, self.face_position_callback) + self.face_tracking_command_sub = rospy.Subscriber( + "/face_tracker/command", String, self.face_tracker_command_callback) self.face_position = Point() + self.prev_time = rospy.Time.now() def face_position_callback(self, msg): self.face_position = msg @@ -100,17 +79,20 @@ class FaceTracker: def start_tracking(self): self.on_tracking = True - self.logger.info("Start Face tracking") + rospy.loginfo("Start face tracking") if self.DEBUG_PRINT else None def stop_tracking(self): self.go_init() self.on_tracking = False - self.logger.info("Stop Face tracking") + rospy.loginfo("Stop face tracking") if self.DEBUG_PRINT else None self.current_face_pan = 0 self.current_face_tilt = 0 self.x_error_sum = 0 self.y_error_sum = 0 + def set_using_head_scan(self, use_scan): + self.use_head_scan = use_scan + def process_tracking(self): if not self.on_tracking: self.face_position.z = 0 @@ -120,7 +102,10 @@ class FaceTracker: if self.face_position.z <= 0: self.count_not_found += 1 if self.count_not_found < self.WAITING_THRESHOLD: - tracking_status = "Waiting" + if self.tracking_status in ["Found", "Waiting"]: + tracking_status = "Waiting" + else: + tracking_status = "NotFound" elif self.count_not_found > self.NOT_FOUND_THRESHOLD: self.scan_face() self.count_not_found = 0 @@ -133,17 +118,18 @@ class FaceTracker: if tracking_status != "Found": self.tracking_status = tracking_status + self.current_face_pan = 0 + self.current_face_tilt = 0 + self.x_error_sum = 0 + self.y_error_sum = 0 return tracking_status x_error = -math.atan(self.face_position.x * math.tan(self.FOV_WIDTH)) y_error = -math.atan(self.face_position.y * math.tan(self.FOV_HEIGHT)) + face_size = self.face_position.z - if self.ros_version == '1': - curr_time = rospy.Time.now() - delta_time = (curr_time - self.prev_time).to_sec() - else: - curr_time = self.node.get_clock().now() - delta_time = (curr_time - self.prev_time).nanoseconds / 1e9 + curr_time = rospy.Time.now() + delta_time = (curr_time - self.prev_time).to_sec() self.prev_time = curr_time x_error_diff = (x_error - self.current_face_pan) / delta_time @@ -154,15 +140,18 @@ class FaceTracker: y_error_target = y_error * self.p_gain + y_error_diff * self.d_gain + self.y_error_sum * self.i_gain if self.DEBUG_PRINT: - self.logger.info(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}") - self.logger.info(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}") - self.logger.info(f"Error sum: {self.x_error_sum * 180 / math.pi} | {self.y_error_sum * 180 / math.pi}") - self.logger.info(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}") + rospy.loginfo(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}") + rospy.loginfo(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}") + rospy.loginfo(f"Error sum: {self.x_error_sum * 180 / math.pi} | {self.y_error_sum * 180 / math.pi}") + rospy.loginfo(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}") self.publish_head_joint(x_error_target, y_error_target) self.current_face_pan = x_error self.current_face_tilt = y_error + self.current_face_bottom = face_size + + self.face_position.z = 0 self.tracking_status = tracking_status return tracking_status @@ -176,7 +165,7 @@ class FaceTracker: head_angle_msg.name = ["head_pan", "head_tilt"] head_angle_msg.position = [pan, tilt] - self.head_joint_pub.publish(head_angle_msg) + self.head_joint_offset_pub.publish(head_angle_msg) def go_init(self): head_angle_msg = JointState() @@ -194,44 +183,12 @@ class FaceTracker: self.head_scan_pub.publish(scan_msg) - def run(self): - if self.ros_version == '1': - rate = rospy.Rate(30) # 30Hz - while not rospy.is_shutdown(): - self.process_tracking() - rate.sleep() - else: - while rclpy.ok(): - self.process_tracking() - rclpy.spin_once(self.node, timeout_sec=0.03) # Approximately 30Hz - -def main(ros_version): - try: - tracker = FaceTracker(ros_version) - tracker.run() - except Exception as e: - if ros_version == '1': - rospy.logerr(f"An error occurred: {e}") - else: - rclpy.shutdown() - print(f"An error occurred: {e}") - if __name__ == '__main__': - ros_version = os.getenv("ROS_VERSION") - if ros_version == "1": - try: - import rospy - main(ros_version) - except rospy.ROSInterruptException: - print("Error in ROS1 main") - elif ros_version == "2": - try: - import rclpy - from rclpy.node import Node - main(ros_version) - except KeyboardInterrupt: - rclpy.shutdown() - print("Error in ROS2 main") - else: - print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") - sys.exit(1) + try: + tracker = FaceTracker() + rate = rospy.Rate(30) + while not rospy.is_shutdown(): + tracker.process_tracking() + rate.sleep() + except rospy.ROSInterruptException: + pass diff --git a/humanoid_robot_intelligence_control_system_configure/src/object_follower.py b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py index 5377ce5..ebe736f 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/object_follower.py +++ b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py @@ -24,9 +24,10 @@ from geometry_msgs.msg import Point from humanoid_robot_intelligence_control_system_walking_module_msgs.msg import WalkingParam from humanoid_robot_intelligence_control_system_walking_module_msgs.srv import GetWalkingParam -from humanoid_robot_intelligence_control_system_detection.follower import calculate_distance_to_object, calculate_footstep, calculate_delta_time +from humanoid_robot_intelligence_control_system_detection.follower import calculate_distance_to, calculate_footstep, calculate_delta_time from humanoid_robot_intelligence_control_system_detection.follower_config import FollowerConfig, FollowerInitializeConfig + class ObjectFollower: def __init__(self, ros_version): self.ros_version = ros_version @@ -116,7 +117,7 @@ class ObjectFollower: self.initialize_config.approach_object_position = "OutOfRange" - distance_to_object = calculate_distance_to_object( + distance_to_object = calculate_distance_to( self.config.CAMERA_HEIGHT, self.initialize_config.current_tilt, self.config.hip_pitch_offset, object_size) distance_to_approach = 0.22 diff --git a/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py b/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py index 2d54fba..4f00592 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py +++ b/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py @@ -15,56 +15,68 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . -import rospy +import os +import sys import math from sensor_msgs.msg import JointState -from std_msgs.msg import String, Float64MultiArray -from humanoid_robot_intelligence_control_system_ball_detector.msg import CircleSetStamped +from std_msgs.msg import String from geometry_msgs.msg import Point +from humanoid_robot_intelligence_control_system_detection.tracker import calculate_error, calculate_error_target, calculate_delta_time +from humanoid_robot_intelligence_control_system_detection.tracker_config import TrackerConfig, TrackerInitializeConfig + class ObjectTracker: - def __init__(self): - rospy.init_node('object_tracker') + def __init__(self, ros_version): + self.ros_version = ros_version + self.initialize_config = TrackerInitializeConfig() + self.config = TrackerConfig() + self.initialize_ros() + self.setup_ros_communication() + + def initialize_ros(self): + if self.ros_version == '1': + rospy.init_node('object_tracker') + self.get_param = rospy.get_param + self.logger = rospy + else: + rclpy.init() + self.node = rclpy.create_node('object_tracker') + self.get_param = self.node.get_parameter + self.logger = self.node.get_logger() - self.FOV_WIDTH = 35.2 * math.pi / 180 - self.FOV_HEIGHT = 21.6 * math.pi / 180 - self.NOT_FOUND_THRESHOLD = 50 - self.WAITING_THRESHOLD = 5 - self.use_head_scan = True - self.count_not_found = 0 - self.on_tracking = False - self.current_object_pan = 0 - self.current_object_tilt = 0 - self.x_error_sum = 0 - self.y_error_sum = 0 - self.current_object_bottom = 0 - self.tracking_status = "NotFound" - self.DEBUG_PRINT = False + self.config.update_from_params(self.get_param) + self.initialize_config.update_from_params(self.get_param) - param_nh = rospy.get_param("~") - self.p_gain = param_nh.get("p_gain", 0.4) - self.i_gain = param_nh.get("i_gain", 0.0) - self.d_gain = param_nh.get("d_gain", 0.0) - - rospy.loginfo(f"Object tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}") - - self.head_joint_offset_pub = rospy.Publisher( - "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", JointState, queue_size=0) - self.head_joint_pub = rospy.Publisher( - "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=0) - self.head_scan_pub = rospy.Publisher( - "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=0) - - self.object_position_sub = rospy.Subscriber( - "/object_detector_node/object_position", Point, self.object_position_callback) - self.object_tracking_command_sub = rospy.Subscriber( - "/object_tracker/command", String, self.object_tracker_command_callback) - - self.object_position = Point() - self.prev_time = rospy.Time.now() + def setup_ros_communication(self): + if self.ros_version == '1': + self.head_joint_offset_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", JointState, queue_size=0) + self.head_joint_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=0) + self.head_scan_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=0) + self.object_position_sub = rospy.Subscriber( + "/object_detector_node/object_position", Point, self.object_position_callback) + self.object_tracking_command_sub = rospy.Subscriber( + "/object_tracker/command", String, self.object_tracker_command_callback) + self.prev_time = rospy.Time.now() + else: + self.head_joint_offset_pub = self.node.create_publisher( + JointState, "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", 10) + self.head_joint_pub = self.node.create_publisher( + JointState, "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 10) + self.head_scan_pub = self.node.create_publisher( + String, "/humanoid_robot_intelligence_control_system/head_control/scan_command", 10) + self.object_position_sub = self.node.create_subscription( + Point, "/object_detector_node/object_position", self.object_position_callback, 10) + self.object_tracking_command_sub = self.node.create_subscription( + String, "/object_tracker/command", self.object_tracker_command_callback, 10) + self.prev_time = self.node.get_clock().now() def object_position_callback(self, msg): - self.object_position = msg + self.initialize_config.object_position = msg + if self.initialize_config.on_tracking: + self.process_tracking() def object_tracker_command_callback(self, msg): if msg.data == "start": @@ -72,88 +84,90 @@ class ObjectTracker: elif msg.data == "stop": self.stop_tracking() elif msg.data == "toggle_start": - if not self.on_tracking: + if not self.initialize_config.on_tracking: self.start_tracking() else: self.stop_tracking() def start_tracking(self): - self.on_tracking = True - rospy.loginfo("Start Object tracking") if self.DEBUG_PRINT else None + self.initialize_config.on_tracking = True + self.logger.info("Start Object tracking") def stop_tracking(self): self.go_init() - self.on_tracking = False - rospy.loginfo("Stop Object tracking") if self.DEBUG_PRINT else None - self.current_object_pan = 0 - self.current_object_tilt = 0 - self.x_error_sum = 0 - self.y_error_sum = 0 + self.initialize_config.on_tracking = False + self.logger.info("Stop Object tracking") + self.initialize_config.current_object_pan = 0 + self.initialize_config.current_object_tilt = 0 + self.initialize_config.x_error_sum = 0 + self.initialize_config.y_error_sum = 0 def set_using_head_scan(self, use_scan): - self.use_head_scan = use_scan + self.config.use_head_scan = use_scan def process_tracking(self): - if not self.on_tracking: - self.object_position.z = 0 - self.count_not_found = 0 + if not self.initialize_config.on_tracking: + self.initialize_config.object_position.z = 0 + self.initialize_config.count_not_found = 0 return "NotFound" - if self.object_position.z <= 0: - self.count_not_found += 1 - if self.count_not_found < self.WAITING_THRESHOLD: - if self.tracking_status in ["Found", "Waiting"]: + if self.initialize_config.object_position.z <= 0: + self.initialize_config.count_not_found += 1 + if self.initialize_config.count_not_found < self.config.WAITING_THRESHOLD: + if self.initialize_config.tracking_status in ["Found", "Waiting"]: tracking_status = "Waiting" else: tracking_status = "NotFound" - elif self.count_not_found > self.NOT_FOUND_THRESHOLD: + elif self.initialize_config.count_not_found > self.config.NOT_FOUND_THRESHOLD: self.scan_object() - self.count_not_found = 0 + self.initialize_config.count_not_found = 0 tracking_status = "NotFound" else: tracking_status = "NotFound" else: - self.count_not_found = 0 + self.initialize_config.count_not_found = 0 tracking_status = "Found" if tracking_status != "Found": - self.tracking_status = tracking_status - self.current_object_pan = 0 - self.current_object_tilt = 0 - self.x_error_sum = 0 - self.y_error_sum = 0 + self.initialize_config.tracking_status = tracking_status + self.initialize_config.current_object_pan = 0 + self.initialize_config.current_object_tilt = 0 + self.initialize_config.x_error_sum = 0 + self.initialize_config.y_error_sum = 0 return tracking_status - x_error = -math.atan(self.object_position.x * math.tan(self.FOV_WIDTH)) - y_error = -math.atan(self.object_position.y * math.tan(self.FOV_HEIGHT)) - object_size = self.object_position.z + x_error, y_error = calculate_error(self.initialize_config.object_position, self.config.FOV_WIDTH, self.config.FOV_HEIGHT) + object_size = self.initialize_config.object_position.z - curr_time = rospy.Time.now() - delta_time = (curr_time - self.prev_time).to_sec() + curr_time = rospy.Time.now() if self.ros_version == '1' else self.node.get_clock().now() + delta_time = calculate_delta_time(curr_time, self.prev_time) self.prev_time = curr_time - x_error_diff = (x_error - self.current_object_pan) / delta_time - y_error_diff = (y_error - self.current_object_tilt) / delta_time - self.x_error_sum += x_error - self.y_error_sum += y_error - x_error_target = x_error * self.p_gain + x_error_diff * self.d_gain + self.x_error_sum * self.i_gain - y_error_target = y_error * self.p_gain + y_error_diff * self.d_gain + self.y_error_sum * self.i_gain + x_error_diff = (x_error - self.initialize_config.current_object_pan) / delta_time + y_error_diff = (y_error - self.initialize_config.current_object_tilt) / delta_time + self.initialize_config.x_error_sum += x_error + self.initialize_config.y_error_sum += y_error + + x_error_target = calculate_error_target(x_error, x_error_diff, self.initialize_config.x_error_sum, + self.config.p_gain, self.config.i_gain, self.config.d_gain) + y_error_target = calculate_error_target(y_error, y_error_diff, self.initialize_config.y_error_sum, + self.config.p_gain, self.config.i_gain, self.config.d_gain) - if self.DEBUG_PRINT: - rospy.loginfo(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}") - rospy.loginfo(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}") - rospy.loginfo(f"Error sum: {self.x_error_sum * 180 / math.pi} | {self.y_error_sum * 180 / math.pi}") - rospy.loginfo(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}") + if self.config.DEBUG_PRINT: + self.logger.info(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}") + self.logger.info(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}") + self.logger.info(f"Error sum: {self.initialize_config.x_error_sum * 180 / math.pi} | {self.initialize_config.y_error_sum * 180 / math.pi}") + self.logger.info(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}") self.publish_head_joint(x_error_target, y_error_target) - self.current_object_pan = x_error - self.current_object_tilt = y_error - self.current_object_bottom = object_size + self.initialize_config.current_object_pan = x_error + self.initialize_config.current_object_tilt = y_error + self.initialize_config.current_object_bottom = object_size - self.object_position.z = 0 + self.initialize_config.object_position.z = 0 - self.tracking_status = tracking_status + self.initialize_config.tracking_status = tracking_status return tracking_status def publish_head_joint(self, pan, tilt): @@ -175,7 +189,7 @@ class ObjectTracker: self.head_joint_pub.publish(head_angle_msg) def scan_object(self): - if not self.use_head_scan: + if not self.config.use_head_scan: return scan_msg = String() @@ -183,12 +197,39 @@ class ObjectTracker: self.head_scan_pub.publish(scan_msg) -if __name__ == '__main__': + def run(self): + if self.ros_version == '1': + rospy.spin() + else: + rclpy.spin(self.node) + +def main(ros_version): try: - tracker = ObjectTracker() - rate = rospy.Rate(30) - while not rospy.is_shutdown(): - tracker.process_tracking() - rate.sleep() - except rospy.ROSInterruptException: - pass + tracker = ObjectTracker(ros_version) + tracker.start_tracking() + tracker.run() + except Exception as e: + if ros_version == '1': + rospy.logerr(f"An error occurred: {e}") + else: + rclpy.shutdown() + print(f"An error occurred: {e}") + +if __name__ == '__main__': + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + import rospy + try: + main(ros_version) + except rospy.ROSInterruptException: + print("Error in ROS1 main") + elif ros_version == "2": + import rclpy + try: + main(ros_version) + except KeyboardInterrupt: + rclpy.shutdown() + print("Error in ROS2 main") + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) From 52d1a300e112f9c2a1f98429e0ffeb1f57a8d15b Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Wed, 24 Jul 2024 01:51:43 -0400 Subject: [PATCH 55/59] latest pushes --- .../CMakeLists.txt | 66 ++++++++ .../package.xml | 46 +++++ .../setup.py | 26 +++ .../src/follower.py | 52 ++++++ .../src/follower_config.py | 159 ++++++++++++++++++ .../src/tracker.py | 30 ++++ .../src/tracker_config.py | 126 ++++++++++++++ 7 files changed, 505 insertions(+) create mode 100644 humanoid_robot_intelligence_control_system_detection/CMakeLists.txt create mode 100644 humanoid_robot_intelligence_control_system_detection/package.xml create mode 100644 humanoid_robot_intelligence_control_system_detection/setup.py create mode 100644 humanoid_robot_intelligence_control_system_detection/src/follower.py create mode 100644 humanoid_robot_intelligence_control_system_detection/src/follower_config.py create mode 100644 humanoid_robot_intelligence_control_system_detection/src/tracker.py create mode 100644 humanoid_robot_intelligence_control_system_detection/src/tracker_config.py diff --git a/humanoid_robot_intelligence_control_system_detection/CMakeLists.txt b/humanoid_robot_intelligence_control_system_detection/CMakeLists.txt new file mode 100644 index 0000000..c427fc3 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/CMakeLists.txt @@ -0,0 +1,66 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_detection) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + ) + + catkin_package( + CATKIN_DEPENDS + rospy + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/follower.py + src/follower_config.py + src/tracker.py + src/tracker_config.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/follower.py + src/follower_config.py + src/tracker.py + src/tracker_config.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_detection/package.xml b/humanoid_robot_intelligence_control_system_detection/package.xml new file mode 100644 index 0000000..fa6c357 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/package.xml @@ -0,0 +1,46 @@ + + + + + humanoid_robot_intelligence_control_system_detection + 0.0.1 + + This Package is for Detection Math + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + + + rclpy + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_detection/setup.py b/humanoid_robot_intelligence_control_system_detection/setup.py new file mode 100644 index 0000000..ca93ba9 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/follower.py', "src/follower_config.py", 'src/tracker.py', "src/tracker_config.py"], + packages=['humanoid_robot_intelligence_control_system_detection'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_detection/src/follower.py b/humanoid_robot_intelligence_control_system_detection/src/follower.py new file mode 100644 index 0000000..8d32487 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/follower.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import math + +def calculate_distance_to_object(camera_height, tilt, hip_pitch_offset, object_size): + distance = camera_height * math.tan(math.pi * 0.5 + tilt - hip_pitch_offset - object_size) + return abs(distance) + +def calculate_footstep(current_x_move, target_distance, current_r_angle, target_angle, delta_time, config): + next_movement = current_x_move + target_distance = max(0, target_distance) + fb_goal = min(target_distance * 0.1, config.MAX_FB_STEP) + config.accum_period_time += delta_time + if config.accum_period_time > (config.curr_period_time / 4): + config.accum_period_time = 0.0 + if (target_distance * 0.1 / 2) < current_x_move: + next_movement -= config.UNIT_FB_STEP + else: + next_movement += config.UNIT_FB_STEP + fb_goal = min(next_movement, fb_goal) + fb_move = max(fb_goal, config.MIN_FB_STEP) + + rl_goal = 0.0 + if abs(target_angle) * 180 / math.pi > 5.0: + rl_offset = abs(target_angle) * 0.2 + rl_goal = min(rl_offset, config.MAX_RL_TURN) + rl_goal = max(rl_goal, config.MIN_RL_TURN) + rl_angle = min(abs(current_r_angle) + config.UNIT_RL_TURN, rl_goal) + if target_angle < 0: + rl_angle *= -1 + else: + rl_angle = 0 + + return fb_move, rl_angle + +def calculate_delta_time(curr_time, prev_time): + return (curr_time - prev_time).to_sec() diff --git a/humanoid_robot_intelligence_control_system_detection/src/follower_config.py b/humanoid_robot_intelligence_control_system_detection/src/follower_config.py new file mode 100644 index 0000000..8bd53d1 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/follower_config.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import math + + +class FollowerConfig: + def __init__(self): + self.FOV_WIDTH = 35.2 * math.pi / 180 + self.FOV_HEIGHT = 21.6 * math.pi / 180 + self.CAMERA_HEIGHT = 0.46 + self.NOT_FOUND_THRESHOLD = 50 + self.MAX_FB_STEP = 40.0 * 0.001 + self.MAX_RL_TURN = 15.0 * math.pi / 180 + self.IN_PLACE_FB_STEP = -3.0 * 0.001 + self.MIN_FB_STEP = 5.0 * 0.001 + self.MIN_RL_TURN = 5.0 * math.pi / 180 + self.UNIT_FB_STEP = 1.0 * 0.001 + self.UNIT_RL_TURN = 0.5 * math.pi / 180 + self.SPOT_FB_OFFSET = 0.0 * 0.001 + self.SPOT_RL_OFFSET = 0.0 * 0.001 + self.SPOT_ANGLE_OFFSET = 0.0 + self.hip_pitch_offset = 7.0 + self.curr_period_time = 0.6 + self.accum_period_time = 0.0 + self.DEBUG_PRINT = False + + def update_from_params(self, get_param): + self.FOV_WIDTH = get_param('fov_width', self.FOV_WIDTH) + self.FOV_HEIGHT = get_param('fov_height', self.FOV_HEIGHT) + self.CAMERA_HEIGHT = get_param('camera_height', self.CAMERA_HEIGHT) + self.NOT_FOUND_THRESHOLD = get_param('not_found_threshold', self.NOT_FOUND_THRESHOLD) + self.MAX_FB_STEP = get_param('max_fb_step', self.MAX_FB_STEP) + self.MAX_RL_TURN = get_param('max_rl_turn', self.MAX_RL_TURN) + self.IN_PLACE_FB_STEP = get_param('in_place_fb_step', self.IN_PLACE_FB_STEP) + self.MIN_FB_STEP = get_param('min_fb_step', self.MIN_FB_STEP) + self.MIN_RL_TURN = get_param('min_rl_turn', self.MIN_RL_TURN) + self.UNIT_FB_STEP = get_param('unit_fb_step', self.UNIT_FB_STEP) + self.UNIT_RL_TURN = get_param('unit_rl_turn', self.UNIT_RL_TURN) + self.SPOT_FB_OFFSET = get_param('spot_fb_offset', self.SPOT_FB_OFFSET) + self.SPOT_RL_OFFSET = get_param('spot_rl_offset', self.SPOT_RL_OFFSET) + self.SPOT_ANGLE_OFFSET = get_param('spot_angle_offset', self.SPOT_ANGLE_OFFSET) + self.hip_pitch_offset = get_param('hip_pitch_offset', self.hip_pitch_offset) + self.curr_period_time = get_param('curr_period_time', self.curr_period_time) + self.DEBUG_PRINT = get_param('debug_print', self.DEBUG_PRINT) + + +class FollowerInitializeConfig: + def __init__(self): + self.count_not_found = 0 + self.count_to_approach = 0 + self.on_tracking = False + self.approach_object_position = "NotFound" + self.current_pan = -10 + self.current_tilt = -10 + self.current_x_move = 0.005 + self.current_r_angle = 0 + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + self.current_object_bottom = 0.0 + self.tracking_status = "NotFound" + self.object_position = None + + + def update_from_params(self, get_param): + self.count_not_found = get_param('initial_count_not_found', self.count_not_found) + self.count_to_approach = get_param('initial_count_to_approach', self.count_to_approach) + self.on_tracking = get_param('initial_on_tracking', self.on_tracking) + self.approach_object_position = get_param('initial_approach_object_position', self.approach_object_position) + self.current_pan = get_param('initial_current_pan', self.current_pan) + self.current_tilt = get_param('initial_current_tilt', self.current_tilt) + self.current_x_move = get_param('initial_current_x_move', self.current_x_move) + self.current_r_angle = get_param('initial_current_r_angle', self.current_r_angle) + self.x_error_sum = get_param('initial_x_error_sum', self.x_error_sum) + self.y_error_sum = get_param('initial_y_error_sum', self.y_error_sum) + self.current_object_bottom = get_param('initial_object_bottom', self.current_object_bottom) + self.tracking_status = get_param('initial_tracking_status', self.tracking_status) + + + def reset(self): + """Reset all values to their initial state.""" + self.__init__() + + def update_object_position(self, x, y, z): + """Update the object position.""" + self.object_position.x = x + self.object_position.y = y + self.object_position.z = z + + def increment_count_not_found(self): + """Increment the count of frames where object was not found.""" + self.count_not_found += 1 + + def reset_count_not_found(self): + """Reset the count of frames where object was not found.""" + self.count_not_found = 0 + + def increment_count_to_approach(self): + """Increment the count to approach.""" + self.count_to_approach += 1 + + def reset_count_to_approach(self): + """Reset the count to approach.""" + self.count_to_approach = 0 + + def set_tracking_status(self, status): + """Set the current tracking status.""" + self.tracking_status = status + + def update_error_sums(self, x_error, y_error): + """Update the cumulative error sums.""" + self.x_error_sum += x_error + self.y_error_sum += y_error + + def reset_error_sums(self): + """Reset the cumulative error sums.""" + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + + def update_current_object_position(self, pan, tilt, bottom): + """Update the current object position.""" + self.current_pan = pan + self.current_tilt = tilt + self.current_object_bottom = bottom + + def update_movement(self, x_move, r_angle): + """Update the current movement parameters.""" + self.current_x_move = x_move + self.current_r_angle = r_angle + + def is_tracking(self): + """Check if tracking is currently active.""" + return self.on_tracking + + def start_tracking(self): + """Start the tracking process.""" + self.on_tracking = True + + def stop_tracking(self): + """Stop the tracking process.""" + self.on_tracking = False + + def set_approach_position(self, position): + """Set the approach position of the object.""" + self.approach_object_position = position diff --git a/humanoid_robot_intelligence_control_system_detection/src/tracker.py b/humanoid_robot_intelligence_control_system_detection/src/tracker.py new file mode 100644 index 0000000..369d84e --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/tracker.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + + +import math + +def calculate_error(object_position, FOV_WIDTH, FOV_HEIGHT): + x_error = -math.atan(object_position.x * math.tan(FOV_WIDTH)) + y_error = -math.atan(object_position.y * math.tan(FOV_HEIGHT)) + return x_error, y_error + +def calculate_error_target(error, error_diff, error_sum, p_gain, i_gain, d_gain): + return error * p_gain + error_diff * d_gain + error_sum * i_gain + +def calculate_delta_time(curr_time, prev_time): + return (curr_time - prev_time).to_sec() diff --git a/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py b/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py new file mode 100644 index 0000000..5310436 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import math + +class TrackerConfig: + def __init__(self): + self.FOV_WIDTH = 35.2 * math.pi / 180 + self.FOV_HEIGHT = 21.6 * math.pi / 180 + self.NOT_FOUND_THRESHOLD = 50 + self.WAITING_THRESHOLD = 5 + self.use_head_scan = True + self.DEBUG_PRINT = False + self.p_gain = 0.4 + self.i_gain = 0.0 + self.d_gain = 0.0 + + def update_from_params(self, get_param): + self.FOV_WIDTH = get_param('fov_width', self.FOV_WIDTH) + self.FOV_HEIGHT = get_param('fov_height', self.FOV_HEIGHT) + self.NOT_FOUND_THRESHOLD = get_param('not_found_threshold', self.NOT_FOUND_THRESHOLD) + self.WAITING_THRESHOLD = get_param('waiting_threshold', self.WAITING_THRESHOLD) + self.use_head_scan = get_param('use_head_scan', self.use_head_scan) + self.DEBUG_PRINT = get_param('debug_print', self.DEBUG_PRINT) + self.p_gain = get_param('p_gain', self.p_gain) + self.i_gain = get_param('i_gain', self.i_gain) + self.d_gain = get_param('d_gain', self.d_gain) + + +class TrackerInitializeConfig: + def __init__(self): + self.count_not_found = 0 + self.on_tracking = False + self.current_object_pan = 0.0 + self.current_object_tilt = 0.0 + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + self.current_object_bottom = 0.0 + self.tracking_status = "NotFound" + self.object_position = None + + + def update_from_params(self, get_param): + self.count_not_found = get_param('initial_count_not_found', self.count_not_found) + self.on_tracking = get_param('initial_on_tracking', self.on_tracking) + self.current_object_pan = get_param('initial_object_pan', self.current_object_pan) + self.current_object_tilt = get_param('initial_object_tilt', self.current_object_tilt) + self.x_error_sum = get_param('initial_x_error_sum', self.x_error_sum) + self.y_error_sum = get_param('initial_y_error_sum', self.y_error_sum) + self.current_object_bottom = get_param('initial_object_bottom', self.current_object_bottom) + self.tracking_status = get_param('initial_tracking_status', self.tracking_status) + + + def reset(self): + """Reset all values to their initial state.""" + self.__init__() + + + def update_object_position(self, x, y, z): + """Update the object position.""" + self.object_position.x = x + self.object_position.y = y + self.object_position.z = z + + + def increment_count_not_found(self): + """Increment the count of frames where object was not found.""" + self.count_not_found += 1 + + + def reset_count_not_found(self): + """Reset the count of frames where object was not found.""" + self.count_not_found = 0 + + + def set_tracking_status(self, status): + """Set the current tracking status.""" + self.tracking_status = status + + + def update_error_sums(self, x_error, y_error): + """Update the cumulative error sums.""" + self.x_error_sum += x_error + self.y_error_sum += y_error + + + def reset_error_sums(self): + """Reset the cumulative error sums.""" + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + + + def update_current_object_position(self, pan, tilt, bottom): + """Update the current object position.""" + self.current_object_pan = pan + self.current_object_tilt = tilt + self.current_object_bottom = bottom + + + def is_tracking(self): + """Check if tracking is currently active.""" + return self.on_tracking + + + def start_tracking(self): + """Start the tracking process.""" + self.on_tracking = True + + + def stop_tracking(self): + """Stop the tracking process.""" + self.on_tracking = False From 07ea79e04bbc1b802510d13cc07a9ae49b3db739 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Wed, 24 Jul 2024 01:53:54 -0400 Subject: [PATCH 56/59] latest pushes --- .../CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt b/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt index 0277ccd..48ef842 100644 --- a/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt @@ -13,7 +13,7 @@ # the License. cmake_minimum_required(VERSION 3.0.2) -project(humanoid_robot_intelligence_control_system_object_detector) +project(humanoid_robot_intelligence_control_system_configure) if($ENV{ROS_VERSION} EQUAL 1) find_package(catkin REQUIRED COMPONENTS From b0e62683ac3550ff38bf1a06e78e2cb13b87381e Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 24 Jul 2024 10:15:33 -0400 Subject: [PATCH 57/59] Update README.md --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 412d3e9..a3dd60f 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,9 @@ - [![Bellande's Internal Sensor Web API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_web_api) - [![Bellande's External Sensor Web API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_web_api) - [![Bellande's Functionality Web API](https://img.shields.io/badge/Bellande's%20Functionality%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_web_api) +- [![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20API-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/bellande_functionality_ros_api) +- [![Bellande's Algorithm through Bellande MODELS](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20MODELS-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/bellande_ros_models) + ## 🤖 Explore Humanoid Robot Intelligence with Us! From c4a68ec22e315aec6209ca2eff0a86e9423996a8 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 25 Jul 2024 10:07:05 -0400 Subject: [PATCH 58/59] latest pushes --- .../src/follower.py | 2 +- .../src/follower_config.py | 86 ++++++++++++++++ .../src/tracker_config.py | 98 +++++++++++++++++++ 3 files changed, 185 insertions(+), 1 deletion(-) diff --git a/humanoid_robot_intelligence_control_system_detection/src/follower.py b/humanoid_robot_intelligence_control_system_detection/src/follower.py index 8d32487..b19ab4c 100644 --- a/humanoid_robot_intelligence_control_system_detection/src/follower.py +++ b/humanoid_robot_intelligence_control_system_detection/src/follower.py @@ -17,7 +17,7 @@ import math -def calculate_distance_to_object(camera_height, tilt, hip_pitch_offset, object_size): +def calculate_distance_to(camera_height, tilt, hip_pitch_offset, object_size): distance = camera_height * math.tan(math.pi * 0.5 + tilt - hip_pitch_offset - object_size) return abs(distance) diff --git a/humanoid_robot_intelligence_control_system_detection/src/follower_config.py b/humanoid_robot_intelligence_control_system_detection/src/follower_config.py index 8bd53d1..ae09c01 100644 --- a/humanoid_robot_intelligence_control_system_detection/src/follower_config.py +++ b/humanoid_robot_intelligence_control_system_detection/src/follower_config.py @@ -58,6 +58,92 @@ class FollowerConfig: self.curr_period_time = get_param('curr_period_time', self.curr_period_time) self.DEBUG_PRINT = get_param('debug_print', self.DEBUG_PRINT) + def reset(self): + """Reset all values to their initial state.""" + self.__init__() + + def set_fov(self, width, height): + """Set the field of view.""" + self.FOV_WIDTH = width + self.FOV_HEIGHT = height + + def set_camera_height(self, height): + """Set the camera height.""" + self.CAMERA_HEIGHT = height + + def set_thresholds(self, not_found_threshold): + """Set the thresholds.""" + self.NOT_FOUND_THRESHOLD = not_found_threshold + + def set_step_parameters(self, max_fb, max_rl, in_place_fb, min_fb, min_rl, unit_fb, unit_rl): + """Set the step parameters.""" + self.MAX_FB_STEP = max_fb + self.MAX_RL_TURN = max_rl + self.IN_PLACE_FB_STEP = in_place_fb + self.MIN_FB_STEP = min_fb + self.MIN_RL_TURN = min_rl + self.UNIT_FB_STEP = unit_fb + self.UNIT_RL_TURN = unit_rl + + def set_spot_offsets(self, fb_offset, rl_offset, angle_offset): + """Set the spot offsets.""" + self.SPOT_FB_OFFSET = fb_offset + self.SPOT_RL_OFFSET = rl_offset + self.SPOT_ANGLE_OFFSET = angle_offset + + def set_hip_pitch_offset(self, offset): + """Set the hip pitch offset.""" + self.hip_pitch_offset = offset + + def set_period_time(self, period_time): + """Set the current period time.""" + self.curr_period_time = period_time + + def update_accum_period_time(self, delta_time): + """Update the accumulated period time.""" + self.accum_period_time += delta_time + if self.accum_period_time > (self.curr_period_time / 4): + self.accum_period_time = 0.0 + return self.accum_period_time + + def reset_accum_period_time(self): + """Reset the accumulated period time.""" + self.accum_period_time = 0.0 + + def set_debug_print(self, debug_print): + """Set the debug print flag.""" + self.DEBUG_PRINT = debug_print + + def get_config_dict(self): + """Return a dictionary of all configuration parameters.""" + return { + 'FOV_WIDTH': self.FOV_WIDTH, + 'FOV_HEIGHT': self.FOV_HEIGHT, + 'CAMERA_HEIGHT': self.CAMERA_HEIGHT, + 'NOT_FOUND_THRESHOLD': self.NOT_FOUND_THRESHOLD, + 'MAX_FB_STEP': self.MAX_FB_STEP, + 'MAX_RL_TURN': self.MAX_RL_TURN, + 'IN_PLACE_FB_STEP': self.IN_PLACE_FB_STEP, + 'MIN_FB_STEP': self.MIN_FB_STEP, + 'MIN_RL_TURN': self.MIN_RL_TURN, + 'UNIT_FB_STEP': self.UNIT_FB_STEP, + 'UNIT_RL_TURN': self.UNIT_RL_TURN, + 'SPOT_FB_OFFSET': self.SPOT_FB_OFFSET, + 'SPOT_RL_OFFSET': self.SPOT_RL_OFFSET, + 'SPOT_ANGLE_OFFSET': self.SPOT_ANGLE_OFFSET, + 'hip_pitch_offset': self.hip_pitch_offset, + 'curr_period_time': self.curr_period_time, + 'accum_period_time': self.accum_period_time, + 'DEBUG_PRINT': self.DEBUG_PRINT + } + + def load_config_dict(self, config_dict): + """Load configuration from a dictionary.""" + for key, value in config_dict.items(): + if hasattr(self, key): + setattr(self, key, value) + + class FollowerInitializeConfig: def __init__(self): diff --git a/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py b/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py index 5310436..43bd55a 100644 --- a/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py +++ b/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py @@ -40,6 +40,104 @@ class TrackerConfig: self.i_gain = get_param('i_gain', self.i_gain) self.d_gain = get_param('d_gain', self.d_gain) + def reset(self): + """Reset all values to their initial state.""" + self.__init__() + + def set_fov(self, width, height): + """Set the field of view.""" + self.FOV_WIDTH = width + self.FOV_HEIGHT = height + + def set_thresholds(self, not_found_threshold, waiting_threshold): + """Set the thresholds.""" + self.NOT_FOUND_THRESHOLD = not_found_threshold + self.WAITING_THRESHOLD = waiting_threshold + + def set_use_head_scan(self, use_head_scan): + """Set whether to use head scan.""" + self.use_head_scan = use_head_scan + + def set_debug_print(self, debug_print): + """Set the debug print flag.""" + self.DEBUG_PRINT = debug_print + + def set_pid_gains(self, p_gain, i_gain, d_gain): + """Set the PID gains.""" + self.p_gain = p_gain + self.i_gain = i_gain + self.d_gain = d_gain + + def get_pid_gains(self): + """Get the PID gains.""" + return self.p_gain, self.i_gain, self.d_gain + + def adjust_pid_gains(self, p_adjust=0, i_adjust=0, d_adjust=0): + """Adjust the PID gains by the given amounts.""" + self.p_gain += p_adjust + self.i_gain += i_adjust + self.d_gain += d_adjust + + def get_config_dict(self): + """Return a dictionary of all configuration parameters.""" + return { + 'FOV_WIDTH': self.FOV_WIDTH, + 'FOV_HEIGHT': self.FOV_HEIGHT, + 'NOT_FOUND_THRESHOLD': self.NOT_FOUND_THRESHOLD, + 'WAITING_THRESHOLD': self.WAITING_THRESHOLD, + 'use_head_scan': self.use_head_scan, + 'DEBUG_PRINT': self.DEBUG_PRINT, + 'p_gain': self.p_gain, + 'i_gain': self.i_gain, + 'd_gain': self.d_gain + } + + def load_config_dict(self, config_dict): + """Load configuration from a dictionary.""" + for key, value in config_dict.items(): + if hasattr(self, key): + setattr(self, key, value) + + def validate_config(self): + """Validate the configuration parameters.""" + assert 0 < self.FOV_WIDTH < math.pi, "FOV_WIDTH must be between 0 and pi" + assert 0 < self.FOV_HEIGHT < math.pi, "FOV_HEIGHT must be between 0 and pi" + assert self.NOT_FOUND_THRESHOLD > 0, "NOT_FOUND_THRESHOLD must be positive" + assert self.WAITING_THRESHOLD > 0, "WAITING_THRESHOLD must be positive" + assert isinstance(self.use_head_scan, bool), "use_head_scan must be a boolean" + assert isinstance(self.DEBUG_PRINT, bool), "DEBUG_PRINT must be a boolean" + assert self.p_gain >= 0, "p_gain must be non-negative" + assert self.i_gain >= 0, "i_gain must be non-negative" + assert self.d_gain >= 0, "d_gain must be non-negative" + + def print_config(self): + """Print the current configuration.""" + for key, value in self.get_config_dict().items(): + print(f"{key}: {value}") + + def to_ros_param(self): + """Convert the configuration to a format suitable for ROS parameters.""" + return { + 'tracker': { + 'fov': { + 'width': self.FOV_WIDTH, + 'height': self.FOV_HEIGHT + }, + 'thresholds': { + 'not_found': self.NOT_FOUND_THRESHOLD, + 'waiting': self.WAITING_THRESHOLD + }, + 'use_head_scan': self.use_head_scan, + 'debug_print': self.DEBUG_PRINT, + 'pid': { + 'p': self.p_gain, + 'i': self.i_gain, + 'd': self.d_gain + } + } + } + + class TrackerInitializeConfig: def __init__(self): From 8ea9a2276056e2cba655d3d7aaed907f14970397 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 25 Jul 2024 15:02:40 -0400 Subject: [PATCH 59/59] latest pushes --- .../CMakeLists.txt | 62 ++++ .../package.xml | 46 +++ .../setup.py | 26 ++ .../src/HumanoidRobotController.py | 327 ++++++++++++++++++ .../src/MovementController.py | 73 ++++ 5 files changed, 534 insertions(+) create mode 100644 humanoid_robot_intelligence_control_system_movement/CMakeLists.txt create mode 100644 humanoid_robot_intelligence_control_system_movement/package.xml create mode 100644 humanoid_robot_intelligence_control_system_movement/setup.py create mode 100644 humanoid_robot_intelligence_control_system_movement/src/HumanoidRobotController.py create mode 100644 humanoid_robot_intelligence_control_system_movement/src/MovementController.py diff --git a/humanoid_robot_intelligence_control_system_movement/CMakeLists.txt b/humanoid_robot_intelligence_control_system_movement/CMakeLists.txt new file mode 100644 index 0000000..ad4b2c6 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_movement/CMakeLists.txt @@ -0,0 +1,62 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_movement) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + ) + + catkin_package( + CATKIN_DEPENDS + rospy + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/MovementController.py + src/HumanoidRobotController.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/MovementController.py + src/HumanoidRobotController.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_movement/package.xml b/humanoid_robot_intelligence_control_system_movement/package.xml new file mode 100644 index 0000000..7cc7687 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_movement/package.xml @@ -0,0 +1,46 @@ + + + + + humanoid_robot_intelligence_control_system_movement + 0.0.1 + + This Package is for Detection Math + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + + + rclpy + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_movement/setup.py b/humanoid_robot_intelligence_control_system_movement/setup.py new file mode 100644 index 0000000..1450a13 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_movement/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=["src/MovementController.py", "src/HumanoidRobotController.py"], + packages=['humanoid_robot_intelligence_control_system_movement'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_movement/src/HumanoidRobotController.py b/humanoid_robot_intelligence_control_system_movement/src/HumanoidRobotController.py new file mode 100644 index 0000000..13adf37 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_movement/src/HumanoidRobotController.py @@ -0,0 +1,327 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import time +import math +from typing import Dict, List, Tuple, Any + +from humanoid_robot_intelligence_control_system_controller.PIDController import PIDController +from humanoid_robot_intelligence_control_system_detection.tracker_config import TrackerConfig +from humanoid_robot_intelligence_control_system_movement.MovementController import MovementController + + +class HumanoidRobotController: + def __init__(self, config: TrackerConfig, controller_type: str = "PID"): + self.config = config + self.controller_type = controller_type + self.controllers: Dict[str, Any] = {} + self.joint_positions: Dict[str, float] = {} + self.target_positions: Dict[str, float] = {} + self.movement_controller = MovementController(config, controller_type) + self.current_balance = 0.0 + self.current_speed = 0.0 + self.current_angle = 0.0 + self.current_position = (0.0, 0.0) + self.last_joint_positions = {} + self.control_loop_time = 0.01 # 100 Hz + + self.initialize_controllers() + self.initialize_sensors() + + + def initialize_controllers(self): + joints = [ + "head_pan", "head_tilt", + "left_shoulder_pitch", "left_shoulder_roll", "left_elbow", + "right_shoulder_pitch", "right_shoulder_roll", "right_elbow", + "left_hip_yaw", "left_hip_roll", "left_hip_pitch", "left_knee", "left_ankle_pitch", "left_ankle_roll", + "right_hip_yaw", "right_hip_roll", "right_hip_pitch", "right_knee", "right_ankle_pitch", "right_ankle_roll" + ] + + for joint in joints: + self.controllers[joint] = self._create_controller(joint) + self.joint_positions[joint] = 0.0 + self.target_positions[joint] = 0.0 + + self.controllers["balance"] = self._create_controller("balance") + self.controllers["walk_forward"] = self._create_controller("walk_forward") + self.controllers["turn"] = self._create_controller("turn") + + self.roll_controller = self._create_controller("roll") + self.pitch_controller = self._create_controller("pitch") + + + def _create_controller(self, controller_name: str) -> Any: + if self.controller_type == "PID": + return PIDController( + getattr(self.config, f"get_{controller_name}_pid_gains")(), + f"{controller_name}_controller", + output_limits=self.config.get_joint_limits(controller_name) + ) + elif self.controller_type == "BellandeController": + return BellandeController( + getattr(self.config, f"get_{controller_name}_bellande_params")(), + f"{controller_name}_controller" + ) + else: + raise ValueError(f"Unsupported controller type: {self.controller_type}") + + + def initialize_sensors(self): + self.imu = None + self.power_sensor = None + self.joint_sensors = None + + + def update_config(self, new_config: TrackerConfig): + self.config = new_config + for joint, controller in self.controllers.items(): + self._update_controller(controller, joint) + self.movement_controller.update_config(new_config) + + + def _update_controller(self, controller: Any, controller_name: str): + if self.controller_type == "PID": + controller.update_config(getattr(self.config, f"get_{controller_name}_pid_gains")()) + elif self.controller_type == "BellandeController": + controller.update_config(getattr(self.config, f"get_{controller_name}_bellande_params")()) + + + def set_target_position(self, joint: str, position: float): + if joint in self.target_positions: + self.target_positions[joint] = position + else: + raise ValueError(f"Unknown joint: {joint}") + + + def update_joint_position(self, joint: str, position: float): + if joint in self.joint_positions: + self.joint_positions[joint] = position + else: + raise ValueError(f"Unknown joint: {joint}") + + + def compute_joint_control(self, joint: str) -> float: + if joint not in self.controllers: + raise ValueError(f"Unknown joint: {joint}") + + return self.controllers[joint].compute( + self.target_positions[joint], + self.joint_positions[joint] + ) + + + def compute_all_joint_controls(self) -> Dict[str, float]: + return {joint: self.compute_joint_control(joint) for joint in self.joint_positions.keys()} + + + def compute_balance_control(self, current_balance: float) -> float: + return self.controllers["balance"].compute(0, current_balance) + + + def compute_walk_forward_control(self, current_speed: float, target_speed: float) -> float: + return self.controllers["walk_forward"].compute(target_speed, current_speed) + + + def compute_turn_control(self, current_angle: float, target_angle: float) -> float: + return self.controllers["turn"].compute(target_angle, current_angle) + + + def reset_all_controllers(self): + for controller in self.controllers.values(): + controller.reset() + + + def update_robot_state(self, joint_positions: Dict[str, float], balance: float, speed: float, angle: float, position: Tuple[float, float]): + for joint, position in joint_positions.items(): + self.update_joint_position(joint, position) + + self.current_balance = balance + self.current_speed = speed + self.current_angle = angle + self.current_position = position + + + def compute_full_robot_control(self, target_speed: float, target_angle: float, target_position: Tuple[float, float]) -> Dict[str, float]: + joint_controls = self.compute_all_joint_controls() + balance_control = self.compute_balance_control(self.current_balance) + walk_control = self.compute_walk_forward_control(self.current_speed, target_speed) + turn_control = self.compute_turn_control(self.current_angle, target_angle) + + translation, rotation = self.movement_controller.compute_path_control(self.current_position, target_position) + + return { + "joints": joint_controls, + "balance": balance_control, + "walk": walk_control, + "turn": turn_control, + "translation": translation, + "rotation": rotation + } + + + def execute_control(self, control: Dict[str, float]): + try: + # Send joint control commands + for joint, value in control['joints'].items(): + self.robot_interface.set_joint_position(joint, value) + + # Send balance control command + self.robot_interface.set_balance_adjustment(control['balance']) + + # Send walk and turn commands + self.robot_interface.set_walk_speed(control['walk']) + self.robot_interface.set_turn_rate(control['turn']) + + # Send translation and rotation commands + self.robot_interface.set_body_translation(control['translation']) + self.robot_interface.set_body_rotation(control['rotation']) + + # Verify that commands were executed + if not self.robot_interface.verify_last_command(): + raise Exception("Failed to execute control commands") + + except Exception as e: + print(f"Error executing control: {e}") + self.emergency_stop() + + + def get_robot_state(self) -> Dict[str, Any]: + joint_positions = self.joint_sensors.get_positions() + imu_data = self.imu.get_data() + return { + "joint_positions": joint_positions, + "balance": imu_data.acceleration.z, + "speed": math.sqrt(imu_data.velocity.x**2 + imu_data.velocity.y**2), + "angle": math.atan2(imu_data.orientation.y, imu_data.orientation.x), + "position": (imu_data.position.x, imu_data.position.y) + } + + + def balance_adjustment(self): + roll, pitch, yaw = self.imu.get_orientation() + roll_rate, pitch_rate, yaw_rate = self.imu.get_angular_velocity() + + roll_correction = self.roll_controller.compute(0, roll) + pitch_correction = self.pitch_controller.compute(0, pitch) + + self.set_target_position("left_hip_roll", self.joint_positions["left_hip_roll"] - roll_correction) + self.set_target_position("right_hip_roll", self.joint_positions["right_hip_roll"] + roll_correction) + self.set_target_position("left_ankle_roll", self.joint_positions["left_ankle_roll"] - roll_correction) + self.set_target_position("right_ankle_roll", self.joint_positions["right_ankle_roll"] + roll_correction) + self.set_target_position("left_ankle_pitch", self.joint_positions["left_ankle_pitch"] - pitch_correction) + self.set_target_position("right_ankle_pitch", self.joint_positions["right_ankle_pitch"] - pitch_correction) + + if abs(pitch) > self.config.get_max_stable_pitch(): + hip_pitch_correction = self.pitch_controller.compute(0, pitch) * 0.5 + self.set_target_position("left_hip_pitch", self.joint_positions["left_hip_pitch"] + hip_pitch_correction) + self.set_target_position("right_hip_pitch", self.joint_positions["right_hip_pitch"] + hip_pitch_correction) + + damping_factor = self.config.get_angular_velocity_damping_factor() + self.set_target_position("left_hip_roll", self.joint_positions["left_hip_roll"] - roll_rate * damping_factor) + self.set_target_position("right_hip_roll", self.joint_positions["right_hip_roll"] + roll_rate * damping_factor) + self.set_target_position("left_ankle_roll", self.joint_positions["left_ankle_roll"] - roll_rate * damping_factor) + self.set_target_position("right_ankle_roll", self.joint_positions["right_ankle_roll"] + roll_rate * damping_factor) + self.set_target_position("left_ankle_pitch", self.joint_positions["left_ankle_pitch"] - pitch_rate * damping_factor) + self.set_target_position("right_ankle_pitch", self.joint_positions["right_ankle_pitch"] - pitch_rate * damping_factor) + + + def check_joint_status(self) -> bool: + for joint, position in self.joint_positions.items(): + limits = self.config.get_joint_limits(joint) + if position < limits[0] or position > limits[1]: + print(f"Joint {joint} out of limits: {position}") + return False + + if joint in self.last_joint_positions: + velocity = (position - self.last_joint_positions[joint]) / self.control_loop_time + if abs(velocity) > self.config.get_max_joint_velocity(joint): + print(f"Joint {joint} velocity too high: {velocity}") + return False + + self.last_joint_positions = self.joint_positions.copy() + return True + + + def check_power_status(self) -> bool: + voltage = self.power_sensor.get_voltage() + current = self.power_sensor.get_current() + + if voltage < self.config.get_min_voltage(): + print(f"Voltage too low: {voltage}V") + return False + if current > self.config.get_max_current(): + print(f"Current too high: {current}A") + return False + + return True + + + def check_sensor_status(self) -> bool: + return self.imu.is_operational() and self.power_sensor.is_operational() and self.joint_sensors.is_operational() + + + def self_diagnosis(self) -> Dict[str, bool]: + return { + "joints": self.check_joint_status(), + "sensors": self.check_sensor_status(), + "power": self.check_power_status(), + } + + + def emergency_stop(self): + self.stop() + self.reset_all_controllers() + + print("Emergency stop activated!") + + + def stop(self): + zero_control = {key: 0.0 for key in self.controllers.keys()} + zero_control.update({"translation": 0.0, "rotation": 0.0}) + self.execute_control(zero_control) + + + def run_control_loop(self): + while True: + try: + loop_start_time = time.time() + + state = self.get_robot_state() + self.update_robot_state(**state) + self.balance_adjustment() + + diagnosis = self.self_diagnosis() + if not all(diagnosis.values()): + print("System check failed. Initiating emergency stop.") + self.emergency_stop() + break + + control = self.compute_full_robot_control(self.current_speed, self.current_angle, self.current_position) + self.execute_control(control) + + loop_end_time = time.time() + loop_duration = loop_end_time - loop_start_time + if loop_duration < self.control_loop_time: + time.sleep(self.control_loop_time - loop_duration) + + except Exception as e: + print(f"Unexpected error: {e}") + self.emergency_stop() + break + + print("HumanoidRobotController shutting down.") diff --git a/humanoid_robot_intelligence_control_system_movement/src/MovementController.py b/humanoid_robot_intelligence_control_system_movement/src/MovementController.py new file mode 100644 index 0000000..d64d2c6 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_movement/src/MovementController.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import math +from typing import Tuple + +from humanoid_robot_intelligence_control_system_detection.PIDController import PIDController +from humanoid_robot_intelligence_control_system_detection.tracker_config import TrackerConfig + + +class MovementController: + def __init__(self, config: TrackerConfig, controller_type: str = "PID"): + self.config = config + self.controller_type = controller_type + self.translation_controller = self._create_controller("translation") + self.rotation_controller = self._create_controller("rotation") + self.path_controller = self._create_controller("path") + + def _create_controller(self, controller_name: str) -> Any: + if self.controller_type == "PID": + return PIDController( + getattr(self.config, f"get_{controller_name}_pid_gains")(), + f"{controller_name}_controller", + output_limits=(-1, 1) + ) + elif self.controller_type == "BellandeController": + return BellandeController( + getattr(self.config, f"get_{controller_name}_bellande_params")(), + f"{controller_name}_controller" + ) + else: + raise ValueError(f"Unsupported controller type: {self.controller_type}") + + def update_config(self, new_config: TrackerConfig): + self.config = new_config + self._update_controller(self.translation_controller, "translation") + self._update_controller(self.rotation_controller, "rotation") + self._update_controller(self.path_controller, "path") + + def _update_controller(self, controller: Any, controller_name: str): + if self.controller_type == "PID": + controller.update_config(getattr(self.config, f"get_{controller_name}_pid_gains")()) + elif self.controller_type == "BellandeController": + controller.update_config(getattr(self.config, f"get_{controller_name}_bellande_params")()) + + def compute_translation_control(self, current_position: float, target_position: float) -> float: + return self.translation_controller.compute(target_position, current_position) + + def compute_rotation_control(self, current_angle: float, target_angle: float) -> float: + return self.rotation_controller.compute(target_angle, current_angle) + + def compute_path_control(self, current_position: Tuple[float, float], target_position: Tuple[float, float]) -> Tuple[float, float]: + dx = target_position[0] - current_position[0] + dy = target_position[1] - current_position[1] + distance = (dx**2 + dy**2)**0.5 + angle = math.atan2(dy, dx) + translation = self.translation_controller.compute(0, -distance) + rotation = self.rotation_controller.compute(angle, 0) + return translation, rotation