From 533811c913a4405a5223b9a720290413b5440323 Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Wed, 31 May 2023 10:32:17 +0530 Subject: [PATCH 01/17] [ Essentials ]: Adds gitignore for ros_setup --- .gitignore | 193 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 193 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..5dda64d --- /dev/null +++ b/.gitignore @@ -0,0 +1,193 @@ +# Created by https://www.gitignore.io/api/c +# Edit at https://www.gitignore.io/?templates=c + +.vscode +### C ### +# Prerequisites +*.d + +# Object files +*.o +*.ko +*.obj +*.elf + +# Linker output +*.ilk +*.map +*.exp + +# Precompiled Headers +*.gch +*.pch + +# Libraries +*.lib +*.a +*.la +*.lo + +# Shared objects (inc. Windows DLLs) +*.dll +*.so +*.so.* +*.dylib + +# Executables +*.exe +*.out +*.app +*.i*86 +*.x86_64 +*.hex + +# Debug files +*.dSYM/ +*.su +*.idb +*.pdb + +# Kernel Module Compile Results +*.mod* +*.cmd +.tmp_versions/ +modules.order +Module.symvers +Mkfile.old +dkms.conf + +#build files test +*/build + +# End of https://www.gitignore.io/api/c + +======= +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +pip-wheel-metadata/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +.python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ \ No newline at end of file From b88196c83a0f0434b6719b89a951a770a650c371 Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Wed, 31 May 2023 10:32:41 +0530 Subject: [PATCH 02/17] [ Initial Commit ] --- README.md | 9 +++++++++ scripts/cost_ugv.py | 2 ++ scripts/path_uav.py | 2 ++ scripts/path_ugv.py | 2 ++ scripts/prediction.py | 2 ++ scripts/reward_uav.py | 2 ++ 6 files changed, 19 insertions(+) create mode 100644 README.md create mode 100644 scripts/cost_ugv.py create mode 100644 scripts/path_uav.py create mode 100644 scripts/path_ugv.py create mode 100644 scripts/prediction.py create mode 100644 scripts/reward_uav.py diff --git a/README.md b/README.md new file mode 100644 index 0000000..4b173a2 --- /dev/null +++ b/README.md @@ -0,0 +1,9 @@ +## Catching the non-cooperative agent + +This project is built upon the current implementation of the [Teaming Heterogeneous Ground and Micro-Aerial Robots for Following +of Non-Cooperative Agents](https://raaslab.org/icra2023-hmrs/assets/contributed_papers/Ori_A_Miller_et_al.pdf) + +Targets: +- [ ] Improving the prediction usiing kalman predictor +- [ ] Using Fuel contraints to gain the optimal path that a UAV and UGV should follow +- [ ] It is not necessary that UAV and UGV should follow same number of points they should be nearly independent \ No newline at end of file diff --git a/scripts/cost_ugv.py b/scripts/cost_ugv.py new file mode 100644 index 0000000..94de072 --- /dev/null +++ b/scripts/cost_ugv.py @@ -0,0 +1,2 @@ +# This code is to get the value for the cost i.e. distance between +# ugv and the person based on the location of the ugv and the person \ No newline at end of file diff --git a/scripts/path_uav.py b/scripts/path_uav.py new file mode 100644 index 0000000..887f0c7 --- /dev/null +++ b/scripts/path_uav.py @@ -0,0 +1,2 @@ +# This will give the path that should be followed based on the shortlisted +# reachable sites by the uav diff --git a/scripts/path_ugv.py b/scripts/path_ugv.py new file mode 100644 index 0000000..5402450 --- /dev/null +++ b/scripts/path_ugv.py @@ -0,0 +1,2 @@ +# This will give the path that should be followed by the ugv based on +# cost function and the reachability of those points with the given speed diff --git a/scripts/prediction.py b/scripts/prediction.py new file mode 100644 index 0000000..d386003 --- /dev/null +++ b/scripts/prediction.py @@ -0,0 +1,2 @@ +# This is a kalman predictor which will get the future state of the person +# using the simple position and velocity state variables diff --git a/scripts/reward_uav.py b/scripts/reward_uav.py new file mode 100644 index 0000000..a2cc2b7 --- /dev/null +++ b/scripts/reward_uav.py @@ -0,0 +1,2 @@ +# This will give the reward based on the reachable states and the +# candidate states provided From 0871b1241934984f04820b4e7cabc920fc01f9dc Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Fri, 2 Jun 2023 15:28:46 +0530 Subject: [PATCH 03/17] Adds a pic of note --- README.md | 4 +- scripts/candidates.py | 58 ++++++++++++++++++++++++++++ scripts/cost_ugv.py | 27 ++++++++++++- scripts/path_uav.py | 29 ++++++++++++++ scripts/path_ugv.py | 29 ++++++++++++++ scripts/prediction.py | 51 ++++++++++++++++++++++++- scripts/reward_uav.py | 89 +++++++++++++++++++++++++++++++++++++++++++ 7 files changed, 283 insertions(+), 4 deletions(-) create mode 100644 scripts/candidates.py diff --git a/README.md b/README.md index 4b173a2..700144f 100644 --- a/README.md +++ b/README.md @@ -6,4 +6,6 @@ of Non-Cooperative Agents](https://raaslab.org/icra2023-hmrs/assets/contributed_ Targets: - [ ] Improving the prediction usiing kalman predictor - [ ] Using Fuel contraints to gain the optimal path that a UAV and UGV should follow -- [ ] It is not necessary that UAV and UGV should follow same number of points they should be nearly independent \ No newline at end of file +- [ ] It is not necessary that UAV and UGV should follow same number of points they should be nearly independent + +![img](https://lh3.googleusercontent.com/8UXw1B8W2pv5L7X4VIjYxJCW99FevBTlxePC9rCUy8Wum401uCN7g5U9q6hapnJf-_HcGprfS4pObYFPwv_4PTFEXgSJl2U-eCagH7KHOYqMj-Eghug_qRIink8FoR7KFIYikL04n4a_u4wM2bc_wMVfI7L8r_8OBLTjzWU2-Y4dLRBUZja81R_rCE7Z0d_HNXUf1dWyX0SJXA3Zcr8mukAELd3DZNEH8A901ivj-TNbhZ1v_eIwIEW_VVQjIQLnbDgGGXOgpmHgZfi4Lklh34S6rKhcfrv_aHCM6IvXGT4zD52oobiUHbHAqGWHesDhDUqdfj1bysOnItPeVaRAScFlFd7Mz4zt1s3Z6BA0bY9a4NAPh881vGUTe4T4ufZB98XQm3OqVV8p4eckag70f19TgJNzdICD-UoY53ZGaREwgS4rU0tDK6rpvLnuO_myRvRaCXo5snw3EJb59vr23HDpcvdUgUwyovID1flbrrtsw4Y1xqP-DkKJJf6KmD6p4M5i4ubRsXLmtAtboXLcpS3zPLbAb9yfpDb15kwpRZjONn-S8hWimfeSVW90iRpQyRN_p8OBzxgsyN3R1T7ZQId5K5gAGVEW8I-_kD5EuAJdWzuOsgXMi7vPGPfCHWBdmegugRZM09bfK-w1vFCnSgF_LZ_TiB5VYXkpG3kYGC7uMnZTE5yLCkLK91VkM6y4kjSym64Nb05Xzck11FxaqopMeQpHEj0CNvgrBcjuSBeppB3YfiBwYGvaL05ts0Wi93VNLyp9Eqkt0pcsGXjEJAs9j9sPO8PH_ZuqYTdjix-Pk4fZJSl7VcemGy0ZRmzMg8P5XZlqszRyR-PZM5-scR349lsvHdLC-Wfmsg4veUlNAEfTlwifTEZKxXmvFnwZDceir9Fft8zTERWJlpabWnGUbkgO9cSebbfyoYoq1x3nFCcCSqZlvoyVLhA-aR-_BeHuAIRMWHLqwVt-cUESpnKh0WRUP0XBrxnFJiRxGpWJGWoyas9QgDI=w1024-h1366-s-no?authuser=0) \ No newline at end of file diff --git a/scripts/candidates.py b/scripts/candidates.py new file mode 100644 index 0000000..3de4389 --- /dev/null +++ b/scripts/candidates.py @@ -0,0 +1,58 @@ +""" + MIT License + + Copyright (c) 2023 Mohd Alqama Shaikh + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +""" + +from prediction import predictor_polynomial + +""" +We first need the current location +Then we need to get all the desired setpoints for UGV and UAV +Then we need to go for the next point that is Update step and do the previous step for the same +Above procedure is repeated until distance between UGV and target is <= 1 +""" +x_previous, y_previous = 0, 0 + +# velocity of target = 1m/s ... Initially i.e. just assumed +# This velocity should be observed by the UAV +vel_target = 1 + +# i.e. we can predict just the next step through the polynomial predictor +time_horizon = 1 + +x_current, y_current = predictor_polynomial( + x_previous, y_previous, x_previous + vel_target, y_previous + vel_target +) + +""" +Now, we need 3 different sets of points +1. Get the location of the target and then choose four points in 1m North, South, East, and west of the target +2. Now, take the radius equal to the time_horizon and choose six equidistant points on the circle +3. Take the radius of another circle equal to twice the time horizon and choose 8 equidistant point from the circle and add them to the candidate locations +""" + +def candidates(): + + + +if __name__ == "__main__": + candidates() \ No newline at end of file diff --git a/scripts/cost_ugv.py b/scripts/cost_ugv.py index 94de072..b4a02ce 100644 --- a/scripts/cost_ugv.py +++ b/scripts/cost_ugv.py @@ -1,2 +1,25 @@ -# This code is to get the value for the cost i.e. distance between -# ugv and the person based on the location of the ugv and the person \ No newline at end of file +""" + MIT License + + Copyright (c) 2023 Mohd Alqama Shaikh + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +""" +# This code is to get the value for the cost i.e. distance between +# ugv and the person based on the location of the ugv and the person diff --git a/scripts/path_uav.py b/scripts/path_uav.py index 887f0c7..29f212f 100644 --- a/scripts/path_uav.py +++ b/scripts/path_uav.py @@ -1,2 +1,31 @@ +""" + MIT License + + Copyright (c) 2023 Mohd Alqama Shaikh + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +""" # This will give the path that should be followed based on the shortlisted # reachable sites by the uav + +""" +We will get a set of candidate goal pose from the candidates.py +and then according to the heuristics for the UAV we will shortlist the reachable +set of candidates for just UAV +""" diff --git a/scripts/path_ugv.py b/scripts/path_ugv.py index 5402450..81ac4cd 100644 --- a/scripts/path_ugv.py +++ b/scripts/path_ugv.py @@ -1,2 +1,31 @@ +""" + MIT License + + Copyright (c) 2023 Mohd Alqama Shaikh + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +""" # This will give the path that should be followed by the ugv based on # cost function and the reachability of those points with the given speed + +""" +We will get a set of candidate goal pose from the candidates.py +and then according to the heuristics for the UGV we will shortlist the reachable +set of candidates for just UGV +""" diff --git a/scripts/prediction.py b/scripts/prediction.py index d386003..c7552d8 100644 --- a/scripts/prediction.py +++ b/scripts/prediction.py @@ -1,2 +1,51 @@ -# This is a kalman predictor which will get the future state of the person +""" + MIT License + + Copyright (c) 2023 Mohd Alqama Shaikh + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +""" + +#! usr/bin/python3 +# This is a polynomial predictor which will get the future state of the person # using the simple position and velocity state variables +""" +Function : predictor_polynomial curve +Inputs : current position of target and a position second before of the target +""" + + +def predictor_polynomial(x0, y0, x1, y1): + global x2, y2 + + x2 = ((x0 + x1) ^ 2) / 2 + y2 = ((y0 + y1) ^ 2) / 2 + + return x2, y2 + + +""" +Function : predictor_kalman +Inputs : current position of target and a position second before of the target +""" + + +def predictor_kalman(): + x2 = 0 + y2 = 0 diff --git a/scripts/reward_uav.py b/scripts/reward_uav.py index a2cc2b7..e9b2061 100644 --- a/scripts/reward_uav.py +++ b/scripts/reward_uav.py @@ -1,2 +1,91 @@ +""" + MIT License + + Copyright (c) 2023 Mohd Alqama Shaikh + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +""" + # This will give the reward based on the reachable states and the # candidate states provided + +from cmath import sqrt, cos, sin +import numpy as np + +# x0 and y0 here are the previous postions of the UAV +x0, y0 = -1, -1 +# x1 and y1 here are the current positons of the UAV +x1, y1 = 0, 0 +# Angle of vision in degrees +theta = 60 +# radius of vision is 5m from the UAV +rov = 5 + + +def dist(x1, y1, x2, y2): + return sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) + + +""" +Information reward calculation: +We need the most reliable field of view possible from a given point to the tracked object + +So, for the same we propose a reward function that focuses on predicting motion of the object over the larger FOV +""" + + +def reward(x0, y0, x1, y1, x_target, y_target): + vec1 = np.array([(x1 - x0), (y1 - y0)], [(y0 - y1), (x1 - x0)]) + vec2 = np.array( + [(x0 - x1) * x_target + (y1 - y0) * y_target], [(x0 - x1) * y1 + (y1 - y0) * x1] + ) + x2, y2 = np.linalg.solve(vec1, vec2)[0], np.linalg.solve(vec1, vec2)[1] + pose = np.array([x2], [y2], [0]) + + z = dist(x1, y1, x2, y2) + pose_left, pose_right = np.matmul( + np.array( + [cos(theta / 2), -sin(theta / 2), 0], + [sin(theta / 2), cos(theta / 2), 0], + [0, 0, 1], + ), + pose, + ), np.matmul( + np.array( + [cos(-theta / 2), -sin(-theta / 2), 0], + [sin(-theta / 2), cos(-theta / 2), 0], + [0, 0, 1], + ), + pose, + ) + + xt, yt = dist(pose_left[0], pose_left[1], x2, y2), dist( + pose_right[0], pose_right[1], x2, y2 + ) + + reward_value = xt * yt * (rov - z) * z + + return reward_value + + +def maximize_reward(list, x0, y0): + max_reward = 0 + max_reward_coordinates = np.array(list[0], list[1]) + for i in range(len(list)): + max_reward = max(reward(x0, y0, x1, y1, list[i][0], list[i][1]), max_reward) From 86831e7e9f98763672d80ad97af5a91f464aaa52 Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Mon, 19 Jun 2023 17:31:28 +0530 Subject: [PATCH 04/17] [ Buildable ]: Launch files are now buildable --- scripts/path_uav.py | 31 - scripts/path_ugv.py | 31 - turtlebots/CMakeLists.txt | 204 +++ turtlebots/launch/tracking.launch | 22 + turtlebots/package.xml | 62 + {scripts => turtlebots/scripts}/candidates.py | 47 +- {scripts => turtlebots/scripts}/cost_ugv.py | 31 + {scripts => turtlebots/scripts}/prediction.py | 9 +- {scripts => turtlebots/scripts}/reward_uav.py | 9 +- turtlebots/scripts/target_pose.py | 74 + turtlebots/urdf/common_properties.xacro | 44 + .../urdf/turtlebot3_burger.gazebo.xacro | 135 ++ turtlebots/urdf/turtlebot3_burger.urdf.xacro | 165 ++ turtlebots/worlds/tracking_one | 1322 +++++++++++++++++ 14 files changed, 2096 insertions(+), 90 deletions(-) delete mode 100644 scripts/path_uav.py delete mode 100644 scripts/path_ugv.py create mode 100644 turtlebots/CMakeLists.txt create mode 100644 turtlebots/launch/tracking.launch create mode 100644 turtlebots/package.xml rename {scripts => turtlebots/scripts}/candidates.py (68%) rename {scripts => turtlebots/scripts}/cost_ugv.py (60%) rename {scripts => turtlebots/scripts}/prediction.py (90%) rename {scripts => turtlebots/scripts}/reward_uav.py (89%) create mode 100644 turtlebots/scripts/target_pose.py create mode 100644 turtlebots/urdf/common_properties.xacro create mode 100644 turtlebots/urdf/turtlebot3_burger.gazebo.xacro create mode 100644 turtlebots/urdf/turtlebot3_burger.urdf.xacro create mode 100644 turtlebots/worlds/tracking_one diff --git a/scripts/path_uav.py b/scripts/path_uav.py deleted file mode 100644 index 29f212f..0000000 --- a/scripts/path_uav.py +++ /dev/null @@ -1,31 +0,0 @@ -""" - MIT License - - Copyright (c) 2023 Mohd Alqama Shaikh - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in all - copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - SOFTWARE. -""" -# This will give the path that should be followed based on the shortlisted -# reachable sites by the uav - -""" -We will get a set of candidate goal pose from the candidates.py -and then according to the heuristics for the UAV we will shortlist the reachable -set of candidates for just UAV -""" diff --git a/scripts/path_ugv.py b/scripts/path_ugv.py deleted file mode 100644 index 81ac4cd..0000000 --- a/scripts/path_ugv.py +++ /dev/null @@ -1,31 +0,0 @@ -""" - MIT License - - Copyright (c) 2023 Mohd Alqama Shaikh - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in all - copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - SOFTWARE. -""" -# This will give the path that should be followed by the ugv based on -# cost function and the reachability of those points with the given speed - -""" -We will get a set of candidate goal pose from the candidates.py -and then according to the heuristics for the UGV we will shortlist the reachable -set of candidates for just UGV -""" diff --git a/turtlebots/CMakeLists.txt b/turtlebots/CMakeLists.txt new file mode 100644 index 0000000..0c62b7c --- /dev/null +++ b/turtlebots/CMakeLists.txt @@ -0,0 +1,204 @@ +cmake_minimum_required(VERSION 3.0.2) +project(turtlebots) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + rospy +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES turtlebots +# CATKIN_DEPENDS rospy +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/turtlebots.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/turtlebots_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_turtlebots.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/turtlebots/launch/tracking.launch b/turtlebots/launch/tracking.launch new file mode 100644 index 0000000..db509d2 --- /dev/null +++ b/turtlebots/launch/tracking.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/turtlebots/package.xml b/turtlebots/package.xml new file mode 100644 index 0000000..b09fb66 --- /dev/null +++ b/turtlebots/package.xml @@ -0,0 +1,62 @@ + + + turtlebots + 0.0.0 + The turtlebots package + + + + + proto + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + rospy + rospy + rospy + + + + + + + + diff --git a/scripts/candidates.py b/turtlebots/scripts/candidates.py similarity index 68% rename from scripts/candidates.py rename to turtlebots/scripts/candidates.py index 3de4389..16823b6 100644 --- a/scripts/candidates.py +++ b/turtlebots/scripts/candidates.py @@ -21,38 +21,39 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. """ - +import numpy as np +from cmath import sin, cos, pi from prediction import predictor_polynomial """ -We first need the current location -Then we need to get all the desired setpoints for UGV and UAV -Then we need to go for the next point that is Update step and do the previous step for the same -Above procedure is repeated until distance between UGV and target is <= 1 +Now, we need 3 different sets of points +1. Get the location of the target and then choose four points in 1m North, South, East, and west of the target +2. Now, take the radius equal to the time_horizon and choose six equidistant points on the circle +3. Take the radius of another circle equal to twice the time horizon and choose 8 equidistant point from the circle and add them to the candidate locations """ -x_previous, y_previous = 0, 0 - -# velocity of target = 1m/s ... Initially i.e. just assumed -# This velocity should be observed by the UAV -vel_target = 1 # i.e. we can predict just the next step through the polynomial predictor time_horizon = 1 -x_current, y_current = predictor_polynomial( - x_previous, y_previous, x_previous + vel_target, y_previous + vel_target -) -""" -Now, we need 3 different sets of points -1. Get the location of the target and then choose four points in 1m North, South, East, and west of the target -2. Now, take the radius equal to the time_horizon and choose six equidistant points on the circle -3. Take the radius of another circle equal to twice the time horizon and choose 8 equidistant point from the circle and add them to the candidate locations -""" +def candidates(x_current, y_current): + candidate_locations = np.empty((2, np.inf)) + + candidate_locations.append(x_current, y_current + 1) + candidate_locations.append(x_current, y_current - 1) + candidate_locations.append(x_current + 1, y_current) + candidate_locations.append(x_current - 1, y_current) -def candidates(): - + for i in range(0, 360, 60): + candidate_locations.append( + (time_horizon * cos(i * pi / 180)) + x_current, + (time_horizon * sin(i * pi / 180)) + y_current, + ) + for i in range(0, 360, 45): + candidate_locations.append( + (2 * time_horizon * cos(i * pi / 180)) + x_current, + (2 * time_horizon * sin(i * pi / 180)) + y_current, + ) -if __name__ == "__main__": - candidates() \ No newline at end of file + return candidate_locations diff --git a/scripts/cost_ugv.py b/turtlebots/scripts/cost_ugv.py similarity index 60% rename from scripts/cost_ugv.py rename to turtlebots/scripts/cost_ugv.py index b4a02ce..605c2b3 100644 --- a/scripts/cost_ugv.py +++ b/turtlebots/scripts/cost_ugv.py @@ -23,3 +23,34 @@ """ # This code is to get the value for the cost i.e. distance between # ugv and the person based on the location of the ugv and the person + +from cmath import sqrt, cos, sin +import numpy as np + +# x1 and y1 here are the current positons of the UGV +x1, y1 = 0, 0 +# Speed of UGV +v1 = 1 # 1 m/s + +""" +Information cost calculation: +We need the most reliable field of view possible from a given point to the tracked object + +So, for the same we propose a cost function that focuses on predicting motion of the object over the larger FOV +""" + + +def dist(x1, y1, x2, y2): + return sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) + + +def minimize_cost(list, x1, y1): + min_cost = np.inf + min_cost_coordinates = np.array(list[0][0], list[0][1]) + + for i in range(len(list)): + if min_cost > dist(x1, y1, list[i][0], list[i][1]): + min_cost = min(dist(x1, y1, list[i][0], list[i][1]), min_cost) + min_cost_coordinates = np.array(list[i][0], list[i][1]) + + return min_cost_coordinates diff --git a/scripts/prediction.py b/turtlebots/scripts/prediction.py similarity index 90% rename from scripts/prediction.py rename to turtlebots/scripts/prediction.py index c7552d8..9517013 100644 --- a/scripts/prediction.py +++ b/turtlebots/scripts/prediction.py @@ -29,15 +29,18 @@ Function : predictor_polynomial curve Inputs : current position of target and a position second before of the target """ +import numpy as np +# This is only about a forward direction def predictor_polynomial(x0, y0, x1, y1): global x2, y2 - x2 = ((x0 + x1) ^ 2) / 2 - y2 = ((y0 + y1) ^ 2) / 2 + x2 = ((x0 + x1 + 1) ^ 2) / 2 + y2 = ((y0 + y1 + 1) ^ 2) / 2 - return x2, y2 + pose_est = np.array([x2, y2]) + return pose_est """ diff --git a/scripts/reward_uav.py b/turtlebots/scripts/reward_uav.py similarity index 89% rename from scripts/reward_uav.py rename to turtlebots/scripts/reward_uav.py index e9b2061..c1ebcb7 100644 --- a/scripts/reward_uav.py +++ b/turtlebots/scripts/reward_uav.py @@ -86,6 +86,11 @@ def reward(x0, y0, x1, y1, x_target, y_target): def maximize_reward(list, x0, y0): max_reward = 0 - max_reward_coordinates = np.array(list[0], list[1]) + max_reward_coordinates = np.array(list[0][0], list[0][1]) + for i in range(len(list)): - max_reward = max(reward(x0, y0, x1, y1, list[i][0], list[i][1]), max_reward) + if max_reward < reward(x0, y0, x1, y1, list[i][0], list[i][1]): + max_reward = max(reward(x0, y0, x1, y1, list[i][0], list[i][1]), max_reward) + max_reward_coordinates = np.array(list[i][0], list[i][1]) + + return max_reward_coordinates diff --git a/turtlebots/scripts/target_pose.py b/turtlebots/scripts/target_pose.py new file mode 100644 index 0000000..a065aac --- /dev/null +++ b/turtlebots/scripts/target_pose.py @@ -0,0 +1,74 @@ +""" + MIT License + + Copyright (c) 2023 Mohd Alqama Shaikh + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +""" +# This will give the path that should be followed based on the shortlisted +# reachable sites by the uav + +""" +We will get a set of candidate goal pose from the candidates.py +and then according to the heuristics for the UAV we will shortlist the reachable +set of candidates for just UAV +""" + +import rospy +import numpy as np + +import candidates, cost_ugv, prediction, reward_uav + +# Subscribe to the position of the first model ( Position of the moving target ), second model ( Position of the UAV ), and the third model ( Position of the UGV ). + +""" +We first need the current location +Then we need to get all the desired setpoints for UGV and UAV +Then we need to go for the next point that is Update step and do the previous step for the same +Above procedure is repeated until distance between UGV and target is <= 1 +""" +x_previous, y_previous = 0, 0 + +# velocity of target = 1m/s ... Initially i.e. just assumed +# This velocity should be observed by the UAV +vel_target = 1 + +x_uav, y_uav = 3, 3 +x_ugv, y_ugv = 1, 1 + +pose_one = np.array([x_previous, y_previous]) + +curr_pose = prediction.predictor_polynomial( + pose_one[0], pose_one[1], pose_one[0] + vel_target, pose_one[1] + vel_target +) + +candidates_locations = candidates.candidates(curr_pose[0], curr_pose[1]) + +reward_coordinates = reward_uav.maximize_reward(candidates_locations, x_uav, y_uav) +cost_coordinates = cost_ugv.minimize_cost(candidates_locations, x_ugv, y_ugv) + +# publish the next pose of the UAV's and UGV's until UGV is less than 1m away from the target +new_x_uav, new_y_uav = reward_coordinates[0], reward_coordinates[1] +new_x_ugv, new_y_ugv = cost_coordinates[0], cost_coordinates[1] + +if reward_uav.dist(new_x_ugv, new_y_uav, curr_pose[0], curr_pose[1]) <= 1: + rospy.signal_shutdown("Target was caught successfully\n") + +if __name__ == "__main__": + callback() diff --git a/turtlebots/urdf/common_properties.xacro b/turtlebots/urdf/common_properties.xacro new file mode 100644 index 0000000..b26a853 --- /dev/null +++ b/turtlebots/urdf/common_properties.xacro @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/turtlebots/urdf/turtlebot3_burger.gazebo.xacro b/turtlebots/urdf/turtlebot3_burger.gazebo.xacro new file mode 100644 index 0000000..2e76946 --- /dev/null +++ b/turtlebots/urdf/turtlebot3_burger.gazebo.xacro @@ -0,0 +1,135 @@ + + + + + + + Gazebo/DarkGrey + + + + 0.1 + 0.1 + 500000.0 + 10.0 + 0.001 + 0.1 + 1 0 0 + Gazebo/FlatBlack + + + + 0.1 + 0.1 + 500000.0 + 10.0 + 0.001 + 0.1 + 1 0 0 + Gazebo/FlatBlack + + + + 0.1 + 0.1 + 1000000.0 + 100.0 + 0.001 + 1.0 + Gazebo/FlatBlack + + + + + true + $(arg imu_visual) + + Gazebo/FlatBlack + + + + + cmd_vel_tb1 + odom_tb1 + odom_tb1 + world + true + base_footprint + false + true + true + false + 30 + wheel_left_joint + wheel_right_joint + 0.160 + 0.066 + 1 + 10 + na + + + + + + true + imu_link_tb1 + imu_link_tb1 + imu + imu_service + 0.0 + 0 + + + gaussian + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + + + + Gazebo/FlatBlack + + 0 0 0 0 0 0 + $(arg laser_visual) + 5 + + + + 360 + 1 + 0.0 + 6.28319 + + + + 0.120 + 3.5 + 0.015 + + + gaussian + 0.0 + 0.01 + + + + scan_tb1 + base_scan_tb1 + + + + + diff --git a/turtlebots/urdf/turtlebot3_burger.urdf.xacro b/turtlebots/urdf/turtlebot3_burger.urdf.xacro new file mode 100644 index 0000000..7f5acdd --- /dev/null +++ b/turtlebots/urdf/turtlebot3_burger.urdf.xacro @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/turtlebots/worlds/tracking_one b/turtlebots/worlds/tracking_one new file mode 100644 index 0000000..4cdc479 --- /dev/null +++ b/turtlebots/worlds/tracking_one @@ -0,0 +1,1322 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + + 1000 + 0.001 + 1 + + + quick + 150 + 0 + 1.4 + 1 + + + 1e-05 + 0.2 + 2000 + 0.01 + + + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + 0.130385 -0.603101 17.253 3.14159 1.57079 3.14159 + orbit + perspective + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + + + -0.00429 0 0.030734 0 -0 0 + 0.944735 + + 0.00717896 + -1.22941e-05 + 0.00057674 + 0.00721804 + -5.01209e-06 + 0.00413858 + + + + -0.032 0 0.08 0 -0 0 + + + 0.14 0.14 0.143 + + + + + + + + + + + + + + + 10 + + + -0.081 1e-06 0.005 -1.57 0 0 + + + 0.03 0.009 0.02 + + + + + + 1e+06 + 100 + 1 + 0.001 + + + + + 0.1 + 0.1 + + + + + + + + 10 + + + -0.017 0 0.1755 0 -0 0 + + + 0.0315 + 0.055 + + + + + + + + + + + + + + + 10 + + + -0.032 0 0.01 0 -0 0 + + + 0.001 0.001 0.001 + /home/proto/mambaforge/envs/ros_env/share/turtlebot3_description/meshes/bases/burger_base.stl + + + + + + + + -0.032 0 0.182 0 -0 0 + + + 0.001 0.001 0.001 + /home/proto/mambaforge/envs/ros_env/share/turtlebot3_description/meshes/sensors/lds.stl + + + + + + + + 1 + 0 + -0.032 0 0.078 0 -0 0 + + + + 0 + 5 + + + + 360 + 1 + 0 + 6.28319 + + + 1 + 0 + 0 + + + + 0.12 + 3.5 + 0.015 + + + gaussian + 0 + 0.01 + + + + scan + base_scan + / + + -0.032 0 0.182 0 -0 0 + + 0 + 0 + 0 + + + 0 0.08 0.033 -1.57 0 0 + base_footprint + wheel_left_link + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 0.0284989 + + 1.11756e-05 + -4.23698e-11 + -5.93817e-09 + 1.11924e-05 + -1.44001e-11 + 2.07126e-05 + + + + 0 0 0 0 -0 0 + + + 0.018 + 0.033 + + + + + + 500000 + 10 + 0.1 + 0.001 + + + + + 0.1 + 0.1 + 1 0 0 + + + + + + + + 10 + + + 0 0 0 1.57 -0 0 + + + 0.001 0.001 0.001 + /home/proto/mambaforge/envs/ros_env/share/turtlebot3_description/meshes/wheels/left_tire.stl + + + + + + + 0 + 0 + 0 + + + 0 -0.08 0.033 -1.57 0 0 + base_footprint + wheel_right_link + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 0.0284989 + + 1.11756e-05 + -4.23698e-11 + -5.93817e-09 + 1.11924e-05 + -1.44001e-11 + 2.07126e-05 + + + + 0 0 0 0 -0 0 + + + 0.018 + 0.033 + + + + + + 500000 + 10 + 0.1 + 0.001 + + + + + 0.1 + 0.1 + 1 0 0 + + + + + + + + 10 + + + 0 0 0 1.57 -0 0 + + + 0.001 0.001 0.001 + /home/proto/mambaforge/envs/ros_env/share/turtlebot3_description/meshes/wheels/right_tire.stl + + + + + + + 0 + 0 + 0 + + + cmd_vel + odom + odom + world + 1 + base_footprint + 0 + 1 + 1 + 0 + 30 + wheel_left_joint + wheel_right_joint + 0.160 + 0.066 + 1 + 10 + na + / + + 0 + + 1 + imu + imu_service + 0.0 + 0 + + + gaussian + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + / + base_footprint + base_footprint + -0.064 0 0.156 + 0 -0 0 + 1 + + 3 1 0 0 -0 0 + + + + + -0.00429 0 0.030734 0 -0 0 + 0.944735 + + 0.00717896 + -1.22941e-05 + 0.00057674 + 0.00721804 + -5.01209e-06 + 0.00413858 + + + + -0.032 0 0.08 0 -0 0 + + + 0.14 0.14 0.143 + + + + + + + + + + + + + + + 10 + + + -0.081 1e-06 0.005 -1.57 0 0 + + + 0.03 0.009 0.02 + + + + + + 1e+06 + 100 + 1 + 0.001 + + + + + 0.1 + 0.1 + + + + + + + + 10 + + + -0.017 0 0.1755 0 -0 0 + + + 0.0315 + 0.055 + + + + + + + + + + + + + + + 10 + + + -0.032 0 0.01 0 -0 0 + + + 0.001 0.001 0.001 + /home/proto/mambaforge/envs/ros_env/share/turtlebot3_description/meshes/bases/burger_base.stl + + + + + + + + -0.032 0 0.182 0 -0 0 + + + 0.001 0.001 0.001 + /home/proto/mambaforge/envs/ros_env/share/turtlebot3_description/meshes/sensors/lds.stl + + + + + + + + 1 + 0 + -0.032 0 0.078 0 -0 0 + + + + 0 + 5 + + + + 360 + 1 + 0 + 6.28319 + + + 1 + 0 + 0 + + + + 0.12 + 3.5 + 0.015 + + + gaussian + 0 + 0.01 + + + + scan + base_scan + / + + -0.032 0 0.182 0 -0 0 + + 0 + 0 + 0 + + + 0 0.08 0.033 -1.57 0 0 + base_footprint + wheel_left_link + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 0.0284989 + + 1.11756e-05 + -4.23698e-11 + -5.93817e-09 + 1.11924e-05 + -1.44001e-11 + 2.07126e-05 + + + + 0 0 0 0 -0 0 + + + 0.018 + 0.033 + + + + + + 500000 + 10 + 0.1 + 0.001 + + + + + 0.1 + 0.1 + 1 0 0 + + + + + + + + 10 + + + 0 0 0 1.57 -0 0 + + + 0.001 0.001 0.001 + /home/proto/mambaforge/envs/ros_env/share/turtlebot3_description/meshes/wheels/left_tire.stl + + + + + + + 0 + 0 + 0 + + + 0 -0.08 0.033 -1.57 0 0 + base_footprint + wheel_right_link + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 0.0284989 + + 1.11756e-05 + -4.23698e-11 + -5.93817e-09 + 1.11924e-05 + -1.44001e-11 + 2.07126e-05 + + + + 0 0 0 0 -0 0 + + + 0.018 + 0.033 + + + + + + 500000 + 10 + 0.1 + 0.001 + + + + + 0.1 + 0.1 + 1 0 0 + + + + + + + + 10 + + + 0 0 0 1.57 -0 0 + + + 0.001 0.001 0.001 + /home/proto/mambaforge/envs/ros_env/share/turtlebot3_description/meshes/wheels/right_tire.stl + + + + + + + 0 + 0 + 0 + + + cmd_vel + odom + odom + world + 1 + base_footprint + 0 + 1 + 1 + 0 + 30 + wheel_left_joint + wheel_right_joint + 0.160 + 0.066 + 1 + 10 + na + / + + 0 + + 1 + imu + imu_service + 0.0 + 0 + + + gaussian + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + / + base_footprint + base_footprint + -0.064 0 0.156 + 0 -0 0 + 1 + + 3 -1 0 0 -0 0 + + + + + -0.00429 0 0.030734 0 -0 0 + 0.944735 + + 0.00717896 + -1.22941e-05 + 0.00057674 + 0.00721804 + -5.01209e-06 + 0.00413858 + + + + -0.032 0 0.08 0 -0 0 + + + 0.14 0.14 0.143 + + + + + + + + + + + + + + + 10 + + + -0.081 1e-06 0.005 -1.57 0 0 + + + 0.03 0.009 0.02 + + + + + + 1e+06 + 100 + 1 + 0.001 + + + + + 0.1 + 0.1 + + + + + + + + 10 + + + -0.017 0 0.1755 0 -0 0 + + + 0.0315 + 0.055 + + + + + + + + + + + + + + + 10 + + + -0.032 0 0.01 0 -0 0 + + + 0.001 0.001 0.001 + /home/proto/mambaforge/envs/ros_env/share/turtlebot3_description/meshes/bases/burger_base.stl + + + + + + + + -0.032 0 0.182 0 -0 0 + + + 0.001 0.001 0.001 + /home/proto/mambaforge/envs/ros_env/share/turtlebot3_description/meshes/sensors/lds.stl + + + + + + + + 1 + 0 + -0.032 0 0.078 0 -0 0 + + + + 0 + 5 + + + + 360 + 1 + 0 + 6.28319 + + + 1 + 0 + 0 + + + + 0.12 + 3.5 + 0.015 + + + gaussian + 0 + 0.01 + + + + scan + base_scan + / + + -0.032 0 0.182 0 -0 0 + + 0 + 0 + 0 + + + 0 0.08 0.033 -1.57 0 0 + base_footprint + wheel_left_link + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 0.0284989 + + 1.11756e-05 + -4.23698e-11 + -5.93817e-09 + 1.11924e-05 + -1.44001e-11 + 2.07126e-05 + + + + 0 0 0 0 -0 0 + + + 0.018 + 0.033 + + + + + + 500000 + 10 + 0.1 + 0.001 + + + + + 0.1 + 0.1 + 1 0 0 + + + + + + + + 10 + + + 0 0 0 1.57 -0 0 + + + 0.001 0.001 0.001 + /home/proto/mambaforge/envs/ros_env/share/turtlebot3_description/meshes/wheels/left_tire.stl + + + + + + + 0 + 0 + 0 + + + 0 -0.08 0.033 -1.57 0 0 + base_footprint + wheel_right_link + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 0.0284989 + + 1.11756e-05 + -4.23698e-11 + -5.93817e-09 + 1.11924e-05 + -1.44001e-11 + 2.07126e-05 + + + + 0 0 0 0 -0 0 + + + 0.018 + 0.033 + + + + + + 500000 + 10 + 0.1 + 0.001 + + + + + 0.1 + 0.1 + 1 0 0 + + + + + + + + 10 + + + 0 0 0 1.57 -0 0 + + + 0.001 0.001 0.001 + /home/proto/mambaforge/envs/ros_env/share/turtlebot3_description/meshes/wheels/right_tire.stl + + + + + + + 0 + 0 + 0 + + + cmd_vel + odom + odom + world + 1 + base_footprint + 0 + 1 + 1 + 0 + 30 + wheel_left_joint + wheel_right_joint + 0.160 + 0.066 + 1 + 10 + na + / + + 0 + + 1 + imu + imu_service + 0.0 + 0 + + + gaussian + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + / + base_footprint + base_footprint + -0.064 0 0.156 + 0 -0 0 + 1 + + -2 -0.5 0 0 -0 0 + + + 118 564000000 + 119 328961017 + 1687111809 670763145 + 118564 + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2.00003 -0.499932 -0.001001 -1.4e-05 0.007706 0.001064 + 1 1 1 + + -2.00003 -0.499932 -0.001001 -1.4e-05 0.007706 0.001064 + 0 1e-06 3.6e-05 3.9e-05 -0.00018 1e-05 + -0 0 -0 0 -0 0 + -0 0 -0 0 -0 0 + + + -1.99986 -0.419931 0.031997 -1.57002 0.005129 0.001062 + -7e-06 -1e-06 3.9e-05 3.8e-05 -0.000204 1.1e-05 + -0 0 -0 0 -0 0 + -0 0 -0 0 -0 0 + + + -1.99969 -0.579931 0.031999 -1.57002 0.01021 0.001066 + -5e-06 -1e-06 3.3e-05 3.8e-05 -0.00016 1e-05 + 0 0 -0 0 -0 0 + 0 0 -0 0 -0 0 + + + + 3.01582 -0.945884 -0.001001 -1.4e-05 0.007706 0.001131 + 1 1 1 + + 3.01582 -0.945884 -0.001001 -1.4e-05 0.007706 0.001131 + 0 1e-06 3.6e-05 3.9e-05 -0.00018 1e-05 + -0 0 -0 0 -0 0 + -0 0 -0 0 -0 0 + + + 3.01599 -0.865884 0.031997 -1.57002 0.007407 0.001131 + -7e-06 -1e-06 3.9e-05 3.8e-05 -0.000206 1.1e-05 + -0 0 -0 0 -0 0 + -0 0 -0 0 -0 0 + + + 3.01617 -1.02588 0.031999 -1.57002 0.012604 0.001135 + -5e-06 -1e-06 3.3e-05 3.8e-05 -0.000161 1.1e-05 + 0 0 -0 0 -0 0 + 0 0 -0 0 -0 0 + + + + 3.01636 0.945476 -0.001001 -1.4e-05 0.007706 0.003198 + 1 1 1 + + 3.01636 0.945476 -0.001001 -1.4e-05 0.007706 0.003198 + 0 1e-06 3.6e-05 3.9e-05 -0.00018 1e-05 + -0 0 -0 0 -0 0 + -0 0 -0 0 -0 0 + + + 3.01635 1.02548 0.031997 -1.57002 -0.003295 0.003189 + -7e-06 -1e-06 3.9e-05 3.8e-05 -0.000199 1e-05 + -0 0 -0 0 -0 0 + -0 0 -0 0 -0 0 + + + 3.01687 0.865477 0.031999 -1.57002 0.002078 0.003193 + -5e-06 -1e-06 3.3e-05 3.8e-05 -0.000155 9e-06 + 0 0 -0 0 -0 0 + 0 0 -0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + From 0c98a3b87d5e30bb6b80b27f7ca18d64edd5b8d1 Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Mon, 19 Jun 2023 17:40:14 +0530 Subject: [PATCH 05/17] [ Minor Changes ] : One turtlebot can be controlled independently --- turtlebots/launch/tracking.launch | 2 +- turtlebots/urdf/turtlebot3_burger.urdf.xacro | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/turtlebots/launch/tracking.launch b/turtlebots/launch/tracking.launch index db509d2..0928ce5 100644 --- a/turtlebots/launch/tracking.launch +++ b/turtlebots/launch/tracking.launch @@ -14,7 +14,7 @@ - + diff --git a/turtlebots/urdf/turtlebot3_burger.urdf.xacro b/turtlebots/urdf/turtlebot3_burger.urdf.xacro index 7f5acdd..72fca16 100644 --- a/turtlebots/urdf/turtlebot3_burger.urdf.xacro +++ b/turtlebots/urdf/turtlebot3_burger.urdf.xacro @@ -1,7 +1,7 @@ - - + + From 59d30262755339c605ba17076b444c610a9a385b Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Wed, 21 Jun 2023 01:16:02 +0530 Subject: [PATCH 06/17] [ Turtlebot Addition ] : Adds another turtlebot and all three are now independent from each other --- README.md | 8 +- turtlebots/launch/tracking.launch | 11 +- .../urdf/turtlebot3_burger.gazebo.xacro | 12 +- turtlebots/urdf/turtlebot3_burger.urdf.xacro | 16 +- .../urdf/turtlebot3_burger_3.gazebo.xacro | 135 ++++++++++++++ .../urdf/turtlebot3_burger_3.urdf.xacro | 165 ++++++++++++++++++ 6 files changed, 327 insertions(+), 20 deletions(-) create mode 100644 turtlebots/urdf/turtlebot3_burger_3.gazebo.xacro create mode 100644 turtlebots/urdf/turtlebot3_burger_3.urdf.xacro diff --git a/README.md b/README.md index 700144f..cc89a46 100644 --- a/README.md +++ b/README.md @@ -8,4 +8,10 @@ Targets: - [ ] Using Fuel contraints to gain the optimal path that a UAV and UGV should follow - [ ] It is not necessary that UAV and UGV should follow same number of points they should be nearly independent -![img](https://lh3.googleusercontent.com/8UXw1B8W2pv5L7X4VIjYxJCW99FevBTlxePC9rCUy8Wum401uCN7g5U9q6hapnJf-_HcGprfS4pObYFPwv_4PTFEXgSJl2U-eCagH7KHOYqMj-Eghug_qRIink8FoR7KFIYikL04n4a_u4wM2bc_wMVfI7L8r_8OBLTjzWU2-Y4dLRBUZja81R_rCE7Z0d_HNXUf1dWyX0SJXA3Zcr8mukAELd3DZNEH8A901ivj-TNbhZ1v_eIwIEW_VVQjIQLnbDgGGXOgpmHgZfi4Lklh34S6rKhcfrv_aHCM6IvXGT4zD52oobiUHbHAqGWHesDhDUqdfj1bysOnItPeVaRAScFlFd7Mz4zt1s3Z6BA0bY9a4NAPh881vGUTe4T4ufZB98XQm3OqVV8p4eckag70f19TgJNzdICD-UoY53ZGaREwgS4rU0tDK6rpvLnuO_myRvRaCXo5snw3EJb59vr23HDpcvdUgUwyovID1flbrrtsw4Y1xqP-DkKJJf6KmD6p4M5i4ubRsXLmtAtboXLcpS3zPLbAb9yfpDb15kwpRZjONn-S8hWimfeSVW90iRpQyRN_p8OBzxgsyN3R1T7ZQId5K5gAGVEW8I-_kD5EuAJdWzuOsgXMi7vPGPfCHWBdmegugRZM09bfK-w1vFCnSgF_LZ_TiB5VYXkpG3kYGC7uMnZTE5yLCkLK91VkM6y4kjSym64Nb05Xzck11FxaqopMeQpHEj0CNvgrBcjuSBeppB3YfiBwYGvaL05ts0Wi93VNLyp9Eqkt0pcsGXjEJAs9j9sPO8PH_ZuqYTdjix-Pk4fZJSl7VcemGy0ZRmzMg8P5XZlqszRyR-PZM5-scR349lsvHdLC-Wfmsg4veUlNAEfTlwifTEZKxXmvFnwZDceir9Fft8zTERWJlpabWnGUbkgO9cSebbfyoYoq1x3nFCcCSqZlvoyVLhA-aR-_BeHuAIRMWHLqwVt-cUESpnKh0WRUP0XBrxnFJiRxGpWJGWoyas9QgDI=w1024-h1366-s-no?authuser=0) \ No newline at end of file +![img](https://lh3.googleusercontent.com/8UXw1B8W2pv5L7X4VIjYxJCW99FevBTlxePC9rCUy8Wum401uCN7g5U9q6hapnJf-_HcGprfS4pObYFPwv_4PTFEXgSJl2U-eCagH7KHOYqMj-Eghug_qRIink8FoR7KFIYikL04n4a_u4wM2bc_wMVfI7L8r_8OBLTjzWU2-Y4dLRBUZja81R_rCE7Z0d_HNXUf1dWyX0SJXA3Zcr8mukAELd3DZNEH8A901ivj-TNbhZ1v_eIwIEW_VVQjIQLnbDgGGXOgpmHgZfi4Lklh34S6rKhcfrv_aHCM6IvXGT4zD52oobiUHbHAqGWHesDhDUqdfj1bysOnItPeVaRAScFlFd7Mz4zt1s3Z6BA0bY9a4NAPh881vGUTe4T4ufZB98XQm3OqVV8p4eckag70f19TgJNzdICD-UoY53ZGaREwgS4rU0tDK6rpvLnuO_myRvRaCXo5snw3EJb59vr23HDpcvdUgUwyovID1flbrrtsw4Y1xqP-DkKJJf6KmD6p4M5i4ubRsXLmtAtboXLcpS3zPLbAb9yfpDb15kwpRZjONn-S8hWimfeSVW90iRpQyRN_p8OBzxgsyN3R1T7ZQId5K5gAGVEW8I-_kD5EuAJdWzuOsgXMi7vPGPfCHWBdmegugRZM09bfK-w1vFCnSgF_LZ_TiB5VYXkpG3kYGC7uMnZTE5yLCkLK91VkM6y4kjSym64Nb05Xzck11FxaqopMeQpHEj0CNvgrBcjuSBeppB3YfiBwYGvaL05ts0Wi93VNLyp9Eqkt0pcsGXjEJAs9j9sPO8PH_ZuqYTdjix-Pk4fZJSl7VcemGy0ZRmzMg8P5XZlqszRyR-PZM5-scR349lsvHdLC-Wfmsg4veUlNAEfTlwifTEZKxXmvFnwZDceir9Fft8zTERWJlpabWnGUbkgO9cSebbfyoYoq1x3nFCcCSqZlvoyVLhA-aR-_BeHuAIRMWHLqwVt-cUESpnKh0WRUP0XBrxnFJiRxGpWJGWoyas9QgDI=w1024-h1366-s-no?authuser=0) + +### Setup + +Environment is free from obstacles (as of now), there are 3 turtlebots one of them is a non-cooperative agent and other two resembles UAV and UGV + +![turtlebot_spawned](https://github.com/aPR0T0/Catching-the-non-cooperative-agent/assets/97826285/72ce76dd-7516-4b29-85f5-5dfbc58ab701) diff --git a/turtlebots/launch/tracking.launch b/turtlebots/launch/tracking.launch index 0928ce5..2b6d53e 100644 --- a/turtlebots/launch/tracking.launch +++ b/turtlebots/launch/tracking.launch @@ -13,10 +13,11 @@ - - + + + - - - + + + diff --git a/turtlebots/urdf/turtlebot3_burger.gazebo.xacro b/turtlebots/urdf/turtlebot3_burger.gazebo.xacro index 2e76946..411389b 100644 --- a/turtlebots/urdf/turtlebot3_burger.gazebo.xacro +++ b/turtlebots/urdf/turtlebot3_burger.gazebo.xacro @@ -1,9 +1,9 @@ - + - + Gazebo/DarkGrey @@ -60,8 +60,8 @@ true false 30 - wheel_left_joint - wheel_right_joint + wheel_left_joint_tb1 + wheel_right_joint_tb1 0.160 0.066 1 @@ -75,8 +75,8 @@ true imu_link_tb1 imu_link_tb1 - imu - imu_service + imu_tb1 + imu_service_tb1 0.0 0 diff --git a/turtlebots/urdf/turtlebot3_burger.urdf.xacro b/turtlebots/urdf/turtlebot3_burger.urdf.xacro index 72fca16..4c32468 100644 --- a/turtlebots/urdf/turtlebot3_burger.urdf.xacro +++ b/turtlebots/urdf/turtlebot3_burger.urdf.xacro @@ -1,5 +1,5 @@ - + @@ -7,11 +7,11 @@ - + - + @@ -37,7 +37,7 @@ - + @@ -69,7 +69,7 @@ - + @@ -101,7 +101,7 @@ - + @@ -124,7 +124,7 @@ - + @@ -132,7 +132,7 @@ - + diff --git a/turtlebots/urdf/turtlebot3_burger_3.gazebo.xacro b/turtlebots/urdf/turtlebot3_burger_3.gazebo.xacro new file mode 100644 index 0000000..527f7f6 --- /dev/null +++ b/turtlebots/urdf/turtlebot3_burger_3.gazebo.xacro @@ -0,0 +1,135 @@ + + + + + + + Gazebo/DarkGrey + + + + 0.1 + 0.1 + 500000.0 + 10.0 + 0.001 + 0.1 + 1 0 0 + Gazebo/FlatBlack + + + + 0.1 + 0.1 + 500000.0 + 10.0 + 0.001 + 0.1 + 1 0 0 + Gazebo/FlatBlack + + + + 0.1 + 0.1 + 1000000.0 + 100.0 + 0.001 + 1.0 + Gazebo/FlatBlack + + + + + true + $(arg imu_visual) + + Gazebo/FlatBlack + + + + + cmd_vel_tb2 + odom_tb2 + odom_tb2 + world + true + base_footprint + false + true + true + false + 30 + wheel_left_joint_tb2 + wheel_right_joint_tb2 + 0.160 + 0.066 + 1 + 10 + na + + + + + + true + imu_link_tb2 + imu_link_tb2 + imu_tb2 + imu_service_tb2 + 0.0 + 0 + + + gaussian + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + + + + Gazebo/FlatBlack + + 0 0 0 0 0 0 + $(arg laser_visual) + 5 + + + + 360 + 1 + 0.0 + 6.28319 + + + + 0.120 + 3.5 + 0.015 + + + gaussian + 0.0 + 0.01 + + + + scan_tb2 + base_scan_tb2 + + + + + diff --git a/turtlebots/urdf/turtlebot3_burger_3.urdf.xacro b/turtlebots/urdf/turtlebot3_burger_3.urdf.xacro new file mode 100644 index 0000000..a143e46 --- /dev/null +++ b/turtlebots/urdf/turtlebot3_burger_3.urdf.xacro @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 57cc7fece9279b85409a73bd841b059f6d666a35 Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Thu, 22 Jun 2023 00:03:47 +0530 Subject: [PATCH 07/17] [Cooking] --- turtlebots/CMakeLists.txt | 1 + turtlebots/launch/tracking.launch | 18 ++++++---- turtlebots/package.xml | 3 ++ turtlebots/scripts/target_pose.py | 58 +++++++++++++++++++++++-------- 4 files changed, 59 insertions(+), 21 deletions(-) diff --git a/turtlebots/CMakeLists.txt b/turtlebots/CMakeLists.txt index 0c62b7c..6f70290 100644 --- a/turtlebots/CMakeLists.txt +++ b/turtlebots/CMakeLists.txt @@ -9,6 +9,7 @@ project(turtlebots) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS rospy + nav_msgs ) ## System dependencies are found with CMake's conventions diff --git a/turtlebots/launch/tracking.launch b/turtlebots/launch/tracking.launch index 2b6d53e..5aac16f 100644 --- a/turtlebots/launch/tracking.launch +++ b/turtlebots/launch/tracking.launch @@ -1,8 +1,14 @@ - - - + + + + + + + + + @@ -17,7 +23,7 @@ - - - + + + diff --git a/turtlebots/package.xml b/turtlebots/package.xml index b09fb66..16ae375 100644 --- a/turtlebots/package.xml +++ b/turtlebots/package.xml @@ -50,8 +50,11 @@ catkin rospy + nav_msgs rospy + nav_msgs rospy + nav_msgs diff --git a/turtlebots/scripts/target_pose.py b/turtlebots/scripts/target_pose.py index a065aac..4b2c16f 100644 --- a/turtlebots/scripts/target_pose.py +++ b/turtlebots/scripts/target_pose.py @@ -24,13 +24,15 @@ # This will give the path that should be followed based on the shortlisted # reachable sites by the uav -""" +"""k We will get a set of candidate goal pose from the candidates.py and then according to the heuristics for the UAV we will shortlist the reachable set of candidates for just UAV """ import rospy +from nav_msgs.msg import Odometry +from std_msgs.msg import Int16MultiArray import numpy as np import candidates, cost_ugv, prediction, reward_uav @@ -48,27 +50,53 @@ # velocity of target = 1m/s ... Initially i.e. just assumed # This velocity should be observed by the UAV vel_target = 1 - +path_uav = np.array([[3,1]]) +path_ugv = np.array([[3,-1]]) x_uav, y_uav = 3, 3 x_ugv, y_ugv = 1, 1 -pose_one = np.array([x_previous, y_previous]) +def odo_sub(odom_data): + x = odom_data.pose.pose.position.x + y = odom_data.pose.pose.position.y + + return x, y + +def vel_sub(odom_data): + vel = odom_data.twist.twist.linear.x + + return vel + +def catcher(): + rospy.init_node('Path_renderer', anonymous=False) + + while not rospy.is_shutdown(): + # creating subscribers and publishers for 3 turtlebots + x_previous, y_previous = rospy.Subscriber("/pose_sub_for_agent", Odometry, odo_sub) + x_ugv, y_ugv = rospy.Subscriber("/pose_sub_for_ugv", Odometry, odo_sub) + x_uav, y_uav = rospy.Subscriber("/pose_sub_for_uav", Odometry, odo_sub) + + vel_target = rospy.Subscriber("/vel_sub_for_agent", Odometry, vel_sub) + + pose_one = np.array([x_previous, y_previous]) -curr_pose = prediction.predictor_polynomial( - pose_one[0], pose_one[1], pose_one[0] + vel_target, pose_one[1] + vel_target -) + curr_pose = prediction.predictor_polynomial( + pose_one[0], pose_one[1], pose_one[0] + vel_target, pose_one[1] + vel_target + ) -candidates_locations = candidates.candidates(curr_pose[0], curr_pose[1]) + candidates_locations = candidates.candidates(curr_pose[0], curr_pose[1]) -reward_coordinates = reward_uav.maximize_reward(candidates_locations, x_uav, y_uav) -cost_coordinates = cost_ugv.minimize_cost(candidates_locations, x_ugv, y_ugv) + reward_coordinates = reward_uav.maximize_reward(candidates_locations, x_uav, y_uav) + cost_coordinates = cost_ugv.minimize_cost(candidates_locations, x_ugv, y_ugv) -# publish the next pose of the UAV's and UGV's until UGV is less than 1m away from the target -new_x_uav, new_y_uav = reward_coordinates[0], reward_coordinates[1] -new_x_ugv, new_y_ugv = cost_coordinates[0], cost_coordinates[1] + # publish the next pose of the UAV's and UGV's until UGV is less than 1m away from the target + new_x_uav, new_y_uav = reward_coordinates[0], reward_coordinates[1] + new_x_ugv, new_y_ugv = cost_coordinates[0], cost_coordinates[1] -if reward_uav.dist(new_x_ugv, new_y_uav, curr_pose[0], curr_pose[1]) <= 1: - rospy.signal_shutdown("Target was caught successfully\n") + if reward_uav.dist(new_x_ugv, new_y_uav, curr_pose[0], curr_pose[1]) <= 1: + rospy.signal_shutdown("Target was caught successfully\n") if __name__ == "__main__": - callback() + try: + catcher() + except rospy.ROSInterruptException: + pass \ No newline at end of file From f8cf4455765e2d147709caa2d9b4cd68173e3b82 Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Thu, 22 Jun 2023 11:00:07 +0530 Subject: [PATCH 08/17] [ Minor Changes ] : changed scripts/ to src/ --- turtlebots/{scripts => src}/candidates.py | 0 turtlebots/{scripts => src}/cost_ugv.py | 0 turtlebots/{scripts => src}/prediction.py | 0 turtlebots/{scripts => src}/reward_uav.py | 0 turtlebots/{scripts => src}/target_pose.py | 0 5 files changed, 0 insertions(+), 0 deletions(-) rename turtlebots/{scripts => src}/candidates.py (100%) rename turtlebots/{scripts => src}/cost_ugv.py (100%) rename turtlebots/{scripts => src}/prediction.py (100%) rename turtlebots/{scripts => src}/reward_uav.py (100%) rename turtlebots/{scripts => src}/target_pose.py (100%) diff --git a/turtlebots/scripts/candidates.py b/turtlebots/src/candidates.py similarity index 100% rename from turtlebots/scripts/candidates.py rename to turtlebots/src/candidates.py diff --git a/turtlebots/scripts/cost_ugv.py b/turtlebots/src/cost_ugv.py similarity index 100% rename from turtlebots/scripts/cost_ugv.py rename to turtlebots/src/cost_ugv.py diff --git a/turtlebots/scripts/prediction.py b/turtlebots/src/prediction.py similarity index 100% rename from turtlebots/scripts/prediction.py rename to turtlebots/src/prediction.py diff --git a/turtlebots/scripts/reward_uav.py b/turtlebots/src/reward_uav.py similarity index 100% rename from turtlebots/scripts/reward_uav.py rename to turtlebots/src/reward_uav.py diff --git a/turtlebots/scripts/target_pose.py b/turtlebots/src/target_pose.py similarity index 100% rename from turtlebots/scripts/target_pose.py rename to turtlebots/src/target_pose.py From d716310d850428524f24c19643e2e2d7c4810650 Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Thu, 22 Jun 2023 12:29:40 +0530 Subject: [PATCH 09/17] [Debugging]: converts some np.arrays to tuples --- turtlebots/src/candidates.py | 18 +++++------ turtlebots/src/reward_uav.py | 6 +--- turtlebots/src/target_pose.py | 59 +++++++++++++++++++++-------------- 3 files changed, 45 insertions(+), 38 deletions(-) diff --git a/turtlebots/src/candidates.py b/turtlebots/src/candidates.py index 16823b6..ade958b 100644 --- a/turtlebots/src/candidates.py +++ b/turtlebots/src/candidates.py @@ -37,23 +37,23 @@ def candidates(x_current, y_current): - candidate_locations = np.empty((2, np.inf)) + candidate_locations = np.empty((2, 10000)) - candidate_locations.append(x_current, y_current + 1) - candidate_locations.append(x_current, y_current - 1) - candidate_locations.append(x_current + 1, y_current) - candidate_locations.append(x_current - 1, y_current) + candidate_locations = [[x_current, y_current + 1]] + candidate_locations.append([x_current, y_current - 1]) + candidate_locations.append([x_current + 1, y_current]) + candidate_locations.append([x_current - 1, y_current]) for i in range(0, 360, 60): candidate_locations.append( - (time_horizon * cos(i * pi / 180)) + x_current, - (time_horizon * sin(i * pi / 180)) + y_current, + [(time_horizon * cos(i * pi / 180)) + x_current, + (time_horizon * sin(i * pi / 180)) + y_current] ) for i in range(0, 360, 45): - candidate_locations.append( + candidate_locations.append([ (2 * time_horizon * cos(i * pi / 180)) + x_current, - (2 * time_horizon * sin(i * pi / 180)) + y_current, + (2 * time_horizon * sin(i * pi / 180)) + y_current] ) return candidate_locations diff --git a/turtlebots/src/reward_uav.py b/turtlebots/src/reward_uav.py index c1ebcb7..fe6efe8 100644 --- a/turtlebots/src/reward_uav.py +++ b/turtlebots/src/reward_uav.py @@ -28,10 +28,6 @@ from cmath import sqrt, cos, sin import numpy as np -# x0 and y0 here are the previous postions of the UAV -x0, y0 = -1, -1 -# x1 and y1 here are the current positons of the UAV -x1, y1 = 0, 0 # Angle of vision in degrees theta = 60 # radius of vision is 5m from the UAV @@ -84,7 +80,7 @@ def reward(x0, y0, x1, y1, x_target, y_target): return reward_value -def maximize_reward(list, x0, y0): +def maximize_reward(list, x0, y0, x1, y1): max_reward = 0 max_reward_coordinates = np.array(list[0][0], list[0][1]) diff --git a/turtlebots/src/target_pose.py b/turtlebots/src/target_pose.py index 4b2c16f..ae77fd8 100644 --- a/turtlebots/src/target_pose.py +++ b/turtlebots/src/target_pose.py @@ -50,49 +50,60 @@ # velocity of target = 1m/s ... Initially i.e. just assumed # This velocity should be observed by the UAV vel_target = 1 -path_uav = np.array([[3,1]]) -path_ugv = np.array([[3,-1]]) -x_uav, y_uav = 3, 3 -x_ugv, y_ugv = 1, 1 +path_uav = [[3, 1]] +path_ugv = [[3,-1]] +x_uav_prev, y_uav_prev = 3, 0 +x_uav, y_uav = 3, 1 +x_ugv, y_ugv = 3,-1 -def odo_sub(odom_data): - x = odom_data.pose.pose.position.x - y = odom_data.pose.pose.position.y +def odo_sub_uav(odom_data): + x_uav = odom_data.pose.pose.position.x + y_uav = odom_data.pose.pose.position.y - return x, y +def odo_sub_ugv(odom_data): + x_ugv = odom_data.pose.pose.position.x + y_ugv = odom_data.pose.pose.position.y -def vel_sub(odom_data): - vel = odom_data.twist.twist.linear.x +def odo_sub(odom_data): + x_previous = odom_data.pose.pose.position.x + y_previous = odom_data.pose.pose.position.y - return vel +def vel_sub(odom_data): + vel_target = odom_data.twist.twist.linear.x def catcher(): rospy.init_node('Path_renderer', anonymous=False) - - while not rospy.is_shutdown(): + global x_uav, y_uav, x_ugv, y_ugv, x_previous, y_previous, x_uav_prev, y_uav_prev, vel_target + while not rospy.is_shutdown(): # creating subscribers and publishers for 3 turtlebots - x_previous, y_previous = rospy.Subscriber("/pose_sub_for_agent", Odometry, odo_sub) - x_ugv, y_ugv = rospy.Subscriber("/pose_sub_for_ugv", Odometry, odo_sub) - x_uav, y_uav = rospy.Subscriber("/pose_sub_for_uav", Odometry, odo_sub) + rospy.Subscriber("/odom", Odometry, odo_sub) + rospy.Subscriber("/odom_tb1", Odometry, odo_sub_ugv) + rospy.Subscriber("/odom_tb2", Odometry, odo_sub_uav) - vel_target = rospy.Subscriber("/vel_sub_for_agent", Odometry, vel_sub) + rospy.Subscriber("/odom", Odometry, vel_sub) - pose_one = np.array([x_previous, y_previous]) + pose_one = [x_previous, y_previous] - curr_pose = prediction.predictor_polynomial( + pose_pred = prediction.predictor_polynomial( pose_one[0], pose_one[1], pose_one[0] + vel_target, pose_one[1] + vel_target ) - candidates_locations = candidates.candidates(curr_pose[0], curr_pose[1]) - - reward_coordinates = reward_uav.maximize_reward(candidates_locations, x_uav, y_uav) + candidates_locations = candidates.candidates(pose_pred[0], pose_pred[1]) + print("Candidate Locations : \n", candidates_locations, "\n") + reward_coordinates = reward_uav.maximize_reward(candidates_locations, x_uav_prev, y_uav_prev, x_uav, y_uav) cost_coordinates = cost_ugv.minimize_cost(candidates_locations, x_ugv, y_ugv) # publish the next pose of the UAV's and UGV's until UGV is less than 1m away from the target new_x_uav, new_y_uav = reward_coordinates[0], reward_coordinates[1] new_x_ugv, new_y_ugv = cost_coordinates[0], cost_coordinates[1] - - if reward_uav.dist(new_x_ugv, new_y_uav, curr_pose[0], curr_pose[1]) <= 1: + path_uav.append([new_x_uav, new_y_uav]) + path_ugv.append([new_x_ugv, new_x_ugv]) + + x_uav_prev = x_uav + y_uav_prev = y_uav + if reward_uav.dist(new_x_ugv, new_y_ugv, pose_pred[0], pose_pred[1]) <= 1: + print("Path of UAV until target is caught: \n", path_uav, "\n") + print("Path of UGV until target is caught: \n", path_ugv, "\n") rospy.signal_shutdown("Target was caught successfully\n") if __name__ == "__main__": From 7856408d29a113838304ad50d81a8091137c42a6 Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Thu, 22 Jun 2023 14:32:39 +0530 Subject: [PATCH 10/17] [ Debugging ] : Mathematical and formatting errors --- turtlebots/src/cost_ugv.py | 6 ++--- turtlebots/src/reward_uav.py | 43 +++++++++++++++++++---------------- turtlebots/src/target_pose.py | 2 +- 3 files changed, 28 insertions(+), 23 deletions(-) diff --git a/turtlebots/src/cost_ugv.py b/turtlebots/src/cost_ugv.py index 605c2b3..ad718fc 100644 --- a/turtlebots/src/cost_ugv.py +++ b/turtlebots/src/cost_ugv.py @@ -41,16 +41,16 @@ def dist(x1, y1, x2, y2): - return sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) + return np.real(sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)) def minimize_cost(list, x1, y1): min_cost = np.inf - min_cost_coordinates = np.array(list[0][0], list[0][1]) + min_cost_coordinates = np.array([list[0][0], list[0][1]]) for i in range(len(list)): if min_cost > dist(x1, y1, list[i][0], list[i][1]): min_cost = min(dist(x1, y1, list[i][0], list[i][1]), min_cost) - min_cost_coordinates = np.array(list[i][0], list[i][1]) + min_cost_coordinates = np.array([list[i][0], list[i][1]]) return min_cost_coordinates diff --git a/turtlebots/src/reward_uav.py b/turtlebots/src/reward_uav.py index fe6efe8..c1e4e77 100644 --- a/turtlebots/src/reward_uav.py +++ b/turtlebots/src/reward_uav.py @@ -25,7 +25,7 @@ # This will give the reward based on the reachable states and the # candidate states provided -from cmath import sqrt, cos, sin +from cmath import sqrt, cos, sin, pi import numpy as np # Angle of vision in degrees @@ -35,7 +35,7 @@ def dist(x1, y1, x2, y2): - return sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) + return np.real(sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)) """ @@ -47,28 +47,33 @@ def dist(x1, y1, x2, y2): def reward(x0, y0, x1, y1, x_target, y_target): - vec1 = np.array([(x1 - x0), (y1 - y0)], [(y0 - y1), (x1 - x0)]) + vec1 = np.array([[(x1 - x0), (y1 - y0)], [(y0 - y1), (x1 - x0)]]) vec2 = np.array( - [(x0 - x1) * x_target + (y1 - y0) * y_target], [(x0 - x1) * y1 + (y1 - y0) * x1] + [[(x0 - x1) * x_target + (y1 - y0) * y_target], [(x0 - x1) * y1 + (y1 - y0) * x1]] ) x2, y2 = np.linalg.solve(vec1, vec2)[0], np.linalg.solve(vec1, vec2)[1] - pose = np.array([x2], [y2], [0]) + pose = np.array([[x2, y2]]) z = dist(x1, y1, x2, y2) - pose_left, pose_right = np.matmul( + + # D E B U G G I N G + # print("Print the dimensions: \n\n\n",np.ndim(vec1), "\t\t\t", np.ndim(vec2), np.ndim( np.array( + # [[cos((theta / 2)*pi/180), -sin((theta / 2)*pi/180)], + # [sin((theta / 2)*pi/180), cos((theta / 2)*pi/180)]] + # )),"\n\n\n") + + pose_left, pose_right = np.dot( np.array( - [cos(theta / 2), -sin(theta / 2), 0], - [sin(theta / 2), cos(theta / 2), 0], - [0, 0, 1], + [[cos((theta / 2)*pi/180), -sin((theta / 2)*pi/180)], + [sin((theta / 2)*pi/180), cos((theta / 2)*pi/180)]] ), - pose, - ), np.matmul( + pose + ), np.dot( np.array( - [cos(-theta / 2), -sin(-theta / 2), 0], - [sin(-theta / 2), cos(-theta / 2), 0], - [0, 0, 1], + [[cos(-(theta / 2)*pi/180), -sin(-(theta / 2)*pi/180)], + [sin(-(theta / 2)*pi/180), cos(-(theta / 2)*pi/180)]] ), - pose, + pose ) xt, yt = dist(pose_left[0], pose_left[1], x2, y2), dist( @@ -76,17 +81,17 @@ def reward(x0, y0, x1, y1, x_target, y_target): ) reward_value = xt * yt * (rov - z) * z - - return reward_value + print(reward_value) + return np.real(reward_value) def maximize_reward(list, x0, y0, x1, y1): max_reward = 0 - max_reward_coordinates = np.array(list[0][0], list[0][1]) + max_reward_coordinates = np.array([list[0][0], list[0][1]]) for i in range(len(list)): if max_reward < reward(x0, y0, x1, y1, list[i][0], list[i][1]): max_reward = max(reward(x0, y0, x1, y1, list[i][0], list[i][1]), max_reward) - max_reward_coordinates = np.array(list[i][0], list[i][1]) + max_reward_coordinates = np.array([list[i][0], list[i][1]]) return max_reward_coordinates diff --git a/turtlebots/src/target_pose.py b/turtlebots/src/target_pose.py index ae77fd8..f7c04ee 100644 --- a/turtlebots/src/target_pose.py +++ b/turtlebots/src/target_pose.py @@ -79,7 +79,7 @@ def catcher(): rospy.Subscriber("/odom", Odometry, odo_sub) rospy.Subscriber("/odom_tb1", Odometry, odo_sub_ugv) rospy.Subscriber("/odom_tb2", Odometry, odo_sub_uav) - + print(x_previous, y_previous) rospy.Subscriber("/odom", Odometry, vel_sub) pose_one = [x_previous, y_previous] From a7232a86bc521370759e055bf2bd21969813e5b2 Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Thu, 22 Jun 2023 15:03:22 +0530 Subject: [PATCH 11/17] [ Debugging ] : Counters addition --- turtlebots/src/target_pose.py | 32 ++++++++++++++++++++++---------- 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/turtlebots/src/target_pose.py b/turtlebots/src/target_pose.py index f7c04ee..4d52b02 100644 --- a/turtlebots/src/target_pose.py +++ b/turtlebots/src/target_pose.py @@ -55,33 +55,42 @@ x_uav_prev, y_uav_prev = 3, 0 x_uav, y_uav = 3, 1 x_ugv, y_ugv = 3,-1 +odo_tb1_count, odo_tb2_count, odo_tb3_count, vel_target_count = 0, 0, 0, 0 def odo_sub_uav(odom_data): + global x_uav, y_uav, odo_tb1_count x_uav = odom_data.pose.pose.position.x y_uav = odom_data.pose.pose.position.y + odo_tb1_count = odo_tb1_count + 1 + print(" odo count ", odo_tb2_count) def odo_sub_ugv(odom_data): + global x_ugv, y_ugv, odo_tb2_count x_ugv = odom_data.pose.pose.position.x y_ugv = odom_data.pose.pose.position.y + odo_tb2_count = odo_tb2_count + 1 def odo_sub(odom_data): + global x_previous, y_previous, odo_tb3_count x_previous = odom_data.pose.pose.position.x y_previous = odom_data.pose.pose.position.y + odo_tb3_count = odo_tb3_count + 1 def vel_sub(odom_data): - vel_target = odom_data.twist.twist.linear.x + global vel_target, vel_target_count + vel_target = odom_data.twist.twist.linear.x + vel_target_count = vel_target_count + 1 def catcher(): + global x_uav, y_uav, x_ugv, y_ugv, x_previous, y_previous, x_uav_prev, y_uav_prev, vel_target, odo_tb1_count, odo_tb2_count, odo_tb3_count, vel_target_count rospy.init_node('Path_renderer', anonymous=False) - global x_uav, y_uav, x_ugv, y_ugv, x_previous, y_previous, x_uav_prev, y_uav_prev, vel_target - while not rospy.is_shutdown(): - # creating subscribers and publishers for 3 turtlebots - rospy.Subscriber("/odom", Odometry, odo_sub) - rospy.Subscriber("/odom_tb1", Odometry, odo_sub_ugv) - rospy.Subscriber("/odom_tb2", Odometry, odo_sub_uav) - print(x_previous, y_previous) - rospy.Subscriber("/odom", Odometry, vel_sub) - + # creating subscribers and publishers for 3 turtlebots + rospy.Subscriber("/odom", Odometry, odo_sub) + rospy.Subscriber("/odom_tb1", Odometry, odo_sub_ugv) + rospy.Subscriber("/odom_tb2", Odometry, odo_sub_uav) + rospy.Subscriber("/odom", Odometry, vel_sub) + if (odo_tb1_count > 1) and (odo_tb2_count > 1) and (odo_tb3_count > 1): + print("This is triggered") pose_one = [x_previous, y_previous] pose_pred = prediction.predictor_polynomial( @@ -105,6 +114,9 @@ def catcher(): print("Path of UAV until target is caught: \n", path_uav, "\n") print("Path of UGV until target is caught: \n", path_ugv, "\n") rospy.signal_shutdown("Target was caught successfully\n") + else: + print("Condition not yet triggered\n") + rospy.spin() if __name__ == "__main__": try: From 085a05c46779a70f3dc6cc634994fdd37bdb6926 Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Sun, 25 Jun 2023 12:50:24 +0530 Subject: [PATCH 12/17] [Debugging] : Using while not rospy.is_shutdown --- turtlebots/src/candidates.py | 0 turtlebots/src/cost_ugv.py | 0 turtlebots/src/prediction.py | 4 +- turtlebots/src/reward_uav.py | 14 +++--- turtlebots/src/target_pose.py | 90 +++++++++++++++++++---------------- 5 files changed, 59 insertions(+), 49 deletions(-) mode change 100644 => 100755 turtlebots/src/candidates.py mode change 100644 => 100755 turtlebots/src/cost_ugv.py mode change 100644 => 100755 turtlebots/src/prediction.py mode change 100644 => 100755 turtlebots/src/reward_uav.py mode change 100644 => 100755 turtlebots/src/target_pose.py diff --git a/turtlebots/src/candidates.py b/turtlebots/src/candidates.py old mode 100644 new mode 100755 diff --git a/turtlebots/src/cost_ugv.py b/turtlebots/src/cost_ugv.py old mode 100644 new mode 100755 diff --git a/turtlebots/src/prediction.py b/turtlebots/src/prediction.py old mode 100644 new mode 100755 index 9517013..f84cd52 --- a/turtlebots/src/prediction.py +++ b/turtlebots/src/prediction.py @@ -36,8 +36,8 @@ def predictor_polynomial(x0, y0, x1, y1): global x2, y2 - x2 = ((x0 + x1 + 1) ^ 2) / 2 - y2 = ((y0 + y1 + 1) ^ 2) / 2 + x2 = ((x0 + x1 + 1) ** 2) / 2 + y2 = ((y0 + y1 + 1) ** 2) / 2 pose_est = np.array([x2, y2]) return pose_est diff --git a/turtlebots/src/reward_uav.py b/turtlebots/src/reward_uav.py old mode 100644 new mode 100755 index c1e4e77..b617901 --- a/turtlebots/src/reward_uav.py +++ b/turtlebots/src/reward_uav.py @@ -49,12 +49,12 @@ def dist(x1, y1, x2, y2): def reward(x0, y0, x1, y1, x_target, y_target): vec1 = np.array([[(x1 - x0), (y1 - y0)], [(y0 - y1), (x1 - x0)]]) vec2 = np.array( - [[(x0 - x1) * x_target + (y1 - y0) * y_target], [(x0 - x1) * y1 + (y1 - y0) * x1]] + [(x0 - x1) * x_target + (y1 - y0) * y_target, (x0 - x1) * y1 + (y1 - y0) * x1 ] ) - x2, y2 = np.linalg.solve(vec1, vec2)[0], np.linalg.solve(vec1, vec2)[1] - pose = np.array([[x2, y2]]) - - z = dist(x1, y1, x2, y2) + print(vec1, "\n", vec2, "\n") + pose = np.linalg.solve(vec1, vec2) + print(pose) + z = dist(x1, y1, pose[0], pose[1]) # D E B U G G I N G # print("Print the dimensions: \n\n\n",np.ndim(vec1), "\t\t\t", np.ndim(vec2), np.ndim( np.array( @@ -76,8 +76,8 @@ def reward(x0, y0, x1, y1, x_target, y_target): pose ) - xt, yt = dist(pose_left[0], pose_left[1], x2, y2), dist( - pose_right[0], pose_right[1], x2, y2 + xt, yt = dist(pose_left[0], pose_left[1], pose[0], pose[1]), dist( + pose_right[0], pose_right[1], pose[0], pose[1] ) reward_value = xt * yt * (rov - z) * z diff --git a/turtlebots/src/target_pose.py b/turtlebots/src/target_pose.py old mode 100644 new mode 100755 index 4d52b02..5bb256c --- a/turtlebots/src/target_pose.py +++ b/turtlebots/src/target_pose.py @@ -59,67 +59,77 @@ def odo_sub_uav(odom_data): global x_uav, y_uav, odo_tb1_count + global vel_target, vel_target_count + # odo_tb1_count, x_uav, y_uav = 0, 3, 1 x_uav = odom_data.pose.pose.position.x y_uav = odom_data.pose.pose.position.y odo_tb1_count = odo_tb1_count + 1 - print(" odo count ", odo_tb2_count) + vel_target = odom_data.twist.twist.linear.x + vel_target_count = vel_target_count + 1 + rospy.loginfo(" odo count "+ str(odo_tb2_count)) def odo_sub_ugv(odom_data): global x_ugv, y_ugv, odo_tb2_count + # odo_tb2_count, x_ugv, y_ugv = 0, 3, -1 x_ugv = odom_data.pose.pose.position.x y_ugv = odom_data.pose.pose.position.y odo_tb2_count = odo_tb2_count + 1 def odo_sub(odom_data): global x_previous, y_previous, odo_tb3_count + # x_previous , y_previous , odo_tb3_count = 0 , 0 , 0 x_previous = odom_data.pose.pose.position.x y_previous = odom_data.pose.pose.position.y odo_tb3_count = odo_tb3_count + 1 -def vel_sub(odom_data): - global vel_target, vel_target_count - vel_target = odom_data.twist.twist.linear.x - vel_target_count = vel_target_count + 1 +rospy.loginfo("odo counts " + str(odo_tb1_count)+ " "+str(odo_tb2_count)+" "+ str(odo_tb3_count)+"\n") def catcher(): - global x_uav, y_uav, x_ugv, y_ugv, x_previous, y_previous, x_uav_prev, y_uav_prev, vel_target, odo_tb1_count, odo_tb2_count, odo_tb3_count, vel_target_count + + global x_previous, y_previous, x_uav, y_uav, x_ugv, y_ugv, odo_tb1_count, odo_tb2_count, odo_tb3_count, x_uav_prev, y_uav_prev + rospy.init_node('Path_renderer', anonymous=False) - # creating subscribers and publishers for 3 turtlebots - rospy.Subscriber("/odom", Odometry, odo_sub) - rospy.Subscriber("/odom_tb1", Odometry, odo_sub_ugv) - rospy.Subscriber("/odom_tb2", Odometry, odo_sub_uav) - rospy.Subscriber("/odom", Odometry, vel_sub) - if (odo_tb1_count > 1) and (odo_tb2_count > 1) and (odo_tb3_count > 1): - print("This is triggered") - pose_one = [x_previous, y_previous] - - pose_pred = prediction.predictor_polynomial( - pose_one[0], pose_one[1], pose_one[0] + vel_target, pose_one[1] + vel_target - ) - - candidates_locations = candidates.candidates(pose_pred[0], pose_pred[1]) - print("Candidate Locations : \n", candidates_locations, "\n") - reward_coordinates = reward_uav.maximize_reward(candidates_locations, x_uav_prev, y_uav_prev, x_uav, y_uav) - cost_coordinates = cost_ugv.minimize_cost(candidates_locations, x_ugv, y_ugv) - - # publish the next pose of the UAV's and UGV's until UGV is less than 1m away from the target - new_x_uav, new_y_uav = reward_coordinates[0], reward_coordinates[1] - new_x_ugv, new_y_ugv = cost_coordinates[0], cost_coordinates[1] - path_uav.append([new_x_uav, new_y_uav]) - path_ugv.append([new_x_ugv, new_x_ugv]) - - x_uav_prev = x_uav - y_uav_prev = y_uav - if reward_uav.dist(new_x_ugv, new_y_ugv, pose_pred[0], pose_pred[1]) <= 1: - print("Path of UAV until target is caught: \n", path_uav, "\n") - print("Path of UGV until target is caught: \n", path_ugv, "\n") - rospy.signal_shutdown("Target was caught successfully\n") - else: - print("Condition not yet triggered\n") - rospy.spin() + + r = rospy.Rate(100); + + while not rospy.is_shutdown(): + # creating subscribers and publishers for 3 turtlebots + rospy.Subscriber("/odom", Odometry, odo_sub) + rospy.Subscriber("/odom_tb1", Odometry, odo_sub_ugv) + rospy.Subscriber("/odom_tb2", Odometry, odo_sub_uav) + + # At least we have some idea about the position of the bots + if (odo_tb1_count > 2 and odo_tb2_count > 2 and odo_tb3_count>2): + print("Odo Counts : ", odo_tb1_count, " ", odo_tb2_count, " ", odo_tb3_count, " \n") + rospy.loginfo("This is triggered\n") + pose_one = [x_previous, y_previous] + + pose_pred = prediction.predictor_polynomial( + pose_one[0], pose_one[1], pose_one[0] + vel_target, pose_one[1] + vel_target + ) + + candidates_locations = candidates.candidates(pose_pred[0], pose_pred[1]) + rospy.loginfo("Candidate Locations : \n" + str(candidates_locations)+ "\n") + reward_coordinates = reward_uav.maximize_reward(candidates_locations, x_uav_prev, y_uav_prev, x_uav, y_uav) + cost_coordinates = cost_ugv.minimize_cost(candidates_locations, x_ugv, y_ugv) + + # publish the next pose of the UAV's and UGV's until UGV is less than 1m away from the target + new_x_uav, new_y_uav = reward_coordinates[0], reward_coordinates[1] + new_x_ugv, new_y_ugv = cost_coordinates[0], cost_coordinates[1] + path_uav.append([new_x_uav, new_y_uav]) + path_ugv.append([new_x_ugv, new_x_ugv]) + + x_uav_prev = x_uav + y_uav_prev = y_uav + + if reward_uav.dist(new_x_ugv, new_y_ugv, pose_pred[0], pose_pred[1]) <= 1: + rospy.loginfo("Path of UAV until target is caught: \n"+ str(path_uav)+ "\n") + rospy.loginfo("Path of UGV until target is caught: \n"+ str(path_ugv)+ "\n") + rospy.signal_shutdown("Target was caught successfully\n") + r.sleep() if __name__ == "__main__": try: catcher() except rospy.ROSInterruptException: - pass \ No newline at end of file + pass \ No newline at end of file From 64e031ee12b698bc9fc12b22ff1456f792c70d29 Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Wed, 28 Jun 2023 15:26:33 +0530 Subject: [PATCH 13/17] [Debugging] : Minimum cost coordinates not yet reflecting --- turtlebots/launch/tracking.launch | 2 +- turtlebots/src/cost_ugv.py | 5 +- turtlebots/src/reward_uav.py | 30 ++++++----- turtlebots/src/target_pose.py | 86 +++++++++++++++++++++---------- 4 files changed, 82 insertions(+), 41 deletions(-) diff --git a/turtlebots/launch/tracking.launch b/turtlebots/launch/tracking.launch index 5aac16f..134f2a1 100644 --- a/turtlebots/launch/tracking.launch +++ b/turtlebots/launch/tracking.launch @@ -1,7 +1,7 @@ - + diff --git a/turtlebots/src/cost_ugv.py b/turtlebots/src/cost_ugv.py index ad718fc..19ea599 100755 --- a/turtlebots/src/cost_ugv.py +++ b/turtlebots/src/cost_ugv.py @@ -50,7 +50,8 @@ def minimize_cost(list, x1, y1): for i in range(len(list)): if min_cost > dist(x1, y1, list[i][0], list[i][1]): + print("Minimizing the cost\n") min_cost = min(dist(x1, y1, list[i][0], list[i][1]), min_cost) - min_cost_coordinates = np.array([list[i][0], list[i][1]]) - + min_cost_coordinates = [list[i][0], list[i][1]] + print("min_cost:\t", min_cost, "\n") return min_cost_coordinates diff --git a/turtlebots/src/reward_uav.py b/turtlebots/src/reward_uav.py index b617901..af44e79 100755 --- a/turtlebots/src/reward_uav.py +++ b/turtlebots/src/reward_uav.py @@ -35,6 +35,7 @@ def dist(x1, y1, x2, y2): + print("Dist:\n", np.real(sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)), "\n") return np.real(sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)) @@ -49,11 +50,11 @@ def dist(x1, y1, x2, y2): def reward(x0, y0, x1, y1, x_target, y_target): vec1 = np.array([[(x1 - x0), (y1 - y0)], [(y0 - y1), (x1 - x0)]]) vec2 = np.array( - [(x0 - x1) * x_target + (y1 - y0) * y_target, (x0 - x1) * y1 + (y1 - y0) * x1 ] + [(x0 - x1) * x_target + (y1 - y0) * y_target, (x0 - x1) * y1 + (y1 - y0) * x1] ) - print(vec1, "\n", vec2, "\n") + print("Vec1:\n", vec1, "\n", "Vec2:\n", vec2, "\n") pose = np.linalg.solve(vec1, vec2) - print(pose) + # print(pose) z = dist(x1, y1, pose[0], pose[1]) # D E B U G G I N G @@ -61,19 +62,23 @@ def reward(x0, y0, x1, y1, x_target, y_target): # [[cos((theta / 2)*pi/180), -sin((theta / 2)*pi/180)], # [sin((theta / 2)*pi/180), cos((theta / 2)*pi/180)]] # )),"\n\n\n") - + pose_left, pose_right = np.dot( np.array( - [[cos((theta / 2)*pi/180), -sin((theta / 2)*pi/180)], - [sin((theta / 2)*pi/180), cos((theta / 2)*pi/180)]] + [ + [cos((theta / 2) * pi / 180), -sin((theta / 2) * pi / 180)], + [sin((theta / 2) * pi / 180), cos((theta / 2) * pi / 180)], + ] ), - pose + pose, ), np.dot( np.array( - [[cos(-(theta / 2)*pi/180), -sin(-(theta / 2)*pi/180)], - [sin(-(theta / 2)*pi/180), cos(-(theta / 2)*pi/180)]] + [ + [cos(-(theta / 2) * pi / 180), -sin(-(theta / 2) * pi / 180)], + [sin(-(theta / 2) * pi / 180), cos(-(theta / 2) * pi / 180)], + ] ), - pose + pose, ) xt, yt = dist(pose_left[0], pose_left[1], pose[0], pose[1]), dist( @@ -81,7 +86,7 @@ def reward(x0, y0, x1, y1, x_target, y_target): ) reward_value = xt * yt * (rov - z) * z - print(reward_value) + print("Reward Value: ", reward_value) return np.real(reward_value) @@ -91,7 +96,8 @@ def maximize_reward(list, x0, y0, x1, y1): for i in range(len(list)): if max_reward < reward(x0, y0, x1, y1, list[i][0], list[i][1]): + # print("Reward is maximized\n") max_reward = max(reward(x0, y0, x1, y1, list[i][0], list[i][1]), max_reward) max_reward_coordinates = np.array([list[i][0], list[i][1]]) - + print("Maximum Reward:\t", max_reward, "\n") return max_reward_coordinates diff --git a/turtlebots/src/target_pose.py b/turtlebots/src/target_pose.py index 5bb256c..b828f9d 100755 --- a/turtlebots/src/target_pose.py +++ b/turtlebots/src/target_pose.py @@ -45,18 +45,19 @@ Then we need to go for the next point that is Update step and do the previous step for the same Above procedure is repeated until distance between UGV and target is <= 1 """ -x_previous, y_previous = 0, 0 +x_previous, y_previous = 0, -1 # velocity of target = 1m/s ... Initially i.e. just assumed # This velocity should be observed by the UAV vel_target = 1 path_uav = [[3, 1]] -path_ugv = [[3,-1]] +path_ugv = [[3, -1]] x_uav_prev, y_uav_prev = 3, 0 x_uav, y_uav = 3, 1 -x_ugv, y_ugv = 3,-1 +x_ugv, y_ugv = 3, -1 odo_tb1_count, odo_tb2_count, odo_tb3_count, vel_target_count = 0, 0, 0, 0 + def odo_sub_uav(odom_data): global x_uav, y_uav, odo_tb1_count global vel_target, vel_target_count @@ -64,9 +65,10 @@ def odo_sub_uav(odom_data): x_uav = odom_data.pose.pose.position.x y_uav = odom_data.pose.pose.position.y odo_tb1_count = odo_tb1_count + 1 - vel_target = odom_data.twist.twist.linear.x + vel_target = odom_data.twist.twist.linear.x vel_target_count = vel_target_count + 1 - rospy.loginfo(" odo count "+ str(odo_tb2_count)) + rospy.loginfo(" odo count " + str(odo_tb2_count)) + def odo_sub_ugv(odom_data): global x_ugv, y_ugv, odo_tb2_count @@ -75,61 +77,93 @@ def odo_sub_ugv(odom_data): y_ugv = odom_data.pose.pose.position.y odo_tb2_count = odo_tb2_count + 1 + def odo_sub(odom_data): global x_previous, y_previous, odo_tb3_count - # x_previous , y_previous , odo_tb3_count = 0 , 0 , 0 + # x_previous , y_previous , odo_tb3_count = 0 , -1 , 0 x_previous = odom_data.pose.pose.position.x y_previous = odom_data.pose.pose.position.y odo_tb3_count = odo_tb3_count + 1 -rospy.loginfo("odo counts " + str(odo_tb1_count)+ " "+str(odo_tb2_count)+" "+ str(odo_tb3_count)+"\n") + +rospy.loginfo( + "odo counts " + + str(odo_tb1_count) + + " " + + str(odo_tb2_count) + + " " + + str(odo_tb3_count) + + "\n" +) + def catcher(): - global x_previous, y_previous, x_uav, y_uav, x_ugv, y_ugv, odo_tb1_count, odo_tb2_count, odo_tb3_count, x_uav_prev, y_uav_prev - rospy.init_node('Path_renderer', anonymous=False) + rospy.init_node("Path_renderer", anonymous=False) - r = rospy.Rate(100); + r = rospy.Rate(100) while not rospy.is_shutdown(): # creating subscribers and publishers for 3 turtlebots rospy.Subscriber("/odom", Odometry, odo_sub) rospy.Subscriber("/odom_tb1", Odometry, odo_sub_ugv) rospy.Subscriber("/odom_tb2", Odometry, odo_sub_uav) - + # At least we have some idea about the position of the bots - if (odo_tb1_count > 2 and odo_tb2_count > 2 and odo_tb3_count>2): - print("Odo Counts : ", odo_tb1_count, " ", odo_tb2_count, " ", odo_tb3_count, " \n") + if odo_tb1_count > 2 and odo_tb2_count > 2 and odo_tb3_count > 2: + print( + "Odo Counts : ", + odo_tb1_count, + " ", + odo_tb2_count, + " ", + odo_tb3_count, + " \n", + ) rospy.loginfo("This is triggered\n") pose_one = [x_previous, y_previous] pose_pred = prediction.predictor_polynomial( - pose_one[0], pose_one[1], pose_one[0] + vel_target, pose_one[1] + vel_target + pose_one[0], + pose_one[1], + pose_one[0] + vel_target, + pose_one[1] + vel_target, ) candidates_locations = candidates.candidates(pose_pred[0], pose_pred[1]) - rospy.loginfo("Candidate Locations : \n" + str(candidates_locations)+ "\n") - reward_coordinates = reward_uav.maximize_reward(candidates_locations, x_uav_prev, y_uav_prev, x_uav, y_uav) - cost_coordinates = cost_ugv.minimize_cost(candidates_locations, x_ugv, y_ugv) - + rospy.loginfo("Candidate Locations : \n" + str(candidates_locations) + "\n") + reward_coordinates = reward_uav.maximize_reward( + candidates_locations, x_uav_prev, y_uav_prev, x_uav, y_uav + ) + print("Here are reward Coordinates:\n", reward_coordinates, "\n") + cost_coordinates = cost_ugv.minimize_cost( + candidates_locations, x_ugv, y_ugv + ) + print("Here are cost coordinates:\n", cost_coordinates, "\n") # publish the next pose of the UAV's and UGV's until UGV is less than 1m away from the target - new_x_uav, new_y_uav = reward_coordinates[0], reward_coordinates[1] - new_x_ugv, new_y_ugv = cost_coordinates[0], cost_coordinates[1] - path_uav.append([new_x_uav, new_y_uav]) - path_ugv.append([new_x_ugv, new_x_ugv]) x_uav_prev = x_uav y_uav_prev = y_uav - if reward_uav.dist(new_x_ugv, new_y_ugv, pose_pred[0], pose_pred[1]) <= 1: - rospy.loginfo("Path of UAV until target is caught: \n"+ str(path_uav)+ "\n") - rospy.loginfo("Path of UGV until target is caught: \n"+ str(path_ugv)+ "\n") + x_uav, y_uav = reward_coordinates[0], reward_coordinates[1] + x_ugv, y_ugv = cost_coordinates[0], cost_coordinates[1] + path_uav.append([x_uav, y_uav]) + path_ugv.append([x_ugv, x_ugv]) + + if reward_uav.dist(x_ugv, y_ugv, pose_pred[0], pose_pred[1]) <= 1: + rospy.loginfo( + "Path of UAV until target is caught: \n" + str(path_uav) + "\n" + ) + rospy.loginfo( + "Path of UGV until target is caught: \n" + str(path_ugv) + "\n" + ) rospy.signal_shutdown("Target was caught successfully\n") r.sleep() + if __name__ == "__main__": try: catcher() except rospy.ROSInterruptException: - pass \ No newline at end of file + pass From efb9dc70357ba6b7b17fdb443e1fbfac86219ec1 Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Wed, 28 Jun 2023 21:37:05 +0530 Subject: [PATCH 14/17] [Initial release with bug] : Debugging for normal cases done --- turtlebots/src/candidates.py | 16 +++--- turtlebots/src/cost_ugv.py | 10 ++-- turtlebots/src/reward_uav.py | 20 +++++--- turtlebots/src/target_pose.py | 96 ++++++++++++++++++----------------- 4 files changed, 79 insertions(+), 63 deletions(-) diff --git a/turtlebots/src/candidates.py b/turtlebots/src/candidates.py index ade958b..4eef878 100755 --- a/turtlebots/src/candidates.py +++ b/turtlebots/src/candidates.py @@ -37,8 +37,6 @@ def candidates(x_current, y_current): - candidate_locations = np.empty((2, 10000)) - candidate_locations = [[x_current, y_current + 1]] candidate_locations.append([x_current, y_current - 1]) candidate_locations.append([x_current + 1, y_current]) @@ -46,14 +44,18 @@ def candidates(x_current, y_current): for i in range(0, 360, 60): candidate_locations.append( - [(time_horizon * cos(i * pi / 180)) + x_current, - (time_horizon * sin(i * pi / 180)) + y_current] + [ + (time_horizon * cos(i * pi / 180)) + x_current, + (time_horizon * sin(i * pi / 180)) + y_current, + ] ) for i in range(0, 360, 45): - candidate_locations.append([ - (2 * time_horizon * cos(i * pi / 180)) + x_current, - (2 * time_horizon * sin(i * pi / 180)) + y_current] + candidate_locations.append( + [ + (2 * time_horizon * cos(i * pi / 180)) + x_current, + (2 * time_horizon * sin(i * pi / 180)) + y_current, + ] ) return candidate_locations diff --git a/turtlebots/src/cost_ugv.py b/turtlebots/src/cost_ugv.py index 19ea599..a3bb089 100755 --- a/turtlebots/src/cost_ugv.py +++ b/turtlebots/src/cost_ugv.py @@ -26,6 +26,7 @@ from cmath import sqrt, cos, sin import numpy as np +from candidates import candidates # x1 and y1 here are the current positons of the UGV x1, y1 = 0, 0 @@ -44,14 +45,17 @@ def dist(x1, y1, x2, y2): return np.real(sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)) -def minimize_cost(list, x1, y1): +def minimize_cost(x1, y1, x_ugv, y_ugv): min_cost = np.inf + + list = candidates(x_ugv, y_ugv) min_cost_coordinates = np.array([list[0][0], list[0][1]]) for i in range(len(list)): if min_cost > dist(x1, y1, list[i][0], list[i][1]): - print("Minimizing the cost\n") + # print("Minimizing the cost\n") min_cost = min(dist(x1, y1, list[i][0], list[i][1]), min_cost) min_cost_coordinates = [list[i][0], list[i][1]] - print("min_cost:\t", min_cost, "\n") + # print("coord", min_cost_coordinates) + # print("min_cost:\t", min_cost, "\n") return min_cost_coordinates diff --git a/turtlebots/src/reward_uav.py b/turtlebots/src/reward_uav.py index af44e79..b782ed2 100755 --- a/turtlebots/src/reward_uav.py +++ b/turtlebots/src/reward_uav.py @@ -26,6 +26,7 @@ # candidate states provided from cmath import sqrt, cos, sin, pi +from candidates import candidates import numpy as np # Angle of vision in degrees @@ -35,7 +36,7 @@ def dist(x1, y1, x2, y2): - print("Dist:\n", np.real(sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)), "\n") + # print("Dist:\n", np.real(sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)), "\n") return np.real(sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)) @@ -52,7 +53,7 @@ def reward(x0, y0, x1, y1, x_target, y_target): vec2 = np.array( [(x0 - x1) * x_target + (y1 - y0) * y_target, (x0 - x1) * y1 + (y1 - y0) * x1] ) - print("Vec1:\n", vec1, "\n", "Vec2:\n", vec2, "\n") + # print("Vec1:\n", vec1, "\n", "Vec2:\n", vec2, "\n") pose = np.linalg.solve(vec1, vec2) # print(pose) z = dist(x1, y1, pose[0], pose[1]) @@ -86,18 +87,23 @@ def reward(x0, y0, x1, y1, x_target, y_target): ) reward_value = xt * yt * (rov - z) * z - print("Reward Value: ", reward_value) + # print("Reward Value: ", reward_value) return np.real(reward_value) -def maximize_reward(list, x0, y0, x1, y1): +def maximize_reward(pose_one, pose_two, x0, y0, x1, y1): + # print(pose_one, pose_two, x0, y0, x1, y1, "\n") max_reward = 0 + + list = candidates(x1, y1) max_reward_coordinates = np.array([list[0][0], list[0][1]]) for i in range(len(list)): - if max_reward < reward(x0, y0, x1, y1, list[i][0], list[i][1]): + if max_reward < reward(x0, y0, list[i][0], list[i][1], pose_one, pose_two): # print("Reward is maximized\n") - max_reward = max(reward(x0, y0, x1, y1, list[i][0], list[i][1]), max_reward) + max_reward = max( + reward(x0, y0, list[i][0], list[i][1], pose_one, pose_two), max_reward + ) max_reward_coordinates = np.array([list[i][0], list[i][1]]) - print("Maximum Reward:\t", max_reward, "\n") + # print("Maximum Reward:\t", max_reward, "\n") return max_reward_coordinates diff --git a/turtlebots/src/target_pose.py b/turtlebots/src/target_pose.py index b828f9d..11e5093 100755 --- a/turtlebots/src/target_pose.py +++ b/turtlebots/src/target_pose.py @@ -67,7 +67,7 @@ def odo_sub_uav(odom_data): odo_tb1_count = odo_tb1_count + 1 vel_target = odom_data.twist.twist.linear.x vel_target_count = vel_target_count + 1 - rospy.loginfo(" odo count " + str(odo_tb2_count)) + # rospy.loginfo(" odo count " + str(odo_tb2_count)) def odo_sub_ugv(odom_data): @@ -86,19 +86,21 @@ def odo_sub(odom_data): odo_tb3_count = odo_tb3_count + 1 -rospy.loginfo( - "odo counts " - + str(odo_tb1_count) - + " " - + str(odo_tb2_count) - + " " - + str(odo_tb3_count) - + "\n" -) +# rospy.loginfo( +# "odo counts " +# + str(odo_tb1_count) +# + " " +# + str(odo_tb2_count) +# + " " +# + str(odo_tb3_count) +# + "\n" +# ) + +count = 0 def catcher(): - global x_previous, y_previous, x_uav, y_uav, x_ugv, y_ugv, odo_tb1_count, odo_tb2_count, odo_tb3_count, x_uav_prev, y_uav_prev + global x_previous, y_previous, x_uav, y_uav, x_ugv, y_ugv, odo_tb1_count, odo_tb2_count, odo_tb3_count, x_uav_prev, y_uav_prev, path_uav, path_ugv rospy.init_node("Path_renderer", anonymous=False) @@ -112,16 +114,16 @@ def catcher(): # At least we have some idea about the position of the bots if odo_tb1_count > 2 and odo_tb2_count > 2 and odo_tb3_count > 2: - print( - "Odo Counts : ", - odo_tb1_count, - " ", - odo_tb2_count, - " ", - odo_tb3_count, - " \n", - ) - rospy.loginfo("This is triggered\n") + # print( + # "Odo Counts : ", + # odo_tb1_count, + # " ", + # odo_tb2_count, + # " ", + # odo_tb3_count, + # " \n", + # ) + # rospy.loginfo("This is triggered\n") pose_one = [x_previous, y_previous] pose_pred = prediction.predictor_polynomial( @@ -131,34 +133,36 @@ def catcher(): pose_one[1] + vel_target, ) - candidates_locations = candidates.candidates(pose_pred[0], pose_pred[1]) - rospy.loginfo("Candidate Locations : \n" + str(candidates_locations) + "\n") - reward_coordinates = reward_uav.maximize_reward( - candidates_locations, x_uav_prev, y_uav_prev, x_uav, y_uav - ) - print("Here are reward Coordinates:\n", reward_coordinates, "\n") - cost_coordinates = cost_ugv.minimize_cost( - candidates_locations, x_ugv, y_ugv - ) - print("Here are cost coordinates:\n", cost_coordinates, "\n") - # publish the next pose of the UAV's and UGV's until UGV is less than 1m away from the target + while reward_uav.dist(x_ugv, y_ugv, pose_pred[0], pose_pred[1]) > 1: + reward_coordinates = reward_uav.maximize_reward( + pose_pred[0], pose_pred[1], x_uav_prev, y_uav_prev, x_uav, y_uav + ) + # print("Here are reward Coordinates:\n", reward_coordinates, "\n") + cost_coordinates = cost_ugv.minimize_cost( + pose_pred[0], pose_pred[1], x_ugv, y_ugv + ) + # print("Here are cost coordinates:\n", cost_coordinates, "\n") + # publish the next pose of the UAV's and UGV's until UGV is less than 1m away from the target - x_uav_prev = x_uav - y_uav_prev = y_uav + x_uav_prev = x_uav + y_uav_prev = y_uav - x_uav, y_uav = reward_coordinates[0], reward_coordinates[1] - x_ugv, y_ugv = cost_coordinates[0], cost_coordinates[1] - path_uav.append([x_uav, y_uav]) - path_ugv.append([x_ugv, x_ugv]) + x_uav, y_uav = reward_coordinates[0], reward_coordinates[1] + x_ugv, y_ugv = cost_coordinates[0], cost_coordinates[1] + + path_uav.append([x_uav, y_uav]) + path_ugv.append([x_ugv, x_ugv]) + + # print(path_uav, "\n", path_ugv, "\n") + + rospy.loginfo( + "Path of UAV until target is caught: \n" + str(path_uav) + "\n" + ) + rospy.loginfo( + "Path of UGV until target is caught: \n" + str(path_ugv) + "\n" + ) + rospy.signal_shutdown("Target was caught successfully\n") - if reward_uav.dist(x_ugv, y_ugv, pose_pred[0], pose_pred[1]) <= 1: - rospy.loginfo( - "Path of UAV until target is caught: \n" + str(path_uav) + "\n" - ) - rospy.loginfo( - "Path of UGV until target is caught: \n" + str(path_ugv) + "\n" - ) - rospy.signal_shutdown("Target was caught successfully\n") r.sleep() From 3b270c4def9d581bb5a9e74fa03ecfe06f66d5e1 Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Wed, 28 Jun 2023 22:35:36 +0530 Subject: [PATCH 15/17] [ Finally done ] --- turtlebots/src/cost_ugv.py | 1 + turtlebots/src/reward_uav.py | 4 +++- turtlebots/src/target_pose.py | 8 ++++---- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/turtlebots/src/cost_ugv.py b/turtlebots/src/cost_ugv.py index a3bb089..427c381 100755 --- a/turtlebots/src/cost_ugv.py +++ b/turtlebots/src/cost_ugv.py @@ -56,6 +56,7 @@ def minimize_cost(x1, y1, x_ugv, y_ugv): # print("Minimizing the cost\n") min_cost = min(dist(x1, y1, list[i][0], list[i][1]), min_cost) min_cost_coordinates = [list[i][0], list[i][1]] + print(min_cost_coordinates, "\n") # print("coord", min_cost_coordinates) # print("min_cost:\t", min_cost, "\n") return min_cost_coordinates diff --git a/turtlebots/src/reward_uav.py b/turtlebots/src/reward_uav.py index b782ed2..1b0f09b 100755 --- a/turtlebots/src/reward_uav.py +++ b/turtlebots/src/reward_uav.py @@ -49,6 +49,8 @@ def dist(x1, y1, x2, y2): def reward(x0, y0, x1, y1, x_target, y_target): + # print(x0, x1, y0, y1, "\n") + vec1 = np.array([[(x1 - x0), (y1 - y0)], [(y0 - y1), (x1 - x0)]]) vec2 = np.array( [(x0 - x1) * x_target + (y1 - y0) * y_target, (x0 - x1) * y1 + (y1 - y0) * x1] @@ -104,6 +106,6 @@ def maximize_reward(pose_one, pose_two, x0, y0, x1, y1): max_reward = max( reward(x0, y0, list[i][0], list[i][1], pose_one, pose_two), max_reward ) - max_reward_coordinates = np.array([list[i][0], list[i][1]]) + max_reward_coordinates = [list[i][0], list[i][1]] # print("Maximum Reward:\t", max_reward, "\n") return max_reward_coordinates diff --git a/turtlebots/src/target_pose.py b/turtlebots/src/target_pose.py index 11e5093..aa8a4e6 100755 --- a/turtlebots/src/target_pose.py +++ b/turtlebots/src/target_pose.py @@ -144,14 +144,14 @@ def catcher(): # print("Here are cost coordinates:\n", cost_coordinates, "\n") # publish the next pose of the UAV's and UGV's until UGV is less than 1m away from the target - x_uav_prev = x_uav - y_uav_prev = y_uav - x_uav, y_uav = reward_coordinates[0], reward_coordinates[1] x_ugv, y_ugv = cost_coordinates[0], cost_coordinates[1] + x_uav_prev = x_uav + y_uav_prev = y_uav + path_uav.append([x_uav, y_uav]) - path_ugv.append([x_ugv, x_ugv]) + path_ugv.append([x_ugv, y_ugv]) # print(path_uav, "\n", path_ugv, "\n") From fc18147401bbe17f88aa4927f1d631954bf6e2bb Mon Sep 17 00:00:00 2001 From: Alqama Shaikh Date: Thu, 29 Jun 2023 11:39:53 +0530 Subject: [PATCH 16/17] [ Minor Corrections ] : Corrections in the prediction functions --- turtlebots/src/prediction.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/turtlebots/src/prediction.py b/turtlebots/src/prediction.py index f84cd52..97ab486 100755 --- a/turtlebots/src/prediction.py +++ b/turtlebots/src/prediction.py @@ -30,14 +30,15 @@ Inputs : current position of target and a position second before of the target """ import numpy as np +from math import sqrt # This is only about a forward direction def predictor_polynomial(x0, y0, x1, y1): global x2, y2 - x2 = ((x0 + x1 + 1) ** 2) / 2 - y2 = ((y0 + y1 + 1) ** 2) / 2 + x2 = sqrt(((x0 + x1 + 1) / 2) ** 2) * abs(x1) / x1 + y2 = sqrt(((y0 + y1 + 1) / 2) ** 2) * abs(y1) / y1 pose_est = np.array([x2, y2]) return pose_est From 34729b3f2835b79bc08daa9b301501686bac2494 Mon Sep 17 00:00:00 2001 From: aPR0T0 Date: Thu, 31 Aug 2023 11:27:48 +0800 Subject: [PATCH 17/17] [ New Python path ] : Path according to mambaforge env (will not work for anybody) --- turtlebots/, | 0 turtlebots/src/target_pose.py | 3 ++- 2 files changed, 2 insertions(+), 1 deletion(-) create mode 100644 turtlebots/, diff --git a/turtlebots/, b/turtlebots/, new file mode 100644 index 0000000..e69de29 diff --git a/turtlebots/src/target_pose.py b/turtlebots/src/target_pose.py index aa8a4e6..1e68fbf 100755 --- a/turtlebots/src/target_pose.py +++ b/turtlebots/src/target_pose.py @@ -1,3 +1,4 @@ +#! /home/proto/mambaforge/envs/ros_env/bin/python3 """ MIT License @@ -24,7 +25,7 @@ # This will give the path that should be followed based on the shortlisted # reachable sites by the uav -"""k +""" We will get a set of candidate goal pose from the candidates.py and then according to the heuristics for the UAV we will shortlist the reachable set of candidates for just UAV