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 diff --git a/README.md b/README.md new file mode 100644 index 0000000..cc89a46 --- /dev/null +++ b/README.md @@ -0,0 +1,17 @@ +## 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 + +![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/, b/turtlebots/, new file mode 100644 index 0000000..e69de29 diff --git a/turtlebots/CMakeLists.txt b/turtlebots/CMakeLists.txt new file mode 100644 index 0000000..6f70290 --- /dev/null +++ b/turtlebots/CMakeLists.txt @@ -0,0 +1,205 @@ +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 + nav_msgs +) + +## 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..134f2a1 --- /dev/null +++ b/turtlebots/launch/tracking.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/turtlebots/package.xml b/turtlebots/package.xml new file mode 100644 index 0000000..16ae375 --- /dev/null +++ b/turtlebots/package.xml @@ -0,0 +1,65 @@ + + + turtlebots + 0.0.0 + The turtlebots package + + + + + proto + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + rospy + nav_msgs + rospy + nav_msgs + rospy + nav_msgs + + + + + + + + diff --git a/turtlebots/src/candidates.py b/turtlebots/src/candidates.py new file mode 100755 index 0000000..4eef878 --- /dev/null +++ b/turtlebots/src/candidates.py @@ -0,0 +1,61 @@ +""" + 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. +""" +import numpy as np +from cmath import sin, cos, pi +from prediction import predictor_polynomial + +""" +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 +""" + +# i.e. we can predict just the next step through the polynomial predictor +time_horizon = 1 + + +def candidates(x_current, 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, + ] + ) + + 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, + ] + ) + + return candidate_locations diff --git a/turtlebots/src/cost_ugv.py b/turtlebots/src/cost_ugv.py new file mode 100755 index 0000000..427c381 --- /dev/null +++ b/turtlebots/src/cost_ugv.py @@ -0,0 +1,62 @@ +""" + 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 + +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 +# 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 np.real(sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)) + + +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") + 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/prediction.py b/turtlebots/src/prediction.py new file mode 100755 index 0000000..97ab486 --- /dev/null +++ b/turtlebots/src/prediction.py @@ -0,0 +1,55 @@ +""" + 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 +""" +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 = 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 + + +""" +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/turtlebots/src/reward_uav.py b/turtlebots/src/reward_uav.py new file mode 100755 index 0000000..1b0f09b --- /dev/null +++ b/turtlebots/src/reward_uav.py @@ -0,0 +1,111 @@ +""" + 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, pi +from candidates import candidates +import numpy as np + +# Angle of vision in degrees +theta = 60 +# radius of vision is 5m from the UAV +rov = 5 + + +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)) + + +""" +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): + # 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] + ) + # 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]) + + # 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) * pi / 180), -sin((theta / 2) * pi / 180)], + [sin((theta / 2) * pi / 180), cos((theta / 2) * pi / 180)], + ] + ), + 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)], + ] + ), + pose, + ) + + 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 + # print("Reward Value: ", reward_value) + return np.real(reward_value) + + +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, list[i][0], list[i][1], pose_one, pose_two): + # print("Reward is maximized\n") + max_reward = max( + reward(x0, y0, list[i][0], list[i][1], pose_one, pose_two), max_reward + ) + 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 new file mode 100755 index 0000000..1e68fbf --- /dev/null +++ b/turtlebots/src/target_pose.py @@ -0,0 +1,174 @@ +#! /home/proto/mambaforge/envs/ros_env/bin/python3 +""" + 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 +from nav_msgs.msg import Odometry +from std_msgs.msg import Int16MultiArray +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, -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]] +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 + 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 + 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 , -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" +# ) + +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, path_uav, path_ugv + + rospy.init_node("Path_renderer", anonymous=False) + + 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, + ) + + 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, 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, y_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") + + r.sleep() + + +if __name__ == "__main__": + try: + catcher() + except rospy.ROSInterruptException: + pass 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..411389b --- /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_tb1 + wheel_right_joint_tb1 + 0.160 + 0.066 + 1 + 10 + na + + + + + + true + imu_link_tb1 + imu_link_tb1 + imu_tb1 + imu_service_tb1 + 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..4c32468 --- /dev/null +++ b/turtlebots/urdf/turtlebot3_burger.urdf.xacro @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + + + +