diff --git a/.gitignore b/.gitignore index 105bef3..f17846f 100644 --- a/.gitignore +++ b/.gitignore @@ -29,3 +29,5 @@ CATKIN_IGNORE # Coveralls .coverage +.idea/ +coms/coms/test1/* diff --git a/.gitmodules b/.gitmodules index 7a697aa..e69de29 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,4 +0,0 @@ -[submodule "turtlebot3_simulations"] - path = turtlebot3_simulations - url = https://github.com/NESTLab/turtlebot3_simulations.git - branch = noetic-devel diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..9137983 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,76 @@ +{ + "files.associations": { + "array": "cpp", + "string": "cpp", + "string_view": "cpp", + "unordered_map": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "any": "cpp", + "atomic": "cpp", + "hash_map": "cpp", + "hash_set": "cpp", + "strstream": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "codecvt": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "map": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "regex": "cpp", + "set": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "shared_mutex": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "variant": "cpp", + "bit": "cpp" + } +} \ No newline at end of file diff --git a/Dockerfile b/Dockerfile old mode 100644 new mode 100755 index 70b3e20..92457e7 --- a/Dockerfile +++ b/Dockerfile @@ -70,13 +70,20 @@ RUN git clone https://github.com/ilpincy/argos3-kheperaiv.git; \ RUN sudo apt-get update; \ sudo apt-get upgrade -y; \ sudo apt-get install -y \ + apt-utils \ python-is-python3 \ ros-noetic-octomap \ ros-noetic-octomap-msgs \ iproute2 \ ros-noetic-catkin \ python3-catkin-tools \ - libtf2-ros-dev + libtf2-ros-dev \ + ros-noetic-global-planner ; + +RUN pip install gym tensorflow ; + +RUN apt install wget +RUN wget https://raw.githubusercontent.com/NESTLab/turtlebot3_simulations/local_costmap_fix/turtlebot3_gazebo/costmap_common_params_burger_local.yaml -P /opt/ros/noetic/share/turtlebot3_navigation/param/ # Add ROS dependent scripts RUN rosdep init; \ @@ -87,6 +94,11 @@ RUN rosdep init; \ echo "export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib/argos3:/opt/ros/noetic/lib" >> ~/.bashrc; \ echo "export ARGOS_PLUGIN_PATH=$HOME/catkin_ws/src/argos_bridge/ros_lib_links" >> ~/.bashrc; \ echo "export ARGOS_PLUGIN_PATH=$ARGOS_PLUGIN_PATH:$HOME/catkin_ws/devel/lib" >> ~/.bashrc + +COPY . /root/catkin_ws/src + EXPOSE 8080 +CMD ["chmod a+rwx /root -R"] + CMD ["/root/catkin_ws/src/entrypoint.sh"] diff --git a/Metrics/Run2/Distance_vs_Time_For_tb3_0.png b/Metrics/Run2/Distance_vs_Time_For_tb3_0.png new file mode 100644 index 0000000..4e985fb Binary files /dev/null and b/Metrics/Run2/Distance_vs_Time_For_tb3_0.png differ diff --git a/Metrics/Run2/Percent_Explored_vs_Distance_For_tb3_0.png b/Metrics/Run2/Percent_Explored_vs_Distance_For_tb3_0.png new file mode 100644 index 0000000..971e142 Binary files /dev/null and b/Metrics/Run2/Percent_Explored_vs_Distance_For_tb3_0.png differ diff --git a/Metrics/Run2/Percent_Explored_vs_Time_For_tb3_0.png b/Metrics/Run2/Percent_Explored_vs_Time_For_tb3_0.png new file mode 100644 index 0000000..c4c25a3 Binary files /dev/null and b/Metrics/Run2/Percent_Explored_vs_Time_For_tb3_0.png differ diff --git a/Metrics/Run2/Screenshot from 2021-12-11 17-33-07.png b/Metrics/Run2/Screenshot from 2021-12-11 17-33-07.png new file mode 100644 index 0000000..88a6e72 Binary files /dev/null and b/Metrics/Run2/Screenshot from 2021-12-11 17-33-07.png differ diff --git a/Metrics/Run3/Distance_vs_Time_For_tb3_0.png b/Metrics/Run3/Distance_vs_Time_For_tb3_0.png new file mode 100644 index 0000000..5c673fd Binary files /dev/null and b/Metrics/Run3/Distance_vs_Time_For_tb3_0.png differ diff --git a/Metrics/Run3/Percent_Explored_vs_Distance_For_tb3_0.png b/Metrics/Run3/Percent_Explored_vs_Distance_For_tb3_0.png new file mode 100644 index 0000000..a0fd71a Binary files /dev/null and b/Metrics/Run3/Percent_Explored_vs_Distance_For_tb3_0.png differ diff --git a/Metrics/Run3/Percent_Explored_vs_Time_For_tb3_0.png b/Metrics/Run3/Percent_Explored_vs_Time_For_tb3_0.png new file mode 100644 index 0000000..7faeb98 Binary files /dev/null and b/Metrics/Run3/Percent_Explored_vs_Time_For_tb3_0.png differ diff --git a/coms/coms/src/nodes/merge_handler.py b/coms/coms/src/nodes/merge_handler.py index 973222b..8c7e69d 100755 --- a/coms/coms/src/nodes/merge_handler.py +++ b/coms/coms/src/nodes/merge_handler.py @@ -66,7 +66,8 @@ def gmapping_cb(self, msg: OccupancyGrid) -> None: if self.logging: self.bag.write('map', msg) - + self.map_header = msg.header + self.map_info = msg.info # breakpoint() MergeHandler.parse_and_save(msg, self.latest_map) # unpack gmapping @@ -153,10 +154,18 @@ def serve_map(self, _) -> GetMapResponse: def create_occupancy_msg(self, seq: int) -> OccupancyGrid: occ = OccupancyGrid() # header - occ.header.seq = seq - # metadata + # occ.header.seq = seq + # occ.header.frame_id = "tb3_"+self.robot_name[-1]+"/map" + # occ.header.stamp = rospy.Time.now() + + # # metadata + # occ.info.resolution = 0.05000000074505806 + # occ.info.origin.orientation.w = 1.0 + occ.header = self.map_header + occ.info = self.map_info occ.info.height = self.latest_map.shape[0] occ.info.width = self.latest_map.shape[1] + # data occ.data = tuple(numpy_to_ros(self.latest_map.flatten())) return occ diff --git a/coms/coms/test1/dummy_file b/coms/coms/test1/dummy_file new file mode 100644 index 0000000..e69de29 diff --git a/docker-compose-image.yaml b/docker-compose-image.yaml new file mode 100755 index 0000000..ab314ae --- /dev/null +++ b/docker-compose-image.yaml @@ -0,0 +1,21 @@ +version: '2' +services: + novnc: + image: tberg1234/2122_multidroneindoorslam:latest + environment: + # Adjust to your screen size + - DISPLAY_WIDTH=1366 + - DISPLAY_HEIGHT=768 + - RUN_XTERM=no + cap_add: + - NET_ADMIN + devices: + - "/dev/net/tun:/dev/net/tun" + - /dev/null:/dev/null + privileged: true + ports: + - "8080:8080" + networks: + - x11 +networks: + x11: diff --git a/frontier_rl/README.txt b/frontier_rl/README.txt new file mode 100755 index 0000000..d6aef87 --- /dev/null +++ b/frontier_rl/README.txt @@ -0,0 +1,34 @@ +INSTRUCTIONS: + +To get the image and start the container: + +docker pull tberg1234/2122_multidroneindoorslam:latest + +docker-compose -f docker-compose-image.yaml up + +To get into the container: + +docker exec -it 2122_multidroneindoorslam_novnc_1 /bin/bash + +Once inside container open 3 shells using the above command and run: + + 1. roslaunch a3c_turtlebot3 setup.launch + 2. rosrun multi_map_ros move_base_reset.py + 3. roslaunch a3c_turtlebot3 start_a2c_training.launch + +Once you run the last command the training will begin. You can view the progress in the web browser (http://localhost:8080/vnc.html), but you can know it is done when the final (3rd) roslaunch finishes cleanly and returns. + +To run trained model on one robo inclusing explorationt: + + 1. roslaunch a3c_turtlebot3 setup.launch + 2. rosrun multi_map_ros move_base_reset.py + 3. roslaunch a3c_turtlebot3 start_trained_a2c_v3.launch + +To run trained model with publishing frontiers for state machine integration: + +rosrun a3c_turtlebot3 a2c_v3_trained_for_state_machine.py +roslaunch a3c_turtlebot3 full_start_trained_a2c_v3_for_state_machine.launch + +MAKE SURE TO DOWNLOAD MODEL FILES WHICH ARE TOO LARGE FOR GIT - in our drive under ML_Model + + diff --git a/frontier_rl/openai_examples_projects/README.md b/frontier_rl/openai_examples_projects/README.md new file mode 100644 index 0000000..16eff96 --- /dev/null +++ b/frontier_rl/openai_examples_projects/README.md @@ -0,0 +1,45 @@ +**Edit a file, create a new file, and clone from Bitbucket in under 2 minutes** + +When you're done, you can delete the content in this README and update the file with details for others getting started with your repository. + +*We recommend that you open this README in another tab as you perform the tasks below. You can [watch our video](https://youtu.be/0ocf7u76WSo) for a full demo of all the steps in this tutorial. Open the video in a new tab to avoid leaving Bitbucket.* + +--- + +## Edit a file + +You’ll start by editing this README file to learn how to edit a file in Bitbucket. + +1. Click **Source** on the left side. +2. Click the README.md link from the list of files. +3. Click the **Edit** button. +4. Delete the following text: *Delete this line to make a change to the README from Bitbucket.* +5. After making your change, click **Commit** and then **Commit** again in the dialog. The commit page will open and you’ll see the change you just made. +6. Go back to the **Source** page. + +--- + +## Create a file + +Next, you’ll add a new file to this repository. + +1. Click the **New file** button at the top of the **Source** page. +2. Give the file a filename of **contributors.txt**. +3. Enter your name in the empty file space. +4. Click **Commit** and then **Commit** again in the dialog. +5. Go back to the **Source** page. + +Before you move on, go ahead and explore the repository. You've already seen the **Source** page, but check out the **Commits**, **Branches**, and **Settings** pages. + +--- + +## Clone a repository + +Use these steps to clone from SourceTree, our client for using the repository command-line free. Cloning allows you to work on your files locally. If you don't yet have SourceTree, [download and install first](https://www.sourcetreeapp.com/). If you prefer to clone from the command line, see [Clone a repository](https://confluence.atlassian.com/x/4whODQ). + +1. You’ll see the clone button under the **Source** heading. Click that button. +2. Now click **Check out in SourceTree**. You may need to create a SourceTree account or log in. +3. When you see the **Clone New** dialog in SourceTree, update the destination path and name if you’d like to and then click **Clone**. +4. Open the directory you just created to see your repository’s files. + +Now that you're more familiar with your Bitbucket repository, go ahead and add a new file locally. You can [push your change back to Bitbucket with SourceTree](https://confluence.atlassian.com/x/iqyBMg), or you can [add, commit,](https://confluence.atlassian.com/x/8QhODQ) and [push from the command line](https://confluence.atlassian.com/x/NQ0zDQ). \ No newline at end of file diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/CMakeLists.txt b/frontier_rl/openai_examples_projects/a3c_turtlebot3/CMakeLists.txt new file mode 100644 index 0000000..54051aa --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/CMakeLists.txt @@ -0,0 +1,202 @@ +cmake_minimum_required(VERSION 3.0.2) +project(a3c_turtlebot3) + +## 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) + +## 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 a3c_turtlebot3 +# CATKIN_DEPENDS other_catkin_pkg +# 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}/a3c_turtlebot3.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/a3c_turtlebot3_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_a3c_turtlebot3.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/frontier_rl/openai_examples_projects/a3c_turtlebot3/config/a2c_params.yaml b/frontier_rl/openai_examples_projects/a3c_turtlebot3/config/a2c_params.yaml new file mode 100755 index 0000000..e21a72a --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/config/a2c_params.yaml @@ -0,0 +1,13 @@ +turtlebot3: #namespace + task_and_robot_environment_name: 'TurtleBot3A2CTesting-v0' + ros_ws_abspath: "/home/taylor/multidrone_slam" #'/root/catkin_ws' + pos_step: 0.016 # increment in position for each command + + #qlearn parameters + alpha: 0.9 + gamma: 0.7 + epsilon: 0.9 + epsilon_discount: 0.999 + nepisodes: 500 + nsteps: 1000 + running_step: 0.06 # Time for each step \ No newline at end of file diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/config/my_turtlebot3_openai_qlearn_params_v2.yaml b/frontier_rl/openai_examples_projects/a3c_turtlebot3/config/my_turtlebot3_openai_qlearn_params_v2.yaml new file mode 100755 index 0000000..f007b01 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/config/my_turtlebot3_openai_qlearn_params_v2.yaml @@ -0,0 +1,14 @@ +turtlebot3: #namespace + task_and_robot_environment_name: 'TurtleBot3WorldMazeTesting-v0' + ros_ws_abspath: "/home/taylor/multidrone_slam" + running_step: 0.04 # amount of time the control will be executed + pos_step: 0.016 # increment in position for each command + + #qlearn parameters + alpha: 0.9 + gamma: 0.7 + epsilon: 0.9 + epsilon_discount: 0.999 + nepisodes: 500 + nsteps: 1000 + running_step: 0.06 # Time for each step \ No newline at end of file diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/full_start_trained_a2c_v3_for_state_machine.launch b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/full_start_trained_a2c_v3_for_state_machine.launch new file mode 100644 index 0000000..bd99924 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/full_start_trained_a2c_v3_for_state_machine.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/full_training.launch b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/full_training.launch new file mode 100755 index 0000000..1d5fff0 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/full_training.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/setup.launch b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/setup.launch new file mode 100755 index 0000000..a9a892f --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/setup.launch @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_a2c_training.launch b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_a2c_training.launch new file mode 100644 index 0000000..535cbcf --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_a2c_training.launch @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_fringe.launch b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_fringe.launch new file mode 100644 index 0000000..3510a24 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_fringe.launch @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_input_publish.launch b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_input_publish.launch new file mode 100644 index 0000000..540c1fb --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_input_publish.launch @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_move_base.launch b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_move_base.launch new file mode 100755 index 0000000..18062dd --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_move_base.launch @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_simulation.launch b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_simulation.launch new file mode 100644 index 0000000..5a5adf4 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_simulation.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_trained_a2c.launch b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_trained_a2c.launch new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_training_v2.launch b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_training_v2.launch new file mode 100644 index 0000000..87afb69 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/launch/start_training_v2.launch @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/package.xml b/frontier_rl/openai_examples_projects/a3c_turtlebot3/package.xml new file mode 100644 index 0000000..31e5547 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/package.xml @@ -0,0 +1,59 @@ + + + a3c_turtlebot3 + 0.0.0 + The a3c_turtlebot3 package + + + + + taylor + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + + + + + + diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/a2c.py b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/a2c.py new file mode 100755 index 0000000..fa30432 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/a2c.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python3.8 + +""" +A minimal Advantage Actor Critic Implementation +Usage: +python3 minA2C.py +""" + +import gym +import tensorflow as tf +import numpy as np +import os + +from gym.vector.utils import spaces +from tensorflow import keras + +import time +from gym import wrappers +# ROS packages required +import rospy +import rospkg +import roslaunch +from openai_ros.openai_ros_common import StartOpenAI_ROS_Environment + +os.environ["CUDA_VISIBLE_DEVICES"] = "0" + +from collections import deque +import time +import random + +# An episode a full game +train_episodes = 100 + +def create_actor(state_shape, action_shape): + learning_rate = 0.001 + init = tf.keras.initializers.HeUniform() + model = keras.Sequential() + # model.add(keras.layers.Dense(24, input_shape=state_shape, activation=tf.keras.layers.LeakyReLU(), kernel_initializer=init)) + model.add(keras.layers.Dense(24, input_shape=state_shape, activation="tanh", kernel_initializer=init)) + model.add(keras.layers.Dense(12, activation="tanh", kernel_initializer=init)) + model.add(keras.layers.Dense(action_shape, activation='softmax', kernel_initializer=init)) + model.compile(loss='categorical_crossentropy', optimizer=tf.keras.optimizers.Adam(lr=learning_rate), metrics=['accuracy']) + return model + +def create_critic(state_shape, output_shape): + learning_rate = 0.001 + init = tf.keras.initializers.HeUniform() + model = keras.Sequential() + model.add(keras.layers.Dense(24, input_shape=state_shape, activation=tf.keras.layers.LeakyReLU(), kernel_initializer=init)) + model.add(keras.layers.Dense(12, activation=tf.keras.layers.LeakyReLU(), kernel_initializer=init)) + model.add(keras.layers.Dense(output_shape, activation='linear', kernel_initializer=init)) + model.compile(loss=tf.keras.losses.MeanSquaredError(), optimizer=tf.keras.optimizers.Adam(lr=learning_rate), metrics=['accuracy']) + return model + +def one_hot_encode_action(action, n_actions): + encoded = np.zeros(n_actions, np.float32) + encoded[action] = 1 + return encoded + +def main(): + + rospy.init_node('turtlebot3_a2c', anonymous=True, log_level=rospy.WARN) + + # Init OpenAI_ROS ENV + task_and_robot_environment_name = rospy.get_param('/turtlebot3/task_and_robot_environment_name') + print("ENV NAME: " + str(task_and_robot_environment_name)) + env = StartOpenAI_ROS_Environment( + task_and_robot_environment_name) + # Create the Gym environment + rospy.loginfo("Gym environment done") + rospy.loginfo("Starting Learning") + + # Set the logging system + rospack = rospkg.RosPack() + pkg_path = rospack.get_path('a3c_turtlebot3') + outdir = pkg_path + '/training_results' + env = wrappers.Monitor(env, outdir, force=True) + rospy.loginfo("Monitor Wrapper started") + + last_time_steps = np.ndarray(0) + + RANDOM_SEED = 7 + tf.random.set_seed(RANDOM_SEED) + + # env = gym.make('MountainCar-v0') + env.seed(RANDOM_SEED) + np.random.seed(RANDOM_SEED) + + actor_checkpoint_path = "/home/taylor/multidrone_slam/src/2122_MultiDroneIndoorSLAM/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_actor/actor_cp.ckpt" #"/root/catkin_ws/src/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_actor/actor_cp.ckpt" + critic_checkpoint_path = "/home/taylor/multidrone_slam/src/2122_MultiDroneIndoorSLAM/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_critic/critic_cp.ckpt" #"/root/catkin_ws/src/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_critic/critic_cp.ckpt" + + # actor_checkpoint_path = "training_actor/actor_cp.ckpt" + # critic_checkpoint_path = "training_critic/critic_cp.ckpt" + + print("ENV SHAPE: " + str(env.observation_space.shape)) + print("ENV SPACE: " + str(env.action_space.n)) + + # number_observations = rospy.get_param('/turtlebot3/n_observations') + # number_actions = rospy.get_param('/turtlebot3/n_actions') + # + # actor = create_actor(env.env.env, env.env.get) + # critic = create_critic(number_observations, 1) + + # print(str(env.env.env._get_obs())) + # print("OBS SHAPE: " + str(np.array(env.env.env._get_obs()).shape)) + + #FIXME: the below lines were working + # actor = create_actor(np.array(env.env.env._get_obs()).shape, env.action_space.n) + # critic = create_critic(np.array(env.env.env._get_obs()).shape, 1) + + actor = create_actor(np.array(np.zeros(env.observation_space.n)).shape, env.action_space.n) + critic = create_critic(np.array(np.zeros(env.observation_space.n)).shape, 1) + + # actor = create_actor(env.observation_space.shape, env.action_space.n) + # critic = create_critic(env.observation_space.shape, 1) + + # FIXME: add this back in + if os.path.exists('training_actor'): + actor.load_weights(actor_checkpoint_path) + critic.load_weights(critic_checkpoint_path) + # print(actor) + # print(critic) + + # X = states, y = actions + X = [] + y = [] + + last_action = 0 + + for episode in range(train_episodes): + + total_training_rewards = 0 + + print("getting obs") + observation = env.env.env.get_obs() + print("resetting gmapping") + env.env.env.reset_gmapping() + print("resetting enviornment") + env.reset() + print("starting move base") + env.env.start_move_base() + print("setting episode start time") + env.env.env.set_start_time() + + done = False + while not done: + + print("continuing episode: " + str(episode)) + # if True: + # env.render() + # observation = env.env.env._get_obs() + observation = np.array(observation) + + # all_zeros = not np.any(observation) + is_default = np.all(observation == 100) + if is_default: + print("default observation hit") + observation = np.array(env.env.env.get_obs()) + + # 0 stays 0 (best) -1 becomes 1 (second best) 100 becomes 2 (worst) + observation = np.where(observation == -1, 1, observation) + observation = np.where(observation == 100, 2, observation) + + # np.savetxt('test1.txt', observation, fmt='%d') + + observation_reshaped = observation.reshape([1, observation.shape[0]]) + + action_probs = actor.predict(observation_reshaped).flatten() + print("*****PROBABILITY: " + str(action_probs)) + # Note we're sampling from the prob distribution instead of using argmax + # action = np.random.choice(env.action_space.n, 1, p=action_probs)[0] + # + # while last_action == action: + # print("returning same action - finding a new one") + # action = np.random.randint(env.action_space.n) + + action = np.argmax(action_probs) + # print("TESTING ACTION IS: " + str(testing_action)) + + # print("ACTION IS: " + str(action) + "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!") + encoded_action = one_hot_encode_action(action, env.action_space.n) + + next_observation, reward, done, info = env.step(action) + next_observation = np.array(next_observation) + next_observation_reshaped = next_observation.reshape([1, next_observation.shape[0]]) + + value_curr = np.asscalar(np.array(critic.predict(observation_reshaped))) + value_next = np.asscalar(np.array(critic.predict(next_observation_reshaped))) + + # Fit on the current observation + discount_factor = .7 + TD_target = reward + (1 - done) * discount_factor * value_next + advantage = critic_target = TD_target - value_curr + print(np.around(action_probs, 2), np.around(value_next - value_curr, 3), 'Advantage:', np.around(advantage, 2)) + advantage_reshaped = np.vstack([advantage]) + TD_target = np.vstack([TD_target]) + critic.train_on_batch(observation_reshaped, TD_target) + #critic.fit(observation_reshaped, TD_target, verbose=0) + + gradient = encoded_action - action_probs + gradient_with_advantage = .0001 * gradient * advantage_reshaped + action_probs + actor.train_on_batch(observation_reshaped, gradient_with_advantage) + #actor.fit(observation_reshaped, gradient_with_advantage, verbose=0) + observation = next_observation + total_training_rewards += reward + + last_action = action + + if done: + print('Total training rewards: {} after n steps = {} with final reward = {}'.format(total_training_rewards, episode, reward)) + total_training_rewards += 1 + + actor.save_weights(actor_checkpoint_path) + critic.save_weights(critic_checkpoint_path) + + print("saved to path") + + print("stopping move base") + env.env.stop_move_base() + + + + + env.close() + +if __name__ == '__main__': + main() diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/a2c_trained.py b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/a2c_trained.py new file mode 100755 index 0000000..c0f1e31 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/a2c_trained.py @@ -0,0 +1,216 @@ +#!/usr/bin/env python3.8 + +""" +A minimal Advantage Actor Critic Implementation +Usage: +python3 minA2C.py +""" + +import gym +import tensorflow as tf +import numpy as np +import os + +from gym.vector.utils import spaces +from tensorflow import keras + +import time +from gym import wrappers +# ROS packages required +import rospy +import rospkg +import roslaunch +from openai_ros.openai_ros_common import StartOpenAI_ROS_Environment + +os.environ["CUDA_VISIBLE_DEVICES"] = "0" + +from collections import deque +import time +import random + +# An episode a full game +train_episodes = 100 + +def create_actor(state_shape, action_shape): + learning_rate = 0.001 + init = tf.keras.initializers.HeUniform() + model = keras.Sequential() + # model.add(keras.layers.Dense(24, input_shape=state_shape, activation=tf.keras.layers.LeakyReLU(), kernel_initializer=init)) + model.add(keras.layers.Dense(24, input_shape=state_shape, activation="tanh", kernel_initializer=init)) + model.add(keras.layers.Dense(12, activation="tanh", kernel_initializer=init)) + model.add(keras.layers.Dense(action_shape, activation='softmax', kernel_initializer=init)) + model.compile(loss='categorical_crossentropy', optimizer=tf.keras.optimizers.Adam(lr=learning_rate), metrics=['accuracy']) + return model + +def create_critic(state_shape, output_shape): + learning_rate = 0.001 + init = tf.keras.initializers.HeUniform() + model = keras.Sequential() + model.add(keras.layers.Dense(24, input_shape=state_shape, activation=tf.keras.layers.LeakyReLU(), kernel_initializer=init)) + model.add(keras.layers.Dense(12, activation=tf.keras.layers.LeakyReLU(), kernel_initializer=init)) + model.add(keras.layers.Dense(output_shape, activation='linear', kernel_initializer=init)) + model.compile(loss=tf.keras.losses.MeanSquaredError(), optimizer=tf.keras.optimizers.Adam(lr=learning_rate), metrics=['accuracy']) + return model + +def one_hot_encode_action(action, n_actions): + encoded = np.zeros(n_actions, np.float32) + encoded[action] = 1 + return encoded + +def main(): + + rospy.init_node('turtlebot3_a2c', anonymous=True, log_level=rospy.WARN) + + # Init OpenAI_ROS ENV + task_and_robot_environment_name = rospy.get_param('/turtlebot3/task_and_robot_environment_name') + print("ENV NAME: " + str(task_and_robot_environment_name)) + env = StartOpenAI_ROS_Environment( + task_and_robot_environment_name) + # Create the Gym environment + rospy.loginfo("Gym environment done") + rospy.loginfo("Starting Learning") + + # Set the logging system + rospack = rospkg.RosPack() + pkg_path = rospack.get_path('a3c_turtlebot3') + outdir = pkg_path + '/training_results' + env = wrappers.Monitor(env, outdir, force=True) + rospy.loginfo("Monitor Wrapper started") + + last_time_steps = np.ndarray(0) + + RANDOM_SEED = 7 + tf.random.set_seed(RANDOM_SEED) + + # env = gym.make('MountainCar-v0') + env.seed(RANDOM_SEED) + np.random.seed(RANDOM_SEED) + + actor_checkpoint_path = "/home/taylor/multidrone_slam/src/2122_MultiDroneIndoorSLAM/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_actor/actor_cp.ckpt" #"/root/catkin_ws/src/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_actor/actor_cp.ckpt" + critic_checkpoint_path = "/home/taylor/multidrone_slam/src/2122_MultiDroneIndoorSLAM/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_critic/critic_cp.ckpt" #"/root/catkin_ws/src/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_critic/critic_cp.ckpt" + + # actor_checkpoint_path = "training_actor/actor_cp.ckpt" + # critic_checkpoint_path = "training_critic/critic_cp.ckpt" + + print("ENV SHAPE: " + str(env.observation_space.shape)) + print("ENV SPACE: " + str(env.action_space.n)) + + # number_observations = rospy.get_param('/turtlebot3/n_observations') + # number_actions = rospy.get_param('/turtlebot3/n_actions') + # + # actor = create_actor(env.env.env, env.env.get) + # critic = create_critic(number_observations, 1) + + # print(str(env.env.env._get_obs())) + # print("OBS SHAPE: " + str(np.array(env.env.env._get_obs()).shape)) + + #FIXME: the below lines were working + # actor = create_actor(np.array(env.env.env._get_obs()).shape, env.action_space.n) + # critic = create_critic(np.array(env.env.env._get_obs()).shape, 1) + + actor = create_actor(np.array(np.zeros(env.observation_space.n)).shape, env.action_space.n) + critic = create_critic(np.array(np.zeros(env.observation_space.n)).shape, 1) + + # actor = create_actor(env.observation_space.shape, env.action_space.n) + # critic = create_critic(env.observation_space.shape, 1) + + # FIXME: add this back in + if os.path.exists('training_actor'): + actor.load_weights(actor_checkpoint_path) + critic.load_weights(critic_checkpoint_path) + # print(actor) + # print(critic) + + # X = states, y = actions + X = [] + y = [] + + last_action = 0 + + all_taken_actions = list() + + total_training_rewards = 0 + + print("getting obs") + observation = env.env.env.get_obs() + print("resetting gmapping") + env.env.env.reset_gmapping() + print("resetting enviornment") + env.reset() + print("starting move base") + env.env.start_move_base() + print("setting episode start time") + env.env.env.set_start_time() + + done = False + while not done: + + # observation = env.env.env._get_obs() + observation = np.array(observation) + + is_default = np.all(observation == 100) + if is_default: + print("default observation hit") + observation = np.array(env.env.env.get_obs()) + + # 0 stays 0 (best) -1 becomes 1 (second best) 100 becomes 2 (worst) + observation = np.where(observation == -1, 1, observation) + observation = np.where(observation == 100, 2, observation) + + observation_reshaped = observation.reshape([1, observation.shape[0]]) + + action_probs = actor.predict(observation_reshaped).flatten() + print("*****PROBABILITY: " + str(action_probs)) + + # action = np.argmax(action_probs) + actions_sorted_indices = action_probs.argsort() + action = actions_sorted_indices[-1] + + print("ACTION: " + str(action)) + + # if int(action) == int(last_action): + # print("ACTION CHANGED BECAUSE DUPLICATE OF LAST ACTION OR NOT VALID") + # for i in range(-2, -1*action_probs.size, -1): + # checking_action = actions_sorted_indices[i] + # print("checking action for index i, action: " + str(i) + ", " + str(checking_action)) + # if checking_action not in all_taken_actions: + # print("ACTION CHANGED BECAUSE DUPLICATE OF LAST ACTION. THIS ACTION HAS NEVER BEEN TAKEN BEFORE AND IS VALID: " + str(checking_action)) + # action = checking_action + # break + + env.gazebo.unpauseSim() + + if int(action) in all_taken_actions or not env.is_valid_action_point(action): + print("ACTION CHANGED BECAUSE DUPLICATE OF LAST ACTION OR NOT VALID") + for i in range(-2, -1*action_probs.size, -1): + checking_action = actions_sorted_indices[i] + print("checking action for index i, action: " + str(i) + ", " + str(checking_action)) + if checking_action not in all_taken_actions and env.is_valid_action_point(checking_action): + print("ACTION CHANGED BECAUSE DUPLICATE OF LAST ACTION. THIS ACTION HAS NEVER BEEN TAKEN BEFORE AND IS VALID: " + str(checking_action)) + action = checking_action + break + + next_observation = env.pseudo_step(action) + + # if np.array_equal(observation, np.array(next_observation)): + # print("OBSERVATION IS THE SAME") + # else: + # print("obs are different :)") + + observation = next_observation + + # print("Sending to trained action point") + # env.env.env.send_to_trained_point(action) + + # observation = env.env.env._get_obs() + + last_action = action + print("SETTING LAST ACTION TO: " + str(action)) + all_taken_actions.append(int(action)) + print("ALL TAKEN ACTIONS: " + str(all_taken_actions)) + + + env.close() + +if __name__ == '__main__': + main() diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/a2c_v2.py b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/a2c_v2.py new file mode 100644 index 0000000..75f3cd3 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/a2c_v2.py @@ -0,0 +1,171 @@ +#!/usr/bin/env python3.8 + +""" +A minimal Advantage Actor Critic Implementation +Usage: +python3 minA2C.py +""" + +import gym +import tensorflow as tf +import numpy as np +import os + +from gym.vector.utils import spaces +from tensorflow import keras + +import time +from gym import wrappers +# ROS packages required +import rospy +import rospkg +import roslaunch +from openai_ros.openai_ros_common import StartOpenAI_ROS_Environment + +os.environ["CUDA_VISIBLE_DEVICES"] = "0" + +import sys +import torch +import torch.nn as nn +import torch.optim as optim +import torch.nn.functional as F +from torch.autograd import Variable +import matplotlib.pyplot as plt +import pandas as pd + +# hyperparameters +hidden_size = 256 +learning_rate = 3e-4 + +# Constants +GAMMA = 0.99 +num_steps = 300 +max_episodes = 3000 + + +class ActorCritic(nn.Module): + def __init__(self, num_inputs, num_actions, hidden_size, learning_rate=3e-4): + super(ActorCritic, self).__init__() + + self.num_actions = num_actions + self.critic_linear1 = nn.Linear(num_inputs, hidden_size) + self.critic_linear2 = nn.Linear(hidden_size, 1) + + self.actor_linear1 = nn.Linear(num_inputs, hidden_size) + self.actor_linear2 = nn.Linear(hidden_size, num_actions) + + def forward(self, state): + state = Variable(torch.from_numpy(state).float().unsqueeze(0)) + value = F.relu(self.critic_linear1(state)) + value = self.critic_linear2(value) + + policy_dist = F.relu(self.actor_linear1(state)) + policy_dist = F.softmax(self.actor_linear2(policy_dist), dim=1) + + return value, policy_dist + + +def a2c(env): + #TODO: add each obs as seperate one + num_inputs = env.observation_space.n + num_outputs = env.action_space.n + + actor_critic = ActorCritic(num_inputs, num_outputs, hidden_size) + ac_optimizer = optim.Adam(actor_critic.parameters(), lr=learning_rate) + + all_lengths = [] + average_lengths = [] + all_rewards = [] + entropy_term = 0 + + for episode in range(max_episodes): + log_probs = [] + values = [] + rewards = [] + + state = env.reset() + for steps in range(num_steps): + value, policy_dist = actor_critic.forward(state) + value = value.detach().numpy()[0, 0] + dist = policy_dist.detach().numpy() + + action = np.random.choice(num_outputs, p=np.squeeze(dist)) + log_prob = torch.log(policy_dist.squeeze(0)[action]) + entropy = -np.sum(np.mean(dist) * np.log(dist)) + new_state, reward, done, _ = env.step(action) + + rewards.append(reward) + values.append(value) + log_probs.append(log_prob) + entropy_term += entropy + state = new_state + + if done or steps == num_steps - 1: + Qval, _ = actor_critic.forward(new_state) + Qval = Qval.detach().numpy()[0, 0] + all_rewards.append(np.sum(rewards)) + all_lengths.append(steps) + average_lengths.append(np.mean(all_lengths[-10:])) + if episode % 10 == 0: + sys.stdout.write("episode: {}, reward: {}, total length: {}, average length: {} \n".format(episode, + np.sum( + rewards), + steps, + average_lengths[ + -1])) + break + + # compute Q values + Qvals = np.zeros_like(values) + for t in reversed(range(len(rewards))): + Qval = rewards[t] + GAMMA * Qval + Qvals[t] = Qval + + # update actor critic + values = torch.FloatTensor(values) + Qvals = torch.FloatTensor(Qvals) + log_probs = torch.stack(log_probs) + + advantage = Qvals - values + actor_loss = (-log_probs * advantage).mean() + critic_loss = 0.5 * advantage.pow(2).mean() + ac_loss = actor_loss + critic_loss + 0.001 * entropy_term + + ac_optimizer.zero_grad() + ac_loss.backward() + ac_optimizer.step() + + # # Plot results + # smoothed_rewards = pd.Series.rolling(pd.Series(all_rewards), 10).mean() + # smoothed_rewards = [elem for elem in smoothed_rewards] + # plt.plot(all_rewards) + # plt.plot(smoothend_rewards) + # plt.plot() + # plt.xlabel('Episode') + # plt.ylabel('Reward') + # plt.show() + # + # plt.plot(all_lengths) + # plt.plot(average_lengths) + # plt.xlabel('Episode') + # plt.ylabel('Episode length') + # plt.show() + + +if __name__ == '__main__': + # Init OpenAI_ROS ENV + task_and_robot_environment_name = rospy.get_param('/turtlebot3/task_and_robot_environment_name') + print("ENV NAME: " + str(task_and_robot_environment_name)) + env = StartOpenAI_ROS_Environment( + task_and_robot_environment_name) + # Create the Gym environment + rospy.loginfo("Gym environment done") + rospy.loginfo("Starting Learning") + + # Set the logging system + rospack = rospkg.RosPack() + pkg_path = rospack.get_path('a3c_turtlebot3') + outdir = pkg_path + '/training_results' + env = wrappers.Monitor(env, outdir, force=True) + rospy.loginfo("Monitor Wrapper started") + a2c(env) diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/a2c_v3_trained_for_state_machine.py b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/a2c_v3_trained_for_state_machine.py new file mode 100755 index 0000000..03dd1fa --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/a2c_v3_trained_for_state_machine.py @@ -0,0 +1,502 @@ +#!/usr/bin/env python3.8 + +import rospy +import numpy as np +import copy +from scipy import signal as sig +import time +import os + +from std_msgs.msg import Header, String +from nav_msgs.msg import OccupancyGrid, GridCells +from geometry_msgs.msg import Point, PoseStamped, Pose, PoseArray +from tf import TransformListener +from rospy.exceptions import ROSTimeMovedBackwardsException, ROSInterruptException + +import torch +import torch.nn as nn +import torch.optim as optim +import torch.nn.functional as F +from torch.autograd import Variable + +os.environ["CUDA_VISIBLE_DEVICES"] = "0" + +SEND_TO_FRONTIERS = False +SEND_STATE_MACHINE_FRONTIERS = True +SM_LIMIT = 10 +REMOVE_THRESHOLD = 4 + +hidden_size = 6144 #12288 +learning_rate = 3e-4 +GAMMA = 0.99 +num_steps = 10 +max_episodes = 3000 + +a1_filepath = "/root/catkin_ws/src/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_v3/actor_1_dict.pth" +a2_filepath = "/root/catkin_ws/src/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_v3/actor_2_dict.pth" +c1_filepath = "/root/catkin_ws/src/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_v3/critic_1_dict.pth" +c2_filepath = "/root/catkin_ws/src/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_v3/critic_2_dict.pth" +ao_filepath = "/root/catkin_ws/src/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_v3/ao_dict.pth" +model_run_time_data = "/root/catkin_ws/src/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_v3/model_run_data.csv" + + +class ActorCritic(nn.Module): + def __init__(self, num_inputs, num_actions, hidden_size, learning_rate=3e-4): + super(ActorCritic, self).__init__() + + self.num_actions = num_actions + self.critic_linear1 = nn.Linear(num_inputs, hidden_size) + self.critic_linear2 = nn.Linear(hidden_size, 1) + + self.actor_linear1 = nn.Linear(num_inputs, hidden_size) + self.actor_linear2 = nn.Linear(hidden_size, num_actions) + + def forward(self, state): + state = Variable(torch.from_numpy(state).float().unsqueeze(0)) + value = F.relu(self.critic_linear1(state)) + value = self.critic_linear2(value) + + policy_dist = F.relu(self.actor_linear1(state)) + policy_dist = F.softmax(self.actor_linear2(policy_dist), dim=1) + + return value, policy_dist + + +class find_frontier: + def __init__(self): + rospy.init_node('find_fontier') + + self.cell_threshold = 0 + self.threshold_multiplier = 2 + + # self.tf_listener_ = TransformListener() + self.weight_output = .75 + self.clusters = list() + + self.max_msg_count_before_start = 10 + self.curr_msg_count = 0 + + self.robot_namespace = '/tb3_0' + + self.publish_next_frontier = False + self.publish_ns = "" + + # self.timer = rospy.Timer(rospy.Duration(5), self.frontier_callback) + + num_inputs = 12288 + print("num inputs: " + str(num_inputs)) + num_outputs = 100 + print("num outputs:" + str(num_outputs)) + + self.desired_dimension_x = 192 + self.desired_dimension_y = 64 + + self.actor_critic = ActorCritic(num_inputs, num_outputs, hidden_size) + + self.init_subscribers_and_publishers(self.robot_namespace) + + rospy.loginfo("FINISHED INIT") + + + def init_subscribers_and_publishers(self, ns): + + self.mapSub = rospy.Subscriber(ns + '/map', OccupancyGrid, self.map_callback) + self.fringe = rospy.Publisher('/map_fringe', GridCells, queue_size=10) + self.final_frontier_pub = rospy.Publisher('/selected_frontier', GridCells, queue_size=10) + self.center_fringe_pub = rospy.Publisher('/selected_center', GridCells, queue_size=10) + self.move_pub = rospy.Publisher(ns + '/move_base_simple/goal', PoseStamped, queue_size=10) + self.move_helper_sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.helper) + self.move_helper_pub = rospy.Publisher(ns + '/move_base_simple/goal', PoseStamped, + queue_size=10) + self.frontier_center_pub = rospy.Publisher(ns + '/frontier_list', PoseArray, queue_size=10) + self.frontier_center_request = rospy.Subscriber('/frontier_request', String, self.request_callback) + + + def request_callback(self, msg): + ns = str(msg.data).replace("\"","") + + if self.robot_namespace != ns: + self.robot_namespace = ns + self.init_subscribers_and_publishers(ns) + + self.publish_ns = ns + self.publish_next_frontier = True + + + def helper(self, msg): + self.move_helper_pub.publish(msg) + + def map_callback(self, msg): + self.raw_map = msg + + print("map received") + + map_height = self.raw_map.info.height + map_width = self.raw_map.info.width + if map_width -1) + + # Takes in a list of (x,y) locations to paint as explored nodes + gridCells = GridCells() + gridCells.cell_width = res + gridCells.cell_height = res + + self.cell_threshold = self.threshold_multiplier*res + + header = Header() + header.frame_id = msg.header.frame_id + gridCells.header = header + + filter = np.asarray([-1,0,1,-2,0,2,-1,0,1]).reshape((3,3)) + edgeX = sig.convolve2d(expl, filter, mode='same', fillvalue=-1) + edgeY = sig.convolve2d(expl, filter.T, mode='same', fillvalue=-1) + edge = np.sqrt(np.add(np.power(edgeX, 2), np.power(edgeY, 2))) + open_cells = np.logical_and((data > -1), (data < 10)) + fringe = np.logical_and((edge >= 2), open_cells) + fringe_list = np.transpose(np.where(fringe)) + + pts = [] + raw_pts = [] + for node in fringe_list: + tup = self.map_to_world(node[0], node[1], msg) + pt = Point(tup[0], tup[1], 0) + raw_pt = (tup[0], tup[1], 0) + pts.append(pt) + raw_pts.append(raw_pt) + + gridCells.cells = pts + self.fringe.publish(gridCells) + return raw_pts + + def map_to_world(self, x, y, my_map): + """ + converts a point from the map to the world + :param x: float of x position + :param y: float of y position + :return: tuple of converted point + @Author tberg1234 (from rbe3002) + """ + resolution = my_map.info.resolution + + originX = my_map.info.origin.position.x + originY = my_map.info.origin.position.y + + # multiply by resolution, then move by origin offset + x = x * resolution + originX + resolution / 2 + y = y * resolution + originY + resolution / 2 + + return (x, y) + + + def cluster_cells(self, gridCells): + + all_frontier_clusters = dict() # keeps track of clusters and the parent + remaining_cells = copy.deepcopy(gridCells) # cells remaining to be assigned to a group + + for a_cell in gridCells: # in remaining_cells worked kinda but it was faster sooo + if remaining_cells: + assigned = False + if all_frontier_clusters: + for key in all_frontier_clusters: + cells_in_cluster = all_frontier_clusters.get(key) + iterable_cells = copy.deepcopy(cells_in_cluster) + for other_cell in iterable_cells: + if np.linalg.norm(np.asarray(a_cell) - np.asarray(other_cell)) <= self.cell_threshold: + if a_cell not in cells_in_cluster: + assigned = True + cells_in_cluster.append(a_cell) + if a_cell in remaining_cells: + remaining_cells.remove(a_cell) + # all_frontier_clusters[key] = cells_in_cluster + else: + assigned = True + + for potential_cell in remaining_cells: # in gridCells + if np.linalg.norm(np.asarray(a_cell) - np.asarray(potential_cell)) <= self.cell_threshold: + if potential_cell not in cells_in_cluster: + cells_in_cluster.append(potential_cell) + if potential_cell in remaining_cells: + remaining_cells.remove(potential_cell) + # all_frontier_clusters[key] = cells_in_cluster + + all_frontier_clusters[key] = cells_in_cluster + + if not assigned: + first_list = list() + first_list.append(a_cell) + remaining_cells.remove(a_cell) + all_frontier_clusters[a_cell] = first_list + + + else: + first_list = list() + first_list.append(a_cell) + remaining_cells.remove(a_cell) + all_frontier_clusters[a_cell] = first_list + + else: + pass + + return all_frontier_clusters + + + def filter_clusters(self, all_frontier_clusters): + length_dict = {key: len(value) for key, value in all_frontier_clusters.items()} + + for k in all_frontier_clusters.keys(): + size = length_dict.get(k) + if size <= REMOVE_THRESHOLD: + key_to_be_removed = k + + all_frontier_clusters = {key: value for key, value \ + in all_frontier_clusters.items() \ + if key is not key_to_be_removed} + + return all_frontier_clusters + + + def calculate_least_cost_frontier(self, all_frontier_clusters): + # max_key, max_value = max(all_frontier_clusters.items(), key=lambda x: len(set(x[1]))) + # return max_key, max_value + + cost_dict = dict() + length_dict = {key: len(value) for key, value in all_frontier_clusters.items()} + robot_position = self.get_current_position_in_map() + + for key in all_frontier_clusters.keys(): + cells = all_frontier_clusters.get(key) + size = length_dict.get(key) + center = np.average(cells, axis=0) + distance = np.linalg.norm(np.asarray(center) - np.asarray(robot_position)) + + cost = self.weight_output*distance + (1-self.weight_output)*(1-size) + cost_dict[key] = cost + + sorted_costs = sorted(cost_dict, key=cost_dict.get, reverse=False) + return all_frontier_clusters.get(sorted_costs[0]) + + + def get_centers(self, all_frontier_clusters): + + cost_dict = dict() + length_dict = {key: len(value) for key, value in all_frontier_clusters.items()} + robot_position = self.get_current_position_in_map() + + for key in all_frontier_clusters.keys(): + cells = all_frontier_clusters.get(key) + size = length_dict.get(key) + center = np.average(cells, axis=0) + distance = np.linalg.norm(np.asarray(center) - np.asarray(robot_position)) + + cost = self.weight_output * distance + (1 - self.weight_output) * (1 - size) + cost_dict[key] = cost + + sorted_costs = sorted(cost_dict, key=cost_dict.get, reverse=False) + + centers = list() + for cost in sorted_costs: + fr = all_frontier_clusters.get(cost) + c = np.average(fr, axis=0) + centers.append(c) + + return centers + + + def send_centers_to_pub(self, centers): + + centers_for_pub = [] + counter = 0 + + for c in centers: + + if counter > SM_LIMIT: + break + + move_msg = Pose() + move_msg.position.x = c[0] + move_msg.position.y = c[1] + move_msg.position.z = c[2] + + move_msg.orientation.x = 0 + move_msg.orientation.y = 0 + move_msg.orientation.z = 0 + move_msg.orientation.w = 1 + + centers_for_pub.append(move_msg) + counter += 1 + + full_msg = PoseArray() + full_msg.poses = centers_for_pub + full_msg.header.frame_id = self.robot_namespace + '/map' + + self.frontier_center_pub.publish(full_msg) + + rospy.loginfo("sent frontier list") + + + def send_to_frontier(self, best_frontier_cells, msg): + + final_frontier = list() + for cell in best_frontier_cells: + final_frontier.append(Point(cell[0], cell[1], cell[2])) + + res = msg.info.resolution + + gridCells = GridCells() + gridCells.cell_width = res + gridCells.cell_height = res + + header = Header() + header.frame_id = msg.header.frame_id + gridCells.header = header + + gridCells.cells = final_frontier + + self.final_frontier_pub.publish(gridCells) + + center = np.average(best_frontier_cells, axis=0) + center_lst = list() + center_lst.append(Point(center[0], center[1], center[2])) + + gridCells.cells = center_lst + + self.center_fringe_pub.publish(gridCells) + + move_msg = PoseStamped() + move_msg.header.frame_id = 'map' + + move_msg.pose.position.x = center[0] + move_msg.pose.position.y = center[1] + move_msg.pose.position.z = center[2] + + rospy.loginfo("SENDING TO FRONTIER: " + str(center)) + + move_msg.pose.orientation.x = 0 + move_msg.pose.orientation.y = 0 + move_msg.pose.orientation.z = 0 + move_msg.pose.orientation.w = 1 + + self.move_pub.publish(move_msg) + + + def get_current_position_in_map(self): + # Position of the robot base in the map + + p1 = PoseStamped() + p1.header.frame_id = self.robot_namespace+"/base_link" + p1.pose.orientation.w = 1.0 # Neutral orientation + # try: + # tf_listener_ = TransformListener() + # p_in_base = tf_listener_.transformPose(self.robot_namespace + "/map", p1) + # except ROSTimeMovedBackwardsException: + # p_in_base = PoseStamped() + # print("ROS Interrupt Exception! Just ignore the exception!") + p_in_base = PoseStamped() + # p_in_base = self.tf_listener_.transformPose(self.robot_namespace+"/map", p1) + return (p_in_base.pose.position.x, p_in_base.pose.position.y, p_in_base.pose.position.z) + + + def a2c(self, observation): + + # normalization + observation = np.where(observation == 0, 1, observation) + observation = np.where(observation == -1, 0, observation) + observation = np.where(observation == 100, -1, observation) + + value, policy_dist = self.actor_critic.forward(observation) + dist = policy_dist.detach().numpy() + + action = np.argmax(np.squeeze(dist)) + print("weight is: " + str(float(action / 100))) + return (action / 100) + + + +if __name__ == '__main__': + node = find_frontier() + + num_inputs = 12288 + print("num inputs: " + str(num_inputs)) + num_outputs = 100 + print("num outputs:" + str(num_outputs)) + + # node.actor_critic = ActorCritic(num_inputs, num_outputs, hidden_size) + node.actor_critic.actor_linear1.load_state_dict(torch.load(a1_filepath)) + node.actor_critic.actor_linear2.load_state_dict(torch.load(a2_filepath)) + node.actor_critic.critic_linear1.load_state_dict(torch.load(c1_filepath)) + node.actor_critic.critic_linear2.load_state_dict(torch.load(c2_filepath)) + node.actor_critic.actor_linear1.eval() + node.actor_critic.actor_linear2.eval() + node.actor_critic.critic_linear1.eval() + node.actor_critic.critic_linear2.eval() + + node.ac_optimizer = optim.Adam(node.actor_critic.parameters(), lr=learning_rate) + node.ac_optimizer.load_state_dict(torch.load(ao_filepath)) + + print("LOADED RL MODELS") + + while not rospy.is_shutdown(): + pass + diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/a3c.py b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/a3c.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/all_ros_stuff_for_trained_model.py b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/all_ros_stuff_for_trained_model.py new file mode 100644 index 0000000..2f2e62f --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/all_ros_stuff_for_trained_model.py @@ -0,0 +1,349 @@ +# +# +# +# +# +# +# +# def send_to_location(self, pt): +# move_msg = PoseStamped() +# move_msg.header.frame_id = self.robot_namespace_no_slash + '/map' +# +# move_msg.pose.position.x = pt[0] +# move_msg.pose.position.y = pt[1] +# move_msg.pose.position.z = 0 +# +# move_msg.pose.orientation.x = 0 +# move_msg.pose.orientation.y = 0 +# move_msg.pose.orientation.z = 0 +# move_msg.pose.orientation.w = 1 +# +# self.visualize_goal_point(move_msg) +# +# if self.check_pose_possible(move_msg): +# print("sending to pose") +# self.move_pub.publish(move_msg) +# self.wait_until_move_achieved(move_msg) +# else: +# rospy.logwarn("Goal pose is impossible to plan to: " + str(move_msg)) +# +# +# def show_location(self, pt): +# move_msg = PoseStamped() +# move_msg.header.frame_id = self.robot_namespace_no_slash + '/map' +# +# move_msg.pose.position.x = pt[0] +# move_msg.pose.position.y = pt[1] +# move_msg.pose.position.z = 0 +# +# move_msg.pose.orientation.x = 0 +# move_msg.pose.orientation.y = 0 +# move_msg.pose.orientation.z = 0 +# move_msg.pose.orientation.w = 1 +# +# self.visualize_goal_point(move_msg) +# +# +# def wait_until_move_achieved(self, goal_pose): +# rospy.loginfo("waiting for move base to finish goal") +# +# # while not rospy.is_shutdown(): +# # curr_odom = self.get_odom() +# # curr_x = curr_odom.pose.pose.position.x +# # curr_y = curr_odom.pose.pose.position.y +# # +# # goal_x = goal_pose.pose.position.x +# # goal_y = goal_pose.pose.position.y +# # +# # curr_map = self.get_map() +# # +# # index_of_robot = int(self.point_to_index((curr_x, curr_y), curr_map)) +# # pt = self.index_to_point(index_of_robot, curr_map) +# # curr_location_map_frame = self.map_to_world(pt[0], pt[1], curr_map) +# # +# # print("CURRENT LOCATION: " + str(curr_location_map_frame)) +# # print("GOAL LOCATION: (" + str(goal_x) + ", " + str(goal_y) + ")") +# # +# # if abs(goal_x-curr_location_map_frame[0]) <= self.move_base_threshold and abs(goal_y-curr_location_map_frame[1]) <= self.move_base_threshold: +# # rospy.loginfo("Robot has reached desired pose") +# # print("Robot has reached desired pose") +# # break +# # +# # # elif self.is_stuck(): +# # # rospy.logerr("Robot is stuck") +# # # print("Robot is stuck") +# # # break +# +# self.record_move_base_response = True +# while not rospy.is_shutdown(): +# +# if self.move_base_result is not None: +# text = self.move_base_result.status.text +# self.record_move_base_response = False +# self.move_base_result = None +# if text == "Goal reached.": +# rospy.logwarn("GOAL REACHED!!!") +# print("GOAL REACHED!!!") +# break +# else: +# rospy.logwarn("GOAL _NOT_ REACHED!!!") +# print("GOAL _NOT_ REACHED!!!") +# self.goal_delete_pub.publish(GoalID()) +# break +# +# # elif self.is_stuck(): +# # rospy.logerr("Robot is stuck") +# # print("Robot is stuck") +# # self.goal_delete_pub.publish(GoalID()) +# # break +# +# +# def check_pose_possible(self, goal_pose): +# move_base_service_topic = self.robot_namespace + '/move_base/make_plan' +# print("waiting for %s service", move_base_service_topic) +# +# rospy.wait_for_service(move_base_service_topic) +# +# req = GetPlan() +# +# curr_odom = self.get_odom() +# start_pose = PoseStamped() +# start_pose.header.frame_id = goal_pose.header.frame_id +# +# start_pose.pose.position.x = curr_odom.pose.pose.position.x +# start_pose.pose.position.y = curr_odom.pose.pose.position.y +# start_pose.pose.position.z = 0 +# +# start_pose.pose.orientation.x = 0 +# start_pose.pose.orientation.y = 0 +# start_pose.pose.orientation.z = 0 +# start_pose.pose.orientation.w = 1 +# +# req.start = start_pose +# req.goal = goal_pose +# req.tolerance = .5 +# +# try: +# get_plan = rospy.ServiceProxy(move_base_service_topic, GetPlan) +# resp = get_plan(req.start, req.goal, req.tolerance) +# rospy.loginfo(resp) +# +# if len(resp.plan.poses) > 0: +# rospy.loginfo("PATH FOUND - goal is possible to get to") +# return True +# else: +# rospy.loginfo("NO path found - goal is NOT possible to get to") +# return False +# +# except rospy.ServiceException as e: +# rospy.logerr("Service call failed: %s" % e) +# rospy.logerr("this means a goal is currently executing - no plan can be made right now") +# return False +# +# +# def wait_until_twist_achieved(self, cmd_vel_value, epsilon, update_rate): +# """ +# We wait for the cmd_vel twist given to be reached by the robot reading +# from the odometry. +# :param cmd_vel_value: Twist we want to wait to reach. +# :param epsilon: Error acceptable in odometry readings. +# :param update_rate: Rate at which we check the odometry. +# :return: +# """ +# rospy.logdebug("START wait_until_twist_achieved...") +# +# rate = rospy.Rate(update_rate) +# start_wait_time = rospy.get_rostime().to_sec() +# end_wait_time = 0.0 +# epsilon = 0.05 +# +# rospy.logdebug("Desired Twist Cmd>>" + str(cmd_vel_value)) +# rospy.logdebug("epsilon>>" + str(epsilon)) +# +# linear_speed = cmd_vel_value.linear.x +# angular_speed = cmd_vel_value.angular.z +# +# linear_speed_plus = linear_speed + epsilon +# linear_speed_minus = linear_speed - epsilon +# angular_speed_plus = angular_speed + epsilon +# angular_speed_minus = angular_speed - epsilon +# +# while not rospy.is_shutdown(): +# current_odometry = self._check_odom_ready() +# # IN turtlebot3 the odometry angular readings are inverted, so we have to invert the sign. +# odom_linear_vel = current_odometry.twist.twist.linear.x +# odom_angular_vel = -1 * current_odometry.twist.twist.angular.z +# +# rospy.logdebug("Linear VEL=" + str(odom_linear_vel) + ", ?RANGE=[" + str(linear_speed_minus) + "," + str( +# linear_speed_plus) + "]") +# rospy.logdebug("Angular VEL=" + str(odom_angular_vel) + ", ?RANGE=[" + str(angular_speed_minus) + "," + str( +# angular_speed_plus) + "]") +# +# linear_vel_are_close = (odom_linear_vel <= linear_speed_plus) and (odom_linear_vel > linear_speed_minus) +# angular_vel_are_close = (odom_angular_vel <= angular_speed_plus) and (odom_angular_vel > angular_speed_minus) +# +# if linear_vel_are_close and angular_vel_are_close: +# rospy.logdebug("Reached Velocity!") +# end_wait_time = rospy.get_rostime().to_sec() +# break +# rospy.logdebug("Not there yet, keep waiting...") +# rate.sleep() +# delta_time = end_wait_time - start_wait_time +# rospy.logdebug("[Wait Time=" + str(delta_time) + "]") +# +# rospy.logdebug("END wait_until_twist_achieved...") +# +# return delta_time +# +# +# def is_stuck(self): +# x_list = list() +# y_list = list() +# z_list = list() +# recorded_odom = self.odom_recorder +# for odom in recorded_odom: +# x_list.append(odom.pose.pose.position.x) +# y_list.append(odom.pose.pose.position.y) +# z_list.append(odom.pose.pose.position.z) +# +# if abs(max(x_list) - min(x_list)) < self.odom_diff_threshold and abs( +# max(y_list) - min(y_list)) < self.odom_diff_threshold and abs( +# max(z_list) - min(z_list)) < self.odom_diff_threshold: +# return True +# else: +# return False +# +# +# def get_odom(self): +# return self.odom +# +# +# def get_imu(self): +# return self.imu +# +# +# def get_laser_scan(self): +# return self.laser_scan +# +# +# def get_frontier_map(self): +# # while not self.frontier_map_receivedt: +# # pass +# return self.frontier_map +# +# +# def get_map(self): +# # while not self.map_received: +# # pass +# return self.map +# +# +# def get_pose_map(self): +# # while not self.pose_map_received: +# # pass +# return self.pose_map +# +# +# def get_frontier_map_set(self): +# return self.frontier_map_received +# +# +# def get_map_set(self): +# return self.map_received +# +# +# def get_pose_map_set(self): +# return self.pose_map_received +# +# +# def pub_reset_gmapping(self): +# reset_msg = String() +# reset_msg.data = 'reset' +# self.reset_gmapping_pub.publish(reset_msg) +# print("Sent reset signal") +# +# +# def visualize_goal_point(self, marker_pose): +# marker = Marker() +# marker.action = Marker.ADD +# marker.header.frame_id = self.robot_namespace_no_slash + "/map" # self.robot_namespace_no_slash + "/base_link" +# marker.id = 0 +# marker.type = Marker.SPHERE +# marker.pose.position.x = marker_pose.pose.position.x +# marker.pose.position.y = marker_pose.pose.position.y +# marker.pose.position.z = 0 +# marker.pose.orientation.x = 0 +# marker.pose.orientation.y = 0 +# marker.pose.orientation.z = 0 +# marker.pose.orientation.w = 1 +# marker.scale.x = 1.0 +# marker.scale.y = 1.0 +# marker.scale.z = 1.0 +# +# marker.color.a = 0.5 +# marker.color.r = 0.0 +# marker.color.g = 1.0 +# marker.color.b = 0.0 +# +# self.visualization_pub.publish(marker) +# +# +# def point_to_index(self, point, my_map): +# """convert a index to a point""" +# pt = self.world_to_map(point[0], point[1], my_map) +# return pt[1] * my_map.info.width + pt[0] +# +# +# def convert_location(self, loc, my_map): +# """converts points to the grid""" +# res = my_map.info.resolution +# Xorigin = my_map.info.origin.position.x +# Yorigin = my_map.info.origin.position.y +# +# # offset from origin and then divide by resolution +# Xcell = int((loc[0] - Xorigin - (res / 2)) / res) +# Ycell = int((loc[1] - Yorigin - (res / 2)) / res) +# return (Xcell, Ycell) +# +# +# def index_to_point(self, index, my_map): +# x = index % int(my_map.info.width) +# y = (index - x) / my_map.info.width +# return (x, y) +# +# +# def map_to_world(self, x, y, my_map): +# """ +# converts a point from the map to the world +# :param x: float of x position +# :param y: float of y position +# :return: tuple of converted point +# """ +# resolution = my_map.info.resolution +# +# originX = my_map.info.origin.position.x +# originY = my_map.info.origin.position.y +# +# # multiply by resolution, then move by origin offset +# x = x * resolution + originX + resolution / 2 +# y = y * resolution + originY + resolution / 2 +# return (x, y) +# +# +# def world_to_map(self, x, y, my_map): +# """ +# converts a point from the world to the map +# :param x: float of x position +# :param y: float of y position +# :return: tuple of converted point +# """ +# +# return self.convert_location((x, y), my_map) +# +# +# def start_move_base(self): +# self.start_move_base_pub.publish("start") +# +# +# def stop_move_base(self): +# self.stop_move_base_pub.publish("stop") \ No newline at end of file diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/example_a2c.py b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/example_a2c.py new file mode 100644 index 0000000..d01af2d --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/example_a2c.py @@ -0,0 +1,121 @@ +""" +A minimal Advantage Actor Critic Implementation +Usage: +python3 minA2C.py +""" + +import gym +import tensorflow as tf +import numpy as np +import os +from tensorflow import keras + +from collections import deque +import time +import random + +RANDOM_SEED = 6 +tf.random.set_seed(RANDOM_SEED) + +env = gym.make('CartPole-v1') +#env = gym.make('MountainCar-v0') +env.seed(RANDOM_SEED) +np.random.seed(RANDOM_SEED) + +print("Action Space: {}".format(env.action_space)) +print("State space: {}".format(env.observation_space)) + +# An episode a full game +train_episodes = 300 + +def create_actor(state_shape, action_shape): + learning_rate = 0.001 + init = tf.keras.initializers.HeUniform() + model = keras.Sequential() + model.add(keras.layers.Dense(24, input_shape=state_shape, activation=tf.keras.layers.LeakyReLU(), kernel_initializer=init)) + model.add(keras.layers.Dense(12, activation=tf.keras.layers.LeakyReLU(), kernel_initializer=init)) + model.add(keras.layers.Dense(action_shape, activation='softmax', kernel_initializer=init)) + model.compile(loss='categorical_crossentropy', optimizer=tf.keras.optimizers.Adam(lr=learning_rate), metrics=['accuracy']) + return model + +def create_critic(state_shape, output_shape): + learning_rate = 0.001 + init = tf.keras.initializers.HeUniform() + model = keras.Sequential() + model.add(keras.layers.Dense(24, input_shape=state_shape, activation=tf.keras.layers.LeakyReLU(), kernel_initializer=init)) + model.add(keras.layers.Dense(12, activation=tf.keras.layers.LeakyReLU(), kernel_initializer=init)) + model.add(keras.layers.Dense(output_shape, activation='linear', kernel_initializer=init)) + model.compile(loss=tf.keras.losses.MeanSquaredError(), optimizer=tf.keras.optimizers.Adam(lr=learning_rate), metrics=['accuracy']) + return model + +def one_hot_encode_action(action, n_actions): + encoded = np.zeros(n_actions, np.float32) + encoded[action] = 1 + return encoded + +def main(): + actor_checkpoint_path = "training_actor/actor_cp.ckpt" + critic_checkpoint_path = "training_critic/critic_cp.ckpt" + + actor = create_actor(env.observation_space.shape, env.action_space.n) + critic = create_critic(env.observation_space.shape, 1) + if os.path.exists('training_actor'): + actor.load_weights(actor_checkpoint_path) + + critic.load_weights(critic_checkpoint_path) + print(actor) + print(critic) + + # X = states, y = actions + X = [] + y = [] + + for episode in range(train_episodes): + total_training_rewards = 0 + observation = env.reset() + done = False + while not done: + if True: + env.render() + + # model dims are (batch, env.observation_space.n) + observation_reshaped = observation.reshape([1, observation.shape[0]]) + action_probs = actor.predict(observation_reshaped).flatten() + # Note we're sampling from the prob distribution instead of using argmax + action = np.random.choice(env.action_space.n, 1, p=action_probs)[0] + encoded_action = one_hot_encode_action(action, env.action_space.n) + + next_observation, reward, done, info = env.step(action) + next_observation_reshaped = next_observation.reshape([1, next_observation.shape[0]]) + + value_curr = np.asscalar(np.array(critic.predict(observation_reshaped))) + value_next = np.asscalar(np.array(critic.predict(next_observation_reshaped))) + + # Fit on the current observation + discount_factor = .7 + TD_target = reward + (1 - done) * discount_factor * value_next + advantage = critic_target = TD_target - value_curr + print(np.around(action_probs, 2), np.around(value_next - value_curr, 3), 'Advantage:', np.around(advantage, 2)) + advantage_reshaped = np.vstack([advantage]) + TD_target = np.vstack([TD_target]) + critic.train_on_batch(observation_reshaped, TD_target) + #critic.fit(observation_reshaped, TD_target, verbose=0) + + gradient = encoded_action - action_probs + gradient_with_advantage = .0001 * gradient * advantage_reshaped + action_probs + actor.train_on_batch(observation_reshaped, gradient_with_advantage) + #actor.fit(observation_reshaped, gradient_with_advantage, verbose=0) + observation = next_observation + total_training_rewards += reward + + if done: + print('Total training rewards: {} after n steps = {} with final reward = {}'.format(total_training_rewards, episode, reward)) + total_training_rewards += 1 + + actor.save_weights(actor_checkpoint_path) + critic.save_weights(critic_checkpoint_path) + + env.close() + +if __name__ == '__main__': + main() diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/example_a2c_v2.py b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/example_a2c_v2.py new file mode 100644 index 0000000..e9c4467 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/example_a2c_v2.py @@ -0,0 +1,110 @@ +import sys +import torch +import gym +import numpy as np +import torch.nn as nn +import torch.optim as optim +import torch.nn.functional as F +from torch.autograd import Variable +import matplotlib.pyplot as plt +import pandas as pd + +# hyperparameters +hidden_size = 256 +learning_rate = 3e-4 + +# Constants +GAMMA = 0.99 +num_steps = 300 +max_episodes = 3000 + + +def a2c(env): + num_inputs = env.observation_space.shape[0] + num_outputs = env.action_space.n + + actor_critic = ActorCritic(num_inputs, num_outputs, hidden_size) + ac_optimizer = optim.Adam(actor_critic.parameters(), lr=learning_rate) + + all_lengths = [] + average_lengths = [] + all_rewards = [] + entropy_term = 0 + + for episode in range(max_episodes): + log_probs = [] + values = [] + rewards = [] + + state = env.reset() + for steps in range(num_steps): + value, policy_dist = actor_critic.forward(state) + value = value.detach().numpy()[0, 0] + dist = policy_dist.detach().numpy() + + action = np.random.choice(num_outputs, p=np.squeeze(dist)) + log_prob = torch.log(policy_dist.squeeze(0)[action]) + entropy = -np.sum(np.mean(dist) * np.log(dist)) + new_state, reward, done, _ = env.step(action) + + rewards.append(reward) + values.append(value) + log_probs.append(log_prob) + entropy_term += entropy + state = new_state + + if done or steps == num_steps - 1: + Qval, _ = actor_critic.forward(new_state) + Qval = Qval.detach().numpy()[0, 0] + all_rewards.append(np.sum(rewards)) + all_lengths.append(steps) + average_lengths.append(np.mean(all_lengths[-10:])) + if episode % 10 == 0: + sys.stdout.write("episode: {}, reward: {}, total length: {}, average length: {} \n".format(episode, + np.sum( + rewards), + steps, + average_lengths[ + -1])) + break + + # compute Q values + Qvals = np.zeros_like(values) + for t in reversed(range(len(rewards))): + Qval = rewards[t] + GAMMA * Qval + Qvals[t] = Qval + + # update actor critic + values = torch.FloatTensor(values) + Qvals = torch.FloatTensor(Qvals) + log_probs = torch.stack(log_probs) + + advantage = Qvals - values + actor_loss = (-log_probs * advantage).mean() + critic_loss = 0.5 * advantage.pow(2).mean() + ac_loss = actor_loss + critic_loss + 0.001 * entropy_term + + ac_optimizer.zero_grad() + ac_loss.backward() + ac_optimizer.step() + + # Plot results + smoothed_rewards = pd.Series.rolling(pd.Series(all_rewards), 10).mean() + smoothed_rewards = [elem for elem in smoothed_rewards] + plt.plot(all_rewards) + plt.plot(smoothed_rewards) + plt.plot() + plt.xlabel('Episode') + plt.ylabel('Reward') + plt.show() + + plt.plot(all_lengths) + plt.plot(average_lengths) + plt.xlabel('Episode') + plt.ylabel('Episode length') + plt.show() + + +if __name__ == "__main__": + env = gym.make("CartPole-v0") + a2c(env) diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/qlearn.py b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/qlearn.py new file mode 100755 index 0000000..d33e6ca --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/qlearn.py @@ -0,0 +1,60 @@ +''' +Q-learning approach for different RL problems +as part of the basic series on reinforcement learning @ +https://github.com/vmayoral/basic_reinforcement_learning + +Inspired by https://gym.openai.com/evaluations/eval_kWknKOkPQ7izrixdhriurA + + @author: Victor Mayoral Vilches +''' +import random + +class QLearn: + def __init__(self, actions, epsilon, alpha, gamma): + self.q = {} + self.epsilon = epsilon # exploration constant + self.alpha = alpha # discount constant + self.gamma = gamma # discount factor + self.actions = actions + + def getQ(self, state, action): + return self.q.get((state, action), 0.0) + + def learnQ(self, state, action, reward, value): + ''' + Q-learning: + Q(s, a) += alpha * (reward(s,a) + max(Q(s') - Q(s,a)) + ''' + oldv = self.q.get((state, action), None) + if oldv is None: + self.q[(state, action)] = reward + else: + self.q[(state, action)] = oldv + self.alpha * (value - oldv) + + def chooseAction(self, state, return_q=False): + q = [self.getQ(state, a) for a in self.actions] + maxQ = max(q) + + if random.random() < self.epsilon: + minQ = min(q); mag = max(abs(minQ), abs(maxQ)) + # add random values to all the actions, recalculate maxQ + q = [q[i] + random.random() * mag - .5 * mag for i in range(len(self.actions))] + maxQ = max(q) + + count = q.count(maxQ) + # In case there're several state-action max values + # we select a random one among them + if count > 1: + best = [i for i in range(len(self.actions)) if q[i] == maxQ] + i = random.choice(best) + else: + i = q.index(maxQ) + + action = self.actions[i] + if return_q: # if they want it, give it! + return action, q + return action + + def learn(self, state1, action1, reward, state2): + maxqnew = max([self.getQ(state2, a) for a in self.actions]) + self.learnQ(state1, action1, reward, reward + self.gamma*maxqnew) \ No newline at end of file diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/start_a3c.py b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/start_a3c.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/start_qlearning_v2.py b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/start_qlearning_v2.py new file mode 100755 index 0000000..8f23a21 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/scripts/start_qlearning_v2.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python3.8 +from functools import reduce + +import gym +import numpy +import time +import qlearn +from gym import wrappers +# ROS packages required +import rospy +import rospkg +from openai_ros.openai_ros_common import StartOpenAI_ROS_Environment + + +if __name__ == '__main__': + + rospy.init_node('turtlebot3_world_qlearn', anonymous=True, log_level=rospy.WARN) + + # Init OpenAI_ROS ENV + task_and_robot_environment_name = rospy.get_param('/turtlebot3/task_and_robot_environment_name') + print("ENV NAME: " + str(task_and_robot_environment_name)) + env = StartOpenAI_ROS_Environment( + task_and_robot_environment_name) + # Create the Gym environment + rospy.loginfo("Gym environment done") + rospy.loginfo("Starting Learning") + + # Set the logging system + rospack = rospkg.RosPack() + pkg_path = rospack.get_path('a3c_turtlebot3') + outdir = pkg_path + '/training_results' + env = wrappers.Monitor(env, outdir, force=True) + rospy.loginfo("Monitor Wrapper started") + + last_time_steps = numpy.ndarray(0) + + # Loads parameters from the ROS param server + # Parameters are stored in a yaml file inside the config directory + # They are loaded at runtime by the launch file + Alpha = rospy.get_param("/turtlebot3/alpha") + Epsilon = rospy.get_param("/turtlebot3/epsilon") + Gamma = rospy.get_param("/turtlebot3/gamma") + epsilon_discount = rospy.get_param("/turtlebot3/epsilon_discount") + nepisodes = rospy.get_param("/turtlebot3/nepisodes") + nsteps = rospy.get_param("/turtlebot3/nsteps") + + running_step = rospy.get_param("/turtlebot3/running_step") + + # Initialises the algorithm that we are going to use for learning + qlearn = qlearn.QLearn(actions=range(env.action_space.n), + alpha=Alpha, gamma=Gamma, epsilon=Epsilon) + initial_epsilon = qlearn.epsilon + + start_time = time.time() + highest_reward = 0 + + # Starts the main training loop: the one about the episodes to do + for x in range(nepisodes): + rospy.logdebug("############### START EPISODE=>" + str(x)) + + cumulated_reward = 0 + done = False + if qlearn.epsilon > 0.05: + qlearn.epsilon *= epsilon_discount + + # Initialize the environment and get first state of the robot + observation = env.reset() + state = ''.join(map(str, observation)) + + # Show on screen the actual situation of the robot + # env.render() + # for each episode, we test the robot for nsteps + for i in range(nsteps): + rospy.logwarn("############### Start Step=>" + str(i)) + # Pick an action based on the current state + action = qlearn.chooseAction(state) + rospy.logdebug("Next action is:%d", action) + # Execute the action in the environment and get feedback + observation, reward, done, info = env.step(action) + + print("OBSERVATION IS: " + str(observation)) + + rospy.logwarn(str(observation) + " " + str(reward)) + cumulated_reward += reward + if highest_reward < cumulated_reward: + highest_reward = cumulated_reward + + nextState = ''.join(map(str, observation)) + + # Make the algorithm learn based on the results + rospy.logdebug("# state we were=>" + str(state)) + rospy.logdebug("# action that we took=>" + str(action)) + rospy.logdebug("# reward that action gave=>" + str(reward)) + rospy.logdebug("# episode cumulated_reward=>" + str(cumulated_reward)) + rospy.logdebug("# State in which we will start next step=>" + str(nextState)) + qlearn.learn(state, action, reward, nextState) + + if not (done): + rospy.logdebug("NOT DONE") + state = nextState + else: + rospy.logdebug("DONE") + last_time_steps = numpy.append(last_time_steps, [int(i + 1)]) + break + rospy.logwarn("############### END Step=>" + str(i)) + #raw_input("Next Step...PRESS KEY") + # rospy.sleep(2.0) + m, s = divmod(int(time.time() - start_time), 60) + h, m = divmod(m, 60) + rospy.logerr(("EP: " + str(x + 1) + " - [alpha: " + str(round(qlearn.alpha, 2)) + " - gamma: " + str( + round(qlearn.gamma, 2)) + " - epsilon: " + str(round(qlearn.epsilon, 2)) + "] - Reward: " + str( + cumulated_reward) + " Time: %d:%02d:%02d" % (h, m, s))) + + rospy.logwarn(("\n|" + str(nepisodes) + "|" + str(qlearn.alpha) + "|" + str(qlearn.gamma) + "|" + str( + initial_epsilon) + "*" + str(epsilon_discount) + "|" + str(highest_reward) + "| PICTURE |")) + + text_file = open(outdir + '/final_values.txt', "w") + n = text_file.write(str("\n|" + str(nepisodes) + "|" + str(qlearn.alpha) + "|" + str(qlearn.gamma) + "|" + str( + initial_epsilon) + "*" + str(epsilon_discount) + "|" + str(highest_reward) + "| PICTURE |")) + text_file.close() + + l = last_time_steps.tolist() + l.sort() + + # print("Parameters: a="+str) + rospy.logwarn("Overall score: {:0.2f}".format(last_time_steps.mean())) + rospy.logwarn("Best 100 score: {:0.2f}".format(reduce(lambda x, y: x + y, l[-100:]) / len(l[-100:]))) + + env.close() \ No newline at end of file diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_actor/actor_cp.ckpt.data-00000-of-00001 b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_actor/actor_cp.ckpt.data-00000-of-00001 new file mode 100644 index 0000000..933017f Binary files /dev/null and b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_actor/actor_cp.ckpt.data-00000-of-00001 differ diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_actor/actor_cp.ckpt.index b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_actor/actor_cp.ckpt.index new file mode 100644 index 0000000..2e2795f Binary files /dev/null and b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_actor/actor_cp.ckpt.index differ diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_actor/checkpoint b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_actor/checkpoint new file mode 100644 index 0000000..73e3fd4 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_actor/checkpoint @@ -0,0 +1,2 @@ +model_checkpoint_path: "actor_cp.ckpt" +all_model_checkpoint_paths: "actor_cp.ckpt" diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_critic/checkpoint b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_critic/checkpoint new file mode 100644 index 0000000..c7988d5 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_critic/checkpoint @@ -0,0 +1,2 @@ +model_checkpoint_path: "critic_cp.ckpt" +all_model_checkpoint_paths: "critic_cp.ckpt" diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_critic/critic_cp.ckpt.data-00000-of-00001 b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_critic/critic_cp.ckpt.data-00000-of-00001 new file mode 100644 index 0000000..c8e82e1 Binary files /dev/null and b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_critic/critic_cp.ckpt.data-00000-of-00001 differ diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_critic/critic_cp.ckpt.index b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_critic/critic_cp.ckpt.index new file mode 100644 index 0000000..836f106 Binary files /dev/null and b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_critic/critic_cp.ckpt.index differ diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_results/final_values.txt b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_results/final_values.txt new file mode 100644 index 0000000..d4578bb --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_results/final_values.txt @@ -0,0 +1,2 @@ + +|100|0.1|0.7|0.9*0.999|252| PICTURE | \ No newline at end of file diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_results/openaigym.episode_batch.0.913343.stats.json b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_results/openaigym.episode_batch.0.913343.stats.json new file mode 100644 index 0000000..5aae791 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_results/openaigym.episode_batch.0.913343.stats.json @@ -0,0 +1 @@ +{"initial_reset_timestamp": null, "timestamps": [], "episode_lengths": [], "episode_rewards": [], "episode_types": []} \ No newline at end of file diff --git a/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_results/openaigym.manifest.0.913343.manifest.json b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_results/openaigym.manifest.0.913343.manifest.json new file mode 100644 index 0000000..dd0d0d9 --- /dev/null +++ b/frontier_rl/openai_examples_projects/a3c_turtlebot3/training_results/openaigym.manifest.0.913343.manifest.json @@ -0,0 +1 @@ +{"stats": "openaigym.episode_batch.0.913343.stats.json", "videos": [], "env_info": {"gym_version": "0.21.0", "env_id": "TurtleBot3A2CTesting-v0"}} \ No newline at end of file diff --git a/frontier_rl/openai_ros/README.md b/frontier_rl/openai_ros/README.md new file mode 100644 index 0000000..a2e0aa0 --- /dev/null +++ b/frontier_rl/openai_ros/README.md @@ -0,0 +1,17 @@ +## Install Dependencies + +Execute the following commands:
+`cd ~/ros_ws/src`
+`git clone https://bitbucket.org/theconstructcore/openai_ros.git`
+`cd ~/ros_ws`
+`catkin_make`
+`source devel/setup.bash`
+`rosdep install openai_ros`
+ + +## Use + + + + + diff --git a/frontier_rl/openai_ros/openai_ros/CHANGELOG.rst b/frontier_rl/openai_ros/openai_ros/CHANGELOG.rst new file mode 100644 index 0000000..8632421 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/CHANGELOG.rst @@ -0,0 +1,100 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package openai_ros +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Fixed issue that asked dependencies of other RobotEnvs and Tasks that we are not using +* Added some changes to dependencies +* Added all dependencies for the package +* Removed dependencies of theconstruct +* Added rewards messages to package +* Corrected error of robot reset +* Removed logs +* Fixed some issues +* Added changes for shadow robot +* Created TaskEnv not tested +* Added done method +* Added actions in Task Env for UR5 +* Added Basic version of the RobotEnv for UR5 +* Removed logs to debug +* Added reset movement to guarantee good start +* Added camera sensors +* Finished untested version of task +* Added done method +* Added action conversion Sawyer +* Added first simple version of Sawyer Robot Env +* Adding movement functions for Sawyer +* Adding movement system for Sawyer +* Added sawyer env barebones +* Corrected missing parameter +* Fixed Bugs +* Added TaskEnv +* Added done method +* Adding Task Env +* Added Wamv Robot Env +* Added minor structure changes +* Added barebones of VMRC env +* Moved to New Clearer Structure +* Added first version of the StayUp Hopper Env +* Debuging code +* Finished first round in TaskEnv for hopper +* Added observations space +* Porting all the functions from old system learning to new openai_ros +* Fixed superfluous line in parrot en and created the first version for hopper +* Added barebones of Hopper and some minor mods to drone +* Made TaskEnv work replacing dynamic action check with hardcoded wait +* Started to test the Task for drone +* Added Version one of the DroneTaskEnv +* Created the done method and its auxiliary methods +* Added New actions +* Corrected bug that broke the openai ros +* Removed file copied where it shoulndt and added parrot to init +* Added landing and takeoff sequences +* Corrected some Errors +* Adding task goto +* Added RobotEnv +* Adding Parrot DRone Env +* Merge branch 'master' into environments-documentation-mig +* Changed reset to world in Turtlebot2 because cameras took for ever to restart in the other methos +* Merged in environments-documentation-mig (pull request #3) + Environments documentation mig + Approved-by: RDaneelOlivaw +* Added changes +* Removed the ACtion stop +* Added correction for the moving system due to inexactitudes of the steering system by friction +* Added some corrections for the reset world +* Corrected Imu Max value too low +* Added new done but still bugs in movement +* Adding rewards and done to make it go to a desired point +* Added world reset option for systems that sim reset breaks the tf and others +* Corrected certain errors +* Added Subscribers for all sensors +* Added Barebones of SumitXL Env +* Added minor improvements to make example work +* Adedd barebones and first version of the turtlebot3 robot env +* Minor changes +* Merged in environments-documentation-mig (pull request #2) + Environments documentation mig + Approved-by: RDaneelOlivaw +* Added fix to deactivate init physiscs parameters +* Fixing bugs +* Added some changes +* Added Task for turtlebot for maze navigation +* Finished version 1 of the turtlebot2 robot env +* Added sensor readings to env +* Added barebones turtlebot2 +* Added basic index +* Added first working docs for modules and the different environments +* Added gitignore to avoud uploading compiled documentation +* Added Documentation support +* Added changes for publishing only the final episode acumulated reward +* Corrected some conversion issues +* Updated code +* Updated code +* Updated code +* Updated code +* Updated code +* Added cartpole envs +* Renamed to openai_ros +* Contributors: RDAneelOlivaw, RDaneelOlivaw, aezquerro, rDaneelOlivaw diff --git a/frontier_rl/openai_ros/openai_ros/CMakeLists.txt b/frontier_rl/openai_ros/openai_ros/CMakeLists.txt new file mode 100644 index 0000000..e895d1d --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/CMakeLists.txt @@ -0,0 +1,198 @@ +cmake_minimum_required(VERSION 2.8.3) +project(openai_ros) + +## 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 + std_msgs + message_generation + move_base_msgs + actionlib_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 run_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 run_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 + RLExperimentInfo.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 + ) + +################################################ +## 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 run_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( + DEPENDS message_runtime rospy gazebo_msgs std_msgs geometry_msgs controller_manager_msgs + # 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}/openai_ros.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/openai_ros_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 +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_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_openai_ros.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/frontier_rl/openai_ros/openai_ros/doc/CHANGELOG.rst b/frontier_rl/openai_ros/openai_ros/doc/CHANGELOG.rst new file mode 100644 index 0000000..6737a32 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package openai_ros +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.0 (2018-07-11) +------------------ +* Not generated with BitBucket diff --git a/frontier_rl/openai_ros/openai_ros/doc/Makefile b/frontier_rl/openai_ros/openai_ros/doc/Makefile new file mode 100644 index 0000000..87afdc8 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/Makefile @@ -0,0 +1,216 @@ +# Makefile for Sphinx documentation +# + +# You can set these variables from the command line. +SPHINXOPTS = +SPHINXBUILD = sphinx-build +PAPER = +BUILDDIR = .build + +# User-friendly check for sphinx-build +ifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1) +$(error The '$(SPHINXBUILD)' command was not found. Make sure you have Sphinx installed, then set the SPHINXBUILD environment variable to point to the full path of the '$(SPHINXBUILD)' executable. Alternatively you can add the directory with the executable to your PATH. If you don't have Sphinx installed, grab it from http://sphinx-doc.org/) +endif + +# Internal variables. +PAPEROPT_a4 = -D latex_paper_size=a4 +PAPEROPT_letter = -D latex_paper_size=letter +ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . +# the i18n builder cannot share the environment and doctrees with the others +I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . + +.PHONY: help +help: + @echo "Please use \`make ' where is one of" + @echo " html to make standalone HTML files" + @echo " dirhtml to make HTML files named index.html in directories" + @echo " singlehtml to make a single large HTML file" + @echo " pickle to make pickle files" + @echo " json to make JSON files" + @echo " htmlhelp to make HTML files and a HTML help project" + @echo " qthelp to make HTML files and a qthelp project" + @echo " applehelp to make an Apple Help Book" + @echo " devhelp to make HTML files and a Devhelp project" + @echo " epub to make an epub" + @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" + @echo " latexpdf to make LaTeX files and run them through pdflatex" + @echo " latexpdfja to make LaTeX files and run them through platex/dvipdfmx" + @echo " text to make text files" + @echo " man to make manual pages" + @echo " texinfo to make Texinfo files" + @echo " info to make Texinfo files and run them through makeinfo" + @echo " gettext to make PO message catalogs" + @echo " changes to make an overview of all changed/added/deprecated items" + @echo " xml to make Docutils-native XML files" + @echo " pseudoxml to make pseudoxml-XML files for display purposes" + @echo " linkcheck to check all external links for integrity" + @echo " doctest to run all doctests embedded in the documentation (if enabled)" + @echo " coverage to run coverage check of the documentation (if enabled)" + +.PHONY: clean +clean: + rm -rf $(BUILDDIR)/* + +.PHONY: html +html: + $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html + @echo + @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." + +.PHONY: dirhtml +dirhtml: + $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml + @echo + @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." + +.PHONY: singlehtml +singlehtml: + $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml + @echo + @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." + +.PHONY: pickle +pickle: + $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle + @echo + @echo "Build finished; now you can process the pickle files." + +.PHONY: json +json: + $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json + @echo + @echo "Build finished; now you can process the JSON files." + +.PHONY: htmlhelp +htmlhelp: + $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp + @echo + @echo "Build finished; now you can run HTML Help Workshop with the" \ + ".hhp project file in $(BUILDDIR)/htmlhelp." + +.PHONY: qthelp +qthelp: + $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp + @echo + @echo "Build finished; now you can run "qcollectiongenerator" with the" \ + ".qhcp project file in $(BUILDDIR)/qthelp, like this:" + @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/openai_ros.qhcp" + @echo "To view the help file:" + @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/openai_ros.qhc" + +.PHONY: applehelp +applehelp: + $(SPHINXBUILD) -b applehelp $(ALLSPHINXOPTS) $(BUILDDIR)/applehelp + @echo + @echo "Build finished. The help book is in $(BUILDDIR)/applehelp." + @echo "N.B. You won't be able to view it unless you put it in" \ + "~/Library/Documentation/Help or install it in your application" \ + "bundle." + +.PHONY: devhelp +devhelp: + $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp + @echo + @echo "Build finished." + @echo "To view the help file:" + @echo "# mkdir -p $$HOME/.local/share/devhelp/openai_ros" + @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/openai_ros" + @echo "# devhelp" + +.PHONY: epub +epub: + $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub + @echo + @echo "Build finished. The epub file is in $(BUILDDIR)/epub." + +.PHONY: latex +latex: + $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex + @echo + @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." + @echo "Run \`make' in that directory to run these through (pdf)latex" \ + "(use \`make latexpdf' here to do that automatically)." + +.PHONY: latexpdf +latexpdf: + $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex + @echo "Running LaTeX files through pdflatex..." + $(MAKE) -C $(BUILDDIR)/latex all-pdf + @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." + +.PHONY: latexpdfja +latexpdfja: + $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex + @echo "Running LaTeX files through platex and dvipdfmx..." + $(MAKE) -C $(BUILDDIR)/latex all-pdf-ja + @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." + +.PHONY: text +text: + $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text + @echo + @echo "Build finished. The text files are in $(BUILDDIR)/text." + +.PHONY: man +man: + $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man + @echo + @echo "Build finished. The manual pages are in $(BUILDDIR)/man." + +.PHONY: texinfo +texinfo: + $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo + @echo + @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." + @echo "Run \`make' in that directory to run these through makeinfo" \ + "(use \`make info' here to do that automatically)." + +.PHONY: info +info: + $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo + @echo "Running Texinfo files through makeinfo..." + make -C $(BUILDDIR)/texinfo info + @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." + +.PHONY: gettext +gettext: + $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale + @echo + @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." + +.PHONY: changes +changes: + $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes + @echo + @echo "The overview file is in $(BUILDDIR)/changes." + +.PHONY: linkcheck +linkcheck: + $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck + @echo + @echo "Link check complete; look for any errors in the above output " \ + "or in $(BUILDDIR)/linkcheck/output.txt." + +.PHONY: doctest +doctest: + $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest + @echo "Testing of doctests in the sources finished, look at the " \ + "results in $(BUILDDIR)/doctest/output.txt." + +.PHONY: coverage +coverage: + $(SPHINXBUILD) -b coverage $(ALLSPHINXOPTS) $(BUILDDIR)/coverage + @echo "Testing of coverage in the sources finished, look at the " \ + "results in $(BUILDDIR)/coverage/python.txt." + +.PHONY: xml +xml: + $(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml + @echo + @echo "Build finished. The XML files are in $(BUILDDIR)/xml." + +.PHONY: pseudoxml +pseudoxml: + $(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml + @echo + @echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml." diff --git a/frontier_rl/openai_ros/openai_ros/doc/cartpole_environment.rst b/frontier_rl/openai_ros/openai_ros/doc/cartpole_environment.rst new file mode 100644 index 0000000..e3996a7 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/cartpole_environment.rst @@ -0,0 +1,31 @@ +CartPole Environment +==================== + +This is a 3D version of the classical **cartpole** controle system. + +.. figure:: img/cartpole_openai.png + :scale: 100 % + :alt: Single Disk Cube Image + + Cartpole in ROSDS. + + +Robot Environment +***************** + +openai_ros.robot_envs.cartpole_env module +----------------------------------------- + +.. automodule:: openai_ros.robot_envs.cartpole_env + :members: + :undoc-members: + :show-inheritance: + + +Task Environments +***************** + +.. toctree:: + :maxdepth: 4 + + openai_ros.task_envs.cartpole_stay_up diff --git a/frontier_rl/openai_ros/openai_ros/doc/conf.py b/frontier_rl/openai_ros/openai_ros/doc/conf.py new file mode 100644 index 0000000..b5c115c --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/conf.py @@ -0,0 +1,309 @@ +# -*- coding: utf-8 -*- +# +# openai_ros documentation build configuration file, created by +# sphinx-quickstart on Tue Jul 10 17:48:23 2018. +# +# This file is execfile()d with the current directory set to its +# containing dir. +# +# Note that not all possible configuration values are present in this +# autogenerated file. +# +# All configuration values have a default; values that are commented out +# serve to show the default. + +import sys +import os +import catkin_pkg.package +catkin_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +catkin_package = catkin_pkg.package.parse_package(os.path.join(catkin_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME)) +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +#sys.path.insert(0, os.path.abspath('.')) +sys.path.insert(0, os.path.abspath('../src')) +sys.path.insert(0, os.path.abspath('/home/rdaneel/catkin_ws/devel/lib/python2.7/dist-packages/')) + + +# -- General configuration ------------------------------------------------ +#autodoc_default_flags = ['members', 'undoc-members', 'private-members', 'special-members', 'inherited-members', 'show-inheritance'] +autodoc_default_flags = ['members', 'undoc-members', 'private-members', 'special-members'] + + +# If your documentation needs a minimal Sphinx version, state it here. +#needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + 'sphinx.ext.autodoc', + 'sphinx.ext.doctest', + 'sphinx.ext.intersphinx', + 'sphinx.ext.todo', + 'sphinx.ext.coverage', + 'sphinx.ext.mathjax', + 'sphinx.ext.ifconfig', + 'sphinx.ext.viewcode', +] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['.templates'] + +# The suffix(es) of source filenames. +# You can specify multiple suffix as a list of string: +# source_suffix = ['.rst', '.md'] +source_suffix = '.rst' + +# The encoding of source files. +#source_encoding = 'utf-8-sig' + +# The master toctree document. +master_doc = 'index' + +# General information about the project. +project = u'openai_ros' +copyright = u'2018, TheConstruct' +author = u'TheConstruct' + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# The short X.Y version. +# version = u'1.0' +version = catkin_package.version +# The full version, including alpha/beta/rc tags. +# release = u'1.0' +release = catkin_package.version + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +# +# This is also used if you do content translation via gettext catalogs. +# Usually you set "language" from the command line for these cases. +language = None + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +#today = '' +# Else, today_fmt is used as the format for a strftime call. +#today_fmt = '%B %d, %Y' + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +exclude_patterns = ['.build'] + +# The reST default role (used for this markup: `text`) to use for all +# documents. +#default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +#add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +#add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +#show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = 'sphinx' + +# A list of ignored prefixes for module index sorting. +#modindex_common_prefix = [] + +# If true, keep warnings as "system message" paragraphs in the built documents. +#keep_warnings = False + +# If true, `todo` and `todoList` produce output, else they produce nothing. +todo_include_todos = True + + +# -- Options for HTML output ---------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +#html_theme = 'haiku' +#html_theme = 'pyramid' +#html_theme = 'sphinxdoc' +html_theme = 'default' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +#html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +#html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +#html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +#html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +#html_logo = None + +# The name of an image file (relative to this directory) to use as a favicon of +# the docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +#html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ['.static'] + +# Add any extra paths that contain custom files (such as robots.txt or +# .htaccess) here, relative to this directory. These files are copied +# directly to the root of the documentation. +#html_extra_path = [] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +#html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +#html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +#html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +#html_additional_pages = {} + +# If false, no module index is generated. +#html_domain_indices = True + +# If false, no index is generated. +#html_use_index = True + +# If true, the index is split into individual pages for each letter. +#html_split_index = False + +# If true, links to the reST sources are added to the pages. +#html_show_sourcelink = True + +# If true, "Created using Sphinx" is shown in the HTML footer. Default is True. +#html_show_sphinx = True + +# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. +#html_show_copyright = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +#html_use_opensearch = '' + +# This is the file name suffix for HTML files (e.g. ".xhtml"). +#html_file_suffix = None + +# Language to be used for generating the HTML full-text search index. +# Sphinx supports the following languages: +# 'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja' +# 'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr' +#html_search_language = 'en' + +# A dictionary with options for the search language support, empty by default. +# Now only 'ja' uses this config value +#html_search_options = {'type': 'default'} + +# The name of a javascript file (relative to the configuration directory) that +# implements a search results scorer. If empty, the default will be used. +#html_search_scorer = 'scorer.js' + +# Output file base name for HTML help builder. +htmlhelp_basename = 'openai_rosdoc' + +# -- Options for LaTeX output --------------------------------------------- + +latex_elements = { +# The paper size ('letterpaper' or 'a4paper'). +#'papersize': 'letterpaper', + +# The font size ('10pt', '11pt' or '12pt'). +#'pointsize': '10pt', + +# Additional stuff for the LaTeX preamble. +#'preamble': '', + +# Latex figure (float) alignment +#'figure_align': 'htbp', +} + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, +# author, documentclass [howto, manual, or own class]). +latex_documents = [ + (master_doc, 'openai_ros.tex', u'openai\\_ros Documentation', + u'TheConstruct', 'manual'), +] + +# The name of an image file (relative to this directory) to place at the top of +# the title page. +#latex_logo = None + +# For "manual" documents, if this is true, then toplevel headings are parts, +# not chapters. +#latex_use_parts = False + +# If true, show page references after internal links. +#latex_show_pagerefs = False + +# If true, show URL addresses after external links. +#latex_show_urls = False + +# Documents to append as an appendix to all manuals. +#latex_appendices = [] + +# If false, no module index is generated. +#latex_domain_indices = True + + +# -- Options for manual page output --------------------------------------- + +# One entry per manual page. List of tuples +# (source start file, name, description, authors, manual section). +man_pages = [ + (master_doc, 'openai_ros', u'openai_ros Documentation', + [author], 1) +] + +# If true, show URL addresses after external links. +#man_show_urls = False + + +# -- Options for Texinfo output ------------------------------------------- + +# Grouping the document tree into Texinfo files. List of tuples +# (source start file, target name, title, author, +# dir menu entry, description, category) +texinfo_documents = [ + (master_doc, 'openai_ros', u'openai_ros Documentation', + author, 'openai_ros', 'One line description of project.', + 'Miscellaneous'), +] + +# Documents to append as an appendix to all manuals. +#texinfo_appendices = [] + +# If false, no module index is generated. +#texinfo_domain_indices = True + +# How to display URL addresses: 'footnote', 'no', or 'inline'. +#texinfo_show_urls = 'footnote' + +# If true, do not generate a @detailmenu in the "Top" node's menu. +#texinfo_no_detailmenu = False + + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = {'https://docs.python.org/': None} diff --git a/frontier_rl/openai_ros/openai_ros/doc/cube_environment.rst b/frontier_rl/openai_ros/openai_ros/doc/cube_environment.rst new file mode 100644 index 0000000..7200c24 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/cube_environment.rst @@ -0,0 +1,31 @@ +Cube Single Disk Environment +============================ + +The **SingleDisk Cube Environment** is based on the **Cubi Robot** created at **ETH Zurich**. +Its a simpler version with only one inertial disk instead of the **three** of the original. + +.. figure:: img/moving_cube_image.png + :scale: 100 % + :alt: Single Disk Cube Image + + Single Disk Cube in ROSDS. + +Robot Environment +***************** + +openai_ros.robot_envs.cube_single_disk_env module +------------------------------------------------- + +.. automodule:: openai_ros.robot_envs.cube_single_disk_env + :members: + :undoc-members: + :show-inheritance: + + +Task Environments +***************** + +.. toctree:: + :maxdepth: 4 + + openai_ros.task_envs.moving_cube diff --git a/frontier_rl/openai_ros/openai_ros/doc/hopper_environment.rst b/frontier_rl/openai_ros/openai_ros/doc/hopper_environment.rst new file mode 100644 index 0000000..b2a16b3 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/hopper_environment.rst @@ -0,0 +1,37 @@ +Hopper Environment +================== + +Its a simulation of a monopod robot. The original 3D model was created by ETH Zurich, +HopperPage_ + +.. _HopperPage: https://github.com/ethz-adrl/towr + +The simulation was created by TheConstructSim. Its the perfect system to test locomotion +algorithms and AI learning for gait research. + +.. figure:: img/hopper.png + :scale: 100 % + :alt: Hopper Red Body Jumping + + Hopper Jumps in ROSDS + + +Robot Environment +***************** + +openai_ros.robot_envs.hopper_env module +--------------------------------------- + +.. automodule:: openai_ros.robot_envs.hopper_env + :members: + :undoc-members: + :show-inheritance: + + +Task Environments +***************** + +.. toctree:: + :maxdepth: 4 + + openai_ros.task_envs.hopper diff --git a/frontier_rl/openai_ros/openai_ros/doc/husarion_environment.rst b/frontier_rl/openai_ros/openai_ros/doc/husarion_environment.rst new file mode 100644 index 0000000..7e82211 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/husarion_environment.rst @@ -0,0 +1,42 @@ +Husarion Environment +==================== + +ROSbot is an autonomous, open source robot platform. +It can be used as a learning platform for Robot Operating System as well as a base for a variety of robotic +applications like inspection robots, custom service robots and others. +Here you have the link to the real robot: RosBotPage_ + +.. _RosBotPage: https://husarion.com/#rosbot + +.. figure:: img/rosbot.jpg + :scale: 10 % + :alt: Real RosBot from Husarion + +The simulation is meant to be a basic wheeled robot for developing navigation and obstacle avoidance AI training. + + +.. figure:: img/rosbot_husarion.png + :scale: 100 % + :alt: Simulated RosBot from Husarion + + + +Robot Environment +***************** + +openai_ros.robot_envs.husarion_env module +----------------------------------------- + +.. automodule:: openai_ros.robot_envs.husarion_env + :members: + :undoc-members: + :show-inheritance: + + +Task Environments +***************** + +.. toctree:: + :maxdepth: 4 + + openai_ros.task_envs.husarion diff --git a/frontier_rl/openai_ros/openai_ros/doc/img/cartpole_openai.png b/frontier_rl/openai_ros/openai_ros/doc/img/cartpole_openai.png new file mode 100644 index 0000000..e408953 Binary files /dev/null and b/frontier_rl/openai_ros/openai_ros/doc/img/cartpole_openai.png differ diff --git a/frontier_rl/openai_ros/openai_ros/doc/img/hopper.png b/frontier_rl/openai_ros/openai_ros/doc/img/hopper.png new file mode 100644 index 0000000..12dd0e0 Binary files /dev/null and b/frontier_rl/openai_ros/openai_ros/doc/img/hopper.png differ diff --git a/frontier_rl/openai_ros/openai_ros/doc/img/moving_cube_image.png b/frontier_rl/openai_ros/openai_ros/doc/img/moving_cube_image.png new file mode 100644 index 0000000..bd5c837 Binary files /dev/null and b/frontier_rl/openai_ros/openai_ros/doc/img/moving_cube_image.png differ diff --git a/frontier_rl/openai_ros/openai_ros/doc/img/parrotdrone.png b/frontier_rl/openai_ros/openai_ros/doc/img/parrotdrone.png new file mode 100644 index 0000000..fb2f6b6 Binary files /dev/null and b/frontier_rl/openai_ros/openai_ros/doc/img/parrotdrone.png differ diff --git a/frontier_rl/openai_ros/openai_ros/doc/img/rosbot.jpg b/frontier_rl/openai_ros/openai_ros/doc/img/rosbot.jpg new file mode 100644 index 0000000..8d30305 Binary files /dev/null and b/frontier_rl/openai_ros/openai_ros/doc/img/rosbot.jpg differ diff --git a/frontier_rl/openai_ros/openai_ros/doc/img/rosbot_husarion.png b/frontier_rl/openai_ros/openai_ros/doc/img/rosbot_husarion.png new file mode 100644 index 0000000..1c00cd2 Binary files /dev/null and b/frontier_rl/openai_ros/openai_ros/doc/img/rosbot_husarion.png differ diff --git a/frontier_rl/openai_ros/openai_ros/doc/img/sawyer_robot.png b/frontier_rl/openai_ros/openai_ros/doc/img/sawyer_robot.png new file mode 100644 index 0000000..fb62186 Binary files /dev/null and b/frontier_rl/openai_ros/openai_ros/doc/img/sawyer_robot.png differ diff --git a/frontier_rl/openai_ros/openai_ros/doc/img/sawyer_sim.png b/frontier_rl/openai_ros/openai_ros/doc/img/sawyer_sim.png new file mode 100644 index 0000000..daa8e1e Binary files /dev/null and b/frontier_rl/openai_ros/openai_ros/doc/img/sawyer_sim.png differ diff --git a/frontier_rl/openai_ros/openai_ros/doc/img/shadow_tc.png b/frontier_rl/openai_ros/openai_ros/doc/img/shadow_tc.png new file mode 100644 index 0000000..edc15e0 Binary files /dev/null and b/frontier_rl/openai_ros/openai_ros/doc/img/shadow_tc.png differ diff --git a/frontier_rl/openai_ros/openai_ros/doc/img/summit_xl.png b/frontier_rl/openai_ros/openai_ros/doc/img/summit_xl.png new file mode 100644 index 0000000..9dcbdc3 Binary files /dev/null and b/frontier_rl/openai_ros/openai_ros/doc/img/summit_xl.png differ diff --git a/frontier_rl/openai_ros/openai_ros/doc/img/summit_xl_sim.png b/frontier_rl/openai_ros/openai_ros/doc/img/summit_xl_sim.png new file mode 100644 index 0000000..1644b90 Binary files /dev/null and b/frontier_rl/openai_ros/openai_ros/doc/img/summit_xl_sim.png differ diff --git a/frontier_rl/openai_ros/openai_ros/doc/img/turtlebot2.png b/frontier_rl/openai_ros/openai_ros/doc/img/turtlebot2.png new file mode 100644 index 0000000..58323b6 Binary files /dev/null and b/frontier_rl/openai_ros/openai_ros/doc/img/turtlebot2.png differ diff --git a/frontier_rl/openai_ros/openai_ros/doc/img/turtlebot3.png b/frontier_rl/openai_ros/openai_ros/doc/img/turtlebot3.png new file mode 100644 index 0000000..61f5583 Binary files /dev/null and b/frontier_rl/openai_ros/openai_ros/doc/img/turtlebot3.png differ diff --git a/frontier_rl/openai_ros/openai_ros/doc/img/turtlebot3_sim.png b/frontier_rl/openai_ros/openai_ros/doc/img/turtlebot3_sim.png new file mode 100644 index 0000000..d99faa9 Binary files /dev/null and b/frontier_rl/openai_ros/openai_ros/doc/img/turtlebot3_sim.png differ diff --git a/frontier_rl/openai_ros/openai_ros/doc/img/wamv.png b/frontier_rl/openai_ros/openai_ros/doc/img/wamv.png new file mode 100644 index 0000000..582c0ab Binary files /dev/null and b/frontier_rl/openai_ros/openai_ros/doc/img/wamv.png differ diff --git a/frontier_rl/openai_ros/openai_ros/doc/index.rst b/frontier_rl/openai_ros/openai_ros/doc/index.rst new file mode 100644 index 0000000..08d25a1 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/index.rst @@ -0,0 +1,30 @@ +.. openai_ros documentation master file, created by + sphinx-quickstart on Tue Jul 10 17:48:23 2018. + You can adapt this file completely to your liking, but it should at least + contain the root `toctree` directive. + +Welcome to openai_ros's documentation! +====================================== + +This ROS package http://wiki.ros.org/openai_ros +Its a structure to make it easy to use OpenAI with ROS and Gazebo. +You will find environments for most of the common robots used in +ROS so that you only have to code the AI part, and don't worry on the +connection to the Physical/Simulated robot. + + +Contents: + +.. toctree:: + :maxdepth: 2 + + openai_ros_core + robot_environments + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`modindex` +* :ref:`search` + diff --git a/frontier_rl/openai_ros/openai_ros/doc/manifest.yaml b/frontier_rl/openai_ros/openai_ros/doc/manifest.yaml new file mode 100644 index 0000000..5bdab42 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/manifest.yaml @@ -0,0 +1,23 @@ +actions: [] +authors: '' +brief: '' +bugtracker: '' +depends: +- catkin +- gazebo_msgs +- controller_manager_msgs +- message_runtime +- std_msgs +- python3-catkin-pkg +- rospy +- message_generation +- geometry_msgs +description: The openai_ros package +license: TODO +maintainers: rdaneel +msgs: +- RLExperimentInfo +package_type: package +repo_url: '' +srvs: [] +url: '' diff --git a/frontier_rl/openai_ros/openai_ros/doc/modules.rst b/frontier_rl/openai_ros/openai_ros/doc/modules.rst new file mode 100644 index 0000000..77824b7 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/modules.rst @@ -0,0 +1,7 @@ +OpenAi Code +=========== + +.. toctree:: + :maxdepth: 4 + + openai_ros diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.cartpole_stay_up.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.cartpole_stay_up.rst new file mode 100644 index 0000000..162c502 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.cartpole_stay_up.rst @@ -0,0 +1,22 @@ +openai_ros.cartpole_stay_up package +=================================== + +Submodules +---------- + +openai_ros.cartpole_stay_up.stay_up module +------------------------------------------ + +.. automodule:: openai_ros.cartpole_stay_up.stay_up + :members: + :undoc-members: + :show-inheritance: + + +Module contents +--------------- + +.. automodule:: openai_ros.cartpole_stay_up + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.moving_cube.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.moving_cube.rst new file mode 100644 index 0000000..e8b6578 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.moving_cube.rst @@ -0,0 +1,22 @@ +openai_ros.moving_cube package +============================== + +Submodules +---------- + +openai_ros.moving_cube.one_disk_walk module +------------------------------------------- + +.. automodule:: openai_ros.moving_cube.one_disk_walk + :members: + :undoc-members: + :show-inheritance: + + +Module contents +--------------- + +.. automodule:: openai_ros.moving_cube + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.robot_envs.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.robot_envs.rst new file mode 100644 index 0000000..d50d73f --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.robot_envs.rst @@ -0,0 +1,110 @@ +openai_ros.robot_envs package +============================= + +Submodules +---------- + +openai_ros.robot_envs.cartpole_env module +----------------------------------------- + +.. automodule:: openai_ros.robot_envs.cartpole_env + :members: + :undoc-members: + :show-inheritance: + +openai_ros.robot_envs.cube_rl_utils module +------------------------------------------ + +.. automodule:: openai_ros.robot_envs.cube_rl_utils + :members: + :undoc-members: + :show-inheritance: + +openai_ros.robot_envs.cube_single_disk_env module +------------------------------------------------- + +.. automodule:: openai_ros.robot_envs.cube_single_disk_env + :members: + :undoc-members: + :show-inheritance: + +openai_ros.robot_envs.hopper_env module +--------------------------------------- + +.. automodule:: openai_ros.robot_envs.hopper_env + :members: + :undoc-members: + :show-inheritance: + +openai_ros.robot_envs.husarion_env module +----------------------------------------- + +.. automodule:: openai_ros.robot_envs.husarion_env + :members: + :undoc-members: + :show-inheritance: + +openai_ros.robot_envs.parrotdrone_env module +-------------------------------------------- + +.. automodule:: openai_ros.robot_envs.parrotdrone_env + :members: + :undoc-members: + :show-inheritance: + +openai_ros.robot_envs.sawyer_env module +--------------------------------------- + +.. automodule:: openai_ros.robot_envs.sawyer_env + :members: + :undoc-members: + :show-inheritance: + +openai_ros.robot_envs.shadow_tc_env module +------------------------------------------ + +.. automodule:: openai_ros.robot_envs.shadow_tc_env + :members: + :undoc-members: + :show-inheritance: + +openai_ros.robot_envs.sumitxl_env module +---------------------------------------- + +.. automodule:: openai_ros.robot_envs.sumitxl_env + :members: + :undoc-members: + :show-inheritance: + +openai_ros.robot_envs.turtlebot2_env module +------------------------------------------- + +.. automodule:: openai_ros.robot_envs.turtlebot2_env + :members: + :undoc-members: + :show-inheritance: + +openai_ros.robot_envs.turtlebot3_env module +------------------------------------------- + +.. automodule:: openai_ros.robot_envs.turtlebot3_env + :members: + :undoc-members: + :show-inheritance: + +openai_ros.robot_envs.wamv_env module +------------------------------------- + +.. automodule:: openai_ros.robot_envs.wamv_env + :members: + :undoc-members: + :show-inheritance: + + +Module contents +--------------- + +.. automodule:: openai_ros.robot_envs + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.rst new file mode 100644 index 0000000..37968c0 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.rst @@ -0,0 +1,70 @@ +openai_ros package +================== + +Subpackages +----------- + +.. toctree:: + + openai_ros.cartpole_stay_up + openai_ros.moving_cube + +Submodules +---------- + +openai_ros.cartpole_env module +------------------------------ + +.. automodule:: openai_ros.cartpole_env + :members: + :undoc-members: + :show-inheritance: + +openai_ros.controllers_connection module +---------------------------------------- + +.. automodule:: openai_ros.controllers_connection + :members: + :undoc-members: + :show-inheritance: + +openai_ros.cube_rl_utils module +------------------------------- + +.. automodule:: openai_ros.cube_rl_utils + :members: + :undoc-members: + :show-inheritance: + +openai_ros.cube_single_disk_env module +-------------------------------------- + +.. automodule:: openai_ros.cube_single_disk_env + :members: + :undoc-members: + :show-inheritance: + +openai_ros.gazebo_connection module +----------------------------------- + +.. automodule:: openai_ros.gazebo_connection + :members: + :undoc-members: + :show-inheritance: + +openai_ros.robot_gazebo_env module +---------------------------------- + +.. automodule:: openai_ros.robot_gazebo_env + :members: + :undoc-members: + :show-inheritance: + + +Module contents +--------------- + +.. automodule:: openai_ros + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.cartpole_stay_up.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.cartpole_stay_up.rst new file mode 100644 index 0000000..c2ebf64 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.cartpole_stay_up.rst @@ -0,0 +1,22 @@ +openai_ros.task_envs.cartpole_stay_up package +============================================= + +Submodules +---------- + +openai_ros.task_envs.cartpole_stay_up.stay_up module +---------------------------------------------------- + +.. automodule:: openai_ros.task_envs.cartpole_stay_up.stay_up + :members: + :undoc-members: + :show-inheritance: + + +Module contents +--------------- + +.. automodule:: openai_ros.task_envs.cartpole_stay_up + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.hopper.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.hopper.rst new file mode 100644 index 0000000..74a6207 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.hopper.rst @@ -0,0 +1,22 @@ +openai_ros.task_envs.hopper package +=================================== + +Submodules +---------- + +openai_ros.task_envs.hopper.hopper_stay_up module +------------------------------------------------- + +.. automodule:: openai_ros.task_envs.hopper.hopper_stay_up + :members: + :undoc-members: + :show-inheritance: + + +Module contents +--------------- + +.. automodule:: openai_ros.task_envs.hopper + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.husarion.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.husarion.rst new file mode 100644 index 0000000..7ec8ab9 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.husarion.rst @@ -0,0 +1,22 @@ +openai_ros.task_envs.husarion package +===================================== + +Submodules +---------- + +openai_ros.task_envs.husarion.husarion_get_to_position_turtlebot_playground module +---------------------------------------------------------------------------------- + +.. automodule:: openai_ros.task_envs.husarion.husarion_get_to_position_turtlebot_playground + :members: + :undoc-members: + :show-inheritance: + + +Module contents +--------------- + +.. automodule:: openai_ros.task_envs.husarion + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.moving_cube.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.moving_cube.rst new file mode 100644 index 0000000..5b5ce99 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.moving_cube.rst @@ -0,0 +1,22 @@ +openai_ros.task_envs.moving_cube package +======================================== + +Submodules +---------- + +openai_ros.task_envs.moving_cube.one_disk_walk module +----------------------------------------------------- + +.. automodule:: openai_ros.task_envs.moving_cube.one_disk_walk + :members: + :undoc-members: + :show-inheritance: + + +Module contents +--------------- + +.. automodule:: openai_ros.task_envs.moving_cube + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.parrotdrone.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.parrotdrone.rst new file mode 100644 index 0000000..a455dff --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.parrotdrone.rst @@ -0,0 +1,22 @@ +openai_ros.task_envs.parrotdrone package +======================================== + +Submodules +---------- + +openai_ros.task_envs.parrotdrone.parrotdrone_goto module +-------------------------------------------------------- + +.. automodule:: openai_ros.task_envs.parrotdrone.parrotdrone_goto + :members: + :undoc-members: + :show-inheritance: + + +Module contents +--------------- + +.. automodule:: openai_ros.task_envs.parrotdrone + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.rst new file mode 100644 index 0000000..e59c8f7 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.rst @@ -0,0 +1,27 @@ +openai_ros.task_envs package +============================ + +Subpackages +----------- + +.. toctree:: + + openai_ros.task_envs.cartpole_stay_up + openai_ros.task_envs.hopper + openai_ros.task_envs.husarion + openai_ros.task_envs.moving_cube + openai_ros.task_envs.parrotdrone + openai_ros.task_envs.sawyer + openai_ros.task_envs.shadow_tc + openai_ros.task_envs.sumit_xl + openai_ros.task_envs.turtlebot2 + openai_ros.task_envs.turtlebot3 + openai_ros.task_envs.wamv + +Module contents +--------------- + +.. automodule:: openai_ros.task_envs + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.sawyer.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.sawyer.rst new file mode 100644 index 0000000..1aaa170 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.sawyer.rst @@ -0,0 +1,22 @@ +openai_ros.task_envs.sawyer package +=================================== + +Submodules +---------- + +openai_ros.task_envs.sawyer.learn_to_touch_cube module +------------------------------------------------------ + +.. automodule:: openai_ros.task_envs.sawyer.learn_to_touch_cube + :members: + :undoc-members: + :show-inheritance: + + +Module contents +--------------- + +.. automodule:: openai_ros.task_envs.sawyer + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.shadow_tc.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.shadow_tc.rst new file mode 100644 index 0000000..8c976a7 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.shadow_tc.rst @@ -0,0 +1,22 @@ +openai_ros.task_envs.shadow_tc package +====================================== + +Submodules +---------- + +openai_ros.task_envs.shadow_tc.learn_to_pick_ball module +-------------------------------------------------------- + +.. automodule:: openai_ros.task_envs.shadow_tc.learn_to_pick_ball + :members: + :undoc-members: + :show-inheritance: + + +Module contents +--------------- + +.. automodule:: openai_ros.task_envs.shadow_tc + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.sumit_xl.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.sumit_xl.rst new file mode 100644 index 0000000..182f81c --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.sumit_xl.rst @@ -0,0 +1,22 @@ +openai_ros.task_envs.sumit_xl package +===================================== + +Submodules +---------- + +openai_ros.task_envs.sumit_xl.sumit_xl_room module +-------------------------------------------------- + +.. automodule:: openai_ros.task_envs.sumit_xl.sumit_xl_room + :members: + :undoc-members: + :show-inheritance: + + +Module contents +--------------- + +.. automodule:: openai_ros.task_envs.sumit_xl + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.turtlebot2.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.turtlebot2.rst new file mode 100644 index 0000000..afbf58c --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.turtlebot2.rst @@ -0,0 +1,22 @@ +openai_ros.task_envs.turtlebot2 package +======================================= + +Submodules +---------- + +openai_ros.task_envs.turtlebot2.turtlebot2_maze module +------------------------------------------------------ + +.. automodule:: openai_ros.task_envs.turtlebot2.turtlebot2_maze + :members: + :undoc-members: + :show-inheritance: + + +Module contents +--------------- + +.. automodule:: openai_ros.task_envs.turtlebot2 + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.turtlebot3.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.turtlebot3.rst new file mode 100644 index 0000000..c6365df --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.turtlebot3.rst @@ -0,0 +1,22 @@ +openai_ros.task_envs.turtlebot3 package +======================================= + +Submodules +---------- + +openai_ros.task_envs.turtlebot3.turtlebot3_world module +------------------------------------------------------- + +.. automodule:: openai_ros.task_envs.turtlebot3.turtlebot3_world + :members: + :undoc-members: + :show-inheritance: + + +Module contents +--------------- + +.. automodule:: openai_ros.task_envs.turtlebot3 + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.wamv.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.wamv.rst new file mode 100644 index 0000000..e20c9ac --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros.task_envs.wamv.rst @@ -0,0 +1,22 @@ +openai_ros.task_envs.wamv package +================================= + +Submodules +---------- + +openai_ros.task_envs.wamv.wamv_nav_twosets_buoys module +------------------------------------------------------- + +.. automodule:: openai_ros.task_envs.wamv.wamv_nav_twosets_buoys + :members: + :undoc-members: + :show-inheritance: + + +Module contents +--------------- + +.. automodule:: openai_ros.task_envs.wamv + :members: + :undoc-members: + :show-inheritance: diff --git a/frontier_rl/openai_ros/openai_ros/doc/openai_ros_core.rst b/frontier_rl/openai_ros/openai_ros/doc/openai_ros_core.rst new file mode 100644 index 0000000..f561f1a --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/openai_ros_core.rst @@ -0,0 +1,29 @@ +OpenAI ROS Core +=============== + +This is the heart of OpenAI ROS. Here you can finc the classes that tie ROS, Gazebo and OpenAi together. + + +openai_ros.robot_gazebo_env module +---------------------------------- + +.. automodule:: openai_ros.robot_gazebo_env + :members: + :undoc-members: + :show-inheritance: + +openai_ros.gazebo_connection module +----------------------------------- + +.. automodule:: openai_ros.gazebo_connection + :members: + :undoc-members: + :show-inheritance: + +openai_ros.controllers_connection module +---------------------------------------- + +.. automodule:: openai_ros.controllers_connection + :members: + :undoc-members: + :show-inheritance: \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/doc/parrotdrone_environment.rst b/frontier_rl/openai_ros/openai_ros/doc/parrotdrone_environment.rst new file mode 100644 index 0000000..6b06e7c --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/parrotdrone_environment.rst @@ -0,0 +1,30 @@ +Parrot Drone Environment +======================== + +Drone simulation to test all your AI algorithms with a flying robot with double cameras and sonar. + +.. figure:: img/parrotdrone.png + :scale: 100 % + :alt: Parrot Drone Simulation + + + +Robot Environment +***************** + +openai_ros.robot_envs.parrotdrone_env module +-------------------------------------------- + +.. automodule:: openai_ros.robot_envs.parrotdrone_env + :members: + :undoc-members: + :show-inheritance: + + +Task Environments +***************** + +.. toctree:: + :maxdepth: 4 + + openai_ros.task_envs.parrotdrone diff --git a/frontier_rl/openai_ros/openai_ros/doc/robot_environments.rst b/frontier_rl/openai_ros/openai_ros/doc/robot_environments.rst new file mode 100644 index 0000000..b3c4da0 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/robot_environments.rst @@ -0,0 +1,20 @@ +Robot Environments +================== + +Here you can find all the **Robot Environments** that we currently support and you can +use with your AI learning algorithms. + +.. toctree:: + :maxdepth: 4 + + cartpole_environment + cube_environment + hopper_environment + husarion_environment + parrotdrone_environment + sawyer_environment + shadow_tc_environment + summitxl_environment + turtlebot2_environment + turtlebot3_environment + wamv_environment \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/doc/sawyer_environment.rst b/frontier_rl/openai_ros/openai_ros/doc/sawyer_environment.rst new file mode 100644 index 0000000..9a3c4e4 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/sawyer_environment.rst @@ -0,0 +1,42 @@ +Sawyer Environment +================== + +Sawyer is the new robot from **Rethink Robotics**. Its a simplified version of the **Baxter Robot**. +Here is the official page for more details: SawyerPage_ + +.. _SawyerPage: https://www.rethinkrobotics.com/sawyer/ + + +.. figure:: img/sawyer_robot.png + :scale: 100 % + :alt: Sawyer Robot + + +This simulation uses all the SDK systems that the real robot does , so anything you develop for +the simulation should work seamlessly with the real robot. + +.. figure:: img/sawyer_sim.png + :scale: 100 % + :alt: Simulated Sawyer + + + +Robot Environment +***************** + +openai_ros.robot_envs.sawyer_env module +--------------------------------------- + +.. automodule:: openai_ros.robot_envs.sawyer_env + :members: + :undoc-members: + :show-inheritance: + + +Task Environments +***************** + +.. toctree:: + :maxdepth: 4 + + openai_ros.task_envs.sawyer diff --git a/frontier_rl/openai_ros/openai_ros/doc/shadow_tc_environment.rst b/frontier_rl/openai_ros/openai_ros/doc/shadow_tc_environment.rst new file mode 100644 index 0000000..9f33f27 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/shadow_tc_environment.rst @@ -0,0 +1,34 @@ +Shadow TC Environment +===================== + +This simulation was created by Ugo_Cupcic_ and ShadowHand_. It uses apart from ShadowsHand systems, +a UR5_ robot arm. + +.. _Ugo_Cupcic: http://ugocupcic.com/ +.. _ShadowHand: https://www.shadowrobot.com/ +.. _UR5 : https://www.universal-robots.com/products/ur5-robot/ + +.. figure:: img/shadow_tc.png + :scale: 100 % + :alt: UR5 with Shadow Claw + + +Robot Environment +***************** + +openai_ros.robot_envs.shadow_tc_env module +------------------------------------------ + +.. automodule:: openai_ros.robot_envs.shadow_tc_env + :members: + :undoc-members: + :show-inheritance: + + +Task Environments +***************** + +.. toctree:: + :maxdepth: 4 + + openai_ros.task_envs.shadow_tc diff --git a/frontier_rl/openai_ros/openai_ros/doc/summitxl_environment.rst b/frontier_rl/openai_ros/openai_ros/doc/summitxl_environment.rst new file mode 100644 index 0000000..e37181b --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/summitxl_environment.rst @@ -0,0 +1,42 @@ +Summit XL Environment +===================== + +Summit_XL_ was created by the great Robotnik_ company. The strong mechanical structure allows to carry much heavier +loads and used for research, surveillance, remote monitoring and military applications. + + +.. _Summit_XL: https://www.robotnik.eu/mobile-robots/summit-xl/ +.. _Robotnik: https://www.robotnik.eu/ + +.. figure:: img/summit_xl.png + :scale: 100 % + :alt: Real Summit XL + + +The simulation is inside a more office like environment. + +.. figure:: img/summit_xl_sim.png + :scale: 100 % + :alt: Simulated Summit XL + + + +Robot Environment +***************** + +openai_ros.robot_envs.sumitxl_env module +---------------------------------------- + +.. automodule:: openai_ros.robot_envs.sumitxl_env + :members: + :undoc-members: + :show-inheritance: + + +Task Environments +***************** + +.. toctree:: + :maxdepth: 4 + + openai_ros.task_envs.sumit_xl diff --git a/frontier_rl/openai_ros/openai_ros/doc/turtlebot2_environment.rst b/frontier_rl/openai_ros/openai_ros/doc/turtlebot2_environment.rst new file mode 100644 index 0000000..f58f0a2 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/turtlebot2_environment.rst @@ -0,0 +1,33 @@ +Turtlebot2 Environment +====================== + +Turtlebot2_ is a classic in ROS robot platforms. In this case, you will find various simulations +with it inside. In ROSDS the default launch one is with an empty worlds and a wall. But you have also +available a world with a maze inside. + +.. _Turtlebot2: https://www.turtlebot.com/turtlebot2/ + +.. figure:: img/turtlebot2.png + :scale: 100 % + :alt: Real RosBot from Husarion + + +Robot Environment +***************** + +openai_ros.robot_envs.turtlebot2_env module +------------------------------------------- + +.. automodule:: openai_ros.robot_envs.turtlebot2_env + :members: + :undoc-members: + :show-inheritance: + + +Task Environments +***************** + +.. toctree:: + :maxdepth: 4 + + openai_ros.task_envs.turtlebot2 diff --git a/frontier_rl/openai_ros/openai_ros/doc/turtlebot3_environment.rst b/frontier_rl/openai_ros/openai_ros/doc/turtlebot3_environment.rst new file mode 100644 index 0000000..2c0a031 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/turtlebot3_environment.rst @@ -0,0 +1,36 @@ +Turtlebot3 Environment +====================== + +Turtlebot3_ is the new version of the classic Turtlebot3 created by ROBOTIS_. It has configurable systems. + +.. _Turtlebot3: http://www.robotis.us/turtlebot-3/ +.. _ROBOTIS: http://www.robotis.us/ + +.. figure:: img/turtlebot3.png + :scale: 100 % + :alt: Real RosBot from Husarion + +.. figure:: img/turtlebot3_sim.png + :scale: 100 % + :alt: Real RosBot from Husarion + + +Robot Environment +***************** + +openai_ros.robot_envs.turtlebot3_env module +------------------------------------------- + +.. automodule:: openai_ros.robot_envs.turtlebot3_env + :members: + :undoc-members: + :show-inheritance: + + +Task Environments +***************** + +.. toctree:: + :maxdepth: 4 + + openai_ros.task_envs.turtlebot3 diff --git a/frontier_rl/openai_ros/openai_ros/doc/wamv_environment.rst b/frontier_rl/openai_ros/openai_ros/doc/wamv_environment.rst new file mode 100644 index 0000000..42d8eea --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/doc/wamv_environment.rst @@ -0,0 +1,36 @@ +Wam-V Environment +================= + +Wam-V_ is the robot used in the RobotX_Maritime_Challenge_. Its a water vehicle that has to navigate autonomously. + +.. _Wam-V: http://www.wam-v.com/robotx/ +.. _RobotX_Maritime_Challenge: https://www.robotx.org/ + + + +.. figure:: img/wamv.png + :scale: 100 % + :alt: Wam-V Simulation + + + + +Robot Environment +***************** + +openai_ros.robot_envs.wamv_env module +------------------------------------- + +.. automodule:: openai_ros.robot_envs.wamv_env + :members: + :undoc-members: + :show-inheritance: + + +Task Environments +***************** + +.. toctree:: + :maxdepth: 4 + + openai_ros.task_envs.wamv diff --git a/frontier_rl/openai_ros/openai_ros/msg/RLExperimentInfo.msg b/frontier_rl/openai_ros/openai_ros/msg/RLExperimentInfo.msg new file mode 100644 index 0000000..a3fe302 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/msg/RLExperimentInfo.msg @@ -0,0 +1,2 @@ +int32 episode_number +float32 episode_reward diff --git a/frontier_rl/openai_ros/openai_ros/package.xml b/frontier_rl/openai_ros/openai_ros/package.xml new file mode 100644 index 0000000..c3fbd2d --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/package.xml @@ -0,0 +1,47 @@ + + + openai_ros + 1.0.0 + The openai_ros package + rdaneel + TODO + + catkin + + rospy + std_msgs + + python3-catkin-pkg + message_generation + + rospy + + rospy + message_runtime + + gazebo_msgs + std_msgs + geometry_msgs + controller_manager_msgs + + + + + + + + + diff --git a/frontier_rl/openai_ros/openai_ros/rosdoc.yaml b/frontier_rl/openai_ros/openai_ros/rosdoc.yaml new file mode 100644 index 0000000..e65bdd9 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/rosdoc.yaml @@ -0,0 +1,2 @@ +- builder: sphinx + sphinx_root_dir: doc \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/setup.py b/frontier_rl/openai_ros/openai_ros/setup.py new file mode 100644 index 0000000..a8853e4 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/setup.py @@ -0,0 +1,12 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['openai_ros'], + package_dir={'': 'src'}, +) + +setup(**setup_args) \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/controllers_connection.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/controllers_connection.py new file mode 100644 index 0000000..38af504 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/controllers_connection.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python + +import rospy +import time +from controller_manager_msgs.srv import SwitchController, SwitchControllerRequest, SwitchControllerResponse + +class ControllersConnection(): + + def __init__(self, namespace, controllers_list): + + rospy.logwarn("Start Init ControllersConnection") + self.controllers_list = controllers_list + self.switch_service_name = '/'+namespace+'/controller_manager/switch_controller' + self.switch_service = rospy.ServiceProxy(self.switch_service_name, SwitchController) + rospy.logwarn("END Init ControllersConnection") + + def switch_controllers(self, controllers_on, controllers_off, strictness=1): + """ + Give the controllers you want to switch on or off. + :param controllers_on: ["name_controler_1", "name_controller2",...,"name_controller_n"] + :param controllers_off: ["name_controler_1", "name_controller2",...,"name_controller_n"] + :return: + """ + rospy.wait_for_service(self.switch_service_name) + + try: + switch_request_object = SwitchControllerRequest() + switch_request_object.start_controllers = controllers_on + switch_request_object.start_controllers = controllers_off + switch_request_object.strictness = strictness + + switch_result = self.switch_service(switch_request_object) + """ + [controller_manager_msgs/SwitchController] + int32 BEST_EFFORT=1 + int32 STRICT=2 + string[] start_controllers + string[] stop_controllers + int32 strictness + --- + bool ok + """ + rospy.logdebug("Switch Result==>"+str(switch_result.ok)) + + return switch_result.ok + + except rospy.ServiceException as e: + print (self.switch_service_name+" service call failed") + + return None + + def reset_controllers(self): + """ + We turn on and off the given controllers + :param controllers_reset: ["name_controler_1", "name_controller2",...,"name_controller_n"] + :return: + """ + reset_result = False + + result_off_ok = self.switch_controllers(controllers_on = [], + controllers_off = self.controllers_list) + + rospy.logdebug("Deactivated Controlers") + + if result_off_ok: + rospy.logdebug("Activating Controlers") + result_on_ok = self.switch_controllers(controllers_on=self.controllers_list, + controllers_off=[]) + if result_on_ok: + rospy.logdebug("Controllers Reseted==>"+str(self.controllers_list)) + reset_result = True + else: + rospy.logdebug("result_on_ok==>" + str(result_on_ok)) + else: + rospy.logdebug("result_off_ok==>" + str(result_off_ok)) + + return reset_result + + def update_controllers_list(self, new_controllers_list): + + self.controllers_list = new_controllers_list \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/gazebo_connection.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/gazebo_connection.py new file mode 100644 index 0000000..0dcc355 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/gazebo_connection.py @@ -0,0 +1,168 @@ +#!/usr/bin/env python + +import rospy +from std_srvs.srv import Empty +from gazebo_msgs.msg import ODEPhysics +from gazebo_msgs.srv import SetPhysicsProperties, SetPhysicsPropertiesRequest +from std_msgs.msg import Float64 +from geometry_msgs.msg import Vector3 + +class GazeboConnection(): + + def __init__(self, start_init_physics_parameters, reset_world_or_sim, max_retry = 20): + + self._max_retry = max_retry + self.unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) + self.pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty) + self.reset_simulation_proxy = rospy.ServiceProxy('/gazebo/reset_simulation', Empty) + self.reset_world_proxy = rospy.ServiceProxy('/gazebo/reset_world', Empty) + + # Setup the Gravity Controle system + service_name = '/gazebo/set_physics_properties' + rospy.logdebug("Waiting for service " + str(service_name)) + rospy.wait_for_service(service_name) + rospy.logdebug("Service Found " + str(service_name)) + + self.set_physics = rospy.ServiceProxy(service_name, SetPhysicsProperties) + self.start_init_physics_parameters = start_init_physics_parameters + self.reset_world_or_sim = reset_world_or_sim + self.init_values() + # We always pause the simulation, important for legged robots learning + self.pauseSim() + + def pauseSim(self): + rospy.logdebug("PAUSING service found...") + paused_done = False + counter = 0 + while not paused_done and not rospy.is_shutdown(): + if counter < self._max_retry: + try: + rospy.logdebug("PAUSING service calling...") + self.pause() + paused_done = True + rospy.logdebug("PAUSING service calling...DONE") + except rospy.ServiceException as e: + counter += 1 + rospy.logerr("/gazebo/pause_physics service call failed") + else: + error_message = "Maximum retries done"+str(self._max_retry)+", please check Gazebo pause service" + rospy.logerr(error_message) + assert False, error_message + + rospy.logdebug("PAUSING FINISH") + + def unpauseSim(self): + rospy.logdebug("UNPAUSING service found...") + unpaused_done = False + counter = 0 + while not unpaused_done and not rospy.is_shutdown(): + if counter < self._max_retry: + try: + rospy.logdebug("UNPAUSING service calling...") + self.unpause() + unpaused_done = True + rospy.logdebug("UNPAUSING service calling...DONE") + except rospy.ServiceException as e: + counter += 1 + rospy.logerr("/gazebo/unpause_physics service call failed...Retrying "+str(counter)) + else: + error_message = "Maximum retries done"+str(self._max_retry)+", please check Gazebo unpause service" + rospy.logerr(error_message) + assert False, error_message + + rospy.logdebug("UNPAUSING FiNISH") + + + def resetSim(self): + """ + This was implemented because some simulations, when reseted the simulation + the systems that work with TF break, and because sometime we wont be able to change them + we need to reset world that ONLY resets the object position, not the entire simulation + systems. + """ + if self.reset_world_or_sim == "SIMULATION": + rospy.logerr("SIMULATION RESET") + self.resetSimulation() + elif self.reset_world_or_sim == "WORLD": + rospy.logerr("WORLD RESET") + self.resetWorld() + elif self.reset_world_or_sim == "NO_RESET_SIM": + rospy.logerr("NO RESET SIMULATION SELECTED") + else: + rospy.logerr("WRONG Reset Option:"+str(self.reset_world_or_sim)) + + def resetSimulation(self): + rospy.wait_for_service('/gazebo/reset_simulation') + try: + self.reset_simulation_proxy() + except rospy.ServiceException as e: + print ("/gazebo/reset_simulation service call failed") + + def resetWorld(self): + rospy.wait_for_service('/gazebo/reset_world') + try: + self.reset_world_proxy() + except rospy.ServiceException as e: + print ("/gazebo/reset_world service call failed") + + def init_values(self): + + self.resetSim() + + if self.start_init_physics_parameters: + rospy.logdebug("Initialising Simulation Physics Parameters") + self.init_physics_parameters() + else: + rospy.logerr("NOT Initialising Simulation Physics Parameters") + + def init_physics_parameters(self): + """ + We initialise the physics parameters of the simulation, like gravity, + friction coeficients and so on. + """ + self._time_step = Float64(0.001) + self._max_update_rate = Float64(1000.0) + + self._gravity = Vector3() + self._gravity.x = 0.0 + self._gravity.y = 0.0 + self._gravity.z = -9.81 + + self._ode_config = ODEPhysics() + self._ode_config.auto_disable_bodies = False + self._ode_config.sor_pgs_precon_iters = 0 + self._ode_config.sor_pgs_iters = 50 + self._ode_config.sor_pgs_w = 1.3 + self._ode_config.sor_pgs_rms_error_tol = 0.0 + self._ode_config.contact_surface_layer = 0.001 + self._ode_config.contact_max_correcting_vel = 0.0 + self._ode_config.cfm = 0.0 + self._ode_config.erp = 0.2 + self._ode_config.max_contacts = 20 + + self.update_gravity_call() + + + def update_gravity_call(self): + + self.pauseSim() + + set_physics_request = SetPhysicsPropertiesRequest() + set_physics_request.time_step = self._time_step.data + set_physics_request.max_update_rate = self._max_update_rate.data + set_physics_request.gravity = self._gravity + set_physics_request.ode_config = self._ode_config + + rospy.logdebug(str(set_physics_request.gravity)) + + result = self.set_physics(set_physics_request) + rospy.logdebug("Gravity Update Result==" + str(result.success) + ",message==" + str(result.status_message)) + + self.unpauseSim() + + def change_gravity(self, x, y, z): + self._gravity.x = x + self._gravity.y = y + self._gravity.z = z + + self.update_gravity_call() \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/openai_ros_common.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/openai_ros_common.py new file mode 100644 index 0000000..e94d6ca --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/openai_ros_common.py @@ -0,0 +1,277 @@ +#!/usr/bin/env python +import gym +from .task_envs.task_envs_list import RegisterOpenAI_Ros_Env +import roslaunch +import rospy +import rospkg +import os +import sys +import subprocess + + +def StartOpenAI_ROS_Environment(task_and_robot_environment_name): + """ + It Does all the stuff that the user would have to do to make it simpler + for the user. + This means: + 0) Registers the TaskEnvironment wanted, if it exists in the Task_Envs. + 2) Checks that the workspace of the user has all that is needed for launching this. + Which means that it will check that the robot spawn launch is there and the worls spawn is there. + 4) Launches the world launch and the robot spawn. + 5) It will import the Gym Env and Make it. + """ + rospy.logwarn("Env: {} will be imported".format( + task_and_robot_environment_name)) + result = RegisterOpenAI_Ros_Env(task_env=task_and_robot_environment_name, + max_episode_steps=10000) + + if result: + rospy.logwarn("Register of Task Env went OK, lets make the env..."+str(task_and_robot_environment_name)) + env = gym.make(task_and_robot_environment_name) + else: + rospy.logwarn("Something Went wrong in the register") + env = None + + return env + + +class ROSLauncher(object): + def __init__(self, rospackage_name, launch_file_name, ros_ws_abspath="/home/user/simulation_ws"): + + self._rospackage_name = rospackage_name + self._launch_file_name = launch_file_name + + self.rospack = rospkg.RosPack() + + # Check Package Exists + try: + pkg_path = self.rospack.get_path(rospackage_name) + rospy.logdebug("Package FOUND...") + except rospkg.common.ResourceNotFound: + rospy.logwarn("Package NOT FOUND, lets Download it...") + pkg_path = self.DownloadRepo(package_name=rospackage_name, + ros_ws_abspath=ros_ws_abspath) + + # Now we check that the Package path is inside the ros_ws_abspath + # This is to force the system to have the packages in that ws, and not in another. + if ros_ws_abspath in pkg_path: + rospy.logdebug("Package FOUND in the correct WS!") + else: + rospy.logwarn("Package FOUND in "+pkg_path + + ", BUT not in the ws="+ros_ws_abspath+", lets Download it...") + pkg_path = self.DownloadRepo(package_name=rospackage_name, + ros_ws_abspath=ros_ws_abspath) + + # If the package was found then we launch + if pkg_path: + rospy.loginfo( + ">>>>>>>>>>Package found in workspace-->"+str(pkg_path)) + launch_dir = os.path.join(pkg_path, "launch") + path_launch_file_name = os.path.join(launch_dir, launch_file_name) + + rospy.logwarn("path_launch_file_name=="+str(path_launch_file_name)) + + source_env_command = "source "+ros_ws_abspath+"/devel/setup.bash;" + roslaunch_command = "roslaunch {0} {1}".format(rospackage_name, launch_file_name) + command = source_env_command+roslaunch_command + rospy.logwarn("Launching command="+str(command)) + + p = subprocess.Popen(command, shell=True) + + state = p.poll() + if state is None: + rospy.loginfo("process is running fine") + elif state < 0: + rospy.loginfo("Process terminated with error") + elif state > 0: + rospy.loginfo("Process terminated without error") + """ + self.uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch.configure_logging(self.uuid) + self.launch = roslaunch.parent.ROSLaunchParent( + self.uuid, [path_launch_file_name]) + self.launch.start() + """ + + + rospy.loginfo(">>>>>>>>>STARTED Roslaunch-->" + + str(self._launch_file_name)) + else: + assert False, "No Package Path was found for ROS apckage ==>" + \ + str(rospackage_name) + + def DownloadRepo(self, package_name, ros_ws_abspath): + """ + This has to be installed + sudo pip install gitpython + """ + commands_to_take_effect = "\nIn a new Shell:::>\ncd "+ros_ws_abspath + \ + "\ncatkin_make\nsource devel/setup.bash\nrospack profile\n" + commands_to_take_effect2 = "\nIn your deeplearning program execute shell catkin_ws:::>\ncd /home/user/catkin_ws\nsource devel/setup.bash\nrospack profile\n" + + ros_ws_src_abspath_src = os.path.join(ros_ws_abspath, "src") + pkg_path = None + # We retrieve the got for the package asked + package_git = None + package_to_branch_dict = {} + + rospy.logdebug("package_name===>"+str(package_name)+"<===") + + if package_name == "moving_cube_description": + + url_git_1 = "https://bitbucket.org/theconstructcore/moving_cube.git" + package_git = [url_git_1] + package_to_branch_dict[url_git_1] = "master" + + url_git_2 = "https://bitbucket.org/theconstructcore/spawn_robot_tools.git" + package_git.append(url_git_2) + package_to_branch_dict[url_git_2] = "master" + + elif package_name == "rosbot_gazebo" or package_name == "rosbot_description": + package_git = [ + "https://bitbucket.org/theconstructcore/rosbot_husarion.git"] + package_git.append( + "https://github.com/paulbovbel/frontier_exploration.git") + + elif package_name == "fetch_gazebo": + package_git = [ + "https://bitbucket.org/theconstructcore/fetch_tc.git"] + + elif package_name == "cartpole_description" or package_name == "cartpole_v0_training": + package_git = [ + "https://bitbucket.org/theconstructcore/cart_pole.git"] + + elif package_name == "legged_robots_sims" or package_name == "legged_robots_description" or package_name == "my_legged_robots_description" or package_name == "my_legged_robots_sims" or package_name == "my_hopper_training": + package_git = ["https://bitbucket.org/theconstructcore/hopper.git"] + + elif package_name == "iri_wam_description" or package_name == "iri_wam_gazebo" or package_name == "iri_wam_reproduce_trajectory" or package_name == "iri_wam_aff_demo": + url_git_1 = "https://bitbucket.org/theconstructcore/iri_wam.git" + package_git = [url_git_1] + package_to_branch_dict[url_git_1] = "noetic" + + url_git_2 = "https://bitbucket.org/theconstructcore/hokuyo_model.git" + package_git.append(url_git_2) + package_to_branch_dict[url_git_2] = "master" + + + elif package_name == "drone_construct" or package_name == "drone_demo" or package_name == "sjtu_drone" or package_name == "custom_teleop" or package_name == "ardrone_as": + url_git_1 = "https://bitbucket.org/theconstructcore/parrot_ardrone.git" + package_git = [url_git_1] + package_to_branch_dict[url_git_1] = "kinetic-gazebo9" + + elif package_name == "sawyer_gazebo": + url_git_1 = "https://bitbucket.org/theconstructcore/sawyer_full.git" + package_git = [url_git_1] + package_to_branch_dict[url_git_1] = "update2019" + + elif package_name == "shadow_gazebo": + + url_git_1 = "https://bitbucket.org/theconstructcore/shadow_robot_smart_grasping_sandbox.git" + package_git = [url_git_1] + package_to_branch_dict[url_git_1] = "melodic-gazebo9" + + + url_git_2 = "https://github.com/ros-industrial/universal_robot.git" + package_git.append(url_git_2) + package_to_branch_dict[url_git_2] = "melodic-devel" + + elif package_name == "summit_xl_gazebo": + + url_git_1 = "https://bitbucket.org/theconstructcore/summit_xl.git" + package_git = [url_git_1] + package_to_branch_dict[url_git_1] = "melodic-gazebo9" + + url_git_2 = "https://github.com/RobotnikAutomation/robotnik_msgs.git" + package_git.append(url_git_2) + package_to_branch_dict[url_git_2] = "master" + + url_git_3 = "https://github.com/tu-darmstadt-ros-pkg/hector_gazebo.git" + package_git.append(url_git_3) + package_to_branch_dict[url_git_3] = "melodic-devel" + + elif package_name == "gym_construct": + package_git = [ + "https://bitbucket.org/theconstructcore/open_ai_gym_construct.git"] + + elif package_name == "turtlebot_gazebo": + + url_git_1 = "https://bitbucket.org/theconstructcore/turtlebot.git" + package_git = [url_git_1] + package_to_branch_dict[url_git_1] = "kinetic-gazebo9" + + + elif package_name == "turtlebot3_gazebo": + + url_git_1 = "https://bitbucket.org/theconstructcore/turtlebot3.git" + package_git = [url_git_1] + package_to_branch_dict[url_git_1] = "master" + + elif package_name == "robotx_gazebo": + url_git_1 = "https://bitbucket.org/theconstructcore/vmrc.git" + package_git = [url_git_1] + package_to_branch_dict[url_git_1] = "master" + + url_git_2 = "https://bitbucket.org/theconstructcore/spawn_robot_tools.git" + package_git.append(url_git_2) + package_to_branch_dict[url_git_2] = "master" + + elif package_name == "fetch_simple_description": + package_git = [ + "https://bitbucket.org/theconstructcore/fetch_simple_simulation.git"] + package_git.append( + "https://bitbucket.org/theconstructcore/spawn_robot_tools.git") + + # ADD HERE THE GITs List To Your Simuation + + else: + rospy.logerr("Package [ >"+package_name + + "< ] is not supported for autodownload, do it manually into >"+str(ros_ws_abspath)) + assert False, "The package "++ \ + " is not supported, please check the package name and the git support in openai_ros_common.py" + + # If a Git for the package is supported + if package_git: + for git_url in package_git: + try: + rospy.logdebug("Lets download git="+git_url + + ", in ws="+ros_ws_src_abspath_src) + if git_url in package_to_branch_dict: + branch_repo_name = package_to_branch_dict[git_url] + git.Git(ros_ws_src_abspath_src).clone(git_url,branch=branch_repo_name) + else: + git.Git(ros_ws_src_abspath_src).clone(git_url) + + rospy.logdebug("Download git="+git_url + + ", in ws="+ros_ws_src_abspath_src+"...DONE") + except git.exc.GitCommandError as e: + rospy.logwarn(str(e)) + rospy.logwarn("The Git "+git_url+" already exists in " + + ros_ws_src_abspath_src+", not downloading") + + # We check that the package is there + try: + pkg_path = self.rospack.get_path(package_name) + rospy.logwarn("The package "+package_name+" was FOUND by ROS.") + + if ros_ws_abspath in pkg_path: + rospy.logdebug("Package FOUND in the correct WS!") + else: + rospy.logwarn("Package FOUND in="+pkg_path + + ", BUT not in the ws="+ros_ws_abspath) + rospy.logerr( + "IMPORTANT!: You need to execute the following commands and rerun to dowloads to take effect.") + rospy.logerr(commands_to_take_effect) + rospy.logerr(commands_to_take_effect2) + sys.exit() + + except rospkg.common.ResourceNotFound: + rospy.logerr("Package "+package_name+" NOT FOUND by ROS.") + # We have to make the user compile and source to make ROS be able to find the new packages + # TODO: Make this automatic + rospy.logerr( + "IMPORTANT!: You need to execute the following commands and rerun to dowloads to take effect.") + rospy.logerr(commands_to_take_effect) + rospy.logerr(commands_to_take_effect2) + sys.exit() + + return pkg_path diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/cartpole_env.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/cartpole_env.py new file mode 100755 index 0000000..9675d31 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/cartpole_env.py @@ -0,0 +1,168 @@ +#!/usr/bin/env python + +import gym +import rospy +import roslaunch +import time +import numpy as np +from gym import utils, spaces +from geometry_msgs.msg import Twist +from std_srvs.srv import Empty +from gym.utils import seeding +from gym.envs.registration import register +import copy +import math +import os + +from sensor_msgs.msg import JointState +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from std_msgs.msg import Float64 +from gazebo_msgs.srv import SetLinkState +from gazebo_msgs.msg import LinkState +from rosgraph_msgs.msg import Clock +from openai_ros import robot_gazebo_env +from openai_ros.openai_ros_common import ROSLauncher + + +class CartPoleEnv(robot_gazebo_env.RobotGazeboEnv): + def __init__( + self, control_type, ros_ws_abspath + ): + + # We launch the ROSlaunch that spawns the robot into the world + ROSLauncher(rospackage_name="cartpole_description", + launch_file_name="put_robot_in_world.launch", + ros_ws_abspath=ros_ws_abspath) + + self.publishers_array = [] + self._base_pub = rospy.Publisher( + '/cartpole_v0/foot_joint_velocity_controller/command', Float64, queue_size=1) + self._pole_pub = rospy.Publisher( + '/cartpole_v0/pole_joint_velocity_controller/command', Float64, queue_size=1) + self.publishers_array.append(self._base_pub) + self.publishers_array.append(self._pole_pub) + + rospy.Subscriber("/cartpole_v0/joint_states", + JointState, self.joints_callback) + + self.control_type = control_type + if self.control_type == "velocity": + self.controllers_list = ['joint_state_controller', + 'pole_joint_velocity_controller', + 'foot_joint_velocity_controller', + ] + + elif self.control_type == "position": + self.controllers_list = ['joint_state_controller', + 'pole_joint_position_controller', + 'foot_joint_position_controller', + ] + + elif self.control_type == "effort": + self.controllers_list = ['joint_state_controller', + 'pole_joint_effort_controller', + 'foot_joint_effort_controller', + ] + + self.robot_name_space = "cartpole_v0" + self.reset_controls = True + + # Seed the environment + self._seed() + self.steps_beyond_done = None + + super(CartPoleEnv, self).__init__( + controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=self.reset_controls + ) + + def joints_callback(self, data): + self.joints = data + + def _seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + # RobotEnv methods + # ---------------------------- + + def _env_setup(self, initial_qpos): + self.init_internal_vars(self.init_pos) + self.set_init_pose() + self.check_all_systems_ready() + + def init_internal_vars(self, init_pos_value): + self.pos = [init_pos_value] + self.joints = None + + def check_publishers_connection(self): + """ + Checks that all the publishers are working + :return: + """ + rate = rospy.Rate(10) # 10hz + while (self._base_pub.get_num_connections() == 0 and not rospy.is_shutdown()): + rospy.logdebug( + "No susbribers to _base_pub yet so we wait and try again") + try: + rate.sleep() + except rospy.ROSInterruptException: + # This is to avoid error when world is rested, time when backwards. + pass + rospy.logdebug("_base_pub Publisher Connected") + + while (self._pole_pub.get_num_connections() == 0 and not rospy.is_shutdown()): + rospy.logdebug( + "No susbribers to _pole_pub yet so we wait and try again") + try: + rate.sleep() + except rospy.ROSInterruptException: + # This is to avoid error when world is rested, time when backwards. + pass + rospy.logdebug("_pole_pub Publisher Connected") + + rospy.logdebug("All Publishers READY") + + def _check_all_systems_ready(self, init=True): + self.base_position = None + while self.base_position is None and not rospy.is_shutdown(): + try: + self.base_position = rospy.wait_for_message( + "/cartpole_v0/joint_states", JointState, timeout=1.0) + rospy.logdebug( + "Current cartpole_v0/joint_states READY=>"+str(self.base_position)) + if init: + # We Check all the sensors are in their initial values + positions_ok = all( + abs(i) <= 1.0e-02 for i in self.base_position.position) + velocity_ok = all( + abs(i) <= 1.0e-02 for i in self.base_position.velocity) + efforts_ok = all( + abs(i) <= 1.0e-01 for i in self.base_position.effort) + base_data_ok = positions_ok and velocity_ok and efforts_ok + rospy.logdebug( + "Checking Init Values Ok=>" + str(base_data_ok)) + except: + rospy.logerr( + "Current cartpole_v0/joint_states not ready yet, retrying for getting joint_states") + rospy.logdebug("ALL SYSTEMS READY") + + def move_joints(self, joints_array): + joint_value = Float64() + joint_value.data = joints_array[0] + rospy.logdebug("Single Base JointsPos>>"+str(joint_value)) + self._base_pub.publish(joint_value) + + def get_clock_time(self): + self.clock_time = None + while self.clock_time is None and not rospy.is_shutdown(): + try: + self.clock_time = rospy.wait_for_message( + "/clock", Clock, timeout=1.0) + rospy.logdebug("Current clock_time READY=>" + + str(self.clock_time)) + except: + rospy.logdebug( + "Current clock_time not ready yet, retrying for getting Current clock_time") + return self.clock_time diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/cube_rl_utils.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/cube_rl_utils.py new file mode 100644 index 0000000..1281fba --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/cube_rl_utils.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python + +import time +import rospy +import math +import copy +import numpy +from std_msgs.msg import Float64 +from sensor_msgs.msg import JointState +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Point +from tf.transformations import euler_from_quaternion + + +class CubeRLUtils(object): + def __init__(self): + + self.check_all_sensors_ready() + + rospy.Subscriber("/moving_cube/joint_states", JointState, self.joints_callback) + rospy.Subscriber("/moving_cube/odom", Odometry, self.odom_callback) + + self._roll_vel_pub = rospy.Publisher('/moving_cube/inertia_wheel_roll_joint_velocity_controller/command', + Float64, queue_size=1) + + self.check_publishers_connection() + + def check_all_sensors_ready(self): + self.disk_joints_data = None + while self.disk_joints_data is None and not rospy.is_shutdown(): + try: + self.disk_joints_data = rospy.wait_for_message("/moving_cube/joint_states", JointState, timeout=1.0) + rospy.loginfo("Current moving_cube/joint_states READY=>" + str(self.disk_joints_data)) + + except: + rospy.logerr("Current moving_cube/joint_states not ready yet, retrying for getting joint_states") + + self.cube_odom_data = None + while self.disk_joints_data is None and not rospy.is_shutdown(): + try: + self.cube_odom_data = rospy.wait_for_message("/moving_cube/odom", Odometry, timeout=1.0) + rospy.loginfo("Current /moving_cube/odom READY=>" + str(self.cube_odom_data)) + + except: + rospy.logerr("Current /moving_cube/odom not ready yet, retrying for getting odom") + rospy.loginfo("ALL SENSORS READY") + + def check_publishers_connection(self): + """ + Checks that all the publishers are working + :return: + """ + rate = rospy.Rate(10) # 10hz + while (self._roll_vel_pub.get_num_connections() == 0 and not rospy.is_shutdown()): + rospy.loginfo("No susbribers to _roll_vel_pub yet so we wait and try again") + try: + rate.sleep() + except rospy.ROSInterruptException: + # This is to avoid error when world is rested, time when backwards. + pass + rospy.loginfo("_base_pub Publisher Connected") + + rospy.loginfo("All Publishers READY") + + def joints_callback(self, data): + self.joints = data + + def odom_callback(self, data): + self.odom = data + + # Reinforcement Learning Utility Code + def move_joints(self, roll_speed): + + joint_speed_value = Float64() + joint_speed_value.data = roll_speed + rospy.loginfo("Single Disk Roll Velocity>>" + str(joint_speed_value)) + self._roll_vel_pub.publish(joint_speed_value) + + def get_cube_state(self): + + # We convert from quaternions to euler + orientation_list = [self.odom.pose.pose.orientation.x, + self.odom.pose.pose.orientation.y, + self.odom.pose.pose.orientation.z, + self.odom.pose.pose.orientation.w] + + roll, pitch, yaw = euler_from_quaternion(orientation_list) + + # We get the distance from the origin + start_position = Point() + start_position.x = 0.0 + start_position.y = 0.0 + start_position.z = 0.0 + + distance = self.get_distance_from_point(start_position, + self.odom.pose.pose.position) + + cube_state = [ + round(self.joints.velocity[0], 1), + round(distance, 1), + round(roll, 1), + round(pitch, 1), + round(yaw, 1) + ] + + return cube_state + + def observation_checks(self, cube_state): + + # MAximum distance to travel permited in meters from origin + max_distance = 2.0 + + if (cube_state[1] > max_distance): + rospy.logerr("Cube Too Far==>" + str(cube_state[1])) + done = True + else: + rospy.loginfo("Cube NOT Too Far==>" + str(cube_state[1])) + done = False + + return done + + def get_distance_from_point(self, pstart, p_end): + """ + Given a Vector3 Object, get distance from current position + :param p_end: + :return: + """ + a = numpy.array((pstart.x, pstart.y, pstart.z)) + b = numpy.array((p_end.x, p_end.y, p_end.z)) + + distance = numpy.linalg.norm(a - b) + + return distance + + def get_reward_for_observations(self, state): + + # We reward it for lower speeds and distance traveled + + speed = state[0] + distance = state[1] + + # Positive Reinforcement + reward_distance = distance * 10.0 + # Negative Reinforcement for magnitude of speed + reward_for_efective_movement = -1 * abs(speed) + + reward = reward_distance + reward_for_efective_movement + + rospy.loginfo("Reward_distance=" + str(reward_distance)) + rospy.loginfo("Reward_for_efective_movement= " + str(reward_for_efective_movement)) + + return reward + + +def cube_rl_systems_test(): + rospy.init_node('cube_rl_systems_test_node', anonymous=True, log_level=rospy.INFO) + cube_rl_utils_object = CubeRLUtils() + + rospy.loginfo("Moving to Speed==>80") + cube_rl_utils_object.move_joints(roll_speed=80.0) + time.sleep(2) + rospy.loginfo("Moving to Speed==>-80") + cube_rl_utils_object.move_joints(roll_speed=-80.0) + time.sleep(2) + rospy.loginfo("Moving to Speed==>0.0") + cube_rl_utils_object.move_joints(roll_speed=0.0) + time.sleep(2) + + cube_state = cube_rl_utils_object.get_cube_state() + done = cube_rl_utils_object.observation_checks(cube_state) + reward = cube_rl_utils_object.get_reward_for_observations(cube_state) + + rospy.loginfo("Done==>" + str(done)) + rospy.loginfo("Reward==>" + str(reward)) + + +if __name__ == "__main__": + cube_rl_systems_test() + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/cube_single_disk_env.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/cube_single_disk_env.py new file mode 100644 index 0000000..d689aa0 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/cube_single_disk_env.py @@ -0,0 +1,165 @@ +#! /usr/bin/env python + +import numpy +import rospy +from openai_ros import robot_gazebo_env +from std_msgs.msg import Float64 +from sensor_msgs.msg import JointState +from nav_msgs.msg import Odometry +from openai_ros.openai_ros_common import ROSLauncher + + +class CubeSingleDiskEnv(robot_gazebo_env.RobotGazeboEnv): + """Superclass for all CubeSingleDisk environments. + """ + + def __init__(self, ros_ws_abspath): + """Initializes a new CubeSingleDisk environment. + + Args: + """ + # We launch the ROSlaunch that spawns the robot into the world + ROSLauncher(rospackage_name="moving_cube_description", + launch_file_name="put_robot_in_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Variables that we give through the constructor. + # None in this case + + # Internal Vars + self.controllers_list = ['joint_state_controller', + 'inertia_wheel_roll_joint_velocity_controller' + ] + + self.robot_name_space = "moving_cube" + + # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv + super(CubeSingleDiskEnv, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=True) + + + # We Start all the ROS related Subscribers and publishers + rospy.Subscriber("/moving_cube/joint_states", JointState, self._joints_callback) + rospy.Subscriber("/moving_cube/odom", Odometry, self._odom_callback) + + self._roll_vel_pub = rospy.Publisher('/moving_cube/inertia_wheel_roll_joint_velocity_controller/command', + Float64, queue_size=1) + + self._check_all_systems_ready() + + # We pause the simulation once everything is ready + self.gazebo.pauseSim() + + # Methods needed by the RobotGazeboEnv + # ---------------------------- + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + self._check_all_sensors_ready() + self._check_publishers_connection() + return True + + # CubeSingleDiskEnv virtual methods + # ---------------------------- + + def _check_all_sensors_ready(self): + self._check_joint_states_ready() + self._check_odom_ready() + rospy.logdebug("ALL SENSORS READY") + + def _check_joint_states_ready(self): + self.joints = None + while self.joints is None and not rospy.is_shutdown(): + try: + self.joints = rospy.wait_for_message( + "/moving_cube/joint_states", JointState, timeout=1.0) + rospy.logdebug( + "Current moving_cube/joint_states READY=>" + str(self.joints)) + + except: + rospy.logerr( + "Current moving_cube/joint_states not ready yet, retrying for getting joint_states") + return self.joints + + def _check_odom_ready(self): + self.odom = None + while self.odom is None and not rospy.is_shutdown(): + try: + self.odom = rospy.wait_for_message( + "/moving_cube/odom", Odometry, timeout=1.0) + rospy.logdebug( + "Current /moving_cube/odom READY=>" + str(self.odom)) + + except: + rospy.logerr( + "Current /moving_cube/odom not ready yet, retrying for getting odom") + + return self.odom + + def _joints_callback(self, data): + self.joints = data + + def _odom_callback(self, data): + self.odom = data + + def _check_publishers_connection(self): + """ + Checks that all the publishers are working + :return: + """ + rate = rospy.Rate(10) # 10hz + while self._roll_vel_pub.get_num_connections() == 0 and not rospy.is_shutdown(): + rospy.logdebug( + "No susbribers to _roll_vel_pub yet so we wait and try again") + try: + rate.sleep() + except rospy.ROSInterruptException: + # This is to avoid error when world is rested, time when backwards. + pass + rospy.logdebug("_roll_vel_pub Publisher Connected") + + rospy.logdebug("All Publishers READY") + + + # Methods that the TrainingEnvironment will need. + # ---------------------------- + def move_joints(self, roll_speed): + joint_speed_value = Float64() + joint_speed_value.data = roll_speed + rospy.logdebug("Single Disk Roll Velocity>>" + str(joint_speed_value)) + self._roll_vel_pub.publish(joint_speed_value) + self.wait_until_roll_is_in_vel(joint_speed_value.data) + + def wait_until_roll_is_in_vel(self, velocity): + + rate = rospy.Rate(10) + start_wait_time = rospy.get_rostime().to_sec() + end_wait_time = 0.0 + epsilon = 0.1 + v_plus = velocity + epsilon + v_minus = velocity - epsilon + while not rospy.is_shutdown(): + joint_data = self._check_joint_states_ready() + roll_vel = joint_data.velocity[0] + rospy.logdebug("VEL=" + str(roll_vel) + + ", ?RANGE=[" + str(v_minus) + ","+str(v_plus)+"]") + are_close = (roll_vel <= v_plus) and (roll_vel > v_minus) + if are_close: + rospy.logdebug("Reached Velocity!") + end_wait_time = rospy.get_rostime().to_sec() + break + rospy.logdebug("Not there yet, keep waiting...") + rate.sleep() + delta_time = end_wait_time - start_wait_time + rospy.logdebug("[Wait Time=" + str(delta_time)+"]") + return delta_time + + def get_joints(self): + return self.joints + + def get_odom(self): + return self.odom diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/fetch_env.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/fetch_env.py new file mode 100755 index 0000000..5554ca5 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/fetch_env.py @@ -0,0 +1,337 @@ +import numpy as np +import rospy +from gazebo_msgs.srv import GetWorldProperties, GetModelState +from sensor_msgs.msg import JointState +from openai_ros import robot_gazebo_env +import sys +import moveit_commander +import moveit_msgs.msg +import geometry_msgs.msg +import trajectory_msgs.msg +from openai_ros.openai_ros_common import ROSLauncher + + +class FetchEnv(robot_gazebo_env.RobotGazeboEnv): + + def __init__(self, ros_ws_abspath): + rospy.logdebug("========= In Fetch Env") + + # We launch the ROSlaunch that spawns the robot into the world + ROSLauncher(rospackage_name="fetch_gazebo", + launch_file_name="put_robot_in_world_HER.launch", + ros_ws_abspath=ros_ws_abspath) + + # this object contains all object's positions!! + self.obj_positions = Obj_Pos() + + self.controllers_list = [] + + self.robot_name_space = "" + self.reset_controls = False + + super(FetchEnv, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=False, + start_init_physics_parameters=False, + reset_world_or_sim="WORLD") + + # We Start all the ROS related Subscribers and publishers + + self.JOINT_STATES_SUBSCRIBER = '/joint_states' + self.join_names = ["joint0", + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6"] + + self.gazebo.unpauseSim() + self._check_all_systems_ready() + + self.joint_states_sub = rospy.Subscriber( + self.JOINT_STATES_SUBSCRIBER, JointState, self.joints_callback) + self.joints = JointState() + + # Start Services + self.move_fetch_object = MoveFetch() + + # Wait until it has reached its Sturtup Position + self.wait_fetch_ready() + + self.gazebo.pauseSim() + # Variables that we give through the constructor. + + rospy.logdebug("========= Out Fetch Env") + + # RobotGazeboEnv virtual methods + # ---------------------------- + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + self._check_all_sensors_ready() + return True + + # FetchEnv virtual methods + # ---------------------------- + + def _check_all_sensors_ready(self): + self._check_joint_states_ready() + + rospy.logdebug("ALL SENSORS READY") + + def _check_joint_states_ready(self): + self.joints = None + while self.joints is None and not rospy.is_shutdown(): + try: + self.joints = rospy.wait_for_message( + self.JOINT_STATES_SUBSCRIBER, JointState, timeout=1.0) + rospy.logdebug( + "Current "+str(self.JOINT_STATES_SUBSCRIBER)+" READY=>" + str(self.joints)) + + except: + rospy.logerr( + "Current "+str(self.JOINT_STATES_SUBSCRIBER)+" not ready yet, retrying....") + return self.joints + + def joints_callback(self, data): + self.joints = data + + def get_joints(self): + return self.joints + + def get_joint_names(self): + return self.joints.name + + def set_trajectory_ee(self, action): + """ + Sets the Pose of the EndEffector based on the action variable. + The action variable contains the position and orientation of the EndEffector. + See create_action + """ + # Set up a trajectory message to publish. + ee_target = geometry_msgs.msg.Pose() + + ee_target.orientation.x = -0.707 + ee_target.orientation.y = 0.0 + ee_target.orientation.z = 0.707 + ee_target.orientation.w = 0.001 + + ee_target.position.x = action[0] + ee_target.position.y = action[1] + ee_target.position.z = action[2] + + result = self.move_fetch_object.ee_traj(ee_target) + return result + + def set_trajectory_joints(self, initial_qpos): + + positions_array = [None] * 7 + positions_array[0] = initial_qpos["joint0"] + positions_array[1] = initial_qpos["joint1"] + positions_array[2] = initial_qpos["joint2"] + positions_array[3] = initial_qpos["joint3"] + positions_array[4] = initial_qpos["joint4"] + positions_array[5] = initial_qpos["joint5"] + positions_array[6] = initial_qpos["joint6"] + + self.move_fetch_object.joint_traj(positions_array) + + return True + + def create_action(self, position, orientation): + """ + position = [x,y,z] + orientation= [x,y,z,w] + """ + + gripper_target = np.array(position) + gripper_rotation = np.array(orientation) + action = np.concatenate([gripper_target, gripper_rotation]) + + return action + + def create_joints_dict(self, joints_positions): + """ + Based on the Order of the positions, they will be assigned to its joint name + names_in_order: + joint0: 0.0 + joint1: 0.0 + joint2: 0.0 + joint3: -1.5 + joint4: 0.0 + joint5: 1.5 + joint6: 0.0 + """ + + assert len(joints_positions) == len( + self.join_names), "Wrong number of joints, there should be "+str(len(self.join_names)) + joints_dict = dict(zip(self.join_names, joints_positions)) + + return joints_dict + + def get_ee_pose(self): + """ + Returns geometry_msgs/PoseStamped + std_msgs/Header header + uint32 seq + time stamp + string frame_id + geometry_msgs/Pose pose + geometry_msgs/Point position + float64 x + float64 y + float64 z + geometry_msgs/Quaternion orientation + float64 x + float64 y + float64 z + float64 w + """ + self.gazebo.unpauseSim() + gripper_pose = self.move_fetch_object.ee_pose() + self.gazebo.pauseSim() + return gripper_pose + + def get_ee_rpy(self): + gripper_rpy = self.move_fetch_object.ee_rpy() + return gripper_rpy + + def wait_fetch_ready(self): + """ + # TODO: Make it wait for this position + Desired Position to wait for + + (0.44291739197591884, + -0.13691381375054146, + -4.498589757905556e-09, + 0.006635104153645881, + 0.0018354466563206273, + 0.0023142971818792546, + 1.3200059164171716, + 1.399964660857453, + -0.19981518020955402, + 1.719961735970255, + 1.0394665737933906e-05, + 1.659980987917125, + -6.067103113238659e-06, + 0.05001918351472232, + 0.050051597253287436) + """ + import time + for i in range(20): + print("WAITING..."+str(i)) + sys.stdout.flush() + time.sleep(1.0) + + print("WAITING...DONE") + + # ParticularEnv methods + # ---------------------------- + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _get_obs(self): + raise NotImplementedError() + + def _is_done(self, observations): + """Checks if episode done based on observations given. + """ + raise NotImplementedError() + + +class Obj_Pos(object): + """ + This object maintains the pose and rotation of the cube in a simulation through Gazebo Service + + """ + + def __init__(self): + world_specs = rospy.ServiceProxy( + '/gazebo/get_world_properties', GetWorldProperties)() + self.time = 0 + self.model_names = world_specs.model_names + self.get_model_state = rospy.ServiceProxy( + '/gazebo/get_model_state', GetModelState) + + def get_states(self): + """ + Returns the ndarray of pose&rotation of the cube + """ + for model_name in self.model_names: + if model_name == "cube": + data = self.get_model_state( + model_name, "world") # gazebo service client + return np.array([ + data.pose.position.x, + data.pose.position.y, + data.pose.position.z, + data.pose.orientation.x, + data.pose.orientation.y, + data.pose.orientation.z + ]) + + +class MoveFetch(object): + def __init__(self): + rospy.logdebug("===== In MoveFetch") + moveit_commander.roscpp_initialize(sys.argv) + self.robot = moveit_commander.RobotCommander() + self.scene = moveit_commander.PlanningSceneInterface() + self.group = moveit_commander.MoveGroupCommander("arm") + rospy.logdebug("===== Out MoveFetch") + + def ee_traj(self, pose): + self.group.set_pose_target(pose) + result = self.execute_trajectory() + return result + + def joint_traj(self, positions_array): + + self.group_variable_values = self.group.get_current_joint_values() + self.group_variable_values[0] = positions_array[0] + self.group_variable_values[1] = positions_array[1] + self.group_variable_values[2] = positions_array[2] + self.group_variable_values[3] = positions_array[3] + self.group_variable_values[4] = positions_array[4] + self.group_variable_values[5] = positions_array[5] + self.group_variable_values[6] = positions_array[6] + self.group.set_joint_value_target(self.group_variable_values) + result = self.execute_trajectory() + + return result + + def execute_trajectory(self): + """ + Assuming that the trajecties has been set to the self objects appropriately + Make a plan to the destination in Homogeneous Space(x,y,z,yaw,pitch,roll) + and returns the result of execution + """ + self.plan = self.group.plan() + result = self.group.go(wait=True) + return result + + def ee_pose(self): + gripper_pose = self.group.get_current_pose() + return gripper_pose + + def ee_rpy(self, request): + gripper_rpy = self.group.get_current_rpy() + return gripper_rpy diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/fetchsimple_env.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/fetchsimple_env.py new file mode 100644 index 0000000..6d06704 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/fetchsimple_env.py @@ -0,0 +1,327 @@ +import numpy as np +import rospy +from std_msgs.msg import Float64 +from sensor_msgs.msg import JointState +from openai_ros import robot_gazebo_env +from openai_ros.openai_ros_common import ROSLauncher + + +class FetchSimpleEnv(robot_gazebo_env.RobotGazeboEnv): + + def __init__(self, ros_ws_abspath): + rospy.logdebug("Entered Fetch Env") + + # We launch the ROSlaunch that spawns the robot into the world + ROSLauncher(rospackage_name="fetch_simple_description", + launch_file_name="put_fetchsimple_in_world.launch", + ros_ws_abspath=ros_ws_abspath) + + self.controllers_list = ["joint_state_controller", + "torso_lift_joint_position_controller", + "bellows_joint_position_controller", + "head_pan_joint_position_controller", + "head_tilt_joint_position_controller", + "shoulder_pan_joint_position_controller", + "shoulder_lift_joint_position_controller", + "upperarm_roll_joint_position_controller", + "elbow_flex_joint_position_controller", + "forearm_roll_joint_position_controller", + "wrist_flex_joint_position_controller", + "wrist_roll_joint_position_controller", + "r_gripper_finger_joint_position_controller", + "l_gripper_finger_joint_position_controller"] + + self.robot_name_space = "fetch" + self.reset_controls = True + + super(FetchSimpleEnv, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=self.reset_controls, + start_init_physics_parameters=False, + reset_world_or_sim="WORLD") + + # We Start all the ROS related Subscribers and publishers + + self.JOINT_STATES_SUBSCRIBER = '/fetch/joint_states' + self.join_names = ["joint0", + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6"] + + self.gazebo.unpauseSim() + # Start Move Fetch Object, that checks all systems are ready + self.move_fetch_object = FetchSimpleMove() + # Wait until Fetch goes to the init pose + self.move_fetch_object.init_position() + + # We pause until the next step + self.gazebo.pauseSim() + + # RobotGazeboEnv virtual methods + # ---------------------------- + + def move_to_init_pose(self): + self.move_fetch_object.init_position() + + def get_joint_limits(self): + return self.move_fetch_object.joint_upper_limits, self.move_fetch_object.joint_lower_limits + + def get_joints_position(self): + return self.move_fetch_object.get_current_joints_position() + + def set_trajectory_joints(self, delta_joints_array): + self.move_fetch_object.delta_joints(delta_joints_array) + return True + + # ParticularEnv methods + # ---------------------------- + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + self.move_fetch_object.check_all_systems_ready() + return True + + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _get_obs(self): + raise NotImplementedError() + + def _is_done(self, observations): + """Checks if episode done based on observations given. + """ + raise NotImplementedError() + + +class FetchSimpleMove(object): + def __init__(self): + rospy.loginfo("Initialising...") + + self.name_joints = ["bellows_joint", + "elbow_flex_joint", + "forearm_roll_joint", + "head_pan_joint", + "head_tilt_joint", + "l_gripper_finger_joint", + "r_gripper_finger_joint", + "shoulder_lift_joint", + "shoulder_pan_joint", + "torso_lift_joint", + "upperarm_roll_joint", + "wrist_flex_joint", + "wrist_roll_joint"] + + self.joint_upper_limits = [0.4, + 2.251, + 6.27, # Was none but placed a limit + 1.57, + 1.45, + 0.05, + 0.05, + 1.518, + 1.6056, + 0.38615, + 6.27, # Was none but placed a limit + 2.16, + 6.27, # Was none but placed a limit + ] + self.joint_lower_limits = [0.0, + -2.251, + 0.0, # Was none but placed a limit + -1.57, + -0.76, + 0.0, + 0.0, + -1.221, + -1.6056, + 0.0, + 0.0, # Was none but placed a limit + -2.16, + 0.0, # Was none but placed a limit + ] + + self.travel_arm_pose = [0.0, + -1.8, # elbow_flex_joint + 0.0, # forearm_roll_joint + 0.0, + 0.0, + 0.05, + 0.04, + 0.7, # shoulder_lift_joint + 1.32, # shoulder_pan_joint + 0.0, # upperarm_roll_joint + 0.0, + -0.4, # wrist_flex_joint + 0.1] + + self.joint_array = len(self.name_joints)*[0.0] + + self.pub_position_array = [] + for joint in self.name_joints: + topic_name = "/fetch/"+joint+"_position_controller/command" + self.pub_position_array.append( + rospy.Publisher(topic_name, Float64, queue_size=1)) + + # Wait for publishers to be ready + self.check_all_systems_ready() + + rospy.Subscriber("/fetch/joint_states", JointState, + self.join_state_callback) + + def check_all_systems_ready(self): + self.wait_publishers_to_be_ready() + self._check_joint_states_ready() + + def _check_joint_states_ready(self): + self.joints_state = None + while self.joints_state is None and not rospy.is_shutdown(): + try: + self.joints_state = rospy.wait_for_message( + "/fetch/joint_states", JointState, timeout=1.0) + rospy.logdebug( + "Current /fetch/joint_states READY=>" + str(self.joints_state)) + + except: + rospy.logerr( + "Current /fetch/joint_states not ready yet, retrying for getting joint_states") + return self.joints_state + + def join_state_callback(self, msg): + self.joints_state = msg + + def get_current_joints_position(self): + return self.joints_state.position + + def init_position(self): + # We wait what it takes to reset pose + self.move_all_joints(joints_pos_array=self.joint_array, time_out=0.0) + + def set_travel_arm_pose(self): + self.move_all_joints(joints_pos_array=self.travel_arm_pose) + + def wait_for_joints_to_get_there(self, desired_pos_array, error=0.2, timeout=3.0): + + time_waiting = 0.0 + frequency = 10.0 + are_equal = False + is_timeout = False + rate = rospy.Rate(frequency) + rospy.logwarn("Waiting for joint to get to the position") + while not are_equal and not is_timeout and not rospy.is_shutdown(): + + current_pos = [self.joints_state.position] + + are_equal = np.allclose(a=current_pos, + b=desired_pos_array, + atol=error) + + rospy.logdebug("are_equal="+str(are_equal)) + rospy.logdebug(str(desired_pos_array)) + rospy.logdebug(str(current_pos)) + + rate.sleep() + if timeout == 0.0: + # We wait what it takes + time_waiting += 0.0 + else: + time_waiting += 1.0 / frequency + is_timeout = time_waiting > timeout + + rospy.logwarn( + "Joints are in the desired position with an erro of "+str(error)) + + def wait_publishers_to_be_ready(self): + + rate_wait = rospy.Rate(10) + rospy.logdebug("Waiting for Publishers to be ready...") + i = 0 + for publisher_obj in self.pub_position_array: + publisher_ready = False + while not publisher_ready: + connection_num = publisher_obj.get_num_connections() + publisher_ready = connection_num > 0 + rospy.logdebug("Pub joint NOT Ready=" + + str(self.name_joints[i])) + rate_wait.sleep() + rospy.logdebug("Publisher for joint Ready=" + + str(self.name_joints[i])) + i += 1 + + def move_all_joints(self, joints_pos_array, time_out=3.0, error=0.2): + + assert len(joints_pos_array) == len( + self.joint_array), "Lengths dont match" + i = 0 + for angle in joints_pos_array: + angle_msg = Float64() + angle_msg.data = angle + # Publish Joint Position + self.pub_position_array[i].publish(angle_msg) + i += 1 + + self.wait_for_joints_to_get_there(self.joint_array, error=error, timeout=time_out) + self.update_joints(new_joints_pos=joints_pos_array) + + def update_joints(self, new_joints_pos): + + i = 0 + + assert len(new_joints_pos) == len( + self.joint_array), "Lengths don't match in Update" + + for new_joint_value in new_joints_pos: + upper = self.joint_upper_limits[i] + lower = self.joint_lower_limits[i] + if upper is None or lower is None: + self.joint_array[i] = new_joint_value + else: + if upper >= new_joint_value >= lower: + self.joint_array[i] = new_joint_value + elif new_joint_value < lower: + self.joint_array[i] = lower + else: + self.joint_array[i] = upper + rospy.logdebug("index =" + str(i)) + rospy.logdebug("length of name_joints =" + + str(len(self.name_joints[i]))) + rospy.logdebug("name_joints=" + str(self.name_joints[i])) + + i += 1 + + def delta_joints(self, delta_array): + """ + delta_array = [bellows_joint, elbow_flex_joint, forearm_roll_joint, head_pan_joint, head_tilt_joint, + l_gripper_finger_joint, r_gripper_finger_joint, shoulder_lift_joint, shoulder_pan_joint, + torso_lift_joint, upperarm_roll_joint, wrist_flex_joint, wrist_roll_joint] + :param delta_array: + :return: + """ + new_pos_array = len(delta_array)*[0.0] + i = 0 + for delta in delta_array: + new_pos_array[i] = self.joint_array[i] + delta + i += 1 + + self.move_all_joints(new_pos_array) + + def get_current_angles(self): + return self.joint_array diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/hopper_env.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/hopper_env.py new file mode 100644 index 0000000..32d95fe --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/hopper_env.py @@ -0,0 +1,345 @@ +import numpy +import rospy +import time +from openai_ros import robot_gazebo_env +from gazebo_msgs.msg import ContactsState +from sensor_msgs.msg import Imu +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Point, Quaternion, Vector3 +from sensor_msgs.msg import JointState +from std_msgs.msg import Float64 +from openai_ros.openai_ros_common import ROSLauncher + + +class HopperEnv(robot_gazebo_env.RobotGazeboEnv): + """Superclass for all HopperEnv environments. + """ + + def __init__(self, ros_ws_abspath): + """ + Initializes a new HopperEnv environment. + + To check any topic we need to have the simulations running, we need to do two things: + 1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations + that are pause for whatever the reason + 2) If the simulation was running already for some reason, we need to reset the controlers. + This has to do with the fact that some plugins with tf, dont understand the reset of the simulation + and need to be reseted to work properly. + + The Sensors: The sensors accesible are the ones considered usefull for AI learning. + + Sensor Topic List: + * /drone/down_camera/image_raw: RGB Camera facing down. + * /drone/front_camera/image_raw: RGB Camera facing front. + * /drone/imu: IMU of the drone giving acceleration and orientation relative to world. + * /drone/sonar: Sonar readings facing front + * /drone/gt_pose: Get position and orientation in Global space + * /drone/gt_vel: Get the linear velocity , the angular doesnt record anything. + + Actuators Topic List: + * /cmd_vel: Move the Drone Around when you have taken off. + * /drone/takeoff: Publish into it to take off + * /drone/land: Publish to make ParrotDrone Land + + Args: + """ + rospy.logdebug("Start HopperEnv INIT...") + + # We launch the ROSlaunch that spawns the robot into the world + ROSLauncher(rospackage_name="legged_robots_sims", + launch_file_name="put_robot_in_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Variables that we give through the constructor. + # None in this case + + # Internal Vars + # Doesnt have any accesibles + self.controllers_list = ['joint_state_controller', + 'haa_joint_position_controller', + 'hfe_joint_position_controller', + 'kfe_joint_position_controller'] + + # It doesnt use namespace + self.robot_name_space = "monoped" + + # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv + super(HopperEnv, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=False, + start_init_physics_parameters=False, + reset_world_or_sim="WORLD") + + rospy.logdebug("HopperEnv unpause1...") + self.gazebo.unpauseSim() + # self.controllers_object.reset_controllers() + + self._check_all_systems_ready() + + # We Start all the ROS related Subscribers and publishers + rospy.Subscriber("/odom", Odometry, self._odom_callback) + # We use the IMU for orientation and linearacceleration detection + rospy.Subscriber("/monoped/imu/data", Imu, self._imu_callback) + # We use it to get the contact force, to know if its in the air or stumping too hard. + rospy.Subscriber("/lowerleg_contactsensor_state", + ContactsState, self._contact_callback) + # We use it to get the joints positions and calculate the reward associated to it + rospy.Subscriber("/monoped/joint_states", JointState, + self._joints_state_callback) + + self.publishers_array = [] + self._haa_joint_pub = rospy.Publisher( + '/monoped/haa_joint_position_controller/command', Float64, queue_size=1) + self._hfe_joint_pub = rospy.Publisher( + '/monoped/hfe_joint_position_controller/command', Float64, queue_size=1) + self._kfe_joint_pub = rospy.Publisher( + '/monoped/kfe_joint_position_controller/command', Float64, queue_size=1) + + self.publishers_array.append(self._haa_joint_pub) + self.publishers_array.append(self._hfe_joint_pub) + self.publishers_array.append(self._kfe_joint_pub) + + self._check_all_publishers_ready() + + self.gazebo.pauseSim() + + rospy.logdebug("Finished HopperEnv INIT...") + + # Methods needed by the RobotGazeboEnv + # ---------------------------- + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + rospy.logdebug("HopperEnv check_all_systems_ready...") + self._check_all_sensors_ready() + rospy.logdebug("END HopperEnv _check_all_systems_ready...") + return True + + # CubeSingleDiskEnv virtual methods + # ---------------------------- + + def _check_all_sensors_ready(self): + rospy.logdebug("START ALL SENSORS READY") + self._check_odom_ready() + self._check_imu_ready() + self._check_lowerleg_contactsensor_state_ready() + self._check_joint_states_ready() + rospy.logdebug("ALL SENSORS READY") + + def _check_odom_ready(self): + self.odom = None + rospy.logdebug("Waiting for /odom to be READY...") + while self.odom is None and not rospy.is_shutdown(): + try: + self.odom = rospy.wait_for_message( + "/odom", Odometry, timeout=1.0) + rospy.logdebug("Current /odom READY=>") + + except: + rospy.logerr( + "Current /odom not ready yet, retrying for getting odom") + return self.odom + + def _check_imu_ready(self): + self.imu = None + rospy.logdebug("Waiting for /monoped/imu/data to be READY...") + while self.imu is None and not rospy.is_shutdown(): + try: + self.imu = rospy.wait_for_message( + "/monoped/imu/data", Imu, timeout=1.0) + rospy.logdebug("Current /monoped/imu/data READY=>") + + except: + rospy.logerr( + "Current /monoped/imu/data not ready yet, retrying for getting imu") + return self.imu + + def _check_lowerleg_contactsensor_state_ready(self): + self.lowerleg_contactsensor_state = None + rospy.logdebug( + "Waiting for /lowerleg_contactsensor_state to be READY...") + while self.lowerleg_contactsensor_state is None and not rospy.is_shutdown(): + try: + self.lowerleg_contactsensor_state = rospy.wait_for_message( + "/lowerleg_contactsensor_state", ContactsState, timeout=1.0) + rospy.logdebug("Current /lowerleg_contactsensor_state READY=>") + + except: + rospy.logerr( + "Current /lowerleg_contactsensor_state not ready yet, retrying for getting lowerleg_contactsensor_state") + return self.lowerleg_contactsensor_state + + def _check_joint_states_ready(self): + self.joint_states = None + rospy.logdebug("Waiting for /monoped/joint_states to be READY...") + while self.joint_states is None and not rospy.is_shutdown(): + try: + self.joint_states = rospy.wait_for_message( + "/monoped/joint_states", JointState, timeout=1.0) + rospy.logdebug("Current /monoped/joint_states READY=>") + + except: + rospy.logerr( + "Current /monoped/joint_states not ready yet, retrying for getting joint_states") + return self.joint_states + + def _odom_callback(self, data): + self.odom = data + + def _imu_callback(self, data): + self.imu = data + + def _contact_callback(self, data): + self.lowerleg_contactsensor_state = data + + def _joints_state_callback(self, data): + self.joint_states = data + + def _check_all_publishers_ready(self): + """ + Checks that all the publishers are working + :return: + """ + rospy.logdebug("START ALL SENSORS READY") + for publisher_object in self.publishers_array: + self._check_pub_connection(publisher_object) + rospy.logdebug("ALL SENSORS READY") + + def _check_pub_connection(self, publisher_object): + + rate = rospy.Rate(10) # 10hz + while publisher_object.get_num_connections() == 0 and not rospy.is_shutdown(): + rospy.logdebug( + "No susbribers to publisher_object yet so we wait and try again") + try: + rate.sleep() + except rospy.ROSInterruptException: + # This is to avoid error when world is rested, time when backwards. + pass + rospy.logdebug("publisher_object Publisher Connected") + + rospy.logdebug("All Publishers READY") + + # Methods that the TrainingEnvironment will need to define here as virtual + # because they will be used in RobotGazeboEnv GrandParentClass and defined in the + # TrainingEnvironment. + # ---------------------------- + + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + raise NotImplementedError() + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _get_obs(self): + raise NotImplementedError() + + def _is_done(self, observations): + """Checks if episode done based on observations given. + """ + raise NotImplementedError() + + # Methods that the TrainingEnvironment will need. + # ---------------------------- + def move_joints(self, joints_array, epsilon=0.05, update_rate=10, time_sleep=0.05, check_position=True): + """ + It will move the Hopper Joints to the given Joint_Array values + """ + i = 0 + for publisher_object in self.publishers_array: + joint_value = Float64() + joint_value.data = joints_array[i] + rospy.logdebug("JointsPos>>"+str(joint_value)) + publisher_object.publish(joint_value) + i += 1 + + if check_position: + self.wait_time_for_execute_movement( + joints_array, epsilon, update_rate) + else: + self.wait_time_movement_hard(time_sleep=time_sleep) + + def wait_time_for_execute_movement(self, joints_array, epsilon, update_rate): + """ + We wait until Joints are where we asked them to be based on the joints_states + :param joints_array:Joints Values in radians of each of the three joints of hopper leg. + :param epsilon: Error acceptable in odometry readings. + :param update_rate: Rate at which we check the joint_states. + :return: + """ + rospy.logdebug("START wait_until_twist_achieved...") + + rate = rospy.Rate(update_rate) + start_wait_time = rospy.get_rostime().to_sec() + end_wait_time = 0.0 + + rospy.logdebug("Desired JointsState>>" + str(joints_array)) + rospy.logdebug("epsilon>>" + str(epsilon)) + + while not rospy.is_shutdown(): + current_joint_states = self._check_joint_states_ready() + + values_to_check = [current_joint_states.position[0], + current_joint_states.position[1], + current_joint_states.position[2]] + + vel_values_are_close = self.check_array_similar( + joints_array, values_to_check, epsilon) + + if vel_values_are_close: + rospy.logdebug("Reached JointStates!") + end_wait_time = rospy.get_rostime().to_sec() + break + rospy.logdebug("Not there yet, keep waiting...") + rate.sleep() + delta_time = end_wait_time - start_wait_time + rospy.logdebug("[Wait Time=" + str(delta_time)+"]") + + rospy.logdebug("END wait_until_jointstate_achieved...") + + return delta_time + + def wait_time_movement_hard(self, time_sleep): + """ + Hard Wait to avoid inconsistencies in times executing actions + """ + rospy.logdebug("Test Wait="+str(time_sleep)) + time.sleep(time_sleep) + + def check_array_similar(self, ref_value_array, check_value_array, epsilon): + """ + It checks if the check_value id similar to the ref_value + """ + rospy.logdebug("ref_value_array="+str(ref_value_array)) + rospy.logdebug("check_value_array="+str(check_value_array)) + return numpy.allclose(ref_value_array, check_value_array, atol=epsilon) + + def get_odom(self): + return self.odom + + def get_imu(self): + return self.imu + + def get_lowerleg_contactsensor_state(self): + return self.lowerleg_contactsensor_state + + def get_joint_states(self): + return self.joint_states diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/husarion_env.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/husarion_env.py new file mode 100644 index 0000000..fa4ffbc --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/husarion_env.py @@ -0,0 +1,361 @@ +import numpy +import rospy +from openai_ros import robot_gazebo_env +from std_msgs.msg import Float64 +from sensor_msgs.msg import JointState +from sensor_msgs.msg import Image +from sensor_msgs.msg import LaserScan +from sensor_msgs.msg import PointCloud2 +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Twist +from openai_ros.openai_ros_common import ROSLauncher + + +class HusarionEnv(robot_gazebo_env.RobotGazeboEnv): + """Superclass for all CubeSingleDisk environments. + """ + + def __init__(self, ros_ws_abspath): + """ + Initializes a new HusarionEnv environment. + Husarion doesnt use controller_manager, therefore we wont reset the + controllers in the standard fashion. For the moment we wont reset them. + + To check any topic we need to have the simulations running, we need to do two things: + 1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations + that are pause for whatever the reason + 2) If the simulation was running already for some reason, we need to reset the controlers. + This has to do with the fact that some plugins with tf, dont understand the reset of the simulation + and need to be reseted to work properly. + + The Sensors: The sensors accesible are the ones considered usefull for AI learning. + + Sensor Topic List: + * /odom : Odometry readings of the Base of the Robot + * /camera/depth/image_raw: 2d Depth image of the depth sensor. + * /camera/depth/points: Pointcloud sensor readings + * /camera/rgb/image_raw: RGB camera + * /scan: Laser Readings + + Actuators Topic List: /cmd_vel, + + Args: + """ + rospy.logerr(">>>>>>>>>>>Start HusarionEnv INIT...") + # Variables that we give through the constructor. + # None in this case + # We launch the ROSlaunch that spawns the robot into the world + ROSLauncher(rospackage_name="rosbot_gazebo", + launch_file_name="put_robot_in_world.launch", + ros_ws_abspath=ros_ws_abspath) + rospy.logerr(">>>>>>>>>>>ROSLAUCHER DONE HusarionEnv INIT...") + # Internal Vars + # Doesnt have any accesibles + self.controllers_list = [] + + # It doesnt use namespace + self.robot_name_space = "" + + # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv + super(HusarionEnv, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=False, + start_init_physics_parameters=False, + reset_world_or_sim="WORLD") + + self.gazebo.unpauseSim() + # self.controllers_object.reset_controllers() + self._check_all_sensors_ready() + + # We Start all the ROS related Subscribers and publishers + rospy.Subscriber("/odom", Odometry, self._odom_callback) + rospy.Subscriber("/camera/depth/image_raw", Image, + self._camera_depth_image_raw_callback) + rospy.Subscriber("/camera/depth/points", PointCloud2, + self._camera_depth_points_callback) + rospy.Subscriber("/camera/rgb/image_raw", Image, + self._camera_rgb_image_raw_callback) + rospy.Subscriber("/scan", LaserScan, + self._laser_scan_callback) + + self._cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) + + self._check_publishers_connection() + + self.gazebo.pauseSim() + + rospy.logdebug("Finished HusarionEnv INIT...") + + # Methods needed by the RobotGazeboEnv + # ---------------------------- + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + self._check_all_sensors_ready() + return True + + # CubeSingleDiskEnv virtual methods + # ---------------------------- + + def _check_all_sensors_ready(self): + rospy.logdebug("START ALL SENSORS READY") + self._check_odom_ready() + # We dont need to check for the moment, takes too long + self._check_camera_depth_image_raw_ready() + self._check_camera_depth_points_ready() + self._check_camera_rgb_image_raw_ready() + self._check_laser_scan_ready() + rospy.logdebug("ALL SENSORS READY") + + def _check_odom_ready(self): + self.odom = None + rospy.logdebug("Waiting for /odom to be READY...") + while self.odom is None and not rospy.is_shutdown(): + try: + self.odom = rospy.wait_for_message( + "/odom", Odometry, timeout=5.0) + rospy.logdebug("Current /odom READY=>") + + except: + rospy.logerr( + "Current /odom not ready yet, retrying for getting odom") + + return self.odom + + def _check_camera_depth_image_raw_ready(self): + self.camera_depth_image_raw = None + rospy.logdebug("Waiting for /camera/depth/image_raw to be READY...") + while self.camera_depth_image_raw is None and not rospy.is_shutdown(): + try: + self.camera_depth_image_raw = rospy.wait_for_message( + "/camera/depth/image_raw", Image, timeout=5.0) + rospy.logdebug("Current /camera/depth/image_raw READY=>") + + except: + rospy.logerr( + "Current /camera/depth/image_raw not ready yet, retrying for getting camera_depth_image_raw") + return self.camera_depth_image_raw + + def _check_camera_depth_points_ready(self): + self.camera_depth_points = None + rospy.logdebug("Waiting for /camera/depth/points to be READY...") + while self.camera_depth_points is None and not rospy.is_shutdown(): + try: + self.camera_depth_points = rospy.wait_for_message( + "/camera/depth/points", PointCloud2, timeout=10.0) + rospy.logdebug("Current /camera/depth/points READY=>") + + except: + rospy.logerr( + "Current /camera/depth/points not ready yet, retrying for getting camera_depth_points") + return self.camera_depth_points + + def _check_camera_rgb_image_raw_ready(self): + self.camera_rgb_image_raw = None + rospy.logdebug("Waiting for /camera/rgb/image_raw to be READY...") + while self.camera_rgb_image_raw is None and not rospy.is_shutdown(): + try: + self.camera_rgb_image_raw = rospy.wait_for_message( + "/camera/rgb/image_raw", Image, timeout=5.0) + rospy.logdebug("Current /camera/rgb/image_raw READY=>") + + except: + rospy.logerr( + "Current /camera/rgb/image_raw not ready yet, retrying for getting camera_rgb_image_raw") + return self.camera_rgb_image_raw + + def _check_laser_scan_ready(self): + self.laser_scan = None + rospy.logdebug("Waiting for /scan to be READY...") + while self.laser_scan is None and not rospy.is_shutdown(): + try: + self.laser_scan = rospy.wait_for_message( + "/scan", LaserScan, timeout=1.0) + rospy.logdebug("Current /scan READY=>") + + except: + rospy.logerr( + "Current /scan not ready yet, retrying for getting laser_scan") + return self.laser_scan + + def _odom_callback(self, data): + self.odom = data + + def _camera_depth_image_raw_callback(self, data): + self.camera_depth_image_raw = data + + def _camera_depth_points_callback(self, data): + self.camera_depth_points = data + + def _camera_rgb_image_raw_callback(self, data): + self.camera_rgb_image_raw = data + + def _laser_scan_callback(self, data): + self.laser_scan = data + + def _check_publishers_connection(self): + """ + Checks that all the publishers are working + :return: + """ + rate = rospy.Rate(10) # 10hz + while self._cmd_vel_pub.get_num_connections() == 0 and not rospy.is_shutdown(): + rospy.logdebug( + "No susbribers to _cmd_vel_pub yet so we wait and try again") + try: + rate.sleep() + except rospy.ROSInterruptException: + # This is to avoid error when world is rested, time when backwards. + pass + rospy.logdebug("_cmd_vel_pub Publisher Connected") + + rospy.logdebug("All Publishers READY") + + # Methods that the TrainingEnvironment will need to define here as virtual + # because they will be used in RobotGazeboEnv GrandParentClass and defined in the + # TrainingEnvironment. + # ---------------------------- + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + raise NotImplementedError() + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _get_obs(self): + raise NotImplementedError() + + def _is_done(self, observations): + """Checks if episode done based on observations given. + """ + raise NotImplementedError() + + # Methods that the TrainingEnvironment will need. + # ---------------------------- + def move_base(self, linear_speed, angular_speed, epsilon=0.05, update_rate=10): + """ + It will move the base based on the linear and angular speeds given. + It will wait untill those twists are achived reading from the odometry topic. + :param linear_speed: Speed in the X axis of the robot base frame + :param angular_speed: Speed of the angular turning of the robot base frame + :param epsilon: Acceptable difference between the speed asked and the odometry readings + :param update_rate: Rate at which we check the odometry. + :return: + """ + cmd_vel_value = Twist() + cmd_vel_value.linear.x = linear_speed + cmd_vel_value.angular.z = angular_speed + rospy.logdebug("Husarion Base Twist Cmd>>" + str(cmd_vel_value)) + self._check_publishers_connection() + self._cmd_vel_pub.publish(cmd_vel_value) + self.wait_until_twist_achieved(cmd_vel_value, + epsilon, + update_rate) + + def wait_until_twist_achieved(self, cmd_vel_value, epsilon, update_rate, angular_speed_noise=0.005): + """ + We wait for the cmd_vel twist given to be reached by the robot reading + Bare in mind that the angular wont be controled , because its too imprecise. + We will only consider to check if its moving or not inside the angular_speed_noise fluctiations it has. + from the odometry. + :param cmd_vel_value: Twist we want to wait to reach. + :param epsilon: Error acceptable in odometry readings. + :param update_rate: Rate at which we check the odometry. + :return: + """ + rospy.logwarn("START wait_until_twist_achieved...") + + rate = rospy.Rate(update_rate) + start_wait_time = rospy.get_rostime().to_sec() + end_wait_time = 0.0 + + rospy.logdebug("Desired Twist Cmd>>" + str(cmd_vel_value)) + rospy.logdebug("epsilon>>" + str(epsilon)) + + linear_speed = cmd_vel_value.linear.x + angular_speed = cmd_vel_value.angular.z + + angular_speed_is = self.check_angular_speed_dir( + angular_speed, angular_speed_noise) + + linear_speed_plus = linear_speed + epsilon + linear_speed_minus = linear_speed - epsilon + + while not rospy.is_shutdown(): + current_odometry = self._check_odom_ready() + odom_linear_vel = current_odometry.twist.twist.linear.x + odom_angular_vel = current_odometry.twist.twist.angular.z + + rospy.logdebug("Linear VEL=" + str(odom_linear_vel) + + ", ?RANGE=[" + str(linear_speed_minus) + ","+str(linear_speed_plus)+"]") + rospy.logdebug("Angular VEL=" + str(odom_angular_vel) + + ", angular_speed asked=[" + str(angular_speed)+"]") + + linear_vel_are_close = (odom_linear_vel <= linear_speed_plus) and ( + odom_linear_vel > linear_speed_minus) + + odom_angular_speed_is = self.check_angular_speed_dir( + odom_angular_vel, angular_speed_noise) + + # We check if its turning in the same diretion or has stopped + angular_vel_are_close = (angular_speed_is == odom_angular_speed_is) + + if linear_vel_are_close and angular_vel_are_close: + rospy.logwarn("Reached Velocity!") + end_wait_time = rospy.get_rostime().to_sec() + break + rospy.logwarn("Not there yet, keep waiting...") + rate.sleep() + delta_time = end_wait_time - start_wait_time + rospy.logdebug("[Wait Time=" + str(delta_time)+"]") + + rospy.logwarn("END wait_until_twist_achieved...") + + return delta_time + + def check_angular_speed_dir(self, angular_speed, angular_speed_noise): + """ + It States if the speed is zero, posititive or negative + """ + # We check if odom angular speed is positive or negative or "zero" + if (-angular_speed_noise < angular_speed <= angular_speed_noise): + angular_speed_is = 0 + elif angular_speed > angular_speed_noise: + angular_speed_is = 1 + elif angular_speed <= angular_speed_noise: + angular_speed_is = -1 + else: + angular_speed_is = 0 + rospy.logerr("Angular Speed has wrong value=="+str(angular_speed)) + + def get_odom(self): + return self.odom + + def get_camera_depth_image_raw(self): + return self.camera_depth_image_raw + + def get_camera_depth_points(self): + return self.camera_depth_points + + def get_camera_rgb_image_raw(self): + return self.camera_rgb_image_raw + + def get_laser_scan(self): + return self.laser_scan diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/iriwam_env.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/iriwam_env.py new file mode 100644 index 0000000..5947832 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/iriwam_env.py @@ -0,0 +1,481 @@ +import numpy +import rospy +import time +import tf +from openai_ros import robot_gazebo_env +from sensor_msgs.msg import Image +from sensor_msgs.msg import LaserScan +from sensor_msgs.msg import PointCloud2 +from control_msgs.msg import JointTrajectoryControllerState +from openai_ros.openai_ros_common import ROSLauncher +import actionlib +from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal +from trajectory_msgs.msg import JointTrajectoryPoint +from moveit_msgs.msg import JointLimits + + +class IriWamEnv(robot_gazebo_env.RobotGazeboEnv): + """Superclass for all IriWamEnv environments. + """ + + def __init__(self, ros_ws_abspath): + """ + Initializes a new IriWamEnv environment. + + To check any topic we need to have the simulations running, we need to do two things: + 1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations + that are pause for whatever the reason + 2) If the simulation was running already for some reason, we need to reset the controlers. + This has to do with the fact that some plugins with tf, dont understand the reset of the simulation + and need to be reseted to work properly. + + The Sensors: The sensors accesible are the ones considered usefull for AI learning. + + Sensor Topic List: + * /camera/depth/image_raw + * /camera/depth/points + * /camera/rgb/image_raw + * /laser_scan: Laser scan of the TCP + * /iri_wam/iri_wam_controller/state, control_msgs/JointTrajectoryControllerState: Gives desired, actual and error. + + Actuators Topic List: + * We publish int the action: /iri_wam/iri_wam_controller/follow_joint_trajectory/goal + + Args: + """ + rospy.logdebug("Start IriWamEnv INIT...") + # Variables that we give through the constructor. + # None in this case + # We launch the ROSlaunch that spawns the robot into the world + + ROSLauncher(rospackage_name="iri_wam_gazebo", + launch_file_name="put_robot_in_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Internal Vars + # Doesnt have any accesibles + self.controllers_list = [] + + # It doesnt use namespace + self.robot_name_space = "" + + # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv + super(IriWamEnv, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=False, + start_init_physics_parameters=False, + reset_world_or_sim="WORLD") + + rospy.logdebug("IriWamEnv unpause...") + self.gazebo.unpauseSim() + + self._check_all_systems_ready() + + rospy.Subscriber("/camera/depth/image_raw", Image, + self._camera_depth_image_raw_callback) + rospy.Subscriber("/camera/depth/points", PointCloud2, + self._camera_depth_points_callback) + rospy.Subscriber("/camera/rgb/image_raw", Image, + self._camera_rgb_image_raw_callback) + rospy.Subscriber("/laser_scan", LaserScan, self._laser_scan_callback) + rospy.Subscriber("/iri_wam/iri_wam_controller/state", + JointTrajectoryControllerState, self._joint_state_callback) + + self._setup_tf_listener() + self._setup_movement_system() + + self.gazebo.pauseSim() + + rospy.logdebug("Finished IriWamEnv INIT...") + + # Methods needed by the RobotGazeboEnv + # ---------------------------- + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + rospy.logdebug("IriWamEnv check_all_systems_ready...") + self._check_all_sensors_ready() + rospy.logdebug("END IriWamEnv _check_all_systems_ready...") + return True + + # CubeSingleDiskEnv virtual methods + # ---------------------------- + + def _check_all_sensors_ready(self): + rospy.logdebug("START ALL SENSORS READY") + # TODO: Here go the sensors like cameras and joint states + # self._check_camera_depth_image_raw_ready() + # self._check_camera_depth_points_ready() + # self._check_camera_rgb_image_raw_ready() + self._check_laser_scan_ready() + rospy.logdebug("ALL SENSORS READY") + + def _check_camera_depth_image_raw_ready(self): + self.camera_depth_image_raw = None + rospy.logdebug("Waiting for /camera/depth/image_raw to be READY...") + while self.camera_depth_image_raw is None and not rospy.is_shutdown(): + try: + self.camera_depth_image_raw = rospy.wait_for_message( + "/camera/depth/image_raw", Image, timeout=5.0) + rospy.logdebug("Current /camera/depth/image_raw READY=>") + + except: + rospy.logerr( + "Current /camera/depth/image_raw not ready yet, retrying for getting camera_depth_image_raw") + return self.camera_depth_image_raw + + def _check_camera_depth_points_ready(self): + self.camera_depth_points = None + rospy.logdebug("Waiting for /camera/depth/points to be READY...") + while self.camera_depth_points is None and not rospy.is_shutdown(): + try: + self.camera_depth_points = rospy.wait_for_message( + "/camera/depth/points", PointCloud2, timeout=10.0) + rospy.logdebug("Current /camera/depth/points READY=>") + + except: + rospy.logerr( + "Current /camera/depth/points not ready yet, retrying for getting camera_depth_points") + return self.camera_depth_points + + def _check_camera_rgb_image_raw_ready(self): + self.camera_rgb_image_raw = None + rospy.logdebug("Waiting for /camera/rgb/image_raw to be READY...") + while self.camera_rgb_image_raw is None and not rospy.is_shutdown(): + try: + self.camera_rgb_image_raw = rospy.wait_for_message( + "/camera/rgb/image_raw", Image, timeout=5.0) + rospy.logdebug("Current /camera/rgb/image_raw READY=>") + + except: + rospy.logerr( + "Current /camera/rgb/image_raw not ready yet, retrying for getting camera_rgb_image_raw") + return self.camera_rgb_image_raw + + def _check_laser_scan_ready(self): + self.laser_scan = None + rospy.logdebug("Waiting for /laser_scan to be READY...") + while self.laser_scan is None and not rospy.is_shutdown(): + try: + self.laser_scan = rospy.wait_for_message( + "/laser_scan", LaserScan, timeout=5.0) + rospy.logdebug("Current /laser_scan READY=>") + + except: + rospy.logerr( + "Current /laser_scan not ready yet, retrying for getting laser_scan") + return self.laser_scan + + def _check_joint_state_ready(self): + self.joint_state = None + rospy.logdebug( + "Waiting for /iri_wam/iri_wam_controller/state to be READY...") + while self.joint_state is None and not rospy.is_shutdown(): + try: + self.joint_state = rospy.wait_for_message( + "/iri_wam/iri_wam_controller/state", JointTrajectoryControllerState, timeout=5.0) + rospy.logdebug( + "Current /iri_wam/iri_wam_controller/state READY=>") + + except: + rospy.logerr( + "Current /iri_wam/iri_wam_controller/state not ready yet, retrying for getting laser_scan") + return self.joint_state + + def _camera_depth_image_raw_callback(self, data): + self.camera_depth_image_raw = data + + def _camera_depth_points_callback(self, data): + self.camera_depth_points = data + + def _camera_rgb_image_raw_callback(self, data): + self.camera_rgb_image_raw = data + + def _laser_scan_callback(self, data): + self.laser_scan = data + + def _joint_state_callback(self, data): + self.joint_state = data + + def _setup_tf_listener(self): + """ + Set ups the TF listener for getting the transforms you ask for. + """ + self.listener = tf.TransformListener() + + def _setup_movement_system(self): + """ + Setup of the movement system. + :return: + """ + self.traj_object = IriWamExecTrajectory() + + # Methods that the TrainingEnvironment will need to define here as virtual + # because they will be used in RobotGazeboEnv GrandParentClass and defined in the + # TrainingEnvironment. + # ---------------------------- + + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + raise NotImplementedError() + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _get_obs(self): + raise NotImplementedError() + + def _is_done(self, observations): + """Checks if episode done based on observations given. + """ + raise NotImplementedError() + + # Methods that the TrainingEnvironment will need. + # ---------------------------- + def move_joints_to_angle_blocking(self, joints_positions_array): + """ + It moves all the joints to the given position and doesnt exit until it reaches that position + :param: joints_positions_array: Its an array that ahas the desired joint positions in radians. The order of the + joints is: + [ "iri_wam_joint_1", + "iri_wam_joint_2", + "iri_wam_joint_3", + "iri_wam_joint_4", + "iri_wam_joint_5", + "iri_wam_joint_6", + "iri_wam_joint_7"] + + """ + self.traj_object.send_joints_positions(joints_positions_array) + + def get_tf_start_to_end_frames(self, start_frame_name, end_frame_name): + """ + Given two frames, it returns the transform from the start_frame_name to the end_frame_name. + It will only return something different to None if the TFs of the Two frames are in TF topic + published and are connected through the TF tree. + :param: start_frame_name: Start Frame of the TF transform + end_frame_name: End Frame of the TF transform + :return: trans,rot of the transform between the start and end frames. + """ + start_frame = "/"+start_frame_name + end_frame = "/"+end_frame_name + + trans, rot = None, None + + try: + (trans, rot) = self.listener.lookupTransform( + start_frame, end_frame, rospy.Time(0)) + except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): + rospy.logerr("TF start to end not ready YET...") + pass + + return trans, rot + + def get_camera_depth_image_raw(self): + return self.camera_depth_image_raw + + def get_camera_depth_points(self): + return self.camera_depth_points + + def get_camera_rgb_image_raw(self): + return self.camera_rgb_image_raw + + def get_laser_scan(self): + return self.laser_scan + + def get_joint_state(self): + return self.joint_state + + def get_joint_limits(self): + """ + name: [iri_wam_joint_1, iri_wam_joint_2, iri_wam_joint_3, iri_wam_joint_4, iri_wam_joint_5, + iri_wam_joint_6, iri_wam_joint_7] +position: [-2.453681702263566e-11, 5.5375411835534294e-05, -2.9760194308892096e-11, -0.0062733383258359865, 1.8740564655672642e-13, 2.6570746959997393e-05, 1.5187850976872141e-13] + """ + + name_array = ["iri_wam_joint_1", + "iri_wam_joint_2", + "iri_wam_joint_3", + "iri_wam_joint_4", + "iri_wam_joint_5", + "iri_wam_joint_6", + "iri_wam_joint_7"] + + up_limits_array = [2.6, + 2.0, + 2.8, + 3.1, + 1.24, + 1.6, + 3.0] + + down_limits_array = [-2.6, + -2.0, + -2.8, + -0.9, + -4.76, + -1.6, + -3.0] + + joint_limits_array = [] + for i in range(len(name_array)): + joint_limits = JointLimits() + joint_limits.min_position = down_limits_array[i] + joint_limits.max_position = up_limits_array[i] + joint_limits_array.append(joint_limits) + + return joint_limits_array + + def init_joint_limits(self): + """ + Get the Joint Limits, in the init fase where we need to unpause the simulation to get them + :return: joint_limits: The Joint Limits Dictionary, with names, angles, vel and effort limits. + """ + joint_limits = self.get_joint_limits() + return joint_limits + + +class IriWamExecTrajectory(object): + + def __init__(self): + + # create the connection to the action server + self.client = actionlib.SimpleActionClient( + '/iri_wam/iri_wam_controller/follow_joint_trajectory', FollowJointTrajectoryAction) + # waits until the action server is up and running + self.client.wait_for_server() + + self.init_goal_message() + + def init_goal_message(self): + """ + We initialise the variable that we will use to send the goals. + We will reuse it because most of the values are fixed. + """ + + self.PENDING = 0 + self.ACTIVE = 1 + self.DONE = 2 + self.WARN = 3 + self.ERROR = 4 + + # We Initialise the GOAL SYETS GOINT TO INIT POSE + # creates a goal to send to the action server + self.goal = FollowJointTrajectoryGoal() + + # We fill in the Goal + + self.goal.trajectory.header.stamp = rospy.Time.now() + self.goal.trajectory.header.frame_id = "iri_wam_link_base" + self.goal.trajectory.joint_names = ["iri_wam_joint_1", + "iri_wam_joint_2", + "iri_wam_joint_3", + "iri_wam_joint_4", + "iri_wam_joint_5", + "iri_wam_joint_6", + "iri_wam_joint_7"] + + # Some of them dont quite coincide with the URDF limits, just because those limits break the simulation. + self.max_values = [2.6, + 2.0, + 2.8, + 3.0, + 1.24, + 1.5, + 3.0] + + self.min_values = [-2.6, + -2.0, + -2.8, + -0.9, + -1.24, + -1.4, + -3.0] + + self.goal.trajectory.points = [] + joint_traj_point = JointTrajectoryPoint() + + # TODO + joint_traj_point.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + joint_traj_point.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + joint_traj_point.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + joint_traj_point.effort = [] + joint_traj_point.time_from_start = rospy.Duration.from_sec(1.0) + + self.goal.trajectory.points.append(joint_traj_point) + + def get_goal(self): + return self.goal + + def feedback_callback(self, feedback): + + rospy.loginfo("##### FEEDBACK ######") + # rospy.loginfo(str(feedback.joint_names)) + # rospy.loginfo(str(feedback.desired.positions)) + # rospy.loginfo(str(feedback.actual.positions)) + rospy.loginfo(str(feedback.error.positions)) + rospy.loginfo("##### ###### ######") + + def send_joints_positions(self, joints_positions_array, seconds_duration=0.05): + rospy.logdebug("send_joints_positions: "+str(joints_positions_array)) + my_goal = self.get_goal() + + my_goal.trajectory.header.stamp = rospy.Time.now() + joint_traj_point = JointTrajectoryPoint() + + # We clamp the values to max and min to avoid asking configurations that IriWam cant reach. + + joint_traj_point.positions = numpy.clip(joints_positions_array, + self.min_values, + self.max_values).tolist() + joint_traj_point.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + joint_traj_point.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + joint_traj_point.effort = [] + joint_traj_point.time_from_start = rospy.Duration.from_sec( + seconds_duration) + + my_goal.trajectory.points = [] + my_goal.trajectory.points.append(joint_traj_point) + + # sends the goal to the action server, specifying which feedback function + # to call when feedback received + self.client.send_goal(my_goal, feedback_cb=self.feedback_callback) + + # Uncomment these lines to test goal preemption: + # self.client.cancel_goal() # would cancel the goal 3 seconds after starting + + # state_result = self.client.get_state() + + # rate = rospy.Rate(10) + + # rospy.loginfo("state_result: "+str(state_result)) + + # while state_result < self.DONE: + # rospy.loginfo( + # "Doing Stuff while waiting for the Server to give a result....") + # rate.sleep() + # state_result = self.client.get_state() + # rospy.loginfo("state_result: "+str(state_result)) + + # rospy.loginfo("[Result] State: "+str(state_result)) + # if state_result == self.ERROR: + # rospy.logerr("Something went wrong in the Server Side") + # if state_result == self.WARN: + # rospy.logdebug("There is a warning in the Server Side") diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/parrotdrone_env.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/parrotdrone_env.py new file mode 100644 index 0000000..dbab951 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/parrotdrone_env.py @@ -0,0 +1,502 @@ +import numpy +import rospy +import time +from openai_ros import robot_gazebo_env +from std_msgs.msg import Float64 +from sensor_msgs.msg import JointState +from sensor_msgs.msg import Image +from sensor_msgs.msg import LaserScan +from sensor_msgs.msg import PointCloud2 +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Twist +from sensor_msgs.msg import Range +from sensor_msgs.msg import Imu +from geometry_msgs.msg import Pose +from std_msgs.msg import Empty +from openai_ros.openai_ros_common import ROSLauncher + + +class ParrotDroneEnv(robot_gazebo_env.RobotGazeboEnv): + """Superclass for all CubeSingleDisk environments. + """ + + def __init__(self, ros_ws_abspath): + """ + Initializes a new ParrotDroneEnv environment. + + To check any topic we need to have the simulations running, we need to do two things: + 1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations + that are pause for whatever the reason + 2) If the simulation was running already for some reason, we need to reset the controlers. + This has to do with the fact that some plugins with tf, dont understand the reset of the simulation + and need to be reseted to work properly. + + The Sensors: The sensors accesible are the ones considered usefull for AI learning. + + Sensor Topic List: + * /drone/down_camera/image_raw: RGB Camera facing down. + * /drone/front_camera/image_raw: RGB Camera facing front. + * /drone/imu: IMU of the drone giving acceleration and orientation relative to world. + * /drone/sonar: Sonar readings facing front + * /drone/gt_pose: Get position and orientation in Global space + * /drone/gt_vel: Get the linear velocity , the angular doesnt record anything. + + Actuators Topic List: + * /cmd_vel: Move the Drone Around when you have taken off. + * /drone/takeoff: Publish into it to take off + * /drone/land: Publish to make ParrotDrone Land + + Args: + """ + rospy.logdebug("Start ParrotDroneEnv INIT...") + + # Variables that we give through the constructor. + # None in this case + + # Internal Vars + # Doesnt have any accesibles + self.controllers_list = [] + + # It doesnt use namespace + self.robot_name_space = "" + + # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv + super(ParrotDroneEnv, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=False, + start_init_physics_parameters=False, + reset_world_or_sim="WORLD") + + self.gazebo.unpauseSim() + + ROSLauncher(rospackage_name="drone_construct", + launch_file_name="put_robot_in_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # self.controllers_object.reset_controllers() + self._check_all_sensors_ready() + + # We Start all the ROS related Subscribers and publishers + rospy.Subscriber("/drone/down_camera/image_raw", Image, + self._down_camera_rgb_image_raw_callback) + rospy.Subscriber("/drone/front_camera/image_raw", Image, + self._front_camera_rgb_image_raw_callback) + rospy.Subscriber("/drone/imu", Imu, self._imu_callback) + rospy.Subscriber("/drone/sonar", Range, self._sonar_callback) + rospy.Subscriber("/drone/gt_pose", Pose, self._gt_pose_callback) + rospy.Subscriber("/drone/gt_vel", Twist, self._gt_vel_callback) + + self._cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) + self._takeoff_pub = rospy.Publisher( + '/drone/takeoff', Empty, queue_size=1) + self._land_pub = rospy.Publisher('/drone/land', Empty, queue_size=1) + + self._check_all_publishers_ready() + + self.gazebo.pauseSim() + + rospy.logdebug("Finished ParrotDroneEnv INIT...") + + # Methods needed by the RobotGazeboEnv + # ---------------------------- + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + self._check_all_sensors_ready() + return True + + # CubeSingleDiskEnv virtual methods + # ---------------------------- + + def _check_all_sensors_ready(self): + rospy.logdebug("START ALL SENSORS READY") + self._check_down_camera_rgb_image_raw_ready() + self._check_front_camera_rgb_image_raw_ready() + self._check_imu_ready() + self._check_sonar_ready() + self._check_gt_pose_ready() + self._check_gt_vel_ready() + rospy.logdebug("ALL SENSORS READY") + + def _check_down_camera_rgb_image_raw_ready(self): + self.down_camera_rgb_image_raw = None + rospy.logdebug( + "Waiting for /drone/down_camera/image_raw to be READY...") + while self.down_camera_rgb_image_raw is None and not rospy.is_shutdown(): + try: + self.down_camera_rgb_image_raw = rospy.wait_for_message( + "/drone/down_camera/image_raw", Image, timeout=5.0) + rospy.logdebug("Current /drone/down_camera/image_raw READY=>") + + except: + rospy.logerr( + "Current /drone/down_camera/image_raw not ready yet, retrying for getting down_camera_rgb_image_raw") + return self.down_camera_rgb_image_raw + + def _check_front_camera_rgb_image_raw_ready(self): + self.front_camera_rgb_image_raw = None + rospy.logdebug( + "Waiting for /drone/front_camera/image_raw to be READY...") + while self.front_camera_rgb_image_raw is None and not rospy.is_shutdown(): + try: + self.front_camera_rgb_image_raw = rospy.wait_for_message( + "/drone/front_camera/image_raw", Image, timeout=5.0) + rospy.logdebug("Current /drone/front_camera/image_raw READY=>") + + except: + rospy.logerr( + "Current /drone/front_camera/image_raw not ready yet, retrying for getting front_camera_rgb_image_raw") + return self.front_camera_rgb_image_raw + + def _check_imu_ready(self): + self.imu = None + rospy.logdebug("Waiting for /drone/imu to be READY...") + while self.imu is None and not rospy.is_shutdown(): + try: + self.imu = rospy.wait_for_message( + "/drone/imu", Imu, timeout=5.0) + rospy.logdebug("Current/drone/imu READY=>") + + except: + rospy.logerr( + "Current /drone/imu not ready yet, retrying for getting imu") + + return self.imu + + def _check_sonar_ready(self): + self.sonar = None + rospy.logdebug("Waiting for /drone/sonar to be READY...") + while self.sonar is None and not rospy.is_shutdown(): + try: + self.sonar = rospy.wait_for_message( + "/drone/sonar", Range, timeout=5.0) + rospy.logdebug("Current/drone/sonar READY=>") + + except: + rospy.logerr( + "Current /drone/sonar not ready yet, retrying for getting sonar") + + return self.sonar + + def _check_gt_pose_ready(self): + self.gt_pose = None + rospy.logdebug("Waiting for /drone/gt_pose to be READY...") + while self.gt_pose is None and not rospy.is_shutdown(): + try: + self.gt_pose = rospy.wait_for_message( + "/drone/gt_pose", Pose, timeout=5.0) + rospy.logdebug("Current /drone/gt_pose READY=>") + + except: + rospy.logerr( + "Current /drone/gt_pose not ready yet, retrying for getting gt_pose") + + return self.gt_pose + + def _check_gt_vel_ready(self): + self.gt_vel = None + rospy.logdebug("Waiting for /drone/gt_vel to be READY...") + while self.gt_vel is None and not rospy.is_shutdown(): + try: + self.gt_vel = rospy.wait_for_message( + "/drone/gt_vel", Twist, timeout=5.0) + rospy.logdebug("Current /drone/gt_vel READY=>") + + except: + rospy.logerr( + "Current /drone/gt_vel not ready yet, retrying for getting gt_vel") + + return self.gt_vel + + def _down_camera_rgb_image_raw_callback(self, data): + self.down_camera_rgb_image_raw = data + + def _front_camera_rgb_image_raw_callback(self, data): + self.front_camera_rgb_image_raw = data + + def _imu_callback(self, data): + self.imu = data + + def _sonar_callback(self, data): + self.sonar = data + + def _gt_pose_callback(self, data): + self.gt_pose = data + + def _gt_vel_callback(self, data): + self.gt_vel = data + + def _check_all_publishers_ready(self): + """ + Checks that all the publishers are working + :return: + """ + rospy.logdebug("START ALL SENSORS READY") + self._check_cmd_vel_pub_connection() + self._check_takeoff_pub_connection() + self._check_land_pub_connection() + rospy.logdebug("ALL SENSORS READY") + + def _check_cmd_vel_pub_connection(self): + + rate = rospy.Rate(10) # 10hz + while self._cmd_vel_pub.get_num_connections() == 0 and not rospy.is_shutdown(): + rospy.logdebug( + "No susbribers to _cmd_vel_pub yet so we wait and try again") + try: + rate.sleep() + except rospy.ROSInterruptException: + # This is to avoid error when world is rested, time when backwards. + pass + rospy.logdebug("_cmd_vel_pub Publisher Connected") + + rospy.logdebug("All Publishers READY") + + def _check_takeoff_pub_connection(self): + + rate = rospy.Rate(10) # 10hz + while self._takeoff_pub.get_num_connections() == 0 and not rospy.is_shutdown(): + rospy.logdebug( + "No susbribers to _takeoff_pub yet so we wait and try again") + try: + rate.sleep() + except rospy.ROSInterruptException: + # This is to avoid error when world is rested, time when backwards. + pass + rospy.logdebug("_takeoff_pub Publisher Connected") + + rospy.logdebug("All Publishers READY") + + def _check_land_pub_connection(self): + + rate = rospy.Rate(10) # 10hz + while self._land_pub.get_num_connections() == 0 and not rospy.is_shutdown(): + rospy.logdebug( + "No susbribers to _land_pub yet so we wait and try again") + try: + rate.sleep() + except rospy.ROSInterruptException: + # This is to avoid error when world is rested, time when backwards. + pass + rospy.logdebug("_land_pub Publisher Connected") + + rospy.logdebug("All Publishers READY") + + # Methods that the TrainingEnvironment will need to define here as virtual + # because they will be used in RobotGazeboEnv GrandParentClass and defined in the + # TrainingEnvironment. + # ---------------------------- + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + raise NotImplementedError() + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _get_obs(self): + raise NotImplementedError() + + def _is_done(self, observations): + """Checks if episode done based on observations given. + """ + raise NotImplementedError() + + # Methods that the TrainingEnvironment will need. + # ---------------------------- + + def takeoff(self): + """ + Sends the takeoff command and checks it has taken of + It unpauses the simulation and pauses again + to allow it to be a self contained action + """ + self.gazebo.unpauseSim() + self._check_takeoff_pub_connection() + + takeoff_cmd = Empty() + self._takeoff_pub.publish(takeoff_cmd) + + # When it takes of value of height is around 1.3 + self.wait_for_height(heigh_value_to_check=0.8, + smaller_than=False, + epsilon=0.05, + update_rate=10) + self.gazebo.pauseSim() + + def land(self): + """ + Sends the Landing command and checks it has landed + It unpauses the simulation and pauses again + to allow it to be a self contained action + """ + self.gazebo.unpauseSim() + + self._check_land_pub_connection() + + land_cmd = Empty() + self._land_pub.publish(land_cmd) + # When Drone is on the floor, the readings are 0.5 + self.wait_for_height(heigh_value_to_check=0.6, + smaller_than=True, + epsilon=0.05, + update_rate=10) + + self.gazebo.pauseSim() + + def wait_for_height(self, heigh_value_to_check, smaller_than, epsilon, update_rate): + """ + Checks if current height is smaller or bigger than a value + :param: smaller_than: If True, we will wait until value is smaller than the one given + """ + + rate = rospy.Rate(update_rate) + start_wait_time = rospy.get_rostime().to_sec() + end_wait_time = 0.0 + + rospy.logdebug("epsilon>>" + str(epsilon)) + + while not rospy.is_shutdown(): + current_gt_pose = self._check_gt_pose_ready() + + current_height = current_gt_pose.position.z + + if smaller_than: + takeoff_height_achieved = current_height <= heigh_value_to_check + rospy.logwarn("SMALLER THAN HEIGHT...current_height=" + + str(current_height)+"<="+str(heigh_value_to_check)) + else: + takeoff_height_achieved = current_height >= heigh_value_to_check + rospy.logwarn("BIGGER THAN HEIGHT...current_height=" + + str(current_height)+">="+str(heigh_value_to_check)) + + if takeoff_height_achieved: + rospy.logwarn("Reached Height!") + end_wait_time = rospy.get_rostime().to_sec() + break + rospy.logwarn("Height Not there yet, keep waiting...") + rate.sleep() + + def move_base(self, linear_speed_vector, angular_speed, epsilon=0.05, update_rate=10): + """ + It will move the base based on the linear and angular speeds given. + It will wait untill those twists are achived reading from the odometry topic. + :param linear_speed_vector: Speed in the XYZ axis of the robot base frame, because drones can move in any direction + :param angular_speed: Speed of the angular turning of the robot base frame, because this drone only turns on the Z axis. + :param epsilon: Acceptable difference between the speed asked and the odometry readings + :param update_rate: Rate at which we check the odometry. + :return: + """ + cmd_vel_value = Twist() + cmd_vel_value.linear.x = linear_speed_vector.x + cmd_vel_value.linear.y = linear_speed_vector.y + cmd_vel_value.linear.z = linear_speed_vector.z + cmd_vel_value.angular.z = angular_speed + rospy.logdebug("TurtleBot2 Base Twist Cmd>>" + str(cmd_vel_value)) + self._check_cmd_vel_pub_connection() + self._cmd_vel_pub.publish(cmd_vel_value) + """ + self.wait_until_twist_achieved(cmd_vel_value, + epsilon, + update_rate) + """ + self.wait_time_for_execute_movement() + + def wait_time_for_execute_movement(self): + """ + Because this Parrot Drone position is global, we really dont have + a way to know if its moving in the direction desired, because it would need + to evaluate the diference in position and speed on the local reference. + """ + time.sleep(1.0) + + def wait_until_twist_achieved(self, cmd_vel_value, epsilon, update_rate): + """ + # TODO: Make it work using TF conversions + We wait for the cmd_vel twist given to be reached by the robot reading + from the odometry. + :param cmd_vel_value: Twist we want to wait to reach. + :param epsilon: Error acceptable in odometry readings. + :param update_rate: Rate at which we check the odometry. + :return: + """ + rospy.logwarn("START wait_until_twist_achieved...") + + rate = rospy.Rate(update_rate) + start_wait_time = rospy.get_rostime().to_sec() + end_wait_time = 0.0 + epsilon = 0.05 + + rospy.logwarn("Desired Twist Cmd>>" + str(cmd_vel_value)) + rospy.logwarn("epsilon>>" + str(epsilon)) + + values_of_ref = [cmd_vel_value.linear.x, + cmd_vel_value.linear.y, + cmd_vel_value.linear.z, + cmd_vel_value.angular.z] + + while not rospy.is_shutdown(): + current_gt_vel = self._check_gt_vel_ready() + + values_to_check = [current_gt_vel.linear.x, + current_gt_vel.linear.y, + current_gt_vel.linear.z, + current_gt_vel.angular.z] + + vel_values_are_close = self.check_array_similar( + values_of_ref, values_to_check, epsilon) + + if vel_values_are_close: + rospy.logwarn("Reached Velocity!") + end_wait_time = rospy.get_rostime().to_sec() + break + rospy.logwarn("Not there yet, keep waiting...") + rate.sleep() + delta_time = end_wait_time - start_wait_time + rospy.logdebug("[Wait Time=" + str(delta_time)+"]") + + rospy.logwarn("END wait_until_twist_achieved...") + + return delta_time + + def check_array_similar(self, ref_value_array, check_value_array, epsilon): + """ + It checks if the check_value id similar to the ref_value + """ + rospy.logwarn("ref_value_array="+str(ref_value_array)) + rospy.logwarn("check_value_array="+str(check_value_array)) + return numpy.allclose(ref_value_array, check_value_array, atol=epsilon) + + def get_down_camera_rgb_image_raw(self): + return self.down_camera_rgb_image_raw + + def get_front_camera_rgb_image_raw(self): + return self.front_camera_rgb_image_raw + + def get_imu(self): + return self.imu + + def get_sonar(self): + return self.sonar + + def get_gt_pose(self): + return self.gt_pose + + def get_gt_vel(self): + return self.gt_vel diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/sawyer_env.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/sawyer_env.py new file mode 100644 index 0000000..2c73d04 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/sawyer_env.py @@ -0,0 +1,381 @@ +import numpy +import rospy +import time +import tf +from openai_ros import robot_gazebo_env +import intera_interface +import intera_external_devices +from intera_interface import CHECK_VERSION +from intera_core_msgs.msg import JointLimits +from sensor_msgs.msg import Image +from openai_ros.openai_ros_common import ROSLauncher + + +class SawyerEnv(robot_gazebo_env.RobotGazeboEnv): + """Superclass for all SawyerEnv environments. + """ + + def __init__(self, ros_ws_abspath): + """ + Initializes a new SawyerEnv environment. + + To check any topic we need to have the simulations running, we need to do two things: + 1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations + that are pause for whatever the reason + 2) If the simulation was running already for some reason, we need to reset the controlers. + This has to do with the fact that some plugins with tf, dont understand the reset of the simulation + and need to be reseted to work properly. + + The Sensors: The sensors accesible are the ones considered usefull for AI learning. + + Sensor Topic List: + * /robot/joint_limits: Odometry of the Base of Wamv + + Actuators Topic List: + * As actuator we will use a class to interface with the movements through commands. + + Args: + """ + rospy.logdebug("Start SawyerEnv INIT...") + # Variables that we give through the constructor. + # None in this case + + # We launch the ROSlaunch that spawns the robot into the world + ROSLauncher(rospackage_name="sawyer_gazebo", + launch_file_name="put_sawyer_in_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Internal Vars + # Doesnt have any accesibles + self.controllers_list = [] + + # It doesnt use namespace + self.robot_name_space = "" + + # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv + super(SawyerEnv, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=False, + start_init_physics_parameters=False, + reset_world_or_sim="WORLD") + + rospy.logdebug("SawyerEnv unpause...") + self.gazebo.unpauseSim() + # self.controllers_object.reset_controllers() + + # TODO: Fill it with the sensors + self._check_all_systems_ready() + + rospy.Subscriber("/io/internal_camera/head_camera/image_raw", + Image, self._head_camera_image_raw_callback) + rospy.Subscriber("/io/internal_camera/right_hand_camera/image_raw", + Image, self._right_hand_camera_image_raw_callback) + + self._setup_tf_listener() + self._setup_movement_system() + + self.gazebo.pauseSim() + + rospy.logdebug("Finished SawyerEnv INIT...") + + # Methods needed by the RobotGazeboEnv + # ---------------------------- + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + rospy.logdebug("SawyerEnv check_all_systems_ready...") + self._check_all_sensors_ready() + rospy.logdebug("END SawyerEnv _check_all_systems_ready...") + return True + + # CubeSingleDiskEnv virtual methods + # ---------------------------- + + def _check_all_sensors_ready(self): + rospy.logdebug("START ALL SENSORS READY") + # TODO: Here go the sensors like cameras and joint states + self._check_head_camera_image_raw_ready() + self._check_right_hand_camera_image_raw_ready() + rospy.logdebug("ALL SENSORS READY") + + def _check_head_camera_image_raw_ready(self): + self.head_camera_image_raw = None + rospy.logdebug( + "Waiting for /io/internal_camera/head_camera/image_raw to be READY...") + while self.head_camera_image_raw is None and not rospy.is_shutdown(): + try: + self.head_camera_image_raw = rospy.wait_for_message( + "/io/internal_camera/head_camera/image_raw", Image, timeout=5.0) + rospy.logdebug( + "Current /io/internal_camera/head_camera/image_raw READY=>") + + except: + rospy.logerr( + "Current /io/internal_camera/head_camera/image_raw not ready yet, retrying for getting head_camera_image_raw") + return self.head_camera_image_raw + + def _check_right_hand_camera_image_raw_ready(self): + self.right_hand_camera_image_raw = None + rospy.logdebug( + "Waiting for /io/internal_camera/right_hand_camera/image_raw to be READY...") + while self.right_hand_camera_image_raw is None and not rospy.is_shutdown(): + try: + self.right_hand_camera_image_raw = rospy.wait_for_message( + "/io/internal_camera/right_hand_camera/image_raw", Image, timeout=5.0) + rospy.logdebug( + "Current /io/internal_camera/right_hand_camera/image_raw READY=>") + + except: + rospy.logerr( + "Current /io/internal_camera/right_hand_camera/image_raw not ready yet, retrying for getting right_hand_camera_image_raw") + return self.right_hand_camera_image_raw + + def _head_camera_image_raw_callback(self, data): + self.head_camera_image_raw = data + + def _right_hand_camera_image_raw_callback(self, data): + self.right_hand_camera_image_raw = data + + def _setup_tf_listener(self): + """ + Set ups the TF listener for getting the transforms you ask for. + """ + self.listener = tf.TransformListener() + + def _setup_movement_system(self): + """ + Setup of the movement system. + :return: + """ + rp = intera_interface.RobotParams() + valid_limbs = rp.get_limb_names() + if not valid_limbs: + rp.log_message(("Cannot detect any limb parameters on this robot. " + "Exiting."), "ERROR") + return + + rospy.loginfo("Valid Sawyer Limbs==>"+str(valid_limbs)) + + print("Getting robot state... ") + rs = intera_interface.RobotEnable(CHECK_VERSION) + init_state = rs.state().enabled + + rospy.loginfo("Enabling robot...") + rs.enable() + self._map_actions_to_movement() + + def _map_actions_to_movement(self, side="right", joint_delta=0.1): + + self.limb = intera_interface.Limb(side) + + try: + self.gripper = intera_interface.Gripper(side + '_gripper') + except: + self.has_gripper = False + rospy.loginfo("The electric gripper is not detected on the robot.") + else: + self.has_gripper = True + + self.joints = self.limb.joint_names() + + self.bindings = { + self.joints[0]+"_increase": (self.set_j, [self.joints[0], joint_delta], self.joints[0]+" increase"), + self.joints[0]+"_decrease": (self.set_j, [self.joints[0], -joint_delta], self.joints[0]+" decrease"), + self.joints[1]+"_increase": (self.set_j, [self.joints[1], joint_delta], self.joints[1]+" increase"), + self.joints[1]+"_decrease": (self.set_j, [self.joints[1], -joint_delta], self.joints[1]+" decrease"), + self.joints[2]+"_increase": (self.set_j, [self.joints[2], joint_delta], self.joints[2]+" increase"), + self.joints[2]+"_decrease": (self.set_j, [self.joints[2], -joint_delta], self.joints[2]+" decrease"), + self.joints[3]+"_increase": (self.set_j, [self.joints[3], joint_delta], self.joints[3]+" increase"), + self.joints[3]+"_decrease": (self.set_j, [self.joints[3], -joint_delta], self.joints[3]+" decrease"), + self.joints[4]+"_increase": (self.set_j, [self.joints[4], joint_delta], self.joints[4]+" increase"), + self.joints[4]+"_decrease": (self.set_j, [self.joints[4], -joint_delta], self.joints[4]+" decrease"), + self.joints[5]+"_increase": (self.set_j, [self.joints[5], joint_delta], self.joints[5]+" increase"), + self.joints[5]+"_decrease": (self.set_j, [self.joints[5], -joint_delta], self.joints[5]+" decrease"), + self.joints[6]+"_increase": (self.set_j, [self.joints[6], joint_delta], self.joints[6]+" increase"), + self.joints[6]+"_decrease": (self.set_j, [self.joints[6], -joint_delta], self.joints[6]+" decrease") + } + if self.has_gripper: + self.bindings.update({ + "close": (self.set_g, "close", side+" gripper close"), + "open": (self.set_g, "open", side+" gripper open"), + "calibrate": (self.set_g, "calibrate", side+" gripper calibrate") + }) + + rospy.loginfo("Controlling joints...") + + # Methods that the TrainingEnvironment will need to define here as virtual + # because they will be used in RobotGazeboEnv GrandParentClass and defined in the + # TrainingEnvironment. + # ---------------------------- + + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + raise NotImplementedError() + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _get_obs(self): + raise NotImplementedError() + + def _is_done(self, observations): + """Checks if episode done based on observations given. + """ + raise NotImplementedError() + + # Methods that the TrainingEnvironment will need. + # ---------------------------- + def execute_movement(self, action_id): + """ + It executed the command given through an id. This will move any joint + of Sawyer, including the gripper if it has it. + :param: action_id: These are the possible action_id values and the action asociated. + + self.joints[0]+"_increase", + self.joints[0]+_decrease, + self.joints[1]+"_increase", + self.joints[1]+"_decrease", + self.joints[2]+"_increase", + self.joints[2]+"_decrease", + self.joints[3]+"_increase", + self.joints[3]+"_decrease", + self.joints[4]+"_increase", + self.joints[4]+"_decrease", + self.joints[5]+"_increase", + self.joints[5]+"_decrease", + self.joints[6]+"_increase", + self.joints[6]+"_decrease", + gripper_close, + gripper_open, + gripper_calibrate + """ + + if action_id in self.bindings: + cmd = self.bindings[action_id] + if action_id == "gripper_close" or action_id == "gripper_open" or action_id == "gripper_calibrate": + cmd[0](cmd[1]) + rospy.loginfo("command: %s" % (cmd[2],)) + else: + # expand binding to something like "self.set_j(right, 'j0', joint_delta)" + cmd[0](*cmd[1]) + rospy.loginfo("command: %s" % (cmd[2],)) + else: + rospy.logerr("NOT VALID key binding, it should be one of these: ") + for key, val in sorted(self.bindings.items(), + key=lambda x: x[1][2]): + rospy.logerr(" %s: %s" % (key, val[2])) + + def set_j(self, joint_name, delta): + current_position = self.limb.joint_angle(joint_name) + joint_command = {joint_name: current_position + delta} + self.limb.set_joint_positions(joint_command) + + def set_g(self, action): + if self.has_gripper: + if action == "close": + self.gripper.close() + elif action == "open": + self.gripper.open() + elif action == "calibrate": + self.gripper.calibrate() + + def move_joints_to_angle_blocking(self, joint_positions_dict, timeout=15.0, threshold=0.008726646): + """ + It moves all the joints to the given position and doesnt exit until it reaches that position + """ + self.limb.move_to_joint_positions(positions=joint_positions_dict, + timeout=15.0, + threshold=0.008726646, + test=None) + + def get_limb_joint_names_array(self): + """ + Returns the Joint Names array of the Limb. + """ + return self.joints + + def get_all_limb_joint_angles(self): + """ + Return dictionary dict({str:float}) with all the joints angles + """ + return self.limb.joint_angles() + + def get_all_limb_joint_efforts(self): + """ + Returns a dictionary dict({str:float}) with all the joints efforts + """ + return self.limb.joint_efforts() + + def get_tf_start_to_end_frames(self, start_frame_name, end_frame_name): + """ + Given two frames, it returns the transform from the start_frame_name to the end_frame_name. + It will only return something different to None if the TFs of the Two frames are in TF topic + published and are connected through the TF tree. + :param: start_frame_name: Start Frame of the TF transform + end_frame_name: End Frame of the TF transform + :return: trans,rot of the transform between the start and end frames. + """ + start_frame = "/"+start_frame_name + end_frame = "/"+end_frame_name + + trans, rot = None, None + while (trans is None or rot is None) and not rospy.is_shutdown(): + try: + (trans, rot) = self.listener.lookupTransform( + start_frame, end_frame, rospy.Time(0)) + except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): + rospy.logerr("TF start to end not ready YET...") + duration_obj = rospy.Duration.from_sec(1.0) + rospy.sleep(duration_obj) + + return trans, rot + + def check_joint_limits_ready(self): + self.joint_limits = None + rospy.logdebug("Waiting for /robot/joint_limits to be READY...") + while self.joint_limits is None and not rospy.is_shutdown(): + try: + self.joint_limits = rospy.wait_for_message( + "/robot/joint_limits", JointLimits, timeout=3.0) + rospy.logdebug("Current /robot/joint_limits READY=>") + + except: + rospy.logerr( + "Current /robot/joint_limits not ready yet, retrying for getting joint_limits") + return self.joint_limits + + def get_joint_limits(self): + return self.joint_limits + + def get_head_camera_image_raw(self): + return self.head_camera_image_raw + + def get_right_hand_camera_image_raw(self): + return self.right_hand_camera_image_raw + + def init_joint_limits(self): + """ + Get the Joint Limits, in the init fase where we need to unpause the simulation to get them + :return: joint_limits: The Joint Limits Dictionary, with names, angles, vel and effort limits. + """ + self.gazebo.unpauseSim() + joint_limits = self.check_joint_limits_ready() + self.gazebo.pauseSim() + return joint_limits diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/shadow_tc_env.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/shadow_tc_env.py new file mode 100644 index 0000000..2dc8070 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/shadow_tc_env.py @@ -0,0 +1,382 @@ +import numpy +import rospy +import time +import tf +from openai_ros import robot_gazebo_env +from sensor_msgs.msg import Imu +from sensor_msgs.msg import JointState +from moveit_msgs.msg import PlanningScene +from openai_ros.openai_ros_common import ROSLauncher + +class ShadowTcEnv(robot_gazebo_env.RobotGazeboEnv): + """Superclass for all ShadowTcEnv environments. + """ + + def __init__(self, ros_ws_abspath): + """ + Initializes a new ShadowTcEnv environment. + + To check any topic we need to have the simulations running, we need to do two things: + 1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations + that are pause for whatever the reason + 2) If the simulation was running already for some reason, we need to reset the controlers. + This has to do with the fact that some plugins with tf, dont understand the reset of the simulation + and need to be reseted to work properly. + + The Sensors: The sensors accesible are the ones considered usefull for AI learning. + + Sensor Topic List: + * /imu/data + * /joint_states + + + Actuators Topic List: + * As actuator we will use a class SmartGrasper to interface. + We use smart_grasping_sandbox smart_grasper.py, to move and get the pose + of the ball and the tool tip. + + Args: + """ + rospy.logdebug("Start ShadowTcEnv INIT...") + # Variables that we give through the constructor. + # None in this case + + # We launch the ROSlaunch that spawns the robot into the world + ROSLauncher(rospackage_name="shadow_gazebo", + launch_file_name="put_shadow_in_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Internal Vars + # Doesnt have any accesibles + self.controllers_list = [] + + # It doesnt use namespace + self.robot_name_space = "" + + # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv + super(ShadowTcEnv, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=False, + start_init_physics_parameters=False, + reset_world_or_sim="NO_RESET_SIM") + + + + rospy.logdebug("ShadowTcEnv unpause...") + self.gazebo.unpauseSim() + #self.controllers_object.reset_controllers() + + self._check_all_systems_ready() + + rospy.Subscriber("/imu/data", Imu, self._imu_callback) + rospy.Subscriber("/joint_states", JointState, self._joints_state_callback) + #rospy.Subscriber('/planning_scene', PlanningScene, self._planning_scene_callback) + + self._setup_smart_grasper() + + self.gazebo.pauseSim() + + rospy.logdebug("Finished ShadowTcEnv INIT...") + + # Methods needed by the RobotGazeboEnv + # ---------------------------- + + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + rospy.logdebug("ShadowTcEnv check_all_systems_ready...") + self._check_all_sensors_ready() + rospy.logdebug("END ShadowTcEnv _check_all_systems_ready...") + return True + + + # CubeSingleDiskEnv virtual methods + # ---------------------------- + + def _check_all_sensors_ready(self): + rospy.logdebug("START ALL SENSORS READY") + self._check_imu_ready() + self._check_joint_states_ready() + #self._check_planning_scene_ready() + + rospy.logdebug("ALL SENSORS READY") + + + def _check_imu_ready(self): + self.imu = None + rospy.logdebug("Waiting for /imu/data to be READY...") + while self.imu is None and not rospy.is_shutdown(): + try: + self.imu = rospy.wait_for_message("/imu/data", Imu, timeout=5.0) + rospy.logdebug("Current/imu/data READY=>") + + except: + rospy.logerr("Current /imu/data not ready yet, retrying for getting imu") + + return self.imu + + def _check_joint_states_ready(self): + self.joint_states = None + rospy.logdebug("Waiting for /joint_states to be READY...") + while self.joint_states is None and not rospy.is_shutdown(): + try: + self.joint_states = rospy.wait_for_message("/joint_states", JointState, timeout=1.0) + rospy.logdebug("Current /joint_states READY=>") + + except: + rospy.logerr("Current /joint_states not ready yet, retrying for getting joint_states") + return self.joint_states + + + def _check_planning_scene_ready(self): + self.planning_scene = None + rospy.logdebug("Waiting for /planning_scene to be READY...") + while self.planning_scene is None and not rospy.is_shutdown(): + try: + self.planning_scene = rospy.wait_for_message('/planning_scene', PlanningScene, timeout=1.0) + rospy.logdebug("Current /planning_scene READY=>") + + except: + rospy.logerr("Current /planning_scene not ready yet, retrying for getting planning_scene") + return self.planning_scene + + + def _imu_callback(self, data): + self.imu = data + + def _joints_state_callback(self, data): + self.joint_states = data + + def _planning_scene_callback(self, data): + self.planning_scene = data + + + def _setup_tf_listener(self): + """ + Set ups the TF listener for getting the transforms you ask for. + """ + self.listener = tf.TransformListener() + + + def _setup_smart_grasper(self): + """ + Setup of the movement system. + :return: + """ + rospy.logdebug("START _setup_smart_grasper") + # We need to tell it to not start a node + from smart_grasping_sandbox.smart_grasper import SmartGrasper + self.sgs = SmartGrasper(init_ros_node=False) + rospy.logdebug("END _setup_smart_grasper") + + # Methods that the TrainingEnvironment will need to define here as virtual + # because they will be used in RobotGazeboEnv GrandParentClass and defined in the + # TrainingEnvironment. + # ---------------------------- + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + raise NotImplementedError() + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _get_obs(self): + raise NotImplementedError() + + def _is_done(self, observations): + """Checks if episode done based on observations given. + """ + raise NotImplementedError() + + # Methods that the TrainingEnvironment will need. + # ---------------------------- + + def open_hand(self): + """ + When called it opens robots hand + """ + self.sgs.open_hand() + + def close_hand(self): + """ + When called it closes robots hand + """ + self.sgs.close_hand() + + def get_ball_pose(self): + """ + Get Ball Pose + return: Ball Pose in the World frame + We unpause and pause the simulation because this calss is a service call. + This means that if the simulation is NOT + running it wont get the Ball information of position. + """ + rospy.logdebug("START get_ball_pose ==>") + self.gazebo.unpauseSim() + ball_pose = self.sgs.get_object_pose() + self.gazebo.pauseSim() + rospy.logdebug("ball_pose ==>"+str(ball_pose)) + rospy.logdebug("STOP get_ball_pose ==>") + + return ball_pose + + def get_tip_pose(self): + """ + Returns the pose of the tip of the TCP + We unpause and pause the simulation because this calss is a service call. + This means that if the simulation is NOT + running it wont get the TCP information of position. + """ + rospy.logdebug("START get_tip_pose ==>") + self.gazebo.unpauseSim() + tcp_pose = self.sgs.get_tip_pose() + self.gazebo.pauseSim() + rospy.logdebug("END get_tip_pose ==>") + return tcp_pose + + def move_tcp_world_frame(self, desired_pose): + """ + Moves the Tool tip TCP to the pose given + Its relative pose to world frame + :param: desired_pose: Pose where you want the TCP to move next + """ + self.sgs.move_tip_absolute(desired_pose) + + def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.): + """ + Moves that increment of XYZ RPY in the world frame + Only state the increment of the variable you want, the rest will + not increment due to the default values + """ + self.sgs.move_tip(x,y,z,roll,pitch,yaw) + + def send_movement_command(self, command, duration=0.2): + """ + Send a dictionnary of joint targets to the arm and hand directly. + To get the available joints names: rostopic echo /joint_states/name -n1 + [H1_F1J1, H1_F1J2, H1_F1J3, H1_F2J1, H1_F2J2, H1_F2J3, H1_F3J1, H1_F3J2, H1_F3J3, + elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint, + wrist_3_joint] + + :param command: a dictionnary of joint names associated with a target: + {"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0} + :param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0 + """ + self.sgs.send_command(command, duration) + + + def set_fingers_colision(self, activate=False): + """ + It activates or deactivates the finger collisions. + It also will triguer the publish into the planning_scene the collisions. + We puase and unpause for the smae exact reason as the get TCP and get ball pos. + Being a service, untill the simulation is unpaused it wont get response. + """ + rospy.logdebug("START get_fingers_colision") + self.sgs.check_fingers_collisions(activate) + rospy.logdebug("END get_fingers_colision") + + + def get_fingers_colision(self, object_collision_name): + """ + Returns the collision of the three fingers + object_collision_name: Here yo ustate the name of the model to check collision + with fingers. + Objects in sim: cricket_ball__link, drill__link + """ + self.gazebo.unpauseSim() + self.set_fingers_colision(True) + planning_scene = self._check_planning_scene_ready() + self.gazebo.pauseSim() + + objects_scene = planning_scene.allowed_collision_matrix.entry_names + colissions_matrix = planning_scene.allowed_collision_matrix.entry_values + + # We look for the Ball object model name in the objects sceen list and get the index: + object_collision_name_index = objects_scene.index(object_collision_name) + + Finger_Links_Names = [ "H1_F1_base_link", + "H1_F1_link_1", + "H1_F1_link_2", + "H1_F1_palm_link", + "H1_F1_tip", + "H1_F2_base_link", + "H1_F2_link_1", + "H1_F2_link_2", + "H1_F2_palm_link", + "H1_F2_tip", + "H1_F3_base_link", + "H1_F3_link_1", + "H1_F3_link_2", + "H1_F3_palm_link", + "H1_F3_tip"] + + + # We get all the index of the model links that are part of the fingers + # We separate by finguer to afterwards be easy to detect that there is contact in all of the finguers + finger1_indices = [i for i, var in enumerate(Finger_Links_Names) if "H1_F1" in var] + finger2_indices = [i for i, var in enumerate(Finger_Links_Names) if "H1_F2" in var] + finger3_indices = [i for i, var in enumerate(Finger_Links_Names) if "H1_F3" in var] + + # Now we search in the entry_value corresponding to the object to check the collision + # With all the rest of objects. + object_collision_array = colissions_matrix[object_collision_name_index].enabled + + # Is there a collision with Finguer1 + f1_collision = False + for finger_index in finger1_indices: + if object_collision_array[finger_index]: + f1_collision = True + break + + # Is there a collision with Finguer2 + f2_collision = False + for finger_index in finger2_indices: + if object_collision_array[finger_index]: + f2_collision = True + break + + # Is there a collision with Finguer3 + f3_collision = False + for finger_index in finger3_indices: + if object_collision_array[finger_index]: + f3_collision = True + break + + finger_collision_dict = { + "f1":f1_collision, + "f2":f2_collision, + "f3":f3_collision + } + + return finger_collision_dict + + def reset_scene(self): + """ + Restarts the simulation and world objects + """ + self.sgs.reset_world() + + + def get_imu(self): + return self.imu + + def get_joint_states(self): + return self.joint_states \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/sumitxl_env.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/sumitxl_env.py new file mode 100644 index 0000000..360138f --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/sumitxl_env.py @@ -0,0 +1,445 @@ +import numpy +import rospy +import time +from openai_ros import robot_gazebo_env +from std_msgs.msg import Float64 +from sensor_msgs.msg import JointState +from sensor_msgs.msg import Image +from sensor_msgs.msg import LaserScan +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import Imu +from sensor_msgs.msg import NavSatFix +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Twist +from geometry_msgs.msg import Vector3Stamped +from openai_ros.openai_ros_common import ROSLauncher + + +class SumitXlEnv(robot_gazebo_env.RobotGazeboEnv): + """Superclass for all CubeSingleDisk environments. + """ + + def __init__(self, ros_ws_abspath): + """ + Initializes a new SumitXlEnv environment. + + Execute a call to service /summit_xl/controller_manager/list_controllers + To get the list of controllers to be restrarted + + To check any topic we need to have the simulations running, we need to do two things: + 1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations + that are pause for whatever the reason + 2) If the simulation was running already for some reason, we need to reset the controlers. + This has to do with the fact that some plugins with tf, dont understand the reset of the simulation + and need to be reseted to work properly. + + The Sensors: The sensors accesible are the ones considered usefull for AI learning. + + Sensor Topic List: + * /gps/fix : GPS position Data + * /gps/fix_velocity: GPS Speed data + * /hokuyo_base/scan: Laser Readings + * /imu/data: Inertial Mesurment Unit data, orientation and acceleration + * /orbbec_astra/depth/image_raw + * /orbbec_astra/depth/points + * /orbbec_astra/rgb/image_raw + * /summit_xl/odom: Odometry + + Actuators Topic List: /cmd_vel, + + Args: + """ + print("Start SumitXlEnv INIT...") + # Variables that we give through the constructor. + # None in this case + + # We launch the ROSlaunch that spawns the robot into the world + ROSLauncher(rospackage_name="summit_xl_gazebo", + launch_file_name="put_robot_in_world.launch", + ros_ws_abspath=ros_ws_abspath) + print("SPAWN DONE SumitXlEnv INIT...") + # Internal Vars + # Doesnt have any accesibles + self.controllers_list = ["joint_read_state_controller", + "joint_blw_velocity_controller", + "joint_brw_velocity_controller", + "joint_flw_velocity_controller", + "joint_frw_velocity_controller" + ] + + # It doesnt use namespace + self.robot_name_space = "summit_xl" + + # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv + print("START OpenAIROS CORE SumitXlEnv INIT...") + super(SumitXlEnv, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=False, + start_init_physics_parameters=False, + reset_world_or_sim="WORLD") + print("DONE OpenAIROS CORE SumitXlEnv INIT...") + self.gazebo.unpauseSim() + # TODO: See why this doesnt work in Summit XL + # self.controllers_object.reset_controllers() + + print("START CHECK SENSORS SumitXlEnv INIT...") + self._check_all_sensors_ready() + print("DONE CHECK SENSORS SumitXlEnv INIT...") + + # We Start all the ROS related Subscribers and publishers + rospy.Subscriber("/gps/fix", NavSatFix, self._gps_fix_callback) + rospy.Subscriber("/gps/fix_velocity", Vector3Stamped, + self._gps_fix_velocity_callback) + + rospy.Subscriber("/orbbec_astra/depth/image_raw", Image, + self._camera_depth_image_raw_callback) + rospy.Subscriber("/orbbec_astra/depth/points", + PointCloud2, self._camera_depth_points_callback) + rospy.Subscriber("/orbbec_astra/rgb/image_raw", Image, + self._camera_rgb_image_raw_callback) + + rospy.Subscriber("/hokuyo_base/scan", LaserScan, + self._laser_scan_callback) + rospy.Subscriber("/imu/data", Imu, self._imu_callback) + rospy.Subscriber("/summit_xl/odom", Odometry, self._odom_callback) + + self._cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) + + print("START CHECK PUBLISHERS SumitXlEnv INIT...") + self._check_publishers_connection() + print("DONE CHECK PUBLISHERS SumitXlEnv INIT...") + + self.gazebo.pauseSim() + + print("Finished SumitXlEnv INIT...") + + # Methods needed by the RobotGazeboEnv + # ---------------------------- + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + self._check_all_sensors_ready() + return True + + # CubeSingleDiskEnv virtual methods + # ---------------------------- + + def _check_all_sensors_ready(self): + print("START ALL SENSORS READY") + self._check_gps_fix_ready() + self._check_gps_fix_velocity_ready() + self._check_camera_depth_image_raw_ready() + self._check_camera_depth_points_ready() + self._check_camera_rgb_image_raw_ready() + self._check_odom_ready() + self._check_imu_ready() + self._check_laser_scan_ready() + print("ALL SENSORS READY") + + def _check_gps_fix_ready(self): + self.gps_fix = None + print("Waiting for /gps/fix to be READY...") + while self.gps_fix is None and not rospy.is_shutdown(): + try: + self.gps_fix = rospy.wait_for_message( + "/gps/fix", NavSatFix, timeout=5.0) + print("Current /gps/fix READY=>") + + except: + rospy.logerr( + "Current /gps/fix not ready yet, retrying for getting odom") + + return self.gps_fix + + def _check_gps_fix_velocity_ready(self): + self.gps_fix_velocity = None + print("Waiting for /gps/fix_velocity to be READY...") + while self.gps_fix_velocity is None and not rospy.is_shutdown(): + try: + self.gps_fix_velocity = rospy.wait_for_message( + "/gps/fix_velocity", Vector3Stamped, timeout=5.0) + print("Current /gps/fix_velocity READY=>") + + except: + rospy.logerr( + "Current /gps/fix_velocity not ready yet, retrying for getting odom") + + return self.gps_fix_velocity + + def _check_camera_depth_image_raw_ready(self): + self.camera_depth_image_raw = None + print("Waiting for /orbbec_astra/depth/image_raw to be READY...") + while self.camera_depth_image_raw is None and not rospy.is_shutdown(): + try: + self.camera_depth_image_raw = rospy.wait_for_message( + "/orbbec_astra/depth/image_raw", Image, timeout=5.0) + print("Current /orbbec_astra/depth/image_raw READY=>") + + except: + rospy.logerr( + "Current /orbbec_astra/depth/image_raw not ready yet, retrying for getting camera_depth_image_raw") + return self.camera_depth_image_raw + + def _check_camera_depth_points_ready(self): + self.camera_depth_points = None + print("Waiting for /orbbec_astra/depth/points to be READY...") + while self.camera_depth_points is None and not rospy.is_shutdown(): + try: + self.camera_depth_points = rospy.wait_for_message( + "/orbbec_astra/depth/points", PointCloud2, timeout=10.0) + print("Current /orbbec_astra/depth/points READY=>") + + except: + rospy.logerr( + "Current /orbbec_astra/depth/points not ready yet, retrying for getting camera_depth_points") + return self.camera_depth_points + + def _check_camera_rgb_image_raw_ready(self): + self.camera_rgb_image_raw = None + print("Waiting for /orbbec_astra/rgb/image_raw to be READY...") + while self.camera_rgb_image_raw is None and not rospy.is_shutdown(): + try: + self.camera_rgb_image_raw = rospy.wait_for_message( + "/orbbec_astra/rgb/image_raw", Image, timeout=5.0) + print("Current /orbbec_astra/rgb/image_raw READY=>") + + except: + rospy.logerr( + "Current /orbbec_astra/rgb/image_raw not ready yet, retrying for getting camera_rgb_image_raw") + return self.camera_rgb_image_raw + + def _check_odom_ready(self): + self.odom = None + print("Waiting for /summit_xl/odom to be READY...") + while self.odom is None and not rospy.is_shutdown(): + try: + self.odom = rospy.wait_for_message( + "/summit_xl/odom", Odometry, timeout=0.5) + print("Current /summit_xl/odom READY=>") + + except: + rospy.logerr( + "Current /summit_xl/odom not ready yet, retrying for getting odom") + + return self.odom + + def _check_imu_ready(self): + self.imu = None + print("Waiting for /imu/data to be READY...") + while self.imu is None and not rospy.is_shutdown(): + try: + self.imu = rospy.wait_for_message( + "/imu/data", Imu, timeout=5.0) + print("Current /imu/data READY=>") + + except: + rospy.logerr( + "Current /imu/data not ready yet, retrying for getting imu") + + return self.imu + + def _check_laser_scan_ready(self): + self.laser_scan = None + print("Waiting for /hokuyo_base/scan to be READY...") + while self.laser_scan is None and not rospy.is_shutdown(): + try: + self.laser_scan = rospy.wait_for_message( + "/hokuyo_base/scan", LaserScan, timeout=1.0) + print("Current /hokuyo_base/scan READY=>") + + except: + rospy.logerr( + "Current /hokuyo_base/scan not ready yet, retrying for getting laser_scan") + return self.laser_scan + + def _gps_fix_callback(self, data): + self.gps_fix = data + + def _gps_fix_velocity_callback(self, data): + self.gps_fix_velocity = data + + def _camera_depth_image_raw_callback(self, data): + self.camera_depth_image_raw = data + + def _camera_depth_points_callback(self, data): + self.camera_depth_points = data + + def _camera_rgb_image_raw_callback(self, data): + self.camera_rgb_image_raw = data + + def _odom_callback(self, data): + self.odom = data + + def _imu_callback(self, data): + self.imu = data + + def _laser_scan_callback(self, data): + self.laser_scan = data + + def _check_publishers_connection(self): + """ + Checks that all the publishers are working + :return: + """ + rate = rospy.Rate(10) # 10hz + while self._cmd_vel_pub.get_num_connections() == 0 and not rospy.is_shutdown(): + print("No susbribers to _cmd_vel_pub yet so we wait and try again") + try: + rate.sleep() + except rospy.ROSInterruptException: + # This is to avoid error when world is rested, time when backwards. + pass + print("_cmd_vel_pub Publisher Connected") + + print("All Publishers READY") + + # Methods that the TrainingEnvironment will need to define here as virtual + # because they will be used in RobotGazeboEnv GrandParentClass and defined in the + # TrainingEnvironment. + # ---------------------------- + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + raise NotImplementedError() + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _get_obs(self): + raise NotImplementedError() + + def _is_done(self, observations): + """Checks if episode done based on observations given. + """ + raise NotImplementedError() + + # Methods that the TrainingEnvironment will need. + # ---------------------------- + def move_base(self, linear_speed, angular_speed, epsilon=0.05, update_rate=10): + """ + It will move the base based on the linear and angular speeds given. + It will wait untill those twists are achived reading from the odometry topic. + :param linear_speed: Speed in the X axis of the robot base frame + :param angular_speed: Speed of the angular turning of the robot base frame + :param epsilon: Acceptable difference between the speed asked and the odometry readings + :param update_rate: Rate at which we check the odometry. + :return: + """ + cmd_vel_value = Twist() + cmd_vel_value.linear.x = linear_speed + cmd_vel_value.angular.z = angular_speed + print("SumitXL Base Twist Cmd>>" + str(cmd_vel_value)) + self._check_publishers_connection() + self._cmd_vel_pub.publish(cmd_vel_value) + time.sleep(0.2) + """ + self.wait_until_twist_achieved(cmd_vel_value, + epsilon, + update_rate) + """ + + def wait_until_twist_achieved(self, cmd_vel_value, epsilon, update_rate): + """ + We wait for the cmd_vel twist given to be reached by the robot reading + from the odometry. + :param cmd_vel_value: Twist we want to wait to reach. + :param epsilon: Error acceptable in odometry readings. + :param update_rate: Rate at which we check the odometry. + :return: + """ + print("START wait_until_twist_achieved...") + + rate = rospy.Rate(update_rate) + start_wait_time = rospy.get_rostime().to_sec() + end_wait_time = 0.0 + + print("Desired Twist Cmd>>" + str(cmd_vel_value)) + print("epsilon>>" + str(epsilon)) + + linear_speed = cmd_vel_value.linear.x + angular_speed = cmd_vel_value.angular.z + + linear_speed_plus = linear_speed + epsilon + linear_speed_minus = linear_speed - epsilon + # Correcting factor for angular based on observations + angular_factor = 2.0 + epsilon_angular_factor = 6.0 + angular_speed_plus = (angular_factor * angular_speed) + \ + (epsilon * epsilon_angular_factor) + angular_speed_minus = (angular_factor * angular_speed) - \ + (epsilon * epsilon_angular_factor) + + while not rospy.is_shutdown(): + current_odometry = self._check_odom_ready() + + odom_linear_vel = current_odometry.twist.twist.linear.x + """ + When asking to turn EX: angular.Z = 0.3 --> Odometry is 0.6 + In linera runs ok. It also flutuates a lot, due to the turning through friction. + Therefore we will have to multiply the angular by 2 and broaden the + accepted error for angular. + """ + odom_angular_vel = current_odometry.twist.twist.angular.z + + print("Linear VEL=" + str(odom_linear_vel) + + ", ?RANGE=[" + str(linear_speed_minus) + ","+str(linear_speed_plus)+"]") + print("Angular VEL=" + str(odom_angular_vel) + + ", ?RANGE=[" + str(angular_speed_minus) + ","+str(angular_speed_plus)+"]") + + linear_vel_are_close = (odom_linear_vel <= linear_speed_plus) and ( + odom_linear_vel > linear_speed_minus) + angular_vel_are_close = (odom_angular_vel <= angular_speed_plus) and ( + odom_angular_vel > angular_speed_minus) + + if linear_vel_are_close and angular_vel_are_close: + print("Reached Velocity!") + end_wait_time = rospy.get_rostime().to_sec() + break + print("Not there yet, keep waiting...") + rate.sleep() + delta_time = end_wait_time - start_wait_time + print("[Wait Time=" + str(delta_time)+"]") + + print("END wait_until_twist_achieved...") + + return delta_time + + def get_gps_fix(self): + return self.gps_fix + + def get_gps_fix_velocity(self): + return self.gps_fix_velocity + + def get_laser_scan(self): + return self.laser_scan + + def get_camera_depth_image_raw(self): + return self.camera_depth_image_raw + + def get_camera_depth_points(self): + return self.camera_depth_points + + def get_camera_rgb_image_raw(self): + return self.camera_rgb_image_raw + + def get_odom(self): + return self.odom + + def get_imu(self): + return self.imu diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/turtlebot2_env.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/turtlebot2_env.py new file mode 100644 index 0000000..1c7b379 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/turtlebot2_env.py @@ -0,0 +1,383 @@ +import numpy +import rospy +import time +from openai_ros import robot_gazebo_env +from std_msgs.msg import Float64 +from sensor_msgs.msg import JointState +from sensor_msgs.msg import Image +from sensor_msgs.msg import LaserScan +from sensor_msgs.msg import PointCloud2 +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Twist +from openai_ros.openai_ros_common import ROSLauncher + + +class TurtleBot2Env(robot_gazebo_env.RobotGazeboEnv): + """Superclass for all CubeSingleDisk environments. + """ + + def __init__(self, ros_ws_abspath): + """ + Initializes a new TurtleBot2Env environment. + Turtlebot2 doesnt use controller_manager, therefore we wont reset the + controllers in the standard fashion. For the moment we wont reset them. + + To check any topic we need to have the simulations running, we need to do two things: + 1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations + that are pause for whatever the reason + 2) If the simulation was running already for some reason, we need to reset the controlers. + This has to do with the fact that some plugins with tf, dont understand the reset of the simulation + and need to be reseted to work properly. + + The Sensors: The sensors accesible are the ones considered usefull for AI learning. + + Sensor Topic List: + * /odom : Odometry readings of the Base of the Robot + * /camera/depth/image_raw: 2d Depth image of the depth sensor. + * /camera/depth/points: Pointcloud sensor readings + * /camera/rgb/image_raw: RGB camera + * /kobuki/laser/scan: Laser Readings + + Actuators Topic List: /cmd_vel, + + Args: + """ + rospy.logdebug("Start TurtleBot2Env INIT...") + + # Variables that we give through the constructor. + # None in this case + + # We launch the ROSlaunch that spawns the robot into the world + ROSLauncher(rospackage_name="turtlebot_gazebo", + launch_file_name="put_robot_in_world.launch", + ros_ws_abspath=ros_ws_abspath) + + + # Internal Vars + # Doesnt have any accesibles + self.controllers_list = [] + + # It doesnt use namespace + self.robot_name_space = "" + + # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv + super(TurtleBot2Env, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=False, + start_init_physics_parameters=False, + reset_world_or_sim="WORLD") + + + + + self.gazebo.unpauseSim() + #self.controllers_object.reset_controllers() + self._check_all_sensors_ready() + + # We Start all the ROS related Subscribers and publishers + rospy.Subscriber("/odom", Odometry, self._odom_callback) + #rospy.Subscriber("/camera/depth/image_raw", Image, self._camera_depth_image_raw_callback) + #rospy.Subscriber("/camera/depth/points", PointCloud2, self._camera_depth_points_callback) + #rospy.Subscriber("/camera/rgb/image_raw", Image, self._camera_rgb_image_raw_callback) + rospy.Subscriber("/kobuki/laser/scan", LaserScan, self._laser_scan_callback) + + self._cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) + + self._check_publishers_connection() + + self.gazebo.pauseSim() + + rospy.logdebug("Finished TurtleBot2Env INIT...") + + # Methods needed by the RobotGazeboEnv + # ---------------------------- + + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + self._check_all_sensors_ready() + return True + + + # CubeSingleDiskEnv virtual methods + # ---------------------------- + + def _check_all_sensors_ready(self): + rospy.logdebug("START ALL SENSORS READY") + self._check_odom_ready() + # We dont need to check for the moment, takes too long + #self._check_camera_depth_image_raw_ready() + #self._check_camera_depth_points_ready() + #self._check_camera_rgb_image_raw_ready() + self._check_laser_scan_ready() + rospy.logdebug("ALL SENSORS READY") + + def _check_odom_ready(self): + self.odom = None + rospy.logdebug("Waiting for /odom to be READY...") + while self.odom is None and not rospy.is_shutdown(): + try: + self.odom = rospy.wait_for_message("/odom", Odometry, timeout=5.0) + rospy.logdebug("Current /odom READY=>") + + except: + rospy.logerr("Current /odom not ready yet, retrying for getting odom") + + return self.odom + + + def _check_camera_depth_image_raw_ready(self): + self.camera_depth_image_raw = None + rospy.logdebug("Waiting for /camera/depth/image_raw to be READY...") + while self.camera_depth_image_raw is None and not rospy.is_shutdown(): + try: + self.camera_depth_image_raw = rospy.wait_for_message("/camera/depth/image_raw", Image, timeout=5.0) + rospy.logdebug("Current /camera/depth/image_raw READY=>") + + except: + rospy.logerr("Current /camera/depth/image_raw not ready yet, retrying for getting camera_depth_image_raw") + return self.camera_depth_image_raw + + + def _check_camera_depth_points_ready(self): + self.camera_depth_points = None + rospy.logdebug("Waiting for /camera/depth/points to be READY...") + while self.camera_depth_points is None and not rospy.is_shutdown(): + try: + self.camera_depth_points = rospy.wait_for_message("/camera/depth/points", PointCloud2, timeout=10.0) + rospy.logdebug("Current /camera/depth/points READY=>") + + except: + rospy.logerr("Current /camera/depth/points not ready yet, retrying for getting camera_depth_points") + return self.camera_depth_points + + + def _check_camera_rgb_image_raw_ready(self): + self.camera_rgb_image_raw = None + rospy.logdebug("Waiting for /camera/rgb/image_raw to be READY...") + while self.camera_rgb_image_raw is None and not rospy.is_shutdown(): + try: + self.camera_rgb_image_raw = rospy.wait_for_message("/camera/rgb/image_raw", Image, timeout=5.0) + rospy.logdebug("Current /camera/rgb/image_raw READY=>") + + except: + rospy.logerr("Current /camera/rgb/image_raw not ready yet, retrying for getting camera_rgb_image_raw") + return self.camera_rgb_image_raw + + + def _check_laser_scan_ready(self): + self.laser_scan = None + rospy.logdebug("Waiting for /kobuki/laser/scan to be READY...") + while self.laser_scan is None and not rospy.is_shutdown(): + try: + self.laser_scan = rospy.wait_for_message("/kobuki/laser/scan", LaserScan, timeout=5.0) + rospy.logdebug("Current /kobuki/laser/scan READY=>") + + except: + rospy.logerr("Current /kobuki/laser/scan not ready yet, retrying for getting laser_scan") + return self.laser_scan + + + def _odom_callback(self, data): + self.odom = data + + def _camera_depth_image_raw_callback(self, data): + self.camera_depth_image_raw = data + + def _camera_depth_points_callback(self, data): + self.camera_depth_points = data + + def _camera_rgb_image_raw_callback(self, data): + self.camera_rgb_image_raw = data + + def _laser_scan_callback(self, data): + self.laser_scan = data + + + def _check_publishers_connection(self): + """ + Checks that all the publishers are working + :return: + """ + rate = rospy.Rate(10) # 10hz + while self._cmd_vel_pub.get_num_connections() == 0 and not rospy.is_shutdown(): + rospy.logdebug("No susbribers to _cmd_vel_pub yet so we wait and try again") + try: + rate.sleep() + except rospy.ROSInterruptException: + # This is to avoid error when world is rested, time when backwards. + pass + rospy.logdebug("_cmd_vel_pub Publisher Connected") + + rospy.logdebug("All Publishers READY") + + # Methods that the TrainingEnvironment will need to define here as virtual + # because they will be used in RobotGazeboEnv GrandParentClass and defined in the + # TrainingEnvironment. + # ---------------------------- + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + raise NotImplementedError() + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _get_obs(self): + raise NotImplementedError() + + def _is_done(self, observations): + """Checks if episode done based on observations given. + """ + raise NotImplementedError() + + # Methods that the TrainingEnvironment will need. + # ---------------------------- + def move_base(self, linear_speed, angular_speed, epsilon=0.05, update_rate=10, min_laser_distance=-1): + """ + It will move the base based on the linear and angular speeds given. + It will wait untill those twists are achived reading from the odometry topic. + :param linear_speed: Speed in the X axis of the robot base frame + :param angular_speed: Speed of the angular turning of the robot base frame + :param epsilon: Acceptable difference between the speed asked and the odometry readings + :param update_rate: Rate at which we check the odometry. + :return: + """ + cmd_vel_value = Twist() + cmd_vel_value.linear.x = linear_speed + cmd_vel_value.angular.z = angular_speed + rospy.logdebug("TurtleBot2 Base Twist Cmd>>" + str(cmd_vel_value)) + self._check_publishers_connection() + self._cmd_vel_pub.publish(cmd_vel_value) + time.sleep(0.2) + #time.sleep(0.02) + """ + self.wait_until_twist_achieved(cmd_vel_value, + epsilon, + update_rate, + min_laser_distance) + """ + + + def wait_until_twist_achieved(self, cmd_vel_value, epsilon, update_rate, min_laser_distance=-1): + """ + We wait for the cmd_vel twist given to be reached by the robot reading + from the odometry. + :param cmd_vel_value: Twist we want to wait to reach. + :param epsilon: Error acceptable in odometry readings. + :param update_rate: Rate at which we check the odometry. + :return: + """ + rospy.logwarn("START wait_until_twist_achieved...") + + rate = rospy.Rate(update_rate) + start_wait_time = rospy.get_rostime().to_sec() + end_wait_time = 0.0 + epsilon = 0.05 + + rospy.logdebug("Desired Twist Cmd>>" + str(cmd_vel_value)) + rospy.logdebug("epsilon>>" + str(epsilon)) + + linear_speed = cmd_vel_value.linear.x + angular_speed = cmd_vel_value.angular.z + + linear_speed_plus = linear_speed + epsilon + linear_speed_minus = linear_speed - epsilon + angular_speed_plus = angular_speed + epsilon + angular_speed_minus = angular_speed - epsilon + + while not rospy.is_shutdown(): + + crashed_into_something = self.has_crashed(min_laser_distance) + + current_odometry = self._check_odom_ready() + odom_linear_vel = current_odometry.twist.twist.linear.x + odom_angular_vel = current_odometry.twist.twist.angular.z + + rospy.logdebug("Linear VEL=" + str(odom_linear_vel) + ", ?RANGE=[" + str(linear_speed_minus) + ","+str(linear_speed_plus)+"]") + rospy.logdebug("Angular VEL=" + str(odom_angular_vel) + ", ?RANGE=[" + str(angular_speed_minus) + ","+str(angular_speed_plus)+"]") + + linear_vel_are_close = (odom_linear_vel <= linear_speed_plus) and (odom_linear_vel > linear_speed_minus) + angular_vel_are_close = (odom_angular_vel <= angular_speed_plus) and (odom_angular_vel > angular_speed_minus) + + if linear_vel_are_close and angular_vel_are_close: + rospy.logwarn("Reached Velocity!") + end_wait_time = rospy.get_rostime().to_sec() + break + + if crashed_into_something: + rospy.logerr("TurtleBot has crashed, stopping movement!") + break + + rospy.logwarn("Not there yet, keep waiting...") + rate.sleep() + delta_time = end_wait_time- start_wait_time + rospy.logdebug("[Wait Time=" + str(delta_time)+"]") + + rospy.logwarn("END wait_until_twist_achieved...") + + return delta_time + + def has_crashed(self, min_laser_distance): + """ + It states based on the laser scan if the robot has crashed or not. + Crashed means that the minimum laser reading is lower than the + min_laser_distance value given. + If min_laser_distance == -1, it returns always false, because its the way + to deactivate this check. + """ + robot_has_crashed = False + + if min_laser_distance != -1: + laser_data = self.get_laser_scan() + for i, item in enumerate(laser_data.ranges): + if item == float ('Inf') or numpy.isinf(item): + pass + elif numpy.isnan(item): + pass + else: + # Has a Non Infinite or Nan Value + if (item < min_laser_distance): + rospy.logerr("TurtleBot HAS CRASHED >>> item=" + str(item)+"< "+str(min_laser_distance)) + robot_has_crashed = True + break + return robot_has_crashed + + + def get_odom(self): + return self.odom + + def get_camera_depth_image_raw(self): + return self.camera_depth_image_raw + + def get_camera_depth_points(self): + return self.camera_depth_points + + def get_camera_rgb_image_raw(self): + return self.camera_rgb_image_raw + + def get_laser_scan(self): + return self.laser_scan + + def reinit_sensors(self): + """ + This method is for the tasks so that when reseting the episode + the sensors values are forced to be updated with the real data and + + """ diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/turtlebot3_env.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/turtlebot3_env.py new file mode 100644 index 0000000..d4c0444 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/turtlebot3_env.py @@ -0,0 +1,299 @@ +import numpy +import rospy +import time +from openai_ros import robot_gazebo_env +from std_msgs.msg import Float64 +from sensor_msgs.msg import JointState +from sensor_msgs.msg import Image +from sensor_msgs.msg import LaserScan +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import Imu +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Twist +from openai_ros.openai_ros_common import ROSLauncher + + +class TurtleBot3Env(robot_gazebo_env.RobotGazeboEnv): + """Superclass for all CubeSingleDisk environments. + """ + + def __init__(self, ros_ws_abspath): + + self.odom_topic = "/odom" + self.scan_topic = "/scan" + self.imu_topic = "/imu" + self.cmd_vel_topic = "/cmd_vel" + + """ + Initializes a new TurtleBot3Env environment. + TurtleBot3 doesnt use controller_manager, therefore we wont reset the + controllers in the standard fashion. For the moment we wont reset them. + + To check any topic we need to have the simulations running, we need to do two things: + 1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations + that are pause for whatever the reason + 2) If the simulation was running already for some reason, we need to reset the controlers. + This has to do with the fact that some plugins with tf, dont understand the reset of the simulation + and need to be reseted to work properly. + + The Sensors: The sensors accesible are the ones considered usefull for AI learning. + + Sensor Topic List: + * /odom : Odometry readings of the Base of the Robot + * /imu: Inertial Mesuring Unit that gives relative accelerations and orientations. + * /scan: Laser Readings + + Actuators Topic List: /cmd_vel, + + Args: + """ + rospy.loginfo("Start TurtleBot3Env INIT...") + # Variables that we give through the constructor. + # None in this case + + # We launch the ROSlaunch that spawns the robot into the world + ROSLauncher(rospackage_name="turtlebot3_gazebo", + launch_file_name="put_robot_in_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Internal Vars + # Doesnt have any accesibles + self.controllers_list = ["imu"] + + # # It doesnt use namespace -- IT DOES NOW -- at top + self.robot_name_space = "" + + # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv + super(TurtleBot3Env, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=False, + start_init_physics_parameters=False) + + + + + self.gazebo.unpauseSim() + #self.controllers_object.reset_controllers() + self._check_all_sensors_ready() + + # We Start all the ROS related Subscribers and publishers + rospy.Subscriber(self.odom_topic, Odometry, self._odom_callback) + rospy.Subscriber(self.imu_topic, Imu, self._imu_callback) + rospy.Subscriber(self.scan_topic, LaserScan, self._laser_scan_callback) + + self._cmd_vel_pub = rospy.Publisher(self.cmd_vel_topic, Twist, queue_size=1) + + self._check_publishers_connection() + + self.gazebo.pauseSim() + + rospy.loginfo("Finished TurtleBot3Env INIT...") + + + # Methods needed by the RobotGazeboEnv + # ---------------------------- + + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + self._check_all_sensors_ready() + return True + + + # CubeSingleDiskEnv virtual methods + # ---------------------------- + + def _check_all_sensors_ready(self): + rospy.logdebug("START ALL SENSORS READY") + self._check_odom_ready() + self._check_imu_ready() + self._check_laser_scan_ready() + rospy.logdebug("ALL SENSORS READY") + + def _check_odom_ready(self): + self.odom = None + rospy.logdebug("Waiting for /odom to be READY...") + while self.odom is None and not rospy.is_shutdown(): + try: + self.odom = rospy.wait_for_message(self.odom_topic, Odometry, timeout=5.0) + rospy.logdebug("Current " + self.odom_topic + " READY=>") + + except: + rospy.logerr("Current " + self.odom_topic + " not ready yet, retrying for getting odom") + + return self.odom + + + def _check_imu_ready(self): + self.imu = None + rospy.logdebug("Waiting for /imu to be READY...") + while self.imu is None and not rospy.is_shutdown(): + try: + self.imu = rospy.wait_for_message(self.imu_topic, Imu, timeout=5.0) + rospy.logdebug("Current " + self.imu_topic + " READY=>") + + except: + rospy.logerr("Current " + self.imu_topic + " not ready yet, retrying for getting imu") + + return self.imu + + + def _check_laser_scan_ready(self): + self.laser_scan = None + rospy.logdebug("Waiting for /scan to be READY...") + while self.laser_scan is None and not rospy.is_shutdown(): + try: + self.laser_scan = rospy.wait_for_message(self.scan_topic, LaserScan, timeout=1.0) + rospy.logdebug("Current " + self.scan_topic + " READY=>") + + except: + rospy.logerr("Current " + self.scan_topic + " not ready yet, retrying for getting laser_scan") + return self.laser_scan + + + def _odom_callback(self, data): + self.odom = data + + def _imu_callback(self, data): + self.imu = data + + def _laser_scan_callback(self, data): + self.laser_scan = data + + + def _check_publishers_connection(self): + """ + Checks that all the publishers are working + :return: + """ + rate = rospy.Rate(10) # 10hz + while self._cmd_vel_pub.get_num_connections() == 0 and not rospy.is_shutdown(): + rospy.logdebug("No susbribers to _cmd_vel_pub yet so we wait and try again") + try: + rate.sleep() + except rospy.ROSInterruptException: + # This is to avoid error when world is rested, time when backwards. + pass + rospy.logdebug("_cmd_vel_pub Publisher Connected") + + rospy.logdebug("All Publishers READY") + + # Methods that the TrainingEnvironment will need to define here as virtual + # because they will be used in RobotGazeboEnv GrandParentClass and defined in the + # TrainingEnvironment. + # ---------------------------- + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + raise NotImplementedError() + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _get_obs(self): + raise NotImplementedError() + + def _is_done(self, observations): + """Checks if episode done based on observations given. + """ + raise NotImplementedError() + + # Methods that the TrainingEnvironment will need. + # ---------------------------- + def move_base(self, linear_speed, angular_speed, epsilon=0.05, update_rate=10): + """ + It will move the base based on the linear and angular speeds given. + It will wait untill those twists are achived reading from the odometry topic. + :param linear_speed: Speed in the X axis of the robot base frame + :param angular_speed: Speed of the angular turning of the robot base frame + :param epsilon: Acceptable difference between the speed asked and the odometry readings + :param update_rate: Rate at which we check the odometry. + :return: + """ + cmd_vel_value = Twist() + cmd_vel_value.linear.x = linear_speed + cmd_vel_value.angular.z = angular_speed + rospy.logdebug("TurtleBot3 Base Twist Cmd>>" + str(cmd_vel_value)) + self._check_publishers_connection() + self._cmd_vel_pub.publish(cmd_vel_value) + #self.wait_until_twist_achieved(cmd_vel_value,epsilon,update_rate) + # Weplace a waitof certain amiunt of time, because this twist achived doesnt work properly + time.sleep(0.2) + + def wait_until_twist_achieved(self, cmd_vel_value, epsilon, update_rate): + """ + We wait for the cmd_vel twist given to be reached by the robot reading + from the odometry. + :param cmd_vel_value: Twist we want to wait to reach. + :param epsilon: Error acceptable in odometry readings. + :param update_rate: Rate at which we check the odometry. + :return: + """ + rospy.logdebug("START wait_until_twist_achieved...") + + rate = rospy.Rate(update_rate) + start_wait_time = rospy.get_rostime().to_sec() + end_wait_time = 0.0 + epsilon = 0.05 + + rospy.logdebug("Desired Twist Cmd>>" + str(cmd_vel_value)) + rospy.logdebug("epsilon>>" + str(epsilon)) + + linear_speed = cmd_vel_value.linear.x + angular_speed = cmd_vel_value.angular.z + + linear_speed_plus = linear_speed + epsilon + linear_speed_minus = linear_speed - epsilon + angular_speed_plus = angular_speed + epsilon + angular_speed_minus = angular_speed - epsilon + + while not rospy.is_shutdown(): + current_odometry = self._check_odom_ready() + # IN turtlebot3 the odometry angular readings are inverted, so we have to invert the sign. + odom_linear_vel = current_odometry.twist.twist.linear.x + odom_angular_vel = -1*current_odometry.twist.twist.angular.z + + rospy.logdebug("Linear VEL=" + str(odom_linear_vel) + ", ?RANGE=[" + str(linear_speed_minus) + ","+str(linear_speed_plus)+"]") + rospy.logdebug("Angular VEL=" + str(odom_angular_vel) + ", ?RANGE=[" + str(angular_speed_minus) + ","+str(angular_speed_plus)+"]") + + linear_vel_are_close = (odom_linear_vel <= linear_speed_plus) and (odom_linear_vel > linear_speed_minus) + angular_vel_are_close = (odom_angular_vel <= angular_speed_plus) and (odom_angular_vel > angular_speed_minus) + + if linear_vel_are_close and angular_vel_are_close: + rospy.logdebug("Reached Velocity!") + end_wait_time = rospy.get_rostime().to_sec() + break + rospy.logdebug("Not there yet, keep waiting...") + rate.sleep() + delta_time = end_wait_time- start_wait_time + rospy.logdebug("[Wait Time=" + str(delta_time)+"]") + + rospy.logdebug("END wait_until_twist_achieved...") + + return delta_time + + + def get_odom(self): + return self.odom + + def get_imu(self): + return self.imu + + def get_laser_scan(self): + return self.laser_scan diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/turtlebot3_env_custom.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/turtlebot3_env_custom.py new file mode 100644 index 0000000..09f94d1 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/turtlebot3_env_custom.py @@ -0,0 +1,300 @@ +import numpy +import rospy +import time +from openai_ros import robot_gazebo_env +from std_msgs.msg import Float64 +from sensor_msgs.msg import JointState +from sensor_msgs.msg import Image +from sensor_msgs.msg import LaserScan +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import Imu +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Twist +from openai_ros.openai_ros_common import ROSLauncher + + +class TurtleBot3Env(robot_gazebo_env.RobotGazeboEnv): + """Superclass for all CubeSingleDisk environments. + """ + + def __init__(self, ros_ws_abspath): + + self.robot_name_space = '/tb3_0' + self.odom_topic = self.robot_name_space + "/odom" + self.scan_topic = self.robot_name_space + "/scan" + self.imu_topic = self.robot_name_space + "/imu" + self.cmd_vel_topic = self.robot_name_space + "/cmd_vel" + + """ + Initializes a new TurtleBot3Env environment. + TurtleBot3 doesnt use controller_manager, therefore we wont reset the + controllers in the standard fashion. For the moment we wont reset them. + + To check any topic we need to have the simulations running, we need to do two things: + 1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations + that are pause for whatever the reason + 2) If the simulation was running already for some reason, we need to reset the controlers. + This has to do with the fact that some plugins with tf, dont understand the reset of the simulation + and need to be reseted to work properly. + + The Sensors: The sensors accesible are the ones considered usefull for AI learning. + + Sensor Topic List: + * /odom : Odometry readings of the Base of the Robot + * /imu: Inertial Mesuring Unit that gives relative accelerations and orientations. + * /scan: Laser Readings + + Actuators Topic List: /cmd_vel, + + Args: + """ + rospy.loginfo("Start TurtleBot3Env INIT...") + # Variables that we give through the constructor. + # None in this case + + # We launch the ROSlaunch that spawns the robot into the world + # ROSLauncher(rospackage_name="turtlebot3_gazebo", + # launch_file_name="put_robot_in_world.launch", + # ros_ws_abspath=ros_ws_abspath) + + # Internal Vars + # Doesnt have any accesibles + self.controllers_list = ["imu"] + + # # It doesnt use namespace -- IT DOES NOW -- at top + # self.robot_name_space = "" + + # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv + super(TurtleBot3Env, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=False, + start_init_physics_parameters=False) + + + + + self.gazebo.unpauseSim() + #self.controllers_object.reset_controllers() + self._check_all_sensors_ready() + + # We Start all the ROS related Subscribers and publishers + rospy.Subscriber(self.odom_topic, Odometry, self._odom_callback) + rospy.Subscriber(self.imu_topic, Imu, self._imu_callback) + rospy.Subscriber(self.scan_topic, LaserScan, self._laser_scan_callback) + + self._cmd_vel_pub = rospy.Publisher(self.cmd_vel_topic, Twist, queue_size=1) + + self._check_publishers_connection() + + self.gazebo.pauseSim() + + rospy.loginfo("Finished TurtleBot3Env INIT...") + + + # Methods needed by the RobotGazeboEnv + # ---------------------------- + + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + self._check_all_sensors_ready() + return True + + + # CubeSingleDiskEnv virtual methods + # ---------------------------- + + def _check_all_sensors_ready(self): + rospy.logdebug("START ALL SENSORS READY") + self._check_odom_ready() + self._check_imu_ready() + self._check_laser_scan_ready() + rospy.logdebug("ALL SENSORS READY") + + def _check_odom_ready(self): + self.odom = None + rospy.logdebug("Waiting for /odom to be READY...") + while self.odom is None and not rospy.is_shutdown(): + try: + self.odom = rospy.wait_for_message(self.odom_topic, Odometry, timeout=5.0) + rospy.logdebug("Current " + self.odom_topic + " READY=>") + + except: + rospy.logerr("Current " + self.odom_topic + " not ready yet, retrying for getting odom") + + return self.odom + + + def _check_imu_ready(self): + self.imu = None + rospy.logdebug("Waiting for /imu to be READY...") + while self.imu is None and not rospy.is_shutdown(): + try: + self.imu = rospy.wait_for_message(self.imu_topic, Imu, timeout=5.0) + rospy.logdebug("Current " + self.imu_topic + " READY=>") + + except: + rospy.logerr("Current " + self.imu_topic + " not ready yet, retrying for getting imu") + + return self.imu + + + def _check_laser_scan_ready(self): + self.laser_scan = None + rospy.logdebug("Waiting for /scan to be READY...") + while self.laser_scan is None and not rospy.is_shutdown(): + try: + self.laser_scan = rospy.wait_for_message(self.scan_topic, LaserScan, timeout=1.0) + rospy.logdebug("Current " + self.scan_topic + " READY=>") + + except: + rospy.logerr("Current " + self.scan_topic + " not ready yet, retrying for getting laser_scan") + return self.laser_scan + + + def _odom_callback(self, data): + self.odom = data + + def _imu_callback(self, data): + self.imu = data + + def _laser_scan_callback(self, data): + self.laser_scan = data + + + def _check_publishers_connection(self): + """ + Checks that all the publishers are working + :return: + """ + rate = rospy.Rate(10) # 10hz + while self._cmd_vel_pub.get_num_connections() == 0 and not rospy.is_shutdown(): + rospy.logdebug("No susbribers to _cmd_vel_pub yet so we wait and try again") + try: + rate.sleep() + except rospy.ROSInterruptException: + # This is to avoid error when world is rested, time when backwards. + pass + rospy.logdebug("_cmd_vel_pub Publisher Connected") + + rospy.logdebug("All Publishers READY") + + # Methods that the TrainingEnvironment will need to define here as virtual + # because they will be used in RobotGazeboEnv GrandParentClass and defined in the + # TrainingEnvironment. + # ---------------------------- + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + raise NotImplementedError() + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _get_obs(self): + raise NotImplementedError() + + def _is_done(self, observations): + """Checks if episode done based on observations given. + """ + raise NotImplementedError() + + # Methods that the TrainingEnvironment will need. + # ---------------------------- + def move_base(self, linear_speed, angular_speed, epsilon=0.05, update_rate=10): + """ + It will move the base based on the linear and angular speeds given. + It will wait untill those twists are achived reading from the odometry topic. + :param linear_speed: Speed in the X axis of the robot base frame + :param angular_speed: Speed of the angular turning of the robot base frame + :param epsilon: Acceptable difference between the speed asked and the odometry readings + :param update_rate: Rate at which we check the odometry. + :return: + """ + cmd_vel_value = Twist() + cmd_vel_value.linear.x = linear_speed + cmd_vel_value.angular.z = angular_speed + rospy.logdebug("TurtleBot3 Base Twist Cmd>>" + str(cmd_vel_value)) + self._check_publishers_connection() + self._cmd_vel_pub.publish(cmd_vel_value) + #self.wait_until_twist_achieved(cmd_vel_value,epsilon,update_rate) + # Weplace a waitof certain amiunt of time, because this twist achived doesnt work properly + time.sleep(0.2) + + def wait_until_twist_achieved(self, cmd_vel_value, epsilon, update_rate): + """ + We wait for the cmd_vel twist given to be reached by the robot reading + from the odometry. + :param cmd_vel_value: Twist we want to wait to reach. + :param epsilon: Error acceptable in odometry readings. + :param update_rate: Rate at which we check the odometry. + :return: + """ + rospy.logdebug("START wait_until_twist_achieved...") + + rate = rospy.Rate(update_rate) + start_wait_time = rospy.get_rostime().to_sec() + end_wait_time = 0.0 + epsilon = 0.05 + + rospy.logdebug("Desired Twist Cmd>>" + str(cmd_vel_value)) + rospy.logdebug("epsilon>>" + str(epsilon)) + + linear_speed = cmd_vel_value.linear.x + angular_speed = cmd_vel_value.angular.z + + linear_speed_plus = linear_speed + epsilon + linear_speed_minus = linear_speed - epsilon + angular_speed_plus = angular_speed + epsilon + angular_speed_minus = angular_speed - epsilon + + while not rospy.is_shutdown(): + current_odometry = self._check_odom_ready() + # IN turtlebot3 the odometry angular readings are inverted, so we have to invert the sign. + odom_linear_vel = current_odometry.twist.twist.linear.x + odom_angular_vel = -1*current_odometry.twist.twist.angular.z + + rospy.logdebug("Linear VEL=" + str(odom_linear_vel) + ", ?RANGE=[" + str(linear_speed_minus) + ","+str(linear_speed_plus)+"]") + rospy.logdebug("Angular VEL=" + str(odom_angular_vel) + ", ?RANGE=[" + str(angular_speed_minus) + ","+str(angular_speed_plus)+"]") + + linear_vel_are_close = (odom_linear_vel <= linear_speed_plus) and (odom_linear_vel > linear_speed_minus) + angular_vel_are_close = (odom_angular_vel <= angular_speed_plus) and (odom_angular_vel > angular_speed_minus) + + if linear_vel_are_close and angular_vel_are_close: + rospy.logdebug("Reached Velocity!") + end_wait_time = rospy.get_rostime().to_sec() + break + rospy.logdebug("Not there yet, keep waiting...") + rate.sleep() + delta_time = end_wait_time- start_wait_time + rospy.logdebug("[Wait Time=" + str(delta_time)+"]") + + rospy.logdebug("END wait_until_twist_achieved...") + + return delta_time + + + def get_odom(self): + return self.odom + + def get_imu(self): + return self.imu + + def get_laser_scan(self): + return self.laser_scan diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/turtlebot3_env_custom_a2c.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/turtlebot3_env_custom_a2c.py new file mode 100755 index 0000000..04e2928 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/turtlebot3_env_custom_a2c.py @@ -0,0 +1,624 @@ +import numpy +import rospy +import time +from openai_ros import robot_gazebo_env +from std_msgs.msg import Float64, String +from sensor_msgs.msg import JointState, Image, LaserScan, PointCloud2, Imu +from nav_msgs.msg import Odometry, OccupancyGrid +from nav_msgs.srv import GetPlan +from geometry_msgs.msg import Twist, PoseStamped +from visualization_msgs.msg import Marker +from openai_ros.openai_ros_common import ROSLauncher +from move_base_msgs.msg import MoveBaseActionResult +from actionlib_msgs.msg import GoalID + + +class TurtleBot3Env(robot_gazebo_env.RobotGazeboEnv): + """Superclass for all CubeSingleDisk environments. + """ + + def __init__(self, ros_ws_abspath): + + self.robot_namespace = '/tb3_0' + self.robot_namespace_no_slash = 'tb3_0' + self.odom_topic = self.robot_namespace + "/odom" + self.scan_topic = self.robot_namespace + "/scan" + self.imu_topic = self.robot_namespace + "/imu" + self.cmd_vel_topic = self.robot_namespace + "/cmd_vel" + self.frontier_topic = self.robot_namespace+'/rl_frontiers' + self.map_topic = self.robot_namespace+'/rl_map' + self.pose_topic = self.robot_namespace+'/rl_pose' + self.raw_map_topic = self.robot_namespace+'/map' + self.reset_gmapping_topic = '/syscommand' + self.reset_gmapping_fully_topic = '/gmapping_finished_resetting' + self.move_base_reached_topic = self.robot_namespace + '/move_base/result' + self.move_base_delete_goal_topic = self.robot_namespace + '/move_base/cancel' + + + """ + Initializes a new TurtleBot3Env environment. + TurtleBot3 doesnt use controller_manager, therefore we wont reset the + controllers in the standard fashion. For the moment we wont reset them. + + To check any topic we need to have the simulations running, we need to do two things: + 1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations + that are pause for whatever the reason + 2) If the simulation was running already for some reason, we need to reset the controlers. + This has to do with the fact that some plugins with tf, dont understand the reset of the simulation + and need to be reseted to work properly. + + The Sensors: The sensors accesible are the ones considered usefull for AI learning. + + Sensor Topic List: + * /odom : Odometry readings of the Base of the Robot + * /imu: Inertial Mesuring Unit that gives relative accelerations and orientations. + * /scan: Laser Readings + + Actuators Topic List: /cmd_vel, + + Args: + """ + rospy.loginfo("Start TurtleBot3Env INIT...") + # Variables that we give through the constructor. + # None in this case + + # We launch the ROSlaunch that spawns the robot into the world + # ROSLauncher(rospackage_name="turtlebot3_gazebo", + # launch_file_name="put_robot_in_world.launch", + # ros_ws_abspath=ros_ws_abspath) + + # Internal Vars + # Doesnt have any accesibles + self.controllers_list = ["imu"] + + # # It doesnt use namespace -- IT DOES NOW -- at top + # self.robot_name_space = "" + + # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv + super(TurtleBot3Env, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_namespace, + reset_controls=False, + start_init_physics_parameters=False) + + self.gazebo.unpauseSim() + #self.controllers_object.reset_controllers() + self._check_all_sensors_ready() + + self.odom_recorder = list() + self.odom_recorder_max = 10 + self.odom_diff_threshold = 0.001 + + # We Start all the ROS related Subscribers and publishers + rospy.Subscriber(self.odom_topic, Odometry, self._odom_callback) + rospy.Subscriber(self.imu_topic, Imu, self._imu_callback) + rospy.Subscriber(self.scan_topic, LaserScan, self._laser_scan_callback) + rospy.Subscriber(self.frontier_topic, OccupancyGrid, self._frontier_callback) + rospy.Subscriber(self.map_topic, OccupancyGrid, self._map_callback) + rospy.Subscriber(self.pose_topic, OccupancyGrid, self._pose_callback) + rospy.Subscriber(self.raw_map_topic, OccupancyGrid, self._raw_map_callback) + rospy.Subscriber(self.reset_gmapping_fully_topic, String, self._gmapping_reset_callback) + rospy.Subscriber(self.move_base_reached_topic, MoveBaseActionResult, self._move_base_result_callback) + + self.stop_move_base_pub = rospy.Publisher('/stop_move_base', String, queue_size=10) + self.start_move_base_pub = rospy.Publisher('/start_move_base', String, queue_size=10) + + self.reset_gmapping_pub = rospy.Publisher(self.reset_gmapping_topic, String, queue_size=10) + + self.frontier_map = OccupancyGrid() + self.map = OccupancyGrid() + self.pose_map = OccupancyGrid() + + self.frontier_map_received = False + self.map_received = False + self.pose_map_received = False + + self._cmd_vel_pub = rospy.Publisher(self.cmd_vel_topic, Twist, queue_size=1) + self._check_publishers_connection() + + self.gazebo.pauseSim() + self.gmapping_fully_reset = False + + self.move_pub = rospy.Publisher(self.robot_namespace+'/move_base_simple/goal', PoseStamped, queue_size=10) + self.visualization_pub = rospy.Publisher("/visualization_marker", Marker, queue_size=10) + self.goal_delete_pub = rospy.Publisher(self.move_base_delete_goal_topic, GoalID, queue_size=10) + + self.move_base_threshold = 0.1 + rospy.set_param(self.robot_namespace + '/move_base/DWAPlannerROS/xy_goal_tolerance', self.move_base_threshold) + + self.record_move_base_response = False + self.move_base_result = None + + rospy.loginfo("Finished TurtleBot3Env INIT...") + + + # Methods needed by the RobotGazeboEnv + # ---------------------------- + + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + self._check_all_sensors_ready() + return True + + + # CubeSingleDiskEnv virtual methods + # ---------------------------- + + def _check_all_sensors_ready(self): + rospy.logdebug("START ALL SENSORS READY") + self._check_odom_ready() + self._check_imu_ready() + self._check_laser_scan_ready() + rospy.logdebug("ALL SENSORS READY") + + def _check_odom_ready(self): + self.odom = None + rospy.logdebug("Waiting for /odom to be READY...") + while self.odom is None and not rospy.is_shutdown(): + try: + self.odom = rospy.wait_for_message(self.odom_topic, Odometry, timeout=5.0) + rospy.logdebug("Current " + self.odom_topic + " READY=>") + + except: + rospy.logerr("Current " + self.odom_topic + " not ready yet, retrying for getting odom") + + return self.odom + + + def _check_imu_ready(self): + self.imu = None + rospy.logdebug("Waiting for /imu to be READY...") + while self.imu is None and not rospy.is_shutdown(): + try: + self.imu = rospy.wait_for_message(self.imu_topic, Imu, timeout=5.0) + rospy.logdebug("Current " + self.imu_topic + " READY=>") + + except: + rospy.logerr("Current " + self.imu_topic + " not ready yet, retrying for getting imu") + + return self.imu + + + def _check_laser_scan_ready(self): + self.laser_scan = None + rospy.logdebug("Waiting for /scan to be READY...") + while self.laser_scan is None and not rospy.is_shutdown(): + try: + self.laser_scan = rospy.wait_for_message(self.scan_topic, LaserScan, timeout=1.0) + rospy.logdebug("Current " + self.scan_topic + " READY=>") + + except: + rospy.logerr("Current " + self.scan_topic + " not ready yet, retrying for getting laser_scan") + return self.laser_scan + + + def _odom_callback(self, data): + self.odom = data + + if len(self.odom_recorder) < self.odom_recorder_max: + self.odom_recorder.append(data) + else: + oldest_odom = self.odom_recorder.pop(0) + self.odom_recorder.append(data) + + # print("Odom Received") + + + def _imu_callback(self, data): + self.imu = data + # print("IMU Received") + + def _laser_scan_callback(self, data): + self.laser_scan = data + # print("Laser Scan Received") + + def _frontier_callback(self, data): + self.frontier_map = data + self.frontier_map_received = True + # print("Frontier Map Received") + + def _map_callback(self, data): + self.map = data + self.map_received = True + # print("Full Occupancy Map Received") + + def _pose_callback(self, data): + self.pose_map = data + self.pose_map_received = True + # print("Pose Map Received") + + def _raw_map_callback(self, data): + print("Raw Map Received") + + def _gmapping_reset_callback(self, data): + if data == "fully reset": + self.gmapping_fully_reset = True + + def _move_base_result_callback(self, data): + if self.record_move_base_response: + self.move_base_result = data + + def _check_publishers_connection(self): + """ + :return: + """ + rate = rospy.Rate(10) # 10hz + while self._cmd_vel_pub.get_num_connections() == 0 and not rospy.is_shutdown(): + rospy.logdebug("No susbribers to _cmd_vel_pub yet so we wait and try again") + try: + rate.sleep() + except rospy.ROSInterruptException: + # This is to avoid error when world is rested, time when backwards. + pass + rospy.logdebug("_cmd_vel_pub Publisher Connected") + + rospy.logdebug("All Publishers READY") + + # Methods that the TrainingEnvironment will need to define here as virtual + # because they will be used in RobotGazeboEnv GrandParentClass and defined in the + # TrainingEnvironment. + # ---------------------------- + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + raise NotImplementedError() + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _get_obs(self): + raise NotImplementedError() + + def _is_done(self, observations): + """Checks if episode done based on observations given. + """ + raise NotImplementedError() + + # Methods that the TrainingEnvironment will need. + # ---------------------------- + def move_base(self, linear_speed, angular_speed, epsilon=0.05, update_rate=10): + """ + It will move the base based on the linear and angular speeds given. + It will wait untill those twists are achived reading from the odometry topic. + :param linear_speed: Speed in the X axis of the robot base frame + :param angular_speed: Speed of the angular turning of the robot base frame + :param epsilon: Acceptable difference between the speed asked and the odometry readings + :param update_rate: Rate at which we check the odometry. + :return: + """ + cmd_vel_value = Twist() + cmd_vel_value.linear.x = linear_speed + cmd_vel_value.angular.z = angular_speed + rospy.logdebug("TurtleBot3 Base Twist Cmd>>" + str(cmd_vel_value)) + self._check_publishers_connection() + self._cmd_vel_pub.publish(cmd_vel_value) + #self.wait_until_twist_achieved(cmd_vel_value,epsilon,update_rate) + # Weplace a waitof certain amiunt of time, because this twist achived doesnt work properly + time.sleep(0.2) + + def send_to_location(self, pt): + + move_msg = PoseStamped() + move_msg.header.frame_id = self.robot_namespace_no_slash + '/map' + + move_msg.pose.position.x = pt[0] + move_msg.pose.position.y = pt[1] + move_msg.pose.position.z = 0 + + move_msg.pose.orientation.x = 0 + move_msg.pose.orientation.y = 0 + move_msg.pose.orientation.z = 0 + move_msg.pose.orientation.w = 1 + + self.visualize_goal_point(move_msg) + + if self.check_pose_possible(move_msg): + print("sending to pose") + self.move_pub.publish(move_msg) + self.wait_until_move_achieved(move_msg) + else: + rospy.logwarn("Goal pose is impossible to plan to: " + str(move_msg)) + + def wait_until_move_achieved(self, goal_pose): + rospy.loginfo("waiting for move base to finish goal") + + # while not rospy.is_shutdown(): + # curr_odom = self.get_odom() + # curr_x = curr_odom.pose.pose.position.x + # curr_y = curr_odom.pose.pose.position.y + # + # goal_x = goal_pose.pose.position.x + # goal_y = goal_pose.pose.position.y + # + # curr_map = self.get_map() + # + # index_of_robot = int(self.point_to_index((curr_x, curr_y), curr_map)) + # pt = self.index_to_point(index_of_robot, curr_map) + # curr_location_map_frame = self.map_to_world(pt[0], pt[1], curr_map) + # + # print("CURRENT LOCATION: " + str(curr_location_map_frame)) + # print("GOAL LOCATION: (" + str(goal_x) + ", " + str(goal_y) + ")") + # + # if abs(goal_x-curr_location_map_frame[0]) <= self.move_base_threshold and abs(goal_y-curr_location_map_frame[1]) <= self.move_base_threshold: + # rospy.loginfo("Robot has reached desired pose") + # print("Robot has reached desired pose") + # break + # + # # elif self.is_stuck(): + # # rospy.logerr("Robot is stuck") + # # print("Robot is stuck") + # # break + + self.record_move_base_response = True + while not rospy.is_shutdown(): + + if self.move_base_result is not None: + text = self.move_base_result.status.text + self.record_move_base_response = False + self.move_base_result = None + if text == "Goal reached.": + rospy.logwarn("GOAL REACHED!!!") + print("GOAL REACHED!!!") + break + else: + rospy.logwarn("GOAL _NOT_ REACHED!!!") + print("GOAL _NOT_ REACHED!!!") + self.goal_delete_pub.publish(GoalID()) + break + + # elif self.is_stuck(): + # rospy.logerr("Robot is stuck") + # print("Robot is stuck") + # self.goal_delete_pub.publish(GoalID()) + # break + + + def check_pose_possible(self, goal_pose): + + move_base_service_topic = self.robot_namespace + '/move_base/make_plan' + print("waiting for %s service", move_base_service_topic) + + rospy.wait_for_service(move_base_service_topic) + + req = GetPlan() + + curr_odom = self.get_odom() + start_pose = PoseStamped() + start_pose.header.frame_id = goal_pose.header.frame_id + + start_pose.pose.position.x = curr_odom.pose.pose.position.x + start_pose.pose.position.y = curr_odom.pose.pose.position.y + start_pose.pose.position.z = 0 + + start_pose.pose.orientation.x = 0 + start_pose.pose.orientation.y = 0 + start_pose.pose.orientation.z = 0 + start_pose.pose.orientation.w = 1 + + req.start = start_pose + req.goal = goal_pose + req.tolerance = .5 + + try: + get_plan = rospy.ServiceProxy(move_base_service_topic, GetPlan) + resp = get_plan(req.start, req.goal, req.tolerance) + rospy.loginfo(resp) + + if len(resp.plan.poses) > 0: + rospy.loginfo("PATH FOUND - goal is possible to get to") + return True + else: + rospy.loginfo("NO path found - goal is NOT possible to get to") + return False + + except rospy.ServiceException as e: + rospy.logerr("Service call failed: %s" % e) + rospy.logerr("this means a goal is currently executing - no plan can be made right now") + return False + + def wait_until_twist_achieved(self, cmd_vel_value, epsilon, update_rate): + """ + We wait for the cmd_vel twist given to be reached by the robot reading + from the odometry. + :param cmd_vel_value: Twist we want to wait to reach. + :param epsilon: Error acceptable in odometry readings. + :param update_rate: Rate at which we check the odometry. + :return: + """ + rospy.logdebug("START wait_until_twist_achieved...") + + rate = rospy.Rate(update_rate) + start_wait_time = rospy.get_rostime().to_sec() + end_wait_time = 0.0 + epsilon = 0.05 + + rospy.logdebug("Desired Twist Cmd>>" + str(cmd_vel_value)) + rospy.logdebug("epsilon>>" + str(epsilon)) + + linear_speed = cmd_vel_value.linear.x + angular_speed = cmd_vel_value.angular.z + + linear_speed_plus = linear_speed + epsilon + linear_speed_minus = linear_speed - epsilon + angular_speed_plus = angular_speed + epsilon + angular_speed_minus = angular_speed - epsilon + + while not rospy.is_shutdown(): + current_odometry = self._check_odom_ready() + # IN turtlebot3 the odometry angular readings are inverted, so we have to invert the sign. + odom_linear_vel = current_odometry.twist.twist.linear.x + odom_angular_vel = -1*current_odometry.twist.twist.angular.z + + rospy.logdebug("Linear VEL=" + str(odom_linear_vel) + ", ?RANGE=[" + str(linear_speed_minus) + ","+str(linear_speed_plus)+"]") + rospy.logdebug("Angular VEL=" + str(odom_angular_vel) + ", ?RANGE=[" + str(angular_speed_minus) + ","+str(angular_speed_plus)+"]") + + linear_vel_are_close = (odom_linear_vel <= linear_speed_plus) and (odom_linear_vel > linear_speed_minus) + angular_vel_are_close = (odom_angular_vel <= angular_speed_plus) and (odom_angular_vel > angular_speed_minus) + + if linear_vel_are_close and angular_vel_are_close: + rospy.logdebug("Reached Velocity!") + end_wait_time = rospy.get_rostime().to_sec() + break + rospy.logdebug("Not there yet, keep waiting...") + rate.sleep() + delta_time = end_wait_time- start_wait_time + rospy.logdebug("[Wait Time=" + str(delta_time)+"]") + + rospy.logdebug("END wait_until_twist_achieved...") + + return delta_time + + + def is_stuck(self): + x_list = list() + y_list = list() + z_list = list() + recorded_odom = self.odom_recorder + for odom in recorded_odom: + x_list.append(odom.pose.pose.position.x) + y_list.append(odom.pose.pose.position.y) + z_list.append(odom.pose.pose.position.z) + + if abs(max(x_list)-min(x_list)) < self.odom_diff_threshold and abs(max(y_list)-min(y_list)) < self.odom_diff_threshold and abs(max(z_list)-min(z_list)) < self.odom_diff_threshold: + return True + else: + return False + + + def get_odom(self): + return self.odom + + def get_imu(self): + return self.imu + + def get_laser_scan(self): + return self.laser_scan + + def get_frontier_map(self): + # while not self.frontier_map_receivedt: + # pass + return self.frontier_map + + def get_map(self): + # while not self.map_received: + # pass + return self.map + + def get_pose_map(self): + # while not self.pose_map_received: + # pass + return self.pose_map + + def get_frontier_map_set(self): + return self.frontier_map_received + + def get_map_set(self): + return self.map_received + + def get_pose_map_set(self): + return self.pose_map_received + + def pub_reset_gmapping(self): + reset_msg = String() + reset_msg.data = 'reset' + self.reset_gmapping_pub.publish(reset_msg) + print("Sent reset signal") + + def visualize_goal_point(self, marker_pose): + + marker = Marker() + marker.action = Marker.ADD + marker.header.frame_id = self.robot_namespace_no_slash + "/map" #self.robot_namespace_no_slash + "/base_link" + marker.id = 0 + marker.type = Marker.SPHERE + marker.pose.position.x = marker_pose.pose.position.x + marker.pose.position.y = marker_pose.pose.position.y + marker.pose.position.z = 0 + marker.pose.orientation.x = 0 + marker.pose.orientation.y = 0 + marker.pose.orientation.z = 0 + marker.pose.orientation.w = 1 + marker.scale.x = 0.25 + marker.scale.y = 0.25 + marker.scale.z = 0.25 + + marker.color.a = 0.5 + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + + self.visualization_pub.publish(marker) + + def point_to_index(self, point, my_map): + """convert a index to a point""" + pt = self.world_to_map(point[0], point[1], my_map) + return pt[1] * my_map.info.width + pt[0] + + def convert_location(self, loc, my_map): + """converts points to the grid""" + res = my_map.info.resolution + Xorigin = my_map.info.origin.position.x + Yorigin = my_map.info.origin.position.y + + # offset from origin and then divide by resolution + Xcell = int((loc[0] - Xorigin - (res / 2)) / res) + Ycell = int((loc[1] - Yorigin - (res / 2)) / res) + return (Xcell, Ycell) + + + def index_to_point(self, index, my_map): + x = index % int(my_map.info.width) + y = (index - x) / my_map.info.width + return (x, y) + + + def map_to_world(self, x, y, my_map): + """ + converts a point from the map to the world + :param x: float of x position + :param y: float of y position + :return: tuple of converted point + """ + resolution = my_map.info.resolution + + originX = my_map.info.origin.position.x + originY = my_map.info.origin.position.y + + # multiply by resolution, then move by origin offset + x = x * resolution + originX + resolution / 2 + y = y * resolution + originY + resolution / 2 + return (x, y) + + def world_to_map(self, x, y, my_map): + """ + converts a point from the world to the map + :param x: float of x position + :param y: float of y position + :return: tuple of converted point + """ + + return self.convert_location((x, y), my_map) + + def start_move_base(self): + self.start_move_base_pub.publish("start") + + def stop_move_base(self): + self.stop_move_base_pub.publish("stop") + + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/wamv_env.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/wamv_env.py new file mode 100644 index 0000000..1ff9f90 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_envs/wamv_env.py @@ -0,0 +1,211 @@ +import numpy +import rospy +import time +from openai_ros import robot_gazebo_env +from nav_msgs.msg import Odometry +from openai_ros.openai_ros_common import ROSLauncher + + +class WamvEnv(robot_gazebo_env.RobotGazeboEnv): + """Superclass for all WamvEnv environments. + """ + + def __init__(self, ros_ws_abspath): + """ + Initializes a new WamvEnv environment. + + To check any topic we need to have the simulations running, we need to do two things: + 1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations + that are pause for whatever the reason + 2) If the simulation was running already for some reason, we need to reset the controlers. + This has to do with the fact that some plugins with tf, dont understand the reset of the simulation + and need to be reseted to work properly. + + The Sensors: The sensors accesible are the ones considered usefull for AI learning. + + Sensor Topic List: + * /wamv/odom: Odometry of the Base of Wamv + + Actuators Topic List: + * /cmd_drive: You publish the speed of the left and right propellers. + + Args: + """ + rospy.logdebug("Start WamvEnv INIT...") + # Variables that we give through the constructor. + # None in this case + + # We launch the ROSlaunch that spawns the robot into the world + ROSLauncher(rospackage_name="robotx_gazebo", + launch_file_name="put_wamv_in_world.launch", + ros_ws_abspath=ros_ws_abspath) + + from robotx_gazebo.msg import UsvDrive + + # Internal Vars + # Doesnt have any accesibles + self.controllers_list = [] + + # It doesnt use namespace + self.robot_name_space = "" + + # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv + super(WamvEnv, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=False, + start_init_physics_parameters=False, + reset_world_or_sim="WORLD") + + + + rospy.logdebug("WamvEnv unpause1...") + self.gazebo.unpauseSim() + #self.controllers_object.reset_controllers() + + self._check_all_systems_ready() + + + # We Start all the ROS related Subscribers and publishers + rospy.Subscriber("/wamv/odom", Odometry, self._odom_callback) + + + self.publishers_array = [] + self._cmd_drive_pub = rospy.Publisher('/cmd_drive', UsvDrive, queue_size=1) + + self.publishers_array.append(self._cmd_drive_pub) + + self._check_all_publishers_ready() + + self.gazebo.pauseSim() + + rospy.logdebug("Finished WamvEnv INIT...") + + # Methods needed by the RobotGazeboEnv + # ---------------------------- + + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + rospy.logdebug("WamvEnv check_all_systems_ready...") + self._check_all_sensors_ready() + rospy.logdebug("END WamvEnv _check_all_systems_ready...") + return True + + + # CubeSingleDiskEnv virtual methods + # ---------------------------- + + def _check_all_sensors_ready(self): + rospy.logdebug("START ALL SENSORS READY") + self._check_odom_ready() + rospy.logdebug("ALL SENSORS READY") + + + def _check_odom_ready(self): + self.odom = None + rospy.logdebug("Waiting for /wamv/odom to be READY...") + while self.odom is None and not rospy.is_shutdown(): + try: + self.odom = rospy.wait_for_message("/wamv/odom", Odometry, timeout=1.0) + rospy.logdebug("Current /wamv/odom READY=>") + + except: + rospy.logerr("Current /wamv/odom not ready yet, retrying for getting odom") + return self.odom + + + + def _odom_callback(self, data): + self.odom = data + + + def _check_all_publishers_ready(self): + """ + Checks that all the publishers are working + :return: + """ + rospy.logdebug("START ALL SENSORS READY") + for publisher_object in self.publishers_array: + self._check_pub_connection(publisher_object) + rospy.logdebug("ALL SENSORS READY") + + def _check_pub_connection(self, publisher_object): + + rate = rospy.Rate(10) # 10hz + while publisher_object.get_num_connections() == 0 and not rospy.is_shutdown(): + rospy.logdebug("No susbribers to publisher_object yet so we wait and try again") + try: + rate.sleep() + except rospy.ROSInterruptException: + # This is to avoid error when world is rested, time when backwards. + pass + rospy.logdebug("publisher_object Publisher Connected") + + rospy.logdebug("All Publishers READY") + + + # Methods that the TrainingEnvironment will need to define here as virtual + # because they will be used in RobotGazeboEnv GrandParentClass and defined in the + # TrainingEnvironment. + # ---------------------------- + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + raise NotImplementedError() + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _get_obs(self): + raise NotImplementedError() + + def _is_done(self, observations): + """Checks if episode done based on observations given. + """ + raise NotImplementedError() + + # Methods that the TrainingEnvironment will need. + # ---------------------------- + def set_propellers_speed(self, right_propeller_speed, left_propeller_speed, time_sleep=1.0): + """ + It will set the speed of each of the two proppelers of wamv. + """ + i = 0 + for publisher_object in self.publishers_array: + from robotx_gazebo.msg import UsvDrive + usv_drive_obj = UsvDrive() + usv_drive_obj.right = right_propeller_speed + usv_drive_obj.left = left_propeller_speed + + rospy.logdebug("usv_drive_obj>>"+str(usv_drive_obj)) + publisher_object.publish(usv_drive_obj) + i += 1 + self.wait_time_for_execute_movement(time_sleep) + + def wait_time_for_execute_movement(self, time_sleep): + """ + Because this Wamv position is global, we really dont have + a way to know if its moving in the direction desired, because it would need + to evaluate the diference in position and speed on the local reference. + """ + time.sleep(time_sleep) + + def get_odom(self): + return self.odom + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_gazebo_env.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_gazebo_env.py new file mode 100644 index 0000000..31a0cd3 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/robot_gazebo_env.py @@ -0,0 +1,201 @@ +import rospy +import gym +from gym.utils import seeding +from .gazebo_connection import GazeboConnection +from .controllers_connection import ControllersConnection +#https://bitbucket.org/theconstructcore/theconstruct_msgs/src/master/msg/RLExperimentInfo.msg +from openai_ros.msg import RLExperimentInfo + +# https://github.com/openai/gym/blob/master/gym/core.py +class RobotGazeboEnv(gym.Env): + + def __init__(self, robot_name_space, controllers_list, reset_controls, start_init_physics_parameters=True, reset_world_or_sim="SIMULATION"): + + # To reset Simulations + rospy.logdebug("START init RobotGazeboEnv") + self.gazebo = GazeboConnection(start_init_physics_parameters,reset_world_or_sim) + self.controllers_object = ControllersConnection(namespace=robot_name_space, controllers_list=controllers_list) + self.reset_controls = reset_controls + self.seed() + + # Set up ROS related variables + self.episode_num = 0 + self.cumulated_episode_reward = 0 + self.reward_pub = rospy.Publisher('/openai/reward', RLExperimentInfo, queue_size=1) + + # We Unpause the simulation and reset the controllers if needed + """ + To check any topic we need to have the simulations running, we need to do two things: + 1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations + that are pause for whatever the reason + 2) If the simulation was running already for some reason, we need to reset the controlers. + This has to do with the fact that some plugins with tf, dont understand the reset of the simulation + and need to be reseted to work properly. + """ + self.gazebo.unpauseSim() + if self.reset_controls: + self.controllers_object.reset_controllers() + + rospy.logdebug("END init RobotGazeboEnv") + + # Env methods + def seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + def step(self, action): + """ + Function executed each time step. + Here we get the action execute it in a time step and retrieve the + observations generated by that action. + :param action: + :return: obs, reward, done, info + """ + + """ + Here we should convert the action num to movement action, execute the action in the + simulation and get the observations result of performing that action. + """ + rospy.logdebug("START STEP OpenAIROS") + + self.gazebo.unpauseSim() + self._set_action(action) + self.gazebo.pauseSim() + obs = self._get_obs() + done = self._is_done(obs) + info = {} + reward = self._compute_reward(obs, done) + self.cumulated_episode_reward += reward + + rospy.logdebug("END STEP OpenAIROS") + + return obs, reward, done, info + + def reset(self): + rospy.logdebug("Reseting RobotGazeboEnvironment") + # obs = self._get_obs() + self._reset_sim() + self._init_env_variables() + self._update_episode() + #FIXME: moved to top - if works delete later + obs = self._get_obs() + rospy.logdebug("END Reseting RobotGazeboEnvironment") + return obs + + def close(self): + """ + Function executed when closing the environment. + Use it for closing GUIS and other systems that need closing. + :return: + """ + rospy.logdebug("Closing RobotGazeboEnvironment") + rospy.signal_shutdown("Closing RobotGazeboEnvironment") + + def _update_episode(self): + """ + Publishes the cumulated reward of the episode and + increases the episode number by one. + :return: + """ + rospy.logwarn("PUBLISHING REWARD...") + self._publish_reward_topic( + self.cumulated_episode_reward, + self.episode_num + ) + rospy.logwarn("PUBLISHING REWARD...DONE="+str(self.cumulated_episode_reward)+",EP="+str(self.episode_num)) + + self.episode_num += 1 + self.cumulated_episode_reward = 0 + + + def _publish_reward_topic(self, reward, episode_number=1): + """ + This function publishes the given reward in the reward topic for + easy access from ROS infrastructure. + :param reward: + :param episode_number: + :return: + """ + reward_msg = RLExperimentInfo() + reward_msg.episode_number = episode_number + reward_msg.episode_reward = reward + self.reward_pub.publish(reward_msg) + + # Extension methods + # ---------------------------- + + def _reset_sim(self): + """Resets a simulation + """ + rospy.logdebug("RESET SIM START") + if self.reset_controls : + rospy.logdebug("RESET CONTROLLERS") + self.gazebo.unpauseSim() + self.controllers_object.reset_controllers() + self._check_all_systems_ready() + self._set_init_pose() + self.gazebo.pauseSim() + self.gazebo.resetSim() + self.gazebo.unpauseSim() + self.controllers_object.reset_controllers() + self._check_all_systems_ready() + self.gazebo.pauseSim() + + else: + rospy.logwarn("DONT RESET CONTROLLERS") + self.gazebo.unpauseSim() + self._check_all_systems_ready() + self._set_init_pose() + self.gazebo.pauseSim() + self.gazebo.resetSim() + self.gazebo.unpauseSim() + self._check_all_systems_ready() + self.gazebo.pauseSim() + + rospy.logdebug("RESET SIM END") + return True + + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + raise NotImplementedError() + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + raise NotImplementedError() + + def _get_obs(self): + """Returns the observation. + """ + raise NotImplementedError() + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _is_done(self, observations): + """Indicates whether or not the episode is done ( the robot has fallen for example). + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _env_setup(self, initial_qpos): + """Initial configuration of the environment. Can be used to configure initial state + and extract information from the simulation. + """ + raise NotImplementedError() + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/cartpole_stay_up/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/cartpole_stay_up/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/cartpole_stay_up/config/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/cartpole_stay_up/config/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/cartpole_stay_up/config/stay_up.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/cartpole_stay_up/config/stay_up.yaml new file mode 100644 index 0000000..8f1c8f8 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/cartpole_stay_up/config/stay_up.yaml @@ -0,0 +1,18 @@ +cartpole_v0: #namespace + + # Cartpole3D environment variables + n_actions: 2 # Number of actions used by algorithm and task + + min_pole_angle: -0.7 #-23 + max_pole_angle: 0.7 #23 + max_base_velocity: 50 + min_base_pose_x: -1.0 + max_base_pose_x: 1.0 + pos_step: 1.0 # increment in position/velocity/effort, depends on the control for each command + running_step: 0.04 # amount of time the control will be executed + init_pos: 0.0 # Position in which the base will start + wait_time: 0.1 # Time to wait in the reset phases + control_type: "velocity" + + + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/cartpole_stay_up/stay_up.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/cartpole_stay_up/stay_up.py new file mode 100644 index 0000000..828f076 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/cartpole_stay_up/stay_up.py @@ -0,0 +1,174 @@ +from gym import utils +from openai_ros.robot_envs import cartpole_env +from gym.envs.registration import register +from gym import error, spaces +import rospy +import math +import numpy as np +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +from openai_ros.openai_ros_common import ROSLauncher +import os + +class CartPoleStayUpEnv(cartpole_env.CartPoleEnv): + def __init__(self): + + ros_ws_abspath = rospy.get_param("/cartpole_v0/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name="cartpole_description", + launch_file_name="start_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/cartpole_stay_up/config", + yaml_file_name="stay_up.yaml") + + self.get_params() + + self.action_space = spaces.Discrete(self.n_actions) + high = np.array([ + 2.5 * 2, + np.finfo(np.float32).max, + 0.7 * 2, + np.finfo(np.float32).max]) + self.observation_space = spaces.Box(-high, high) + + # TODO: Remove when working + """ + cartpole_env.CartPoleEnv.__init__( + self, control_type=self.control_type + ) + """ + + # Here we will add any init functions prior to starting the MyRobotEnv + super(CartPoleStayUpEnv, self).__init__(control_type=self.control_type, + ros_ws_abspath=ros_ws_abspath) + + def get_params(self): + # get configuration parameters + self.n_actions = rospy.get_param('/cartpole_v0/n_actions') + self.min_pole_angle = rospy.get_param('/cartpole_v0/min_pole_angle') + self.max_pole_angle = rospy.get_param('/cartpole_v0/max_pole_angle') + self.max_base_velocity = rospy.get_param( + '/cartpole_v0/max_base_velocity') + self.min_base_pose_x = rospy.get_param('/cartpole_v0/min_base_pose_x') + self.max_base_pose_x = rospy.get_param('/cartpole_v0/max_base_pose_x') + self.pos_step = rospy.get_param('/cartpole_v0/pos_step') + self.running_step = rospy.get_param('/cartpole_v0/running_step') + self.init_pos = rospy.get_param('/cartpole_v0/init_pos') + self.wait_time = rospy.get_param('/cartpole_v0/wait_time') + self.control_type = rospy.get_param('/cartpole_v0/control_type') + + def _set_action(self, action): + + # Take action + if action == 0: # LEFT + rospy.loginfo("GO LEFT...") + self.pos[0] -= self.pos_step + elif action == 1: # RIGHT + rospy.loginfo("GO RIGHT...") + self.pos[0] += self.pos_step + elif action == 2: # LEFT BIG + rospy.loginfo("GO LEFT BIG...") + self.pos[0] -= self.pos_step * 10 + elif action == 3: # RIGHT BIG + rospy.loginfo("GO RIGHT BIG...") + self.pos[0] += self.pos_step * 10 + + # Apply action to simulation. + rospy.loginfo("MOVING TO POS=="+str(self.pos)) + + # 1st: unpause simulation + #rospy.logdebug("Unpause SIM...") + # self.gazebo.unpauseSim() + + self.move_joints(self.pos) + rospy.logdebug( + "Wait for some time to execute movement, time="+str(self.running_step)) + rospy.sleep(self.running_step) # wait for some time + rospy.logdebug( + "DONE Wait for some time to execute movement, time=" + str(self.running_step)) + + # 3rd: pause simulation + #rospy.logdebug("Pause SIM...") + # self.gazebo.pauseSim() + + def _get_obs(self): + + data = self.joints + # base_postion base_velocity pole angle pole velocity + #obs = [round(data.position[1],1), round(data.velocity[1],1), round(data.position[0],1), round(data.velocity[0],1)] + obs = [data.position[1], data.velocity[1], + data.position[0], data.velocity[0]] + + return np.array(obs) + + def _is_done(self, observations): + done = False + data = self.joints + + rospy.loginfo("BASEPOSITION=="+str(observations[0])) + rospy.loginfo("POLE ANGLE==" + str(observations[2])) + # check if the base is still within the ranges of (-2, 2) + if (self.min_base_pose_x >= observations[0] or observations[0] >= self.max_base_pose_x): + rospy.logerr("Base Outside Limits==>min="+str(self.min_base_pose_x) + + ",pos="+str(observations[0])+",max="+str(self.max_base_pose_x)) + done = True + # check if pole has toppled over + if (self.min_pole_angle >= observations[2] or observations[2] >= self.max_pole_angle): + rospy.logerr( + "Pole Angle Outside Limits==>min=" + str(self.min_pole_angle) + ",pos=" + str(observations[2]) + ",max=" + str( + self.max_pole_angle)) + done = True + + rospy.loginfo("FINISHED get _is_done") + + return done + + def _compute_reward(self, observations, done): + """ + Gives more points for staying upright, gets data from given observations to avoid + having different data than other previous functions + :return:reward + """ + rospy.logdebug("START _compute_reward") + + if not done: + reward = 1.0 + elif self.steps_beyond_done is None: + # Pole just fell! + self.steps_beyond_done = 0 + reward = 1.0 + else: + if self.steps_beyond_done == 0: + logger.warning("You are calling 'step()' even though this environment has already returned done = True. You should always call 'reset()' once you receive 'done = True' -- any further steps are undefined behavior.") + self.steps_beyond_done += 1 + reward = 0.0 + + rospy.logdebug("END _compute_reward") + + return reward + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + self.steps_beyond_done = None + + def _set_init_pose(self): + """ + Sets joints to initial position [0,0,0] + :return: + """ + + self.check_publishers_connection() + + # Reset Internal pos variable + self.init_internal_vars(self.init_pos) + self.move_joints(self.pos) diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/config/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/config/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/config/fetch_push.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/config/fetch_push.yaml new file mode 100644 index 0000000..9ee0827 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/config/fetch_push.yaml @@ -0,0 +1,36 @@ +fetch: #namespace + + # Fetch Task Realated parameters + n_actions: 6 # X+/-,Y+/-,Z+/- + n_observations: 4 # XYZ of the TCP and the distance from GOAL + position_ee_max: 1.0 + position_ee_min: -1.0 + + init_pos: # This has to be validated in the fetch_moveit_test.py in fetch_openai_ros_example or something that tests this pos is possible + joint0: 0.0 + joint1: -0.8 + joint2: 0.0 + joint3: 1.6 + joint4: 0.0 + joint5: 0.8 + joint6: 0.0 + + + setup_ee_pos: # This has to be validated in the fetch_moveit_test.py in fetch_openai_ros_example or something that tests this pos is possible + x: 0.598 + y: 0.005 + z: 0.9 + + goal_ee_pos: # This has to be validated in the fetch_moveit_test.py in fetch_openai_ros_example or something that tests this pos is possible + x: 2.0 + y: 2.0 + z: 2.0 + + position_delta: 0.1 # Increments of Decrements in the X/Y/Z positions each action step + + step_punishment: -1 + closer_reward: 10 + impossible_movement_punishement: -100 + reached_goal_reward: 100 + + max_distance: 3.0 # Maximum distance from EE to the desired GOAL EE \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/config/fetch_test.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/config/fetch_test.yaml new file mode 100644 index 0000000..2b724a4 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/config/fetch_test.yaml @@ -0,0 +1,35 @@ +fetch: #namespace + + # Fetch Task Realated parameters + n_actions: 6 # X+/-,Y+/-,Z+/- + n_observations: 4 # XYZ of the TCP and the distance from GOAL + position_ee_max: 1.0 + position_ee_min: -1.0 + + init_pos: # This has to be validated in the fetch_moveit_test.py in fetch_openai_ros_example or something that tests this pos is possible + joint0: 0.0 + joint1: 0.0 + joint2: 0.0 + joint3: 0.0 + joint4: 0.0 + joint5: 0.0 + joint6: 0.0 + + setup_ee_pos: # This has to be validated in the fetch_moveit_test.py in fetch_openai_ros_example or something that tests this pos is possible + x: 0.598 + y: 0.005 + z: 0.9 + + goal_ee_pos: # This has to be validated in the fetch_moveit_test.py in fetch_openai_ros_example or something that tests this pos is possible + x: 2.0 + y: 2.0 + z: 2.0 + + position_delta: 0.1 # Increments of Decrements in the X/Y/Z positions each action step + + step_punishment: -1 + closer_reward: 10 + impossible_movement_punishement: -100 + reached_goal_reward: 100 + + max_distance: 3.0 # Maximum distance from EE to the desired GOAL EE \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/config/fetchsimple_test.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/config/fetchsimple_test.yaml new file mode 100644 index 0000000..bd30937 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/config/fetchsimple_test.yaml @@ -0,0 +1,32 @@ +fetch: #namespace + + # Fetch Task Realated parameters + n_actions: 6 # elbow_flex_joint+/-,shoulder_lift_joint+/-,shoulder_pan_joint+/- + n_observations: 3 # XYZ of the TCP and the distance from GOAL + + max_iterations: 20 + + init_pos: # This has to be validated in the fetch_moveit_test.py in fetch_openai_ros_example or something that tests this pos is possible + bellows_joint: 0.0 + elbow_flex_joint: 0.0 + forearm_roll_joint: 0.0 + head_pan_joint: 0.0 + head_tilt_joint: 0.0 + l_gripper_finger_joint: 0.0 + r_gripper_finger_joint: 0.0 + shoulder_lift_joint: 0.0 + shoulder_pan_joint: 0.0 + torso_lift_joint: 0.0 + upperarm_roll_joint: 0.0 + wrist_flex_joint: 0.0 + wrist_roll_joint: 0.0 + + goal_pos: # This has to be validated in the fetch_moveit_test.py in fetch_openai_ros_example or something that tests this pos is possible + elbow_flex_joint: -1.8 + shoulder_lift_joint: 0.7 + shoulder_pan_joint: 1.32 + + + position_delta: 0.1 # Increments of Decrements in the positions each action step + + reached_goal_reward: 100 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/fetch_pick_and_place_task.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/fetch_pick_and_place_task.py new file mode 100644 index 0000000..e2eb0c2 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/fetch_pick_and_place_task.py @@ -0,0 +1,214 @@ +from gym import utils +import copy +import rospy +import numpy as np +from gym import spaces +from openai_ros.robot_envs import fetchsimple_env +from gym.envs.registration import register +import numpy as np +from sensor_msgs.msg import JointState +from openai_ros.openai_ros_common import ROSLauncher +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +import os + + +class FetchPickAndPlaceEnv(fetchsimple_env.FetchSimpleEnv, utils.EzPickle): + def __init__(self): + + # Launch the Task Simulated-Environment + # This is the path where the simulation files are, + # the Task and the Robot gits will be downloaded if not there + + ros_ws_abspath = rospy.get_param("/fetch/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name="fetch_simple_description", + launch_file_name="start_HER_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file relative to this TaskEnvironment + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/fetch/config", + yaml_file_name="fetchsimple_test.yaml") + + super(FetchPickAndPlaceEnv, self).__init__(ros_ws_abspath) + + rospy.logdebug("Entered FetchPickAndPlaceEnv Env") + self.get_params() + + self.action_space = spaces.Discrete(self.n_actions) + + observations_high_range = np.array( + self.upper_array_observations) + observations_low_range = np.array( + self.lower_array_observations) + + self.observation_space = spaces.Box( + observations_low_range, observations_high_range) + + def get_params(self): + # get configuration parameters + + self.n_actions = rospy.get_param('/fetch/n_actions') + self.n_max_iterations = rospy.get_param('/fetch/max_iterations') + + self.init_pos = rospy.get_param('/fetch/init_pos') + + init_pos_dict = rospy.get_param('/fetch/init_pos') + self.init_pos = [init_pos_dict["bellows_joint"], + init_pos_dict["elbow_flex_joint"], + init_pos_dict["forearm_roll_joint"], + init_pos_dict["head_pan_joint"], + init_pos_dict["head_tilt_joint"], + init_pos_dict["l_gripper_finger_joint"], + init_pos_dict["r_gripper_finger_joint"], + init_pos_dict["shoulder_lift_joint"], + init_pos_dict["shoulder_pan_joint"], + + init_pos_dict["torso_lift_joint"], + init_pos_dict["upperarm_roll_joint"], + init_pos_dict["wrist_flex_joint"], + init_pos_dict["wrist_roll_joint"] + ] + + goal_pos_dict = rospy.get_param('/fetch/goal_pos') + self.goal_pos = [goal_pos_dict["elbow_flex_joint"], + goal_pos_dict["shoulder_lift_joint"], + goal_pos_dict["shoulder_pan_joint"]] + + self.position_delta = rospy.get_param('/fetch/position_delta') + self.reached_goal_reward = rospy.get_param( + '/fetch/reached_goal_reward') + + upper_array, lower_array = self.get_joint_limits() + self.upper_array_observations = [ + upper_array[1], upper_array[7], upper_array[8]] + self.lower_array_observations = [ + lower_array[1], lower_array[7], lower_array[8]] + + self.n_observations = len(self.upper_array_observations) + + def _set_init_pose(self): + """Sets the Robot in its init pose + The Simulation will be unpaused for this purpose. + """ + # Check because it seems its not being used + rospy.logdebug("Init Pos:") + rospy.logdebug(self.init_pos) + + """ + # Init Joint Pose + rospy.logdebug("Moving To SETUP Joints ") + self.movement_result = self.set_trajectory_joints(self.init_pos) + """ + + # We test the Desired Goal + + # INIT POSE + rospy.logdebug("Moving To Init Pose ") + self.move_to_init_pose() + self.last_action = "INIT" + + return True + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + The simulation will be paused, therefore all the data retrieved has to be + from a system that doesnt need the simulation running, like variables where the + callbackas have stored last know sesnor data. + :return: + """ + rospy.logdebug("Init Env Variables...") + self.interations_done = 0 + rospy.logdebug("Init Env Variables...END") + + def _set_action(self, action): + + delta_gripper_target = [0.0]*len(self.init_pos) + rospy.logwarn("==== ROBOT ENV RECEIVED ACTION: "+str(action)) + + delta_gripper_target[1] += action[0] + delta_gripper_target[7] += action[1] + delta_gripper_target[8] += action[2] + + # # We convert action ints in increments/decrements of one of the axis XYZ + # if action == 0: # elbow_flex_joint+ + # delta_gripper_target[1] += self.position_delta + # self.last_action = "elbow_flex_joint+" + # elif action == 1: # elbow_flex_joint- + # delta_gripper_target[1] -= self.position_delta + # self.last_action = "elbow_flex_joint-" + # elif action == 2: # shoulder_lift_joint+ + # delta_gripper_target[7] += self.position_delta + # self.last_action = "shoulder_lift_joint+" + # elif action == 3: # shoulder_lift_joint- + # delta_gripper_target[7] -= self.position_delta + # self.last_action = "shoulder_lift_joint-" + # elif action == 4: # shoulder_pan_joint+ + # delta_gripper_target[8] += self.position_delta + # self.last_action = "shoulder_pan_joint+" + # elif action == 5: # shoulder_pan_joint- + # delta_gripper_target[8] -= self.position_delta + # self.last_action = "shoulder_pan_joint-" + + self.movement_result = self.set_trajectory_joints(delta_gripper_target) + + # rospy.logwarn("END Set Action ==>" + str(action) + + # ", NAME=" + str(self.last_action)) + + def _get_obs(self): + """ + It returns the Position of the TCP/EndEffector as observation. + And the distance from the desired point + Orientation for the moment is not considered + """ + + joints_position = self.get_joints_position() + obs_joints_position = { + 'observation': np.array([joints_position[1]]), + 'achieved_goal': np.array([joints_position[7]]), + 'desired_goal': np.array([joints_position[8]]), + } + + # obs_joints_position = [joints_position[1], + # joints_position[7], joints_position[8]] + + rospy.logdebug("OBSERVATIONS====>>>>>>>"+str(obs_joints_position)) + + return obs_joints_position + + def _is_done(self, observations): + """ + If the latest Action didnt succeed, it means that tha position asked was imposible therefore the episode must end. + It will also end if it reaches its goal. + """ + + done = np.allclose(a=observations, + b=self.goal_pos, + atol=0.2) + + self.interations_done += 1 + + if self.interations_done >= self.n_max_iterations: + done = True + + return done + + def _compute_reward(self, observations, done): + """ + We punish each step that it passes without achieveing the goal. + Punishes differently if it reached a position that is imposible to move to. + Rewards getting to a position close to the goal. + """ + reward = 1.0 / \ + (np.linalg.norm(np.array(observations) - np.array(self.goal_pos))) + if done: + reward += self.reached_goal_reward + rospy.logwarn(">>>REWARD>>>"+str(reward)) + + return reward diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/fetch_push.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/fetch_push.py new file mode 100644 index 0000000..e732834 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/fetch_push.py @@ -0,0 +1,273 @@ +from gym import utils +import copy +import rospy +from gym import spaces +from openai_ros.robot_envs import fetch_env +from gym.envs.registration import register +import numpy as np +from sensor_msgs.msg import JointState +from openai_ros.openai_ros_common import ROSLauncher +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +import os + + +class FetchPushEnv(fetch_env.FetchEnv, utils.EzPickle): + def __init__(self): + + # Launch the Task Simulated-Environment + # This is the path where the simulation files are, + # the Task and the Robot gits will be downloaded if not there + + ros_ws_abspath = rospy.get_param("/fetch/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name="fetch_simple_description", + launch_file_name="start_HER_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file relative to this TaskEnvironment + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/fetch/config", + yaml_file_name="fetch_push.yaml") + + self.get_params() + + # TODO: this must be continuous action space... don't follow the old implementation. + self.action_space = spaces.Discrete(self.n_actions) + + observations_high_range = np.array( + [self.position_ee_max]*self.n_observations) + observations_low_range = np.array( + [self.position_ee_min]*self.n_observations) + + observations_high_dist = np.array([self.max_distance]) + observations_low_dist = np.array([0.0]) + + high = np.concatenate( + [observations_high_range, observations_high_dist]) + low = np.concatenate([observations_low_range, observations_low_dist]) + + self.observation_space = spaces.Box(low, high) + + # TODO: Clean up + # fetch_env.FetchEnv.__init__(self) + super(FetchPushEnv, self).__init__(ros_ws_abspath) + + def get_params(self): + """ + get configuration parameters + + """ + self.sim_time = rospy.get_time() + self.n_actions = rospy.get_param('/fetch/n_actions') + self.n_observations = rospy.get_param('/fetch/n_observations') + self.position_ee_max = rospy.get_param('/fetch/position_ee_max') + self.position_ee_min = rospy.get_param('/fetch/position_ee_min') + + self.init_pos = rospy.get_param('/fetch/init_pos') + self.setup_ee_pos = rospy.get_param('/fetch/setup_ee_pos') + self.goal_ee_pos = rospy.get_param('/fetch/goal_ee_pos') + + self.position_delta = rospy.get_param('/fetch/position_delta') + self.step_punishment = rospy.get_param('/fetch/step_punishment') + self.closer_reward = rospy.get_param('/fetch/closer_reward') + self.impossible_movement_punishement = rospy.get_param( + '/fetch/impossible_movement_punishement') + self.reached_goal_reward = rospy.get_param( + '/fetch/reached_goal_reward') + + self.max_distance = rospy.get_param('/fetch/max_distance') + + self.goal = np.array( + [self.goal_ee_pos["x"], self.goal_ee_pos["y"], self.goal_ee_pos["z"]]) + self.rot_ctrl = np.array([1., 0., 1., 0.]) + + self.prev_grip_pos = np.zeros(3) + self.prev_object_pos = np.zeros(3) + self.prev_object_rot = np.zeros(3) + + def _set_init_pose(self): + """ + Sets the Robot in its init pose + The Simulation will be unpaused for this purpose. + """ + if not self.set_trajectory_joints(self.init_pos): + assert False, "Initialisation is failed...." + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + The simulation will be paused, therefore all the data retrieved has to be + from a system that doesnt need the simulation running, like variables where the + callbackas have stored last know sesnor data. + :return: + """ + rospy.logdebug("Init Env Variables...") + rospy.logdebug("Init Env Variables...END") + + def _set_action(self, action): + rospy.logwarn("=== Action: {}".format(action)) + pos_ctrl, gripper_ctrl = action[:3], action[3] + + """ + Since the implementation of self.set_trajectory_ee(in fetch_env.py) ONLY takes the position of the EE(end-effector) + Action only contains: the destination of the EE in the world frame + + TODO: Talk to Miguel regarding the modification of the basic implementation of self.set_trajectory_ee(in fetch_env.py) + + below code is similar to the original implementaion of OpenAI + """ + # gripper_ctrl = np.array([gripper_ctrl, gripper_ctrl]) + # action = np.concatenate([pos_ctrl, self.rot_ctrl, gripper_ctrl]) + + # TODO: After speak to Miguel, let's not use this, USE above action!! + action = pos_ctrl + + self.movement_result = self.set_trajectory_ee(action) + if not self.movement_result: + assert False, "movement_result failed with the action of : " + \ + str(action) + + def _get_obs(self): + """ + It returns the Position of the TCP/EndEffector as observation. + And the distance from the desired point + Orientation for the moment is not considered + + Note: + - In original code(https://github.com/openai/gym/blob/master/gym/envs/robotics/fetch_env.py#L91), + the term (xvelp) is used and it means positional velocity in world frame + """ + grip_pos_v = self.get_ee_pose() + grip_pos = np.array([grip_pos_v.pose.position.x, + grip_pos_v.pose.position.y, grip_pos_v.pose.position.z]) + dt = self.get_elapsed_time() + # Velocity(position) = Distance/Time + grip_velp = (grip_pos - self.prev_grip_pos)/dt + # the pose and rotation of the cube/box on a table + object_data = self.obj_positions.get_states() + object_pos = object_data[:3] # pose of cube + object_rot = object_data[3:] # rotation of cube + object_velp = (object_pos - self.prev_object_pos) / \ + dt # Velocity(position) = Distance/Time + object_velr = (object_rot - self.prev_object_rot) / \ + dt # Velocity(rotation) = Rotation/Time + object_rel_pos = object_pos - grip_pos + # Unknown meaning of this operation(https://github.com/openai/gym/blob/master/gym/envs/robotics/fetch_env.py#L102) + object_velp -= grip_velp + + """ + TODO: Ask Miguel the meaning of the two variables below + + 1. gripper_state => https: // github.com / openai / gym / blob / master / gym / envs / robotics / fetch_env.py # L105 + 2. gripper_vel => https: // github.com / openai / gym / blob / master / gym / envs / robotics / fetch_env.py # L106 + + """ + gripper_state = np.zeros(0) # temp workaround + gripper_vel = np.zeros(0) # temp workaround + + achieved_goal = np.squeeze(object_pos.copy()) + obs = np.concatenate([ + grip_pos, # absolute position of gripper + object_pos.ravel(), # absolute position of object + object_rel_pos.ravel(), # relative position of object from gripper + gripper_state, # gripper state + object_rot.ravel(), # rotations of object + object_velp.ravel(), # positional velocities of object + object_velr.ravel(), # rotational velocities of object + grip_velp, # distance between fingers + gripper_vel, # velocities of gripper + ]) + + rospy.logdebug("OBSERVATIONS====>>>>>>>"+str(obs)) + # Update the previous properties + self.prev_grip_pos = grip_pos + self.prev_object_pos = object_pos + self.prev_object_rot = object_rot + + return { + 'observation': obs.copy(), + 'achieved_goal': achieved_goal.copy(), + 'desired_goal': self.goal.copy(), + } + + def get_elapsed_time(self): + """ + Returns the elapsed time since the beginning of the simulation + Then maintains the current time as "previous time" to calculate the elapsed time again + """ + current_time = rospy.get_time() + dt = self.sim_time - current_time + self.sim_time = current_time + return dt + + def _is_done(self, observations): + """ + If the latest Action didnt succeed, it means that tha position asked was imposible therefore the episode must end. + It will also end if it reaches its goal. + """ + + current_pos = observations[:3] + + done = self.calculate_if_done( + self.movement_result, self.goal, current_pos) + + return done + + def _compute_reward(self, observations, done): + """ + Given a success of the execution of the action + Calculate the reward: binary => 1 for success, 0 for failure + """ + current_pos = observations[:3] + # TODO: ask Miguel, why we need this + new_dist_from_des_pos_ee = observations[-1] + + if self.movement_result: + position_similar = np.all(np.isclose( + self.goal, current_pos, atol=1e-02)) + if position_similar: + reward = self.reached_goal_reward + rospy.logwarn("Reached a Desired Position!") + else: + reward = 0 + else: + # TODO: Ask Miguel about the purpose of having "self.impossible_movement_punishement" + reward = self.impossible_movement_punishement + rospy.logwarn("Reached a TCP position not reachable") + rospy.logwarn(">>>REWARD>>>"+str(reward)) + + return reward + + def calculate_if_done(self, movement_result, goal, current_pos): + """ + It calculated whather it has finished or not + """ + done = False + + if movement_result: + # check if the end-effector located within a threshold + # TODO: check this threshold is alined with the paper one + position_similar = np.all( + np.isclose(goal, current_pos, atol=1e-02)) + + if position_similar: + done = True + rospy.logdebug("Reached a Desired Position!") + else: + # or if the end-effector reaches the maximum strechable point + done = True + rospy.logdebug("Reached a TCP position not reachable") + + return done + + def calculate_distance_between(self, v1, v2): + """ + Calculated the Euclidian distance between two vectors given as python lists. + """ + dist = np.linalg.norm(np.array(v1) - np.array(v2)) + return dist diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/fetch_simple_task.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/fetch_simple_task.py new file mode 100644 index 0000000..9ac95f6 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/fetch_simple_task.py @@ -0,0 +1,201 @@ +from gym import utils +import copy +import rospy +from gym import spaces +from openai_ros.robot_envs import fetchsimple_env +from gym.envs.registration import register +import numpy as np +from sensor_msgs.msg import JointState +from openai_ros.openai_ros_common import ROSLauncher +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +import os + +class FetchSimpleTestEnv(fetchsimple_env.FetchSimpleEnv, utils.EzPickle): + def __init__(self): + + # Launch the Task Simulated-Environment + # This is the path where the simulation files are, + # the Task and the Robot gits will be downloaded if not there + + ros_ws_abspath = rospy.get_param("/fetch/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name="fetch_simple_description", + launch_file_name="start_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file relative to this TaskEnvironment + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/fetch/config", + yaml_file_name="fetchsimple_test.yaml") + + super(FetchSimpleTestEnv, self).__init__(ros_ws_abspath) + + rospy.logdebug("Entered FetchSimpleTestEnv Env") + self.get_params() + + self.action_space = spaces.Discrete(self.n_actions) + + observations_high_range = np.array( + self.upper_array_observations) + observations_low_range = np.array( + self.lower_array_observations) + + self.observation_space = spaces.Box(observations_low_range, observations_high_range) + + + + def get_params(self): + # get configuration parameters + + self.n_actions = rospy.get_param('/fetch/n_actions') + self.n_max_iterations = rospy.get_param('/fetch/max_iterations') + + + self.init_pos = rospy.get_param('/fetch/init_pos') + + init_pos_dict = rospy.get_param('/fetch/init_pos') + self.init_pos = [init_pos_dict["bellows_joint"], + init_pos_dict["elbow_flex_joint"], + init_pos_dict["forearm_roll_joint"], + init_pos_dict["head_pan_joint"], + init_pos_dict["head_tilt_joint"], + init_pos_dict["l_gripper_finger_joint"], + init_pos_dict["r_gripper_finger_joint"], + init_pos_dict["shoulder_lift_joint"], + init_pos_dict["shoulder_pan_joint"], + + init_pos_dict["torso_lift_joint"], + init_pos_dict["upperarm_roll_joint"], + init_pos_dict["wrist_flex_joint"], + init_pos_dict["wrist_roll_joint"] + ] + + + goal_pos_dict = rospy.get_param('/fetch/goal_pos') + self.goal_pos = [goal_pos_dict["elbow_flex_joint"], + goal_pos_dict["shoulder_lift_joint"], + goal_pos_dict["shoulder_pan_joint"]] + + self.position_delta = rospy.get_param('/fetch/position_delta') + self.reached_goal_reward = rospy.get_param( + '/fetch/reached_goal_reward') + + upper_array, lower_array = self.get_joint_limits() + self.upper_array_observations = [upper_array[1],upper_array[7],upper_array[8]] + self.lower_array_observations = [lower_array[1], lower_array[7], lower_array[8]] + + self.n_observations = len(self.upper_array_observations) + + + def _set_init_pose(self): + """Sets the Robot in its init pose + The Simulation will be unpaused for this purpose. + """ + # Check because it seems its not being used + rospy.logdebug("Init Pos:") + rospy.logdebug(self.init_pos) + + """ + # Init Joint Pose + rospy.logdebug("Moving To SETUP Joints ") + self.movement_result = self.set_trajectory_joints(self.init_pos) + """ + + # We test the Desired Goal + + # INIT POSE + rospy.logdebug("Moving To Init Pose ") + self.move_to_init_pose() + self.last_action = "INIT" + + return True + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + The simulation will be paused, therefore all the data retrieved has to be + from a system that doesnt need the simulation running, like variables where the + callbackas have stored last know sesnor data. + :return: + """ + rospy.logdebug("Init Env Variables...") + self.interations_done = 0 + rospy.logdebug("Init Env Variables...END") + + def _set_action(self, action): + + delta_gripper_target = [0.0]*len(self.init_pos) + + # We convert action ints in increments/decrements of one of the axis XYZ + if action == 0: # elbow_flex_joint+ + delta_gripper_target[1] += self.position_delta + self.last_action = "elbow_flex_joint+" + elif action == 1: # elbow_flex_joint- + delta_gripper_target[1] -= self.position_delta + self.last_action = "elbow_flex_joint-" + elif action == 2: # shoulder_lift_joint+ + delta_gripper_target[7] += self.position_delta + self.last_action = "shoulder_lift_joint+" + elif action == 3: # shoulder_lift_joint- + delta_gripper_target[7] -= self.position_delta + self.last_action = "shoulder_lift_joint-" + elif action == 4: # shoulder_pan_joint+ + delta_gripper_target[8] += self.position_delta + self.last_action = "shoulder_pan_joint+" + elif action == 5: # shoulder_pan_joint- + delta_gripper_target[8] -= self.position_delta + self.last_action = "shoulder_pan_joint-" + + self.movement_result = self.set_trajectory_joints(delta_gripper_target) + + rospy.logwarn("END Set Action ==>" + str(action) + + ", NAME=" + str(self.last_action)) + + def _get_obs(self): + """ + It returns the Position of the TCP/EndEffector as observation. + And the distance from the desired point + Orientation for the moment is not considered + """ + + joints_position = self.get_joints_position() + obs_joints_position = [joints_position[1],joints_position[7],joints_position[8]] + + rospy.logdebug("OBSERVATIONS====>>>>>>>"+str(obs_joints_position)) + + return obs_joints_position + + def _is_done(self, observations): + """ + If the latest Action didnt succeed, it means that tha position asked was imposible therefore the episode must end. + It will also end if it reaches its goal. + """ + + done = np.allclose(a=observations, + b=self.goal_pos, + atol=0.2) + + self.interations_done += 1 + + if self.interations_done >= self.n_max_iterations: + done = True + + return done + + def _compute_reward(self, observations, done): + """ + We punish each step that it passes without achieveing the goal. + Punishes differently if it reached a position that is imposible to move to. + Rewards getting to a position close to the goal. + """ + reward = 1.0 / (np.linalg.norm(np.array(observations) - np.array(self.goal_pos))) + if done: + reward += self.reached_goal_reward + rospy.logwarn(">>>REWARD>>>"+str(reward)) + + return reward diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/fetch_test_task.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/fetch_test_task.py new file mode 100644 index 0000000..714176b --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/fetch/fetch_test_task.py @@ -0,0 +1,299 @@ +from gym import utils +import copy +import rospy +from gym import spaces +from openai_ros.robot_envs import fetch_env +from gym.envs.registration import register +import numpy as np +from sensor_msgs.msg import JointState +from openai_ros.openai_ros_common import ROSLauncher +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +import os + +class FetchTestEnv(fetch_env.FetchEnv, utils.EzPickle): + def __init__(self): + + # Launch the Task Simulated-Environment + # This is the path where the simulation files are, + # the Task and the Robot gits will be downloaded if not there + + ros_ws_abspath = rospy.get_param("/fetch/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name="fetch_gazebo", + launch_file_name="start_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file relative to this TaskEnvironment + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/fetch/config", + yaml_file_name="fetch_test.yaml") + + rospy.logdebug("Entered FetchTestEnv Env") + self.get_params() + + self.action_space = spaces.Discrete(self.n_actions) + + observations_high_range = np.array( + [self.position_ee_max]*self.n_observations) + observations_low_range = np.array( + [self.position_ee_min]*self.n_observations) + + observations_high_dist = np.array([self.max_distance]) + observations_low_dist = np.array([0.0]) + + high = np.concatenate( + [observations_high_range, observations_high_dist]) + low = np.concatenate([observations_low_range, observations_low_dist]) + + self.observation_space = spaces.Box(low, high) + + # TODO: Clean up + # fetch_env.FetchEnv.__init__(self) + super(FetchTestEnv, self).__init__(ros_ws_abspath) + + def get_params(self): + # get configuration parameters + + self.n_actions = rospy.get_param('/fetch/n_actions') + self.n_observations = rospy.get_param('/fetch/n_observations') + self.position_ee_max = rospy.get_param('/fetch/position_ee_max') + self.position_ee_min = rospy.get_param('/fetch/position_ee_min') + + self.init_pos = rospy.get_param('/fetch/init_pos') + self.setup_ee_pos = rospy.get_param('/fetch/setup_ee_pos') + self.goal_ee_pos = rospy.get_param('/fetch/goal_ee_pos') + + self.position_delta = rospy.get_param('/fetch/position_delta') + self.step_punishment = rospy.get_param('/fetch/step_punishment') + self.closer_reward = rospy.get_param('/fetch/closer_reward') + self.impossible_movement_punishement = rospy.get_param( + '/fetch/impossible_movement_punishement') + self.reached_goal_reward = rospy.get_param( + '/fetch/reached_goal_reward') + + self.max_distance = rospy.get_param('/fetch/max_distance') + + self.desired_position = [self.goal_ee_pos["x"], + self.goal_ee_pos["y"], self.goal_ee_pos["z"]] + self.gripper_rotation = [1., 0., 1., 0.] + + def _set_init_pose(self): + """Sets the Robot in its init pose + The Simulation will be unpaused for this purpose. + """ + # Check because it seems its not being used + rospy.logdebug("Init Pos:") + rospy.logdebug(self.init_pos) + + """ + # Init Joint Pose + rospy.logdebug("Moving To SETUP Joints ") + self.movement_result = self.set_trajectory_joints(self.init_pos) + """ + + # We test the Desired Goal + + # INIT POSE + rospy.logdebug("Moving To TEST DESIRED GOAL Position ") + action = self.create_action( + self.desired_position, self.gripper_rotation) + self.movement_result = self.set_trajectory_ee(action) + + if self.movement_result: + # INIT POSE + rospy.logdebug("Moving To SETUP Position ") + self.last_gripper_target = [ + self.setup_ee_pos["x"], self.setup_ee_pos["y"], self.setup_ee_pos["z"]] + action = self.create_action( + self.last_gripper_target, self.gripper_rotation) + self.movement_result = self.set_trajectory_ee(action) + + self.current_dist_from_des_pos_ee = self.calculate_distance_between( + self.desired_position, self.last_gripper_target) + rospy.logdebug("INIT DISTANCE FROM GOAL==>" + + str(self.current_dist_from_des_pos_ee)) + else: + assert False, "Desired GOAL EE is not possible" + + self.last_action = "INIT" + + rospy.logdebug("Init Pose Results ==>"+str(self.movement_result)) + + return self.movement_result + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + The simulation will be paused, therefore all the data retrieved has to be + from a system that doesnt need the simulation running, like variables where the + callbackas have stored last know sesnor data. + :return: + """ + rospy.logdebug("Init Env Variables...") + rospy.logdebug("Init Env Variables...END") + + def _set_action(self, action): + + delta_gripper_target = [0.0]*len(self.last_gripper_target) + + # We convert action ints in increments/decrements of one of the axis XYZ + if action == 0: # X+ + delta_gripper_target[0] += self.position_delta + self.last_action = "X+" + elif action == 1: # X- + delta_gripper_target[0] -= self.position_delta + self.last_action = "X-" + elif action == 2: # Y+ + delta_gripper_target[1] += self.position_delta + self.last_action = "Y+" + elif action == 3: # Y- + delta_gripper_target[1] -= self.position_delta + self.last_action = "Y-" + elif action == 4: # Z+ + delta_gripper_target[2] += self.position_delta + self.last_action = "Z+" + elif action == 5: # Z- + delta_gripper_target[2] -= self.position_delta + self.last_action = "Z-" + + gripper_target = copy.deepcopy(self.last_gripper_target) + gripper_target[0] += delta_gripper_target[0] + gripper_target[1] += delta_gripper_target[1] + gripper_target[2] += delta_gripper_target[2] + + # Apply action to simulation. + action_end_effector = self.create_action( + gripper_target, self.gripper_rotation) + self.movement_result = self.set_trajectory_ee(action_end_effector) + if self.movement_result: + # If the End Effector Positioning was succesfull, we replace the last one with the new one. + self.last_gripper_target = copy.deepcopy(gripper_target) + else: + rospy.logerr("Impossible End Effector Position...." + + str(gripper_target)) + + rospy.logwarn("END Set Action ==>"+str(action) + + ", NAME="+str(self.last_action)) + + def _get_obs(self): + """ + It returns the Position of the TCP/EndEffector as observation. + And the distance from the desired point + Orientation for the moment is not considered + """ + + grip_pos = self.get_ee_pose() + grip_pos_array = [grip_pos.pose.position.x, + grip_pos.pose.position.y, grip_pos.pose.position.z] + obs = grip_pos_array + + new_dist_from_des_pos_ee = self.calculate_distance_between( + self.desired_position, grip_pos_array) + + obs.append(new_dist_from_des_pos_ee) + + rospy.logdebug("OBSERVATIONS====>>>>>>>"+str(obs)) + + return obs + + def _is_done(self, observations): + """ + If the latest Action didnt succeed, it means that tha position asked was imposible therefore the episode must end. + It will also end if it reaches its goal. + """ + + current_pos = observations[:3] + + done = self.calculate_if_done( + self.movement_result, self.desired_position, current_pos) + + return done + + def _compute_reward(self, observations, done): + """ + We punish each step that it passes without achieveing the goal. + Punishes differently if it reached a position that is imposible to move to. + Rewards getting to a position close to the goal. + """ + current_pos = observations[:3] + new_dist_from_des_pos_ee = observations[-1] + + reward = self.calculate_reward( + self.movement_result, self.desired_position, current_pos, new_dist_from_des_pos_ee) + rospy.logwarn(">>>REWARD>>>"+str(reward)) + + return reward + + def calculate_if_done(self, movement_result, desired_position, current_pos): + """ + It calculated whather it has finished or not + """ + done = False + + if movement_result: + + position_similar = np.all(np.isclose( + desired_position, current_pos, atol=1e-02)) + + if position_similar: + done = True + rospy.logdebug("Reached a Desired Position!") + else: + done = True + rospy.logdebug("Reached a TCP position not reachable") + + return done + + def calculate_reward(self, movement_result, desired_position, current_pos, new_dist_from_des_pos_ee): + """ + It calculated whather it has finished or nota and how much reward to give + """ + + if movement_result: + position_similar = np.all(np.isclose( + desired_position, current_pos, atol=1e-02)) + + # Calculating Distance + rospy.logwarn("desired_position="+str(desired_position)) + rospy.logwarn("current_pos="+str(current_pos)) + rospy.logwarn("self.current_dist_from_des_pos_ee=" + + str(self.current_dist_from_des_pos_ee)) + rospy.logwarn("new_dist_from_des_pos_ee=" + + str(new_dist_from_des_pos_ee)) + + delta_dist = new_dist_from_des_pos_ee - self.current_dist_from_des_pos_ee + if position_similar: + reward = self.reached_goal_reward + rospy.logwarn("Reached a Desired Position!") + else: + if delta_dist < 0: + reward = self.closer_reward + rospy.logwarn( + "CLOSER To Desired Position!="+str(delta_dist)) + else: + reward = self.step_punishment + rospy.logwarn( + "FURTHER FROM Desired Position!"+str(delta_dist)) + + else: + reward = self.impossible_movement_punishement + rospy.logwarn("Reached a TCP position not reachable") + + # We update the distance + self.current_dist_from_des_pos_ee = new_dist_from_des_pos_ee + rospy.logdebug("Updated Distance from GOAL==" + + str(self.current_dist_from_des_pos_ee)) + + return reward + + def calculate_distance_between(self, v1, v2): + """ + Calculated the Euclidian distance between two vectors given as python lists. + """ + dist = np.linalg.norm(np.array(v1)-np.array(v2)) + return dist diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/hopper/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/hopper/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/hopper/config/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/hopper/config/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/hopper/config/hopper_stay_up.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/hopper/config/hopper_stay_up.yaml new file mode 100644 index 0000000..f9d97f2 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/hopper/config/hopper_stay_up.yaml @@ -0,0 +1,44 @@ +monoped: #namespace + + n_actions: 6 # 1-2) Increment/Decrement haa_joint + # 3-4) Increment/Decrement hfe_joint + # 5-6) Increment/Decrement kfe_joint + + init_joint_states: + haa_joint: 0.0 + hfe_joint: 0.0 + kfe_joint: 0.0 + + desired_point: + x: 0.0 + y: 0.0 + z: 1.0 + accepted_error_in_des_pos: 0.1 # Accepted error un meters of the desired position + + desired_yaw: 0.0 # Desired yaw in radians for the hopper to stay + joint_increment_value: 0.05 # in radians + accepted_joint_error: 0.1 # Radians of accepted error in the joints, should be bigger than the joint increment + update_rate: 20 # Hz frequency in whihc we check that the joints are in the correct place, the bigger the more precise but more overhead + number_decimals_precision_obs: 1 + desired_force: 7.08 # In Newtons, normal contact force when stanting still with 9.81 gravity + max_x_pos: 3.0 # Maximum Position of the base in X axis in World + max_y_pos: 3.0 # Maximum Position of the base in Y axis in World + max_height: 3.0 # in meters + min_height: 0.5 # in meters + distance_from_desired_point_max: 3.0 # Distance that we consider maximum to be from the desired point + max_incl: 1.57 # Maximum acceptable roll and pitch for the hoppers base. More is conidered a fallen robot + max_contact_force: 14.16 # The maxixmum contact force that we consoder acceptable. + maximum_haa_joint: 1.6 # Max and Min value ( sign change ) based on the URDF joint values + maximum_hfe_joint: 1.6 # Max and Min value ( sign change ) based on the URDF joint values + maximum_kfe_joint: 0.0 # Max based on the URDF joint values + min_kfe_joint: -1.6 # Min value based on the URDF joint values + + rewards_weight: + weight_joint_position: 1.0 # Weight for joint positions ( joints in the zero is perfect ) + weight_contact_force: 1.0 # Weight for contact force similar to desired ( weight of monoped ) + weight_orientation: 1.0 # Weight for orientation ( vertical is perfect ) + weight_distance_from_des_point: 1.0 # Weight for distance from desired point ( on the point is perfect ) + + alive_reward: 100.0 # reward + done_reward: 1000.0 # reward + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/hopper/hopper_stay_up.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/hopper/hopper_stay_up.py new file mode 100644 index 0000000..fe4b27e --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/hopper/hopper_stay_up.py @@ -0,0 +1,646 @@ +import rospy +import numpy +from gym import spaces +from openai_ros.robot_envs import hopper_env +from gym.envs.registration import register +from geometry_msgs.msg import Point +from geometry_msgs.msg import Vector3 +from tf.transformations import euler_from_quaternion +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +from openai_ros.openai_ros_common import ROSLauncher +import os + +class HopperStayUpEnv(hopper_env.HopperEnv): + def __init__(self): + """ + Make Hopper Learn how to Stay Up indefenitly + """ + + # Only variable needed to be set here + """ + For this version, we consider 6 actions + 1-2) Increment/Decrement haa_joint + 3-4) Increment/Decrement hfe_joint + 5-6) Increment/Decrement kfe_joint + """ + rospy.logdebug("Start HopperStayUpEnv INIT...") + + # This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there + ros_ws_abspath = rospy.get_param("/monoped/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name="legged_robots_sims", + launch_file_name="start_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/hopper/config", + yaml_file_name="hopper_stay_up.yaml") + + number_actions = rospy.get_param('/monoped/n_actions') + self.action_space = spaces.Discrete(number_actions) + + # We set the reward range, which is not compulsory but here we do it. + self.reward_range = (-numpy.inf, numpy.inf) + + # Actions and Observations + + self.init_joint_states = Vector3() + self.init_joint_states.x = rospy.get_param( + '/monoped/init_joint_states/haa_joint') + self.init_joint_states.y = rospy.get_param( + '/monoped/init_joint_states/hfe_joint') + self.init_joint_states.z = rospy.get_param( + '/monoped/init_joint_states/kfe_joint') + + # Get Desired Point to Get + self.desired_point = Point() + self.desired_point.x = rospy.get_param("/monoped/desired_point/x") + self.desired_point.y = rospy.get_param("/monoped/desired_point/y") + self.desired_point.z = rospy.get_param("/monoped/desired_point/z") + self.accepted_error_in_des_pos = rospy.get_param( + "/monoped/accepted_error_in_des_pos") + + self.desired_yaw = rospy.get_param("/monoped/desired_yaw") + + self.joint_increment_value = rospy.get_param( + "/monoped/joint_increment_value") + self.init_move_time = rospy.get_param("/monoped/init_move_time", 1.0) + self.move_time = rospy.get_param("/monoped/move_time", 0.05) + self.check_position = rospy.get_param("/monoped/check_position", True) + + self.accepted_joint_error = rospy.get_param( + "/monoped/accepted_joint_error") + self.update_rate = rospy.get_param("/monoped/update_rate") + + self.dec_obs = rospy.get_param( + "/monoped/number_decimals_precision_obs") + + self.desired_force = rospy.get_param("/monoped/desired_force") + + self.max_x_pos = rospy.get_param("/monoped/max_x_pos") + self.max_y_pos = rospy.get_param("/monoped/max_y_pos") + + self.min_height = rospy.get_param("/monoped/min_height") + self.max_height = rospy.get_param("/monoped/max_height") + + self.distance_from_desired_point_max = rospy.get_param( + "/monoped/distance_from_desired_point_max") + + self.max_incl_roll = rospy.get_param("/monoped/max_incl") + self.max_incl_pitch = rospy.get_param("/monoped/max_incl") + self.max_contact_force = rospy.get_param("/monoped/max_contact_force") + + self.maximum_haa_joint = rospy.get_param("/monoped/maximum_haa_joint") + self.maximum_hfe_joint = rospy.get_param("/monoped/maximum_hfe_joint") + self.maximum_kfe_joint = rospy.get_param("/monoped/maximum_kfe_joint") + self.min_kfe_joint = rospy.get_param("/monoped/min_kfe_joint") + + # We place the Maximum and minimum values of observations + self.joint_ranges_array = {"maximum_haa": self.maximum_haa_joint, + "minimum_haa_joint": -self.maximum_haa_joint, + "maximum_hfe_joint": self.maximum_hfe_joint, + "minimum_hfe_joint": self.maximum_hfe_joint, + "maximum_kfe_joint": self.maximum_kfe_joint, + "min_kfe_joint": self.min_kfe_joint + } + + high = numpy.array([self.distance_from_desired_point_max, + self.max_incl_roll, + self.max_incl_pitch, + 3.14, + self.max_contact_force, + self.maximum_haa_joint, + self.maximum_hfe_joint, + self.maximum_kfe_joint, + self.max_x_pos, + self.max_y_pos, + self.max_height + ]) + + low = numpy.array([0.0, + -1*self.max_incl_roll, + -1*self.max_incl_pitch, + -1*3.14, + 0.0, + -1*self.maximum_haa_joint, + -1*self.maximum_hfe_joint, + self.min_kfe_joint, + -1*self.max_x_pos, + -1*self.max_y_pos, + self.min_height + ]) + + self.observation_space = spaces.Box(low, high) + + rospy.logdebug("ACTION SPACES TYPE===>"+str(self.action_space)) + rospy.logdebug("OBSERVATION SPACES TYPE===>" + + str(self.observation_space)) + + # Rewards + self.weight_joint_position = rospy.get_param( + "/monoped/rewards_weight/weight_joint_position") + self.weight_contact_force = rospy.get_param( + "/monoped/rewards_weight/weight_contact_force") + self.weight_orientation = rospy.get_param( + "/monoped/rewards_weight/weight_orientation") + self.weight_distance_from_des_point = rospy.get_param( + "/monoped/rewards_weight/weight_distance_from_des_point") + + self.alive_reward = rospy.get_param("/monoped/alive_reward") + self.done_reward = rospy.get_param("/monoped/done_reward") + + # Here we will add any init functions prior to starting the MyRobotEnv + super(HopperStayUpEnv, self).__init__(ros_ws_abspath) + + rospy.logdebug("END HopperStayUpEnv INIT...") + + def _set_init_pose(self): + """ + Sets the Robot in its init linear and angular speeds + and lands the robot. Its preparing it to be reseted in the world. + """ + + joints_array = [self.init_joint_states.x, + self.init_joint_states.y, + self.init_joint_states.z] + + self.move_joints(joints_array, + epsilon=self.accepted_joint_error, + update_rate=self.update_rate, + time_sleep=self.init_move_time, + check_position=self.check_position) + + return True + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + + # For Info Purposes + self.cumulated_reward = 0.0 + # We get the initial pose to mesure the distance from the desired point. + odom = self.get_odom() + self.previous_distance_from_des_point = self.get_distance_from_desired_point( + odom.pose.pose.position) + + def _set_action(self, action): + """ + It sets the joints of monoped based on the action integer given + based on the action number given. + :param action: The action integer that sets what movement to do next. + """ + + rospy.logdebug("Start Set Action ==>"+str(action)) + + # We get current Joints values + joint_states = self.get_joint_states() + joint_states_position = joint_states.position + rospy.logdebug("get_action_to_position>>>"+str(joint_states_position)) + + action_position = [0.0, 0.0, 0.0] + + rospy.logdebug( + "OLD-JOINT-STATE [haa,hfa,kfe]>>>"+str(joint_states_position)) + + if action == 0: # Increment haa_joint + rospy.logdebug("Increment haa_joint") + action_position[0] = joint_states_position[0] + \ + self.joint_increment_value + action_position[1] = joint_states_position[1] + action_position[2] = joint_states_position[2] + elif action == 1: # Decrement haa_joint + rospy.logdebug("Decrement haa_joint") + action_position[0] = joint_states_position[0] - \ + self.joint_increment_value + action_position[1] = joint_states_position[1] + action_position[2] = joint_states_position[2] + elif action == 2: # Increment hfe_joint + rospy.logdebug("Increment hfe_joint") + action_position[0] = joint_states_position[0] + action_position[1] = joint_states_position[1] + \ + self.joint_increment_value + action_position[2] = joint_states_position[2] + elif action == 3: # Decrement hfe_joint + rospy.logdebug("Decrement hfe_joint") + action_position[0] = joint_states_position[0] + action_position[1] = joint_states_position[1] - \ + self.joint_increment_value + action_position[2] = joint_states_position[2] + elif action == 4: # Increment kfe_joint + rospy.logdebug("Increment kfe_joint") + action_position[0] = joint_states_position[0] + action_position[1] = joint_states_position[1] + action_position[2] = joint_states_position[2] + \ + self.joint_increment_value + elif action == 5: # Decrement kfe_joint + rospy.logdebug("Decrement kfe_joint") + action_position[0] = joint_states_position[0] + action_position[1] = joint_states_position[1] + action_position[2] = joint_states_position[2] - \ + self.joint_increment_value + + rospy.logdebug("NEW-JOINT-STATE [haa,hfa,kfe]>>>"+str(action_position)) + rospy.logdebug("JOINT-RANGES>>>"+str(self.joint_ranges_array)) + + rospy.logdebug("START ACTION EXECUTE>>>"+str(action)) + # We tell monoped where to place its joints next + self.move_joints(action_position, + epsilon=self.accepted_joint_error, + update_rate=self.update_rate, + time_sleep=self.move_time, + check_position=self.check_position) + rospy.logdebug("END ACTION EXECUTE>>>"+str(action)) + + rospy.logdebug("END Set Action ==>"+str(action)) + + def _get_obs(self): + """ + Here we define what sensor data defines our robots observations + To know which Variables we have access to, we need to read the + HopperEnv API DOCS + Returns the state of the robot needed for OpenAI QLearn Algorithm + The state will be defined by an array of the: + 1) distance from desired point in meters + 2) The pitch orientation in radians + 3) the Roll orientation in radians + 4) the Yaw orientation in radians + 5) Force in contact sensor in Newtons + 6-7-8) State of the 3 joints in radians + 9) Height of the Base + + observation = [distance_from_desired_point, + base_roll, + base_pitch, + base_yaw, + force_magnitude, + joint_states_haa, + joint_states_hfe, + joint_states_kfe, + height_base] + :return: observation + """ + rospy.logdebug("Start Get Observation ==>") + + distance_from_desired_point = self.get_distance_from_desired_point( + self.desired_point) + + base_orientation = self.get_base_rpy() + base_roll = base_orientation.x + base_pitch = base_orientation.y + base_yaw = base_orientation.z + + force_magnitude = self.get_contact_force_magnitude() + + joint_states = self.get_joint_states() + joint_states_haa = joint_states.position[0] + joint_states_hfe = joint_states.position[1] + joint_states_kfe = joint_states.position[2] + + odom = self.get_odom() + base_position = odom.pose.pose.position + + observation = [] + observation.append(round(distance_from_desired_point, self.dec_obs)) + observation.append(round(base_roll, self.dec_obs)) + observation.append(round(base_pitch, self.dec_obs)) + observation.append(round(base_yaw, self.dec_obs)) + observation.append(round(force_magnitude, self.dec_obs)) + observation.append(round(joint_states_haa, self.dec_obs)) + observation.append(round(joint_states_hfe, self.dec_obs)) + observation.append(round(joint_states_kfe, self.dec_obs)) + + observation.append(round(base_position.x, self.dec_obs)) + observation.append(round(base_position.y, self.dec_obs)) + observation.append(round(base_position.z, self.dec_obs)) # height + + return observation + + def _is_done(self, observations): + """ + We consider the episode done if: + 1) The Monopeds height is lower than a threshhold + 2) The Orientation is outside a threshold + """ + + height_base = observations[10] + + monoped_height_ok = self.monoped_height_ok(height_base) + monoped_orientation_ok = self.monoped_orientation_ok() + + done = not(monoped_height_ok and monoped_orientation_ok) + + return done + + def _compute_reward(self, observations, done): + """ + We Base the rewards in if its done or not and we base it on + the joint poisition, effort, contact force, orientation and distance from desired point. + :return: + """ + + joints_state_array = observations[5:8] + r1 = self.calculate_reward_joint_position( + joints_state_array, self.weight_joint_position) + # Desired Force in Newtons, taken form idle contact with 9.81 gravity. + + force_magnitude = observations[4] + r2 = self.calculate_reward_contact_force( + force_magnitude, self.weight_contact_force) + + rpy_array = observations[1:4] + r3 = self.calculate_reward_orientation( + rpy_array, self.weight_orientation) + + current_position = Point() + current_position.x = observations[8] + current_position.y = observations[9] + current_position.z = observations[10] + r4 = self.calculate_reward_distance_from_des_point( + current_position, self.weight_distance_from_des_point) + + # The sign depend on its function. + total_reward = self.alive_reward - r1 - r2 - r3 - r4 + + rospy.logdebug("###############") + rospy.logdebug("alive_bonus=" + str(self.alive_reward)) + rospy.logdebug("r1 joint_position=" + str(r1)) + rospy.logdebug("r2 contact_force=" + str(r2)) + rospy.logdebug("r3 orientation=" + str(r3)) + rospy.logdebug("r4 distance=" + str(r4)) + rospy.logdebug("total_reward=" + str(total_reward)) + rospy.logdebug("###############") + + return total_reward + + # Internal TaskEnv Methods + + def is_in_desired_position(self, current_position, epsilon=0.05): + """ + It return True if the current position is similar to the desired poistion + """ + + is_in_desired_pos = False + + x_pos_plus = self.desired_point.x + epsilon + x_pos_minus = self.desired_point.x - epsilon + y_pos_plus = self.desired_point.y + epsilon + y_pos_minus = self.desired_point.y - epsilon + + x_current = current_position.x + y_current = current_position.y + + x_pos_are_close = (x_current <= x_pos_plus) and ( + x_current > x_pos_minus) + y_pos_are_close = (y_current <= y_pos_plus) and ( + y_current > y_pos_minus) + + is_in_desired_pos = x_pos_are_close and y_pos_are_close + + rospy.logdebug("###### IS DESIRED POS ? ######") + rospy.logdebug("current_position"+str(current_position)) + rospy.logdebug("x_pos_plus"+str(x_pos_plus) + + ",x_pos_minus="+str(x_pos_minus)) + rospy.logdebug("y_pos_plus"+str(y_pos_plus) + + ",y_pos_minus="+str(y_pos_minus)) + rospy.logdebug("x_pos_are_close"+str(x_pos_are_close)) + rospy.logdebug("y_pos_are_close"+str(y_pos_are_close)) + rospy.logdebug("is_in_desired_pos"+str(is_in_desired_pos)) + rospy.logdebug("############") + + return is_in_desired_pos + + def is_inside_workspace(self, current_position): + """ + Check if the monoped is inside the Workspace defined + """ + is_inside = False + + rospy.logdebug("##### INSIDE WORK SPACE? #######") + rospy.logdebug("XYZ current_position"+str(current_position)) + rospy.logdebug("work_space_x_max"+str(self.work_space_x_max) + + ",work_space_x_min="+str(self.work_space_x_min)) + rospy.logdebug("work_space_y_max"+str(self.work_space_y_max) + + ",work_space_y_min="+str(self.work_space_y_min)) + rospy.logdebug("work_space_z_max"+str(self.work_space_z_max) + + ",work_space_z_min="+str(self.work_space_z_min)) + rospy.logdebug("############") + + if current_position.x > self.work_space_x_min and current_position.x <= self.work_space_x_max: + if current_position.y > self.work_space_y_min and current_position.y <= self.work_space_y_max: + if current_position.z > self.work_space_z_min and current_position.z <= self.work_space_z_max: + is_inside = True + + return is_inside + + def sonar_detected_something_too_close(self, sonar_value): + """ + Detects if there is something too close to the monoped front + """ + rospy.logdebug("##### SONAR TOO CLOSE? #######") + rospy.logdebug("sonar_value"+str(sonar_value) + + ",min_sonar_value="+str(self.min_sonar_value)) + rospy.logdebug("############") + + too_close = sonar_value < self.min_sonar_value + + return too_close + + def monoped_has_flipped(self, current_orientation): + """ + Based on the orientation RPY given states if the monoped has flipped + """ + has_flipped = True + + self.max_roll = rospy.get_param("/monoped/max_roll") + self.max_pitch = rospy.get_param("/monoped/max_pitch") + + rospy.logdebug("#### HAS FLIPPED? ########") + rospy.logdebug("RPY current_orientation"+str(current_orientation)) + rospy.logdebug("max_roll"+str(self.max_roll) + + ",min_roll="+str(-1*self.max_roll)) + rospy.logdebug("max_pitch"+str(self.max_pitch) + + ",min_pitch="+str(-1*self.max_pitch)) + rospy.logdebug("############") + + if current_orientation.x > -1*self.max_roll and current_orientation.x <= self.max_roll: + if current_orientation.y > -1*self.max_pitch and current_orientation.y <= self.max_pitch: + has_flipped = False + + return has_flipped + + def get_distance_from_desired_point(self, current_position): + """ + Calculates the distance from the current position to the desired point + :param start_point: + :return: + """ + distance = self.get_distance_from_point(current_position, + self.desired_point) + + return distance + + def get_distance_from_point(self, pstart, p_end): + """ + Given a Vector3 Object, get distance from current position + :param p_end: + :return: + """ + a = numpy.array((pstart.x, pstart.y, pstart.z)) + b = numpy.array((p_end.x, p_end.y, p_end.z)) + + distance = numpy.linalg.norm(a - b) + + return distance + + def get_orientation_euler(self, quaternion_vector): + # We convert from quaternions to euler + orientation_list = [quaternion_vector.x, + quaternion_vector.y, + quaternion_vector.z, + quaternion_vector.w] + + roll, pitch, yaw = euler_from_quaternion(orientation_list) + return roll, pitch, yaw + + def get_base_rpy(self): + + imu = self.get_imu() + base_orientation = imu.orientation + + euler_rpy = Vector3() + euler = euler_from_quaternion([base_orientation.x, + base_orientation.y, + base_orientation.z, + base_orientation.w] + ) + euler_rpy.x = euler[0] + euler_rpy.y = euler[1] + euler_rpy.z = euler[2] + + return euler_rpy + + def get_contact_force_magnitude(self): + """ + You will see that because the X axis is the one pointing downwards, it will be the one with + higher value when touching the floor + For a Robot of total mas of 0.55Kg, a gravity of 9.81 m/sec**2, Weight = 0.55*9.81=5.39 N + Falling from around 5centimetres ( negligible height ), we register peaks around + Fx = 7.08 N + :return: + """ + # We get the Contact Sensor data + lowerleg_contactsensor_state = self.get_lowerleg_contactsensor_state() + # We extract what we need that is only the total_wrench force + contact_force = self.get_contact_force(lowerleg_contactsensor_state) + # We create an array with each component XYZ + contact_force_np = numpy.array( + (contact_force.x, contact_force.y, contact_force.z)) + # We calculate the magnitude of the Force Vector, array. + force_magnitude = numpy.linalg.norm(contact_force_np) + + return force_magnitude + + def get_contact_force(self, lowerleg_contactsensor_state): + """ + /lowerleg_contactsensor_state/states[0]/contact_positions ==> PointContact in World + /lowerleg_contactsensor_state/states[0]/contact_normals ==> NormalContact in World + + ==> One is an array of all the forces, the other total, + and are relative to the contact link referred to in the sensor. + /lowerleg_contactsensor_state/states[0]/wrenches[] + /lowerleg_contactsensor_state/states[0]/total_wrench + :return: + """ + + # We create an empty element , in case there is no contact. + contact_force = Vector3() + for state in lowerleg_contactsensor_state.states: + self.contact_force = state.total_wrench.force + + return contact_force + + def monoped_height_ok(self, height_base): + + height_ok = self.min_height <= height_base < self.max_height + return height_ok + + def monoped_orientation_ok(self): + + orientation_rpy = self.get_base_rpy() + roll_ok = self.max_incl_roll > abs(orientation_rpy.x) + pitch_ok = self.max_incl_pitch > abs(orientation_rpy.y) + orientation_ok = roll_ok and pitch_ok + return orientation_ok + + def calculate_reward_joint_position(self, joints_state_array, weight=1.0): + """ + We calculate reward base on the joints configuration. The more near 0 the better. + :return: + """ + acumulated_joint_pos = 0.0 + for joint_pos in joints_state_array: + # Abs to remove sign influence, it doesnt matter the direction of turn. + acumulated_joint_pos += abs(joint_pos) + rospy.logdebug( + "calculate_reward_joint_position>>acumulated_joint_pos=" + str(acumulated_joint_pos)) + reward = weight * acumulated_joint_pos + rospy.logdebug( + "calculate_reward_joint_position>>reward=" + str(reward)) + return reward + + def calculate_reward_contact_force(self, force_magnitude, weight=1.0): + """ + We calculate reward base on the contact force. + The nearest to the desired contact force the better. + We use exponential to magnify big departures from the desired force. + Default ( 7.08 N ) desired force was taken from reading of the robot touching + the ground from a negligible height of 5cm. + :return: + """ + force_displacement = force_magnitude - self.desired_force + + rospy.logdebug( + "calculate_reward_contact_force>>force_magnitude=" + str(force_magnitude)) + rospy.logdebug( + "calculate_reward_contact_force>>force_displacement=" + str(force_displacement)) + # Abs to remove sign + reward = weight * abs(force_displacement) + rospy.logdebug("calculate_reward_contact_force>>reward=" + str(reward)) + return reward + + def calculate_reward_orientation(self, rpy_array, weight=1.0): + """ + We calculate the reward based on the orientation. + The more its closser to 0 the better because it means its upright + desired_yaw is the yaw that we want it to be. + to praise it to have a certain orientation, here is where to set it. + :param: rpy_array: Its an array with Roll Pitch and Yaw in place 0, 1 and 2 respectively. + :return: + """ + + yaw_displacement = rpy_array[2] - self.desired_yaw + acumulated_orientation_displacement = abs( + rpy_array[0]) + abs(rpy_array[1]) + abs(yaw_displacement) + reward = weight * acumulated_orientation_displacement + rospy.logdebug("calculate_reward_orientation>>reward=" + str(reward)) + return reward + + def calculate_reward_distance_from_des_point(self, current_position, weight=1.0): + """ + We calculate the distance from the desired point. + The closser the better + :param weight: + :return:reward + """ + distance = self.get_distance_from_desired_point(current_position) + reward = weight * distance + rospy.logdebug("calculate_reward_orientation>>reward=" + str(reward)) + return reward diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/husarion/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/husarion/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/husarion/config/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/husarion/config/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/husarion/config/husarion_get_to_position_turtlebot_playground.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/husarion/config/husarion_get_to_position_turtlebot_playground.yaml new file mode 100644 index 0000000..32cad20 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/husarion/config/husarion_get_to_position_turtlebot_playground.yaml @@ -0,0 +1,42 @@ +husarion: #namespace + n_actions: 4 # Forwards,TurnLeft,TurnRight, Backwards + init_linear_forward_speed: 0.0 # Initial linear speed in which we start each episode + init_linear_turn_speed: 0.0 # Initial angular speed in shich we start each episode + linear_forward_speed: 0.5 # Spwwed for ging fowards + linear_turn_speed: 0.0 # Lienare speed when turning + angular_speed: 1.5 # Angular speed when turning Left or Right + new_ranges: 40 # How many laser readings we jump in each observation reading, the bigger the less laser resolution + max_laser_value: 6.0 # Value considered Ok, no wall + min_laser_value: 0.3 # Value considered there is an obstacle or crashed + work_space: # 2D cube in which Husarion Cant surpass. + x_max: 3.0 + x_min: -7.0 + y_max: 3.0 + y_min: -5.0 + desired_pose: + x: -2.94 + y: 3.92 + precision: 1 # Number of decimals that we will accept in all sensor readings + move_base_precision: 0.2 # We allow this difference between the asked linear speed and the real one. Note that angular is not because controller is too imprecise in sim. + closer_to_point_reward: 10 # We give points for getting closer to the desired point + alive_reward: 0 # Point we give to just not not end the episode + end_episode_points: 200 # Points given when ending an episode + + + + + + + + + + + + + + + + + + + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/husarion/husarion_get_to_position_turtlebot_playground.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/husarion/husarion_get_to_position_turtlebot_playground.py new file mode 100644 index 0000000..f3b155d --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/husarion/husarion_get_to_position_turtlebot_playground.py @@ -0,0 +1,554 @@ +import rospy +import numpy +from gym import spaces +from openai_ros.robot_envs import husarion_env +from geometry_msgs.msg import Vector3 +from geometry_msgs.msg import Point +from tf.transformations import euler_from_quaternion +from sensor_msgs.msg import LaserScan +from std_msgs.msg import Header +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +from openai_ros.openai_ros_common import ROSLauncher +import os + +class HusarionGetToPosTurtleBotPlayGroundEnv(husarion_env.HusarionEnv): + def __init__(self): + """ + This Task Env is designed for having the husarion in the husarion world + closed room with columns. + It will learn how to move around without crashing. + """ + # Launch the Task Simulated-Environment + # This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there + ros_ws_abspath = rospy.get_param("/husarion/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name="rosbot_gazebo", + launch_file_name="start_world_maze.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/husarion/config", + yaml_file_name="husarion_get_to_position_turtlebot_playground.yaml") + + # Only variable needed to be set here + number_actions = rospy.get_param('/husarion/n_actions') + self.action_space = spaces.Discrete(number_actions) + + # We set the reward range, which is not compulsory but here we do it. + self.reward_range = (-numpy.inf, numpy.inf) + + # Actions and Observations + self.init_linear_forward_speed = rospy.get_param( + '/husarion/init_linear_forward_speed') + self.init_linear_turn_speed = rospy.get_param( + '/husarion/init_linear_turn_speed') + + self.linear_forward_speed = rospy.get_param( + '/husarion/linear_forward_speed') + self.linear_turn_speed = rospy.get_param('/husarion/linear_turn_speed') + self.angular_speed = rospy.get_param('/husarion/angular_speed') + + self.new_ranges = rospy.get_param('/husarion/new_ranges') + self.max_laser_value = rospy.get_param('/husarion/max_laser_value') + self.min_laser_value = rospy.get_param('/husarion/min_laser_value') + + self.work_space_x_max = rospy.get_param("/husarion/work_space/x_max") + self.work_space_x_min = rospy.get_param("/husarion/work_space/x_min") + self.work_space_y_max = rospy.get_param("/husarion/work_space/y_max") + self.work_space_y_min = rospy.get_param("/husarion/work_space/y_min") + + # Get Desired Point to Get + self.desired_position = Point() + self.desired_position.x = rospy.get_param("/husarion/desired_pose/x") + self.desired_position.y = rospy.get_param("/husarion/desired_pose/y") + + self.precision = rospy.get_param('/husarion/precision') + self.precision_epsilon = 1.0 / (10.0 * self.precision) + + self.move_base_precision = rospy.get_param( + '/husarion/move_base_precision') + + # We create the arrays for the laser readings + # We also create the arrays for the odometry readings + # We join them toeguether. + + # Here we will add any init functions prior to starting the MyRobotEnv + super(HusarionGetToPosTurtleBotPlayGroundEnv, + self).__init__(ros_ws_abspath) + + laser_scan = self._check_laser_scan_ready() + num_laser_readings = int(len(laser_scan.ranges)/self.new_ranges) + high_laser = numpy.full((num_laser_readings), self.max_laser_value) + low_laser = numpy.full((num_laser_readings), self.min_laser_value) + + # We place the Maximum and minimum values of the X,Y and YAW of the odometry + # The odometry yaw can be any value in the circunference. + high_odometry = numpy.array([self.work_space_x_max, + self.work_space_y_max, + 3.14]) + low_odometry = numpy.array([self.work_space_x_min, + self.work_space_y_min, + -1*3.14]) + + # Now we fetch the max and min of the Desired Position in 2D XY + # We use the exact same as the workspace, just because make no sense + # Consider points outside the workspace + high_des_pos = numpy.array([self.work_space_x_max, + self.work_space_y_max + ]) + low_des_pos = numpy.array([self.work_space_x_min, + self.work_space_y_min + ]) + + # We join both arrays + high = numpy.concatenate([high_laser, high_odometry, high_des_pos]) + low = numpy.concatenate([low_laser, low_odometry, low_des_pos]) + + self.observation_space = spaces.Box(low, high) + + rospy.logdebug("ACTION SPACES TYPE===>"+str(self.action_space)) + rospy.logdebug("OBSERVATION SPACES TYPE===>" + + str(self.observation_space)) + + # Rewards + self.closer_to_point_reward = rospy.get_param( + "/husarion/closer_to_point_reward") + self.alive_reward = rospy.get_param("/husarion/alive_reward") + self.end_episode_points = rospy.get_param( + "/husarion/end_episode_points") + + self.cumulated_steps = 0.0 + + self.laser_filtered_pub = rospy.Publisher( + '/rosbot/laser/scan_filtered', LaserScan, queue_size=1) + + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + self.move_base(self.init_linear_forward_speed, + self.init_linear_turn_speed, + epsilon=self.move_base_precision, + update_rate=10) + + return True + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + # For Info Purposes + self.cumulated_reward = 0.0 + + self.index = 0 + + odometry = self.get_odom() + self.previous_distance_from_des_point = self.get_distance_from_desired_point( + odometry.pose.pose.position, self.desired_position) + + def _set_action(self, action): + """ + This set action will Set the linear and angular speed of the SumitXl + based on the action number given. + :param action: The action integer that set s what movement to do next. + """ + + rospy.logdebug("Start Set Action ==>"+str(action)) + # We convert the actions to speed movements to send to the parent class CubeSingleDiskEnv + if action == 0: # FORWARD + linear_speed = self.linear_forward_speed + angular_speed = 0.0 + last_action = "FORWARDS" + elif action == 1: # LEFT + linear_speed = self.linear_turn_speed + angular_speed = self.angular_speed + last_action = "TURN_LEFT" + elif action == 2: # RIGHT + linear_speed = self.linear_turn_speed + angular_speed = -1*self.angular_speed + last_action = "TURN_RIGHT" + elif action == 3: # BACKWARDS + linear_speed = self.linear_forward_speed + angular_speed = 0.0 + last_action = "BACKWARDS" + + # We tell Husarion the linear and angular speed to set to execute + self.move_base(linear_speed, angular_speed, + epsilon=self.move_base_precision, update_rate=10) + + rospy.logdebug("END Set Action ==>"+str(action) + + ", ACTION="+str(last_action)) + + def _get_obs(self): + """ + Here we define what sensor data defines our robots observations + To know which Variables we have acces to, we need to read the + HusarionEnv API DOCS + :return: + """ + rospy.logdebug("Start Get Observation ==>") + # We get the laser scan data + laser_scan = self.get_laser_scan() + + discretized_laser_scan = self.discretize_scan_observation(laser_scan, + self.new_ranges + ) + # We get the odometry so that SumitXL knows where it is. + odometry = self.get_odom() + x_position = odometry.pose.pose.position.x + y_position = odometry.pose.pose.position.y + + # We get the orientation of the cube in RPY + roll, pitch, yaw = self.get_orientation_euler() + # We round to only two decimals to avoid very big Observation space + # We only want the X and Y position and the Yaw + odometry_array = [round(x_position, 1), + round(y_position, 1), + round(yaw, 1)] + + # We fetch also the desired position because it conditions the learning + # It also make it dynamic, because we can change the desired position and the + # learning will be able to adapt. + desired_position = [round(self.desired_position.x, 1), + round(self.desired_position.y, 1)] + + # We concatenate all the lists. + observations = discretized_laser_scan + odometry_array + desired_position + + rospy.logwarn("Observations==>"+str(observations)) + rospy.logwarn("END Get Observation ==>") + + return observations + + def _is_done(self, observations): + """ + We consider that the episode has finished when: + 1) Husarion has moved ouside the workspace defined. + 2) Husarion is too close to an object + 3) Husarion has reached the desired position + """ + + # We fetch data through the observations + # Its all the array except from the last four elements, which are XY odom and XY des_pos + laser_readings = observations[:-5] + + current_position = Point() + current_position.x = observations[-5] + current_position.y = observations[-4] + current_position.z = 0.0 + + desired_position = Point() + desired_position.x = observations[-2] + desired_position.y = observations[-1] + desired_position.z = 0.0 + + rospy.logwarn("is DONE? laser_readings=" + str(laser_readings)) + rospy.logwarn("is DONE? current_position=" + str(current_position)) + rospy.logwarn("is DONE? desired_position=" + str(desired_position)) + + too_close_to_object = self.check_husarion_has_crashed(laser_readings) + inside_workspace = self.check_inside_workspace(current_position) + reached_des_pos = self.check_reached_desired_position(current_position, + desired_position, + self.precision_epsilon) + + is_done = too_close_to_object or not( + inside_workspace) or reached_des_pos + + rospy.logwarn("####################") + rospy.logwarn("too_close_to_object=" + str(too_close_to_object)) + rospy.logwarn("inside_workspace=" + str(inside_workspace)) + rospy.logwarn("reached_des_pos=" + str(reached_des_pos)) + rospy.logwarn("is_done=" + str(is_done)) + rospy.logwarn("######## END DONE ##") + + return is_done + + def _compute_reward(self, observations, done): + """ + We will reward the following behaviours: + 1) The distance to the desired point has increase from last step + 2) The robot has reached the desired point + + We will penalise the following behaviours: + 1) Ending the episode without reaching the desired pos. That means it has crashed + or it has gone outside the workspace + + """ + + laser_readings = observations[:-5] + + current_position = Point() + current_position.x = observations[-5] + current_position.y = observations[-4] + current_position.z = 0.0 + + desired_position = Point() + desired_position.x = observations[-2] + desired_position.y = observations[-1] + desired_position.z = 0.0 + + distance_from_des_point = self.get_distance_from_desired_point( + current_position, desired_position) + + distance_difference = distance_from_des_point - \ + self.previous_distance_from_des_point + + rospy.logwarn("current_position=" + str(current_position)) + rospy.logwarn("desired_point=" + str(desired_position)) + + rospy.logwarn("total_distance_from_des_point=" + + str(self.previous_distance_from_des_point)) + rospy.logwarn("distance_from_des_point=" + + str(distance_from_des_point)) + rospy.logwarn("distance_difference=" + str(distance_difference)) + + if not done: + # If there has been a decrease in the distance to the desired point, we reward it + if distance_difference < 0.0: + rospy.logwarn("DECREASE IN DISTANCE GOOD") + reward = self.closer_to_point_reward + else: + reward = self.alive_reward + else: + + reached_des_pos = self.check_reached_desired_position(current_position, + desired_position, + self.precision_epsilon) + + if reached_des_pos: + reward = self.end_episode_points + rospy.logwarn( + "GOT TO DESIRED POINT ; DONE, reward=" + str(reward)) + else: + reward = -1*self.end_episode_points + rospy.logerr( + "SOMETHING WENT WRONG ; DONE, reward=" + str(reward)) + + self.previous_distance_from_des_point = distance_from_des_point + + rospy.logwarn("reward=" + str(reward)) + self.cumulated_reward += reward + rospy.logdebug("Cumulated_reward=" + str(self.cumulated_reward)) + self.cumulated_steps += 1 + rospy.logdebug("Cumulated_steps=" + str(self.cumulated_steps)) + + return reward + + # Internal TaskEnv Methods + def update_desired_pos(self, new_position): + """ + With this method you can change the desired position that you want + Usarion to be that initialy is set through rosparams loaded through + a yaml file possibly. + :new_position: Type Point, because we only value the position. + """ + self.desired_position.x = new_position.x + self.desired_position.y = new_position.y + + def discretize_scan_observation(self, data, new_ranges): + """ + Discards all the laser readings that are not multiple in index of new_ranges + value. + """ + + discretized_ranges = [] + mod = len(data.ranges)/new_ranges + + filtered_range = [] + + rospy.logdebug("data=" + str(data)) + rospy.logdebug("new_ranges=" + str(new_ranges)) + rospy.logdebug("mod=" + str(mod)) + + nan_value = (self.min_laser_value + self.min_laser_value) / 2.0 + + for i, item in enumerate(data.ranges): + if (i % mod == 0): + if item == float('Inf') or numpy.isinf(item): + rospy.logerr("Infinite Value=" + str(item) + + "Assigning Max value") + discretized_ranges.append(self.max_laser_value) + elif numpy.isnan(item): + rospy.logerr("Nan Value=" + str(item) + + "Assigning MIN value") + discretized_ranges.append(self.min_laser_value) + else: + # We clamp the laser readings + if item > self.max_laser_value: + rospy.logwarn("Item Bigger Than MAX, CLAMPING=>" + + str(item)+", MAX="+str(self.max_laser_value)) + discretized_ranges.append( + round(self.max_laser_value, 1)) + elif item < self.min_laser_value: + rospy.logwarn("Item smaller Than MIN, CLAMPING=>" + + str(item)+", MIN="+str(self.min_laser_value)) + discretized_ranges.append( + round(self.min_laser_value, 1)) + else: + rospy.logwarn( + "Normal Item, no processing=>" + str(item)) + discretized_ranges.append(round(item, 1)) + # We add last value appended + filtered_range.append(discretized_ranges[-1]) + else: + # We add value zero + filtered_range.append(0.0) + + rospy.logwarn( + ">>>>>>>>>>>>>>>>>>>>>>discretized_ranges=>" + str(discretized_ranges)) + + self.publish_filtered_laser_scan(laser_original_data=data, + new_filtered_laser_range=filtered_range) + + return discretized_ranges + + def get_orientation_euler(self): + # We convert from quaternions to euler + orientation_list = [self.odom.pose.pose.orientation.x, + self.odom.pose.pose.orientation.y, + self.odom.pose.pose.orientation.z, + self.odom.pose.pose.orientation.w] + + roll, pitch, yaw = euler_from_quaternion(orientation_list) + return roll, pitch, yaw + + def get_distance_from_desired_point(self, current_position, desired_position): + """ + Calculates the distance from the current position to the desired point + :param current_position: + :param desired_position: + :return: + """ + distance = self.get_distance_from_point(current_position, + desired_position) + + return distance + + def get_distance_from_point(self, pstart, p_end): + """ + Given a Vector3 Object, get distance from current position + :param p_end: + :return: + """ + a = numpy.array((pstart.x, pstart.y, pstart.z)) + b = numpy.array((p_end.x, p_end.y, p_end.z)) + + distance = numpy.linalg.norm(a - b) + + return distance + + def check_husarion_has_crashed(self, laser_readings): + """ + Based on the laser readings we check if any laser readingdistance is below + the minimum distance acceptable. + """ + husarion_has_crashed = False + + for laser_distance in laser_readings: + rospy.logwarn("laser_distance==>"+str(laser_distance)) + if laser_distance == self.min_laser_value: + husarion_has_crashed = True + rospy.logwarn("HAS CRASHED==>"+str(laser_distance) + + ", min="+str(self.min_laser_value)) + break + + elif laser_distance < self.min_laser_value: + rospy.logerr("Value of laser shouldnt be lower than min==>" + + str(laser_distance)+", min="+str(self.min_laser_value)) + elif laser_distance > self.max_laser_value: + rospy.logerr("Value of laser shouldnt be higher than max==>" + + str(laser_distance)+", max="+str(self.min_laser_value)) + + return husarion_has_crashed + + def check_inside_workspace(self, current_position): + """ + We check that the current position is inside the given workspace. + """ + is_inside = False + + rospy.logwarn("##### INSIDE WORK SPACE? #######") + rospy.logwarn("XYZ current_position"+str(current_position)) + rospy.logwarn("work_space_x_max"+str(self.work_space_x_max) + + ",work_space_x_min="+str(self.work_space_x_min)) + rospy.logwarn("work_space_y_max"+str(self.work_space_y_max) + + ",work_space_y_min="+str(self.work_space_y_min)) + rospy.logwarn("############") + + if current_position.x > self.work_space_x_min and current_position.x <= self.work_space_x_max: + if current_position.y > self.work_space_y_min and current_position.y <= self.work_space_y_max: + is_inside = True + + return is_inside + + def check_reached_desired_position(self, current_position, desired_position, epsilon=0.1): + """ + It return True if the current position is similar to the desired poistion + """ + + is_in_desired_pos = False + + x_pos_plus = desired_position.x + epsilon + x_pos_minus = desired_position.x - epsilon + y_pos_plus = desired_position.y + epsilon + y_pos_minus = desired_position.y - epsilon + + x_current = current_position.x + y_current = current_position.y + + x_pos_are_close = (x_current <= x_pos_plus) and ( + x_current > x_pos_minus) + y_pos_are_close = (y_current <= y_pos_plus) and ( + y_current > y_pos_minus) + + is_in_desired_pos = x_pos_are_close and y_pos_are_close + + rospy.logdebug("###### IS DESIRED POS ? ######") + rospy.logdebug("epsilon==>"+str(epsilon)) + rospy.logdebug("current_position"+str(current_position)) + rospy.logdebug("x_pos_plus"+str(x_pos_plus) + + ",x_pos_minus="+str(x_pos_minus)) + rospy.logdebug("y_pos_plus"+str(y_pos_plus) + + ",y_pos_minus="+str(y_pos_minus)) + rospy.logdebug("x_pos_are_close"+str(x_pos_are_close)) + rospy.logdebug("y_pos_are_close"+str(y_pos_are_close)) + rospy.logdebug("is_in_desired_pos"+str(is_in_desired_pos)) + rospy.logdebug("############") + + return is_in_desired_pos + + def publish_filtered_laser_scan(self, laser_original_data, new_filtered_laser_range): + + length_range = len(laser_original_data.ranges) + length_intensities = len(laser_original_data.intensities) + + laser_filtered_object = LaserScan() + + h = Header() + # Note you need to call rospy.init_node() before this will work + h.stamp = rospy.Time.now() + h.frame_id = "chassis" + + laser_filtered_object.header = h + laser_filtered_object.angle_min = laser_original_data.angle_min + laser_filtered_object.angle_max = laser_original_data.angle_max + laser_filtered_object.angle_increment = laser_original_data.angle_increment + laser_filtered_object.time_increment = laser_original_data.time_increment + laser_filtered_object.scan_time = laser_original_data.scan_time + laser_filtered_object.range_min = laser_original_data.range_min + laser_filtered_object.range_max = laser_original_data.range_max + + laser_filtered_object.ranges = [] + laser_filtered_object.intensities = [] + for item in new_filtered_laser_range: + laser_filtered_object.ranges.append(item) + laser_filtered_object.intensities.append(item) + + self.laser_filtered_pub.publish(laser_filtered_object) diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/iriwam/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/iriwam/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/iriwam/config/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/iriwam/config/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/iriwam/config/tcp_to_bowl.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/iriwam/config/tcp_to_bowl.yaml new file mode 100644 index 0000000..a4ae0c0 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/iriwam/config/tcp_to_bowl.yaml @@ -0,0 +1,33 @@ +iriwam: #namespace + + n_actions: 13 # Increase and decrease 7 joints. + + init_joints: # Init pose so that camera can see the laser element .["iri_wam_joint_1","iri_wam_joint_2","iri_wam_joint_3","iri_wam_joint_4","iri_wam_joint_5","iri_wam_joint_6","iri_wam_joint_7"] + iri_wam_joint_1: 0.0 + iri_wam_joint_2: 1.1 + iri_wam_joint_3: 0.0 + iri_wam_joint_4: 1.1 + iri_wam_joint_5: 0.0 + iri_wam_joint_6: 0.0 + iri_wam_joint_7: 0.0 + + work_space: # 3D cube in which Sawyers TCP ( right_electric_gripper_base frame) is allowed to move in + x_max: 1.1 + x_min: 0.0 + y_max: 1.0 + y_min: -1.0 + z_max: 1.3 + z_min: 0.3 + + + joint_increment_value: 0.1 # Increments of the joints each action does. + + max_distance_from_red_bowl: 145 # Maximum distace in pixels of the laser tip to the bowl. Bigger than this the episode finishes. + min_distance_from_red_bowl: 30 # Distance value that we consider it Over the red bowl + + min_laser_distance: 0.3 # Laser distance minimum for the center laser beam. Smaller, and its considered to be close enough. + + number_decimals_precision_obs: 1 + + done_reward: 1000.0 # reward + closer_to_block_reward: 100.0 # reward \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/iriwam/tcp_to_bowl.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/iriwam/tcp_to_bowl.py new file mode 100644 index 0000000..8666bdb --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/iriwam/tcp_to_bowl.py @@ -0,0 +1,568 @@ +import rospy +import numpy +from gym import spaces +from openai_ros.robot_envs import iriwam_env +from gym.envs.registration import register +from geometry_msgs.msg import Point +from geometry_msgs.msg import Vector3 +from tf.transformations import euler_from_quaternion +import cv2 +from cv_bridge import CvBridge, CvBridgeError +from sensor_msgs.msg import Image +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +from openai_ros.openai_ros_common import ROSLauncher +import os + +class IriWamTcpToBowlEnv(iriwam_env.IriWamEnv): + def __init__(self): + """ + Make iriwam learn how pick up a cube + """ + # We set an initial value imposible to ach to reward at least the first time + self.previous_distance_from_block = 100.0 + + # This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there + ros_ws_abspath = rospy.get_param("/iriwam/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name="iri_wam_gazebo", + launch_file_name="start_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/iriwam/config", + yaml_file_name="tcp_to_bowl.yaml") + + # We execute this one before because there are some functions that this + # TaskEnv uses that use variables from the parent class, like the effort limit fetch. + super(IriWamTcpToBowlEnv, self).__init__(ros_ws_abspath) + + # Here we will add any init functions prior to starting the MyRobotEnv + + # Only variable needed to be set here + + rospy.logdebug("Start IriWamTcpToBowlEnv INIT...") + number_actions = rospy.get_param('/iriwam/n_actions') + self.action_space = spaces.Discrete(number_actions) + + # We set the reward range, which is not compulsory but here we do it. + self.reward_range = (-numpy.inf, numpy.inf) + + self.iri_wam_joint_1 = rospy.get_param( + "/iriwam/init_joints/iri_wam_joint_1") + self.iri_wam_joint_2 = rospy.get_param( + "/iriwam/init_joints/iri_wam_joint_2") + self.iri_wam_joint_3 = rospy.get_param( + "/iriwam/init_joints/iri_wam_joint_3") + self.iri_wam_joint_4 = rospy.get_param( + "/iriwam/init_joints/iri_wam_joint_4") + self.iri_wam_joint_5 = rospy.get_param( + "/iriwam/init_joints/iri_wam_joint_5") + self.iri_wam_joint_6 = rospy.get_param( + "/iriwam/init_joints/iri_wam_joint_6") + self.iri_wam_joint_7 = rospy.get_param( + "/iriwam/init_joints/iri_wam_joint_7") + + self.init_joints_positions_array = [self.iri_wam_joint_1, + self.iri_wam_joint_2, + self.iri_wam_joint_3, + self.iri_wam_joint_4, + self.iri_wam_joint_5, + self.iri_wam_joint_6, + self.iri_wam_joint_7] + + self.init_joints_x_max = rospy.get_param( + "/iriwam/work_space/x_max") + self.init_joints_y_max = rospy.get_param( + "/iriwam/work_space/y_max") + self.init_joints_z_max = rospy.get_param( + "/iriwam/work_space/z_max") + self.init_joints_x_min = rospy.get_param( + "/iriwam/work_space/x_min") + self.init_joints_y_min = rospy.get_param( + "/iriwam/work_space/y_min") + self.init_joints_z_min = rospy.get_param( + "/iriwam/work_space/z_min") + + self.joint_increment_value = rospy.get_param( + "/iriwam/joint_increment_value") + + self.max_distance_from_red_bowl = rospy.get_param( + "/iriwam/max_distance_from_red_bowl") + self.min_distance_from_red_bowl = rospy.get_param( + "/iriwam/min_distance_from_red_bowl") + + self.min_laser_distance = rospy.get_param("/iriwam/min_laser_distance") + + self.dec_obs = rospy.get_param("/iriwam/number_decimals_precision_obs") + + # We place the Maximum and minimum values of observations + # TODO: Fill when get_observations is done. + """ + We supose that its all these: + head_pan, right_gripper_l_finger_joint, right_gripper_r_finger_joint, right_j0, right_j1, + right_j2, right_j3, right_j4, right_j5, right_j6 + + Plus the first three are the block_to_tcp vector + """ + + # We fetch the limits of the joinst to get the effort and angle limits + self.joint_limits = self.init_joint_limits() + + high = numpy.array([self.init_joints_x_max, + self.init_joints_y_max, + self.init_joints_z_max, + self.joint_limits[0].max_position, + self.joint_limits[1].max_position, + self.joint_limits[2].max_position, + self.joint_limits[3].max_position, + self.joint_limits[4].max_position, + self.joint_limits[5].max_position, + self.joint_limits[6].max_position + ]) + + low = numpy.array([self.init_joints_x_min, + self.init_joints_y_min, + self.init_joints_z_min, + self.joint_limits[0].min_position, + self.joint_limits[1].min_position, + self.joint_limits[2].min_position, + self.joint_limits[3].min_position, + self.joint_limits[4].min_position, + self.joint_limits[5].min_position, + self.joint_limits[6].min_position + ]) + + self.observation_space = spaces.Box(low, high) + + rospy.logdebug("ACTION SPACES TYPE===>"+str(self.action_space)) + rospy.logdebug("OBSERVATION SPACES TYPE===>" + + str(self.observation_space)) + + # Rewards + + self.done_reward = rospy.get_param("/iriwam/done_reward") + self.closer_to_block_reward = rospy.get_param( + "/iriwam/closer_to_block_reward") + + self.cumulated_steps = 0.0 + + # We init the CVBridge object + self.bridge_object = CvBridge() + + rospy.logdebug("END IriWamTcpToBowlEnv INIT...") + + def _set_init_pose(self): + """ + Sets the two proppelers speed to 0.0 and waits for the time_sleep + to allow the action to be executed + """ + + # We set the angles of the IriWam to the init pose: + self.move_joints_to_angle_blocking(self.init_joints_positions_array) + + self.joints = [] + for joint_value in self.init_joints_positions_array: + self.joints.append(joint_value) + + return True + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + + # For Info Purposes + self.cumulated_reward = 0.0 + + image_data = self.get_camera_rgb_image_raw() + self.previous_distance_from_bowl = self.get_magnitud_tcp_to_block( + data=image_data) + + def _set_action(self, action): + """ + It sets the joints of iriwam based on the action integer given + based on the action number given. + :param action: The action integer that sets what movement to do next. + """ + + rospy.logdebug("Start Set Action ==>"+str(action)) + + if action == 0: # Increase joint_0 + self.joints[0] += self.joint_increment_value + elif action == 1: # Decrease joint_0 + self.joints[0] -= self.joint_increment_value + elif action == 2: # Increase joint_1 + self.joints[1] += self.joint_increment_value + elif action == 3: # Decrease joint_1 + self.joints[1] -= self.joint_increment_value + elif action == 4: # Increase joint_2 + self.joints[2] += self.joint_increment_value + elif action == 5: # Decrease joint_2 + self.joints[2] -= self.joint_increment_value + elif action == 6: # Increase joint_3 + self.joints[3] += self.joint_increment_value + elif action == 7: # Decrease joint_3 + self.joints[3] -= self.joint_increment_value + elif action == 8: # Increase joint_4 + self.joints[4] += self.joint_increment_value + elif action == 9: # Decrease joint_4 + self.joints[4] -= self.joint_increment_value + elif action == 10: # Increase joint_5 + self.joints[5] += self.joint_increment_value + elif action == 11: # Decrease joint_5 + self.joints[5] -= self.joint_increment_value + elif action == 12: # Increase joint_6 + self.joints[6] += self.joint_increment_value + elif action == 13: # Decrease joint_6 + self.joints[6] -= self.joint_increment_value + else: + rospy.logdebug("Action not supported="+str(action)) + + # We tell iriwam the action to perform + self.move_joints_to_angle_blocking(self.joints) + + def _get_obs(self): + """ + Here we define what sensor data defines our robots observations + To know which Variables we have access to, we need to read the + iriwamEnv API DOCS. + :return: observation + """ + rospy.logdebug("Start Get Observation ==>") + + # We get Join state currently of all the joints + join_state = self.get_joint_state() + joints_angles_array = join_state.actual.positions + joints_angles_array_round = numpy.around( + joints_angles_array, decimals=self.dec_obs) + + # We get the Laser reading of the center laser ray, only one + laser_data = self.get_laser_scan() + center_laser_distance = laser_data.ranges[int( + len(laser_data.ranges)/2)] + center_laser_distance_array = [ + round(center_laser_distance, self.dec_obs)] + + # We get the distance laser tip to the red bowl using the image blob detection system + image_data = self.get_camera_rgb_image_raw() + distance_from_bowl = self.get_magnitud_tcp_to_block(data=image_data) + distance_from_bowl_array = [round(distance_from_bowl, self.dec_obs)] + + # We concatenate the two rounded arrays and convert them to standard Python list + observation = numpy.concatenate( + (joints_angles_array_round, center_laser_distance_array, distance_from_bowl_array), axis=0).tolist() + + return observation + + def _is_done(self, observations): + """ + We consider the episode done if: + 1) The iriwam end effector to bowl distance exceeds the maximum + 2) The iriwam end effector to bowl distance reaches the minimum and laser distance is lower than minimum + + """ + + distance_laser = observations[-2] + magnitude_image = observations[-1] + + has_reached_the_block = self.reached_bowl( + distance_laser, magnitude_image) + too_faraway_bowl_b = self.too_faraway_bowl(distance_laser, magnitude_image) + + done = has_reached_the_block or too_faraway_bowl_b + + rospy.logdebug("#### IS DONE ? ####") + rospy.logwarn("done ?="+str(done)+",has_reached_the_block="+str(has_reached_the_block)+",too_faraway_bowl_b="+str(too_faraway_bowl_b)) + rospy.logdebug("#### #### ####") + + return done + + def _compute_reward(self, observations, done): + """ + We Base the rewards in if its done or not and we base it on + if the distance to the block has increased or not. + :return: + """ + + tf_tcp_to_block_vector = Vector3() + tf_tcp_to_block_vector.x = observations[0] + tf_tcp_to_block_vector.y = observations[1] + tf_tcp_to_block_vector.z = observations[2] + + distance_block_to_tcp = self.get_magnitud_tf_tcp_to_block( + tf_tcp_to_block_vector) + distance_difference = distance_block_to_tcp - self.previous_distance_from_block + + if not done: + + # If there has been a decrease in the distance to the desired point, we reward it + if distance_difference < 0.0: + rospy.logdebug("DECREASE IN DISTANCE GOOD") + reward = self.closer_to_block_reward + else: + rospy.logerr("ENCREASE IN DISTANCE BAD") + #reward = -1*self.closer_to_block_reward + reward = 0.0 + + else: + distance_laser = observations[-2] + magnitude_image = observations[-1] + + reached_the_bowl = self.reached_bowl(distance_laser, magnitude_image) + if reached_the_bowl: + reward = self.done_reward + else: + reward = -1*self.done_reward + + self.previous_distance_from_block = distance_block_to_tcp + + rospy.logdebug("reward=" + str(reward)) + self.cumulated_reward += reward + rospy.logdebug("Cumulated_reward=" + str(self.cumulated_reward)) + self.cumulated_steps += 1 + rospy.logdebug("Cumulated_steps=" + str(self.cumulated_steps)) + + return reward + + # Internal TaskEnv Methods + + def is_arm_stuck(self, joints_efforts_dict): + """ + Checks if the efforts in the arm joints exceed certain theshhold + We will only check the joints_0,1,2,3,4,5,6 + """ + is_arm_stuck = False + + for joint_name in self.joint_limits.joint_names: + if joint_name in joints_efforts_dict: + + effort_value = joints_efforts_dict[joint_name] + index = self.joint_limits.joint_names.index(joint_name) + effort_limit = self.joint_limits.effort[index] + + rospy.logdebug("Joint Effort ==>Name="+str(joint_name) + + ",Effort="+str(effort_value)+",Limit="+str(effort_limit)) + + if abs(effort_value) > effort_limit: + is_arm_stuck = True + rospy.logerr("Joint Effort TOO MUCH ==>" + + str(joint_name)+","+str(effort_value)) + break + else: + rospy.logdebug("Joint Effort is ok==>" + + str(joint_name)+","+str(effort_value)) + else: + rospy.logdebug( + "Joint Name is not in the effort dict==>"+str(joint_name)) + + return is_arm_stuck + + def reached_bowl(self, distance_laser, magnitude_image): + """ + It return True if the distance red by the laser is smaller than minimum and + the distance by image is smaller than minimum + """ + + laser_close_enough = (distance_laser <= self.min_laser_distance) + magnitude_image_enough = ( + magnitude_image <= self.min_distance_from_red_bowl) + rospy.logwarn("reached_bowl condition, magnitude_image==>"+str(magnitude_image)+"<=?"+str(self.min_distance_from_red_bowl)) + + # reached_block_b = laser_close_enough and magnitude_image_enough + reached_block_b = magnitude_image_enough + + rospy.logdebug("###### REACHED Bowl ? ######") + # rospy.logdebug("laser_close_enough==>"+str(laser_close_enough)) + rospy.logdebug("magnitude_image_enough==>"+str(magnitude_image_enough)) + rospy.logdebug("reached_block_b==>"+str(reached_block_b)) + rospy.logdebug("############") + + return reached_block_b + + def too_faraway_bowl(self, distance_laser, magnitude_image): + """ + It return True if the distance by image is bigger than maximum + """ + + laser_close_enough = (distance_laser <= self.min_laser_distance) + magnitude_image_too_big = ( + magnitude_image <= self.max_distance_from_red_bowl) + + too_faraway_b = laser_close_enough and magnitude_image_too_big + + rospy.logdebug("###### Too Faar from bowl ? ######") + rospy.logdebug("magnitude_image_too_big==>" + + str(magnitude_image_too_big)) + rospy.logdebug("too_faraway_b==>"+str(too_faraway_b)) + rospy.logdebug("############") + + return too_faraway_b + + def get_distance_from_desired_point(self, current_position): + """ + Calculates the distance from the current position to the desired point + :param start_point: + :return: + """ + distance = self.get_distance_from_point(current_position, + self.desired_point) + + return distance + + def get_distance_from_point(self, pstart, p_end): + """ + Given a Vector3 Object, get distance from current position + :param p_end: + :return: + """ + a = numpy.array((pstart.x, pstart.y, pstart.z)) + b = numpy.array((p_end.x, p_end.y, p_end.z)) + + distance = numpy.linalg.norm(a - b) + + return distance + + def get_magnitud_tf_tcp_to_block(self, translation_vector): + """ + Given a Vector3 Object, get the magnitud + :param p_end: + :return: + """ + a = numpy.array((translation_vector.x, + translation_vector.y, + translation_vector.z)) + + distance = numpy.linalg.norm(a) + + return distance + + def get_orientation_euler(self, quaternion_vector): + # We convert from quaternions to euler + orientation_list = [quaternion_vector.x, + quaternion_vector.y, + quaternion_vector.z, + quaternion_vector.w] + + roll, pitch, yaw = euler_from_quaternion(orientation_list) + return roll, pitch, yaw + + def is_inside_workspace(self, current_position): + """ + Check if the iriwam is inside the Workspace defined + """ + is_inside = False + + rospy.logdebug("##### INSIDE WORK SPACE? #######") + rospy.logdebug("XYZ current_position"+str(current_position)) + rospy.logdebug("init_joints_x_max"+str(self.init_joints_x_max) + + ",init_joints_x_min="+str(self.init_joints_x_min)) + rospy.logdebug("init_joints_y_max"+str(self.init_joints_y_max) + + ",init_joints_y_min="+str(self.init_joints_y_min)) + rospy.logdebug("init_joints_z_max"+str(self.init_joints_z_max) + + ",init_joints_z_min="+str(self.init_joints_z_min)) + rospy.logdebug("############") + + if current_position.x > self.init_joints_x_min and current_position.x <= self.init_joints_x_max: + if current_position.y > self.init_joints_y_min and current_position.y <= self.init_joints_y_max: + if current_position.z > self.init_joints_z_min and current_position.z <= self.init_joints_z_max: + is_inside = True + + return is_inside + + def get_magnitud_tcp_to_block(self, data): + """ + Retrieves the distance end effector laser element to the red bowl through the + image data given + + :param: data: RGB image data + :return: magnitude: Distance in pixels from the center of the black blob ( the laser) + To the center of the red blob ( the red bowl) + Bear in mind that if the laser tip goes out of the cameras view, it will give a false positive + """ + + try: + # We select bgr8 because its the OpneCV encoding by default + cv_image = self.bridge_object.imgmsg_to_cv2( + data, desired_encoding="bgr8") + except CvBridgeError as e: + print(e) + cv_image = None + + if cv_image is not None: + # We get image dimensions and crop the parts of the image we don't need + # Bear in mind that because the first value of the image matrix is start and second value is down limit. + # Select the limits so that it gets the line not too close and not too far, and the minimum portion possible + # To make process faster. + height, width, channels = cv_image.shape + descentre = int(-height/2) + rows_to_watch = int(height) + aux1 = int((height)/2+descentre) + aux2 = int((height) / 2+(descentre+rows_to_watch)) + rospy.logdebug("aux1="+str(aux1)+",aux2="+str(aux2)+",width"+str(width)) + crop_img = cv_image[aux1:aux2][1:width] + + # Convert from RGB to HSV + hsv = cv2.cvtColor(crop_img, cv2.COLOR_BGR2HSV) + + # We track two colours, the RedBowl and the Black tip of IriWam Arm ( laser ) which is black. + + # RED BOWL + + lower_red = numpy.array([0, 204, 100]) + upper_red = numpy.array([0, 255, 255]) + + # Threshold the HSV image to get only yellow colors + mask = cv2.inRange(hsv, lower_red, upper_red) + + # Calculate centroid of the blob of binary image using ImageMoments + m = cv2.moments(mask, False) + try: + cx_red, cy_red = m['m10']/m['m00'], m['m01']/m['m00'] + except ZeroDivisionError: + cy_red, cx_red = height/2, width/2 + + # Black Laser + lower_black = numpy.array([0, 0, 0]) + upper_black = numpy.array([0, 0, 10]) + + # Threshold the HSV image to get only yellow colors + mask_black = cv2.inRange(hsv, lower_black, upper_black) + + # Calculate centroid of the blob of binary image using ImageMoments + m = cv2.moments(mask_black, False) + try: + cx_black, cy_black = m['m10']/m['m00'], m['m01']/m['m00'] + except ZeroDivisionError: + cy_black, cx_black = height/2, width/2 + + # Bitwise-AND mask and original image + res_black = cv2.bitwise_and(crop_img, crop_img) + + # Draw the centroid in the resultut image + cv2.circle(res_black, (int(cx_red), int(cy_red)), 10, (255, 0, 0), -1) + cv2.circle(res_black, (int(cx_black), int(cy_black)), + 10, (0, 255, 0), -1) + + cv2.imshow("RES BLACK", res_black) + + cv2.waitKey(1) + + error_x = cx_red - cx_black + error_y = cy_red - cy_black + error_array = numpy.array([error_x, error_y]) + magnitude = numpy.linalg.norm(error_array) + rospy.logdebug("Magnitude==>"+str(magnitude)) + else: + magnitude = 10.0 + + return magnitude + + def clean_up(self): + cv2.destroyAllWindows() diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/moving_cube/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/moving_cube/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/moving_cube/config/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/moving_cube/config/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/moving_cube/config/one_disk_walk.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/moving_cube/config/one_disk_walk.yaml new file mode 100644 index 0000000..ae89008 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/moving_cube/config/one_disk_walk.yaml @@ -0,0 +1,19 @@ +moving_cube: #namespace + # Cube Realated parameters + roll_speed_fixed_value: 50.0 # High Propeller Speed + roll_speed_increment_value: 10.0 # Low Propeller Speed + max_distance: 10.0 # Maximum Base Turn Angular speed + max_pitch_angle: 0.3 + max_y_linear_speed: 100 + max_yaw_angle: 0.3 + init_cube_pose: # 3D cube in which Drone is allowed to move + x: 0.0 + y: 0.0 + z: 0.0 + move_distance_reward_weight: 100.0 # reward + y_linear_speed_reward_weight: 100.0 + y_axis_angle_reward_weight: 100.0 + end_episode_points: -10.0 + roll_reward_weight: 120 + init_roll_vel: 10 + n_actions: 3 # Number of actions used by algorithm and task \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/moving_cube/one_disk_walk.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/moving_cube/one_disk_walk.py new file mode 100644 index 0000000..6509d8f --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/moving_cube/one_disk_walk.py @@ -0,0 +1,305 @@ +import rospy +import numpy +import math +from gym import spaces +from openai_ros.robot_envs import cube_single_disk_env +from geometry_msgs.msg import Point +from tf.transformations import euler_from_quaternion +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +from openai_ros.openai_ros_common import ROSLauncher +import os + +class MovingCubeOneDiskWalkEnv(cube_single_disk_env.CubeSingleDiskEnv): + def __init__(self): + + # Launch the Task Simulated-Environment + # This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there + ros_ws_abspath = rospy.get_param("/moving_cube/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name = "moving_cube_description", + launch_file_name = "start_world.launch", + ros_ws_abspath=ros_ws_abspath) + + + + # Load Params from the desired Yaml file + LoadYamlFileParamsTest( rospackage_name = "openai_ros", + rel_path_from_package_to_file = "src/openai_ros/task_envs/moving_cube/config", + yaml_file_name = "one_disk_walk.yaml") + + + # Only variable needed to be set here + number_actions = rospy.get_param('/moving_cube/n_actions') + self.action_space = spaces.Discrete(number_actions) + + + #number_observations = rospy.get_param('/moving_cube/n_observations') + """ + We set the Observation space for the 6 observations + cube_observations = [ + round(current_disk_roll_vel, 0), + round(y_distance, 1), + round(roll, 1), + round(pitch, 1), + round(y_linear_speed,1), + round(yaw, 1), + ] + """ + + # Actions and Observations + self.roll_speed_fixed_value = rospy.get_param('/moving_cube/roll_speed_fixed_value') + self.roll_speed_increment_value = rospy.get_param('/moving_cube/roll_speed_increment_value') + self.max_distance = rospy.get_param('/moving_cube/max_distance') + max_roll = 2 * math.pi + self.max_pitch_angle = rospy.get_param('/moving_cube/max_pitch_angle') + self.max_y_linear_speed = rospy.get_param('/moving_cube/max_y_linear_speed') + self.max_yaw_angle = rospy.get_param('/moving_cube/max_yaw_angle') + + + high = numpy.array([ + self.roll_speed_fixed_value, + self.max_distance, + max_roll, + self.max_pitch_angle, + self.max_y_linear_speed, + self.max_y_linear_speed, + ]) + + self.observation_space = spaces.Box(-high, high) + + rospy.logwarn("ACTION SPACES TYPE===>"+str(self.action_space)) + rospy.logwarn("OBSERVATION SPACES TYPE===>"+str(self.observation_space)) + + # Variables that we retrieve through the param server, loded when launch training launch. + self.init_roll_vel = rospy.get_param("/moving_cube/init_roll_vel") + + + # Get Observations + self.start_point = Point() + self.start_point.x = rospy.get_param("/moving_cube/init_cube_pose/x") + self.start_point.y = rospy.get_param("/moving_cube/init_cube_pose/y") + self.start_point.z = rospy.get_param("/moving_cube/init_cube_pose/z") + + # Rewards + self.move_distance_reward_weight = rospy.get_param("/moving_cube/move_distance_reward_weight") + self.y_linear_speed_reward_weight = rospy.get_param("/moving_cube/y_linear_speed_reward_weight") + self.y_axis_angle_reward_weight = rospy.get_param("/moving_cube/y_axis_angle_reward_weight") + self.end_episode_points = rospy.get_param("/moving_cube/end_episode_points") + + self.roll_reward_weight = rospy.get_param("/moving_cube/roll_reward_weight") + self.cumulated_steps = 0.0 + + # Here we will add any init functions prior to starting the MyRobotEnv + super(MovingCubeOneDiskWalkEnv, self).__init__(ros_ws_abspath) + + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + self.move_joints(self.init_roll_vel) + + return True + + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + self.total_distance_moved = 0.0 + self.current_y_distance = self.get_y_dir_distance_from_start_point(self.start_point) + self.pre_roll_angle = 0 + self.roll_turn_speed = rospy.get_param('/moving_cube/init_roll_vel') + # For Info Purposes + self.cumulated_reward = 0.0 + #self.cumulated_steps = 0.0 + + + def _set_action(self, action): + + # We convert the actions to speed movements to send to the parent class CubeSingleDiskEnv + if action == 0:# Move Speed Wheel Forwards + self.roll_turn_speed = self.roll_speed_fixed_value + elif action == 1:# Move Speed Wheel Backwards + self.roll_turn_speed = -1*self.roll_speed_fixed_value + elif action == 2:# Stop Speed Wheel + self.roll_turn_speed = 0.0 + elif action == 3:# Increment Speed + self.roll_turn_speed += self.roll_speed_increment_value + elif action == 4:# Decrement Speed + self.roll_turn_speed -= self.roll_speed_increment_value + + # We clamp Values to maximum + rospy.logdebug("roll_turn_speed before clamp=="+str(self.roll_turn_speed)) + self.roll_turn_speed = numpy.clip(self.roll_turn_speed, + -1*self.roll_speed_fixed_value, + self.roll_speed_fixed_value) + rospy.logdebug("roll_turn_speed after clamp==" + str(self.roll_turn_speed)) + + # We tell the OneDiskCube to spin the RollDisk at the selected speed + self.move_joints(self.roll_turn_speed) + + def _get_obs(self): + """ + Here we define what sensor data defines our robots observations + To know which Variables we have acces to, we need to read the + MyCubeSingleDiskEnv API DOCS + :return: + """ + + # We get the orientation of the cube in RPY + roll, pitch, yaw = self.get_orientation_euler() + + # We get the distance from the origin + #distance = self.get_distance_from_start_point(self.start_point) + y_distance = self.get_y_dir_distance_from_start_point(self.start_point) + + # We get the current speed of the Roll Disk + current_disk_roll_vel = self.get_roll_velocity() + + # We get the linear speed in the y axis + y_linear_speed = self.get_y_linear_speed() + + + cube_observations = [ + round(current_disk_roll_vel, 0), + round(y_distance, 1), + round(roll, 1), + round(pitch, 1), + round(y_linear_speed,1), + round(yaw, 1) + ] + + rospy.logdebug("Observations==>"+str(cube_observations)) + + return cube_observations + + + def _is_done(self, observations): + + pitch_angle = observations[3] + yaw_angle = observations[5] + + if abs(pitch_angle) > self.max_pitch_angle: + rospy.logerr("WRONG Cube Pitch Orientation==>" + str(pitch_angle)) + done = True + else: + rospy.logdebug("Cube Pitch Orientation Ok==>" + str(pitch_angle)) + if abs(yaw_angle) > self.max_yaw_angle: + rospy.logerr("WRONG Cube Yaw Orientation==>" + str(yaw_angle)) + done = True + else: + rospy.logdebug("Cube Yaw Orientation Ok==>" + str(yaw_angle)) + done = False + + return done + + def _compute_reward(self, observations, done): + + if not done: + + y_distance_now = observations[1] + delta_distance = y_distance_now - self.current_y_distance + rospy.logdebug("y_distance_now=" + str(y_distance_now)+", current_y_distance=" + str(self.current_y_distance)) + rospy.logdebug("delta_distance=" + str(delta_distance)) + reward_distance = delta_distance * self.move_distance_reward_weight + self.current_y_distance = y_distance_now + + y_linear_speed = observations[4] + rospy.logdebug("y_linear_speed=" + str(y_linear_speed)) + reward_y_axis_speed = y_linear_speed * self.y_linear_speed_reward_weight + + # Negative Reward for yaw different from zero. + yaw_angle = observations[5] + rospy.logdebug("yaw_angle=" + str(yaw_angle)) + + # Worst yaw is 90 and 270 degrees, best 0 and 180. We use sin function for giving reward. + sin_yaw_angle = math.sin(yaw_angle) + rospy.logdebug("sin_yaw_angle=" + str(sin_yaw_angle)) + reward_y_axis_angle = -1 * abs(sin_yaw_angle) * self.y_axis_angle_reward_weight + + #Rolling reward + roll_angle = observations[2] + roll_reward = math.sin(abs(self.pre_roll_angle - roll_angle)) * self.roll_reward_weight + self.pre_roll_angle = roll_angle + + # We are not intereseted in decimals of the reward, doesnt give any advatage. + reward = round(reward_distance, 0) + round(reward_y_axis_speed, 0) + round(reward_y_axis_angle, 0) + rospy.logdebug("reward_distance=" + str(reward_distance)) + rospy.logdebug("reward_y_axis_speed=" + str(reward_y_axis_speed)) + rospy.logdebug("reward_y_axis_angle=" + str(reward_y_axis_angle)) + rospy.logdebug("reward=" + str(reward)) + else: + reward = -1*self.end_episode_points + + + self.cumulated_reward += reward + rospy.logdebug("Cumulated_reward=" + str(self.cumulated_reward)) + self.cumulated_steps += 1 + rospy.logdebug("Cumulated_steps=" + str(self.cumulated_steps)) + + return reward + + + # Internal TaskEnv Methods + def get_y_dir_distance_from_start_point(self, start_point): + """ + Calculates the distance from the given point and the current position + given by odometry. In this case the increase or decrease in y. + :param start_point: + :return: + """ + y_dist_dir = self.odom.pose.pose.position.y - start_point.y + + return y_dist_dir + + + def get_distance_from_start_point(self, start_point): + """ + Calculates the distance from the given point and the current position + given by odometry + :param start_point: + :return: + """ + distance = self.get_distance_from_point(start_point, + self.odom.pose.pose.position) + + return distance + + def get_distance_from_point(self, pstart, p_end): + """ + Given a Vector3 Object, get distance from current position + :param p_end: + :return: + """ + a = numpy.array((pstart.x, pstart.y, pstart.z)) + b = numpy.array((p_end.x, p_end.y, p_end.z)) + + distance = numpy.linalg.norm(a - b) + + return distance + + def get_orientation_euler(self): + # We convert from quaternions to euler + orientation_list = [self.odom.pose.pose.orientation.x, + self.odom.pose.pose.orientation.y, + self.odom.pose.pose.orientation.z, + self.odom.pose.pose.orientation.w] + + roll, pitch, yaw = euler_from_quaternion(orientation_list) + return roll, pitch, yaw + + def get_roll_velocity(self): + # We get the current joint roll velocity + roll_vel = self.joints.velocity[0] + return roll_vel + + def get_y_linear_speed(self): + # We get the current joint roll velocity + y_linear_speed = self.odom.twist.twist.linear.y + return y_linear_speed + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/parrotdrone/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/parrotdrone/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/parrotdrone/config/parrotdrone_goto.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/parrotdrone/config/parrotdrone_goto.yaml new file mode 100644 index 0000000..5765b60 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/parrotdrone/config/parrotdrone_goto.yaml @@ -0,0 +1,44 @@ +drone: #namespace + + n_actions: 6 # We have 3 actions, Forwards,TurnLeft,TurnRight, STOP + + linear_forward_speed: 0.5 # Spwwed for ging fowards + angular_turn_speed: 0.05 # Lienare speed when turning + angular_speed: 0.3 # Angular speed when turning Left or Right + + init_linear_speed_vector: + x: 0.0 + y: 0.0 + z: 0.0 + + init_angular_turn_speed: 0.0 # Initial angular speed in shich we start each episode + + + min_sonar_value: 0.5 # Minimum meters below wich we consider we have crashed + max_sonar_value: 5.0 # This can be retrieved form the sonar topic + + work_space: # 3D cube in which Drone is allowed to move + x_max: 10.0 + x_min: -1.0 + y_max: 5.0 + y_min: -1.0 + z_max: 3.0 + z_min: -1.0 + + max_roll: 1.57 # Max roll after which we end the episode + max_pitch: 1.57 # Max roll after which we end the episode + max_yaw: inf # Max yaw, its 4 because its bigger the pi, its a complete turn actually the maximum + + desired_pose: + x: 9.0 + y: 4.0 + z: 2.0 + + desired_point_epsilon: 1.0 # Error acceptable to consider that it has reached the desired point + + + closer_to_point_reward: 10 # We give points for getting closer to the desired point + not_ending_point_reward: 1 # Points given if we just dont crash + end_episode_points: 200 # Points given when ending an episode + + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/parrotdrone/parrotdrone_goto.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/parrotdrone/parrotdrone_goto.py new file mode 100644 index 0000000..007fe0a --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/parrotdrone/parrotdrone_goto.py @@ -0,0 +1,463 @@ +import rospy +import numpy +from gym import spaces +from openai_ros.robot_envs import parrotdrone_env +from gym.envs.registration import register +from geometry_msgs.msg import Point +from geometry_msgs.msg import Vector3 +from tf.transformations import euler_from_quaternion +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +from openai_ros.openai_ros_common import ROSLauncher +import os + +class ParrotDroneGotoEnv(parrotdrone_env.ParrotDroneEnv): + def __init__(self): + """ + Make parrotdrone learn how to navigate to get to a point + """ + ros_ws_abspath = rospy.get_param("/drone/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name="drone_construct", + launch_file_name="start_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/parrotdrone/config", + yaml_file_name="parrotdrone_goto.yaml") + + # Only variable needed to be set here + number_actions = rospy.get_param('/drone/n_actions') + self.action_space = spaces.Discrete(number_actions) + + # We set the reward range, which is not compulsory but here we do it. + self.reward_range = (-numpy.inf, numpy.inf) + + # Actions and Observations + self.linear_forward_speed = rospy.get_param( + '/drone/linear_forward_speed') + self.angular_turn_speed = rospy.get_param('/drone/angular_turn_speed') + self.angular_speed = rospy.get_param('/drone/angular_speed') + + self.init_linear_speed_vector = Vector3() + self.init_linear_speed_vector.x = rospy.get_param( + '/drone/init_linear_speed_vector/x') + self.init_linear_speed_vector.y = rospy.get_param( + '/drone/init_linear_speed_vector/y') + self.init_linear_speed_vector.z = rospy.get_param( + '/drone/init_linear_speed_vector/z') + + self.init_angular_turn_speed = rospy.get_param( + '/drone/init_angular_turn_speed') + + self.min_sonar_value = rospy.get_param('/drone/min_sonar_value') + self.max_sonar_value = rospy.get_param('/drone/max_sonar_value') + + # Get WorkSpace Cube Dimensions + self.work_space_x_max = rospy.get_param("/drone/work_space/x_max") + self.work_space_x_min = rospy.get_param("/drone/work_space/x_min") + self.work_space_y_max = rospy.get_param("/drone/work_space/y_max") + self.work_space_y_min = rospy.get_param("/drone/work_space/y_min") + self.work_space_z_max = rospy.get_param("/drone/work_space/z_max") + self.work_space_z_min = rospy.get_param("/drone/work_space/z_min") + + # Maximum RPY values + self.max_roll = rospy.get_param("/drone/max_roll") + self.max_pitch = rospy.get_param("/drone/max_pitch") + self.max_yaw = rospy.get_param("/drone/max_yaw") + + # Get Desired Point to Get + self.desired_point = Point() + self.desired_point.x = rospy.get_param("/drone/desired_pose/x") + self.desired_point.y = rospy.get_param("/drone/desired_pose/y") + self.desired_point.z = rospy.get_param("/drone/desired_pose/z") + + self.desired_point_epsilon = rospy.get_param( + "/drone/desired_point_epsilon") + + # We place the Maximum and minimum values of the X,Y,Z,R,P,Yof the pose + + high = numpy.array([self.work_space_x_max, + self.work_space_y_max, + self.work_space_z_max, + self.max_roll, + self.max_pitch, + self.max_yaw, + self.max_sonar_value]) + + low = numpy.array([self.work_space_x_min, + self.work_space_y_min, + self.work_space_z_min, + -1*self.max_roll, + -1*self.max_pitch, + -numpy.inf, + self.min_sonar_value]) + + self.observation_space = spaces.Box(low, high) + + rospy.logdebug("ACTION SPACES TYPE===>"+str(self.action_space)) + rospy.logdebug("OBSERVATION SPACES TYPE===>" + + str(self.observation_space)) + + # Rewards + self.closer_to_point_reward = rospy.get_param( + "/drone/closer_to_point_reward") + self.not_ending_point_reward = rospy.get_param( + "/drone/not_ending_point_reward") + self.end_episode_points = rospy.get_param("/drone/end_episode_points") + + self.cumulated_steps = 0.0 + + # Here we will add any init functions prior to starting the MyRobotEnv + super(ParrotDroneGotoEnv, self).__init__(ros_ws_abspath) + + def _set_init_pose(self): + """ + Sets the Robot in its init linear and angular speeds + and lands the robot. Its preparing it to be reseted in the world. + """ + #raw_input("INIT SPEED PRESS") + self.move_base(self.init_linear_speed_vector, + self.init_angular_turn_speed, + epsilon=0.05, + update_rate=10) + # We Issue the landing command to be sure it starts landing + #raw_input("LAND PRESS") + # self.land() + + return True + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + #raw_input("TakeOFF PRESS") + # We TakeOff before sending any movement commands + self.takeoff() + + # For Info Purposes + self.cumulated_reward = 0.0 + # We get the initial pose to mesure the distance from the desired point. + gt_pose = self.get_gt_pose() + self.previous_distance_from_des_point = self.get_distance_from_desired_point( + gt_pose.position) + + def _set_action(self, action): + """ + This set action will Set the linear and angular speed of the drone + based on the action number given. + :param action: The action integer that set s what movement to do next. + """ + + rospy.logdebug("Start Set Action ==>"+str(action)) + # We convert the actions to speed movements to send to the parent class of Parrot + linear_speed_vector = Vector3() + angular_speed = 0.0 + + if action == 0: # FORWARDS + linear_speed_vector.x = self.linear_forward_speed + self.last_action = "FORWARDS" + elif action == 1: # BACKWARDS + linear_speed_vector.x = -1*self.linear_forward_speed + self.last_action = "BACKWARDS" + elif action == 2: # STRAFE_LEFT + linear_speed_vector.y = self.linear_forward_speed + self.last_action = "STRAFE_LEFT" + elif action == 3: # STRAFE_RIGHT + linear_speed_vector.y = -1*self.linear_forward_speed + self.last_action = "STRAFE_RIGHT" + elif action == 4: # UP + linear_speed_vector.z = self.linear_forward_speed + self.last_action = "UP" + elif action == 5: # DOWN + linear_speed_vector.z = -1*self.linear_forward_speed + self.last_action = "DOWN" + + # We tell drone the linear and angular speed to set to execute + self.move_base(linear_speed_vector, + angular_speed, + epsilon=0.05, + update_rate=10) + + rospy.logdebug("END Set Action ==>"+str(action)) + + def _get_obs(self): + """ + Here we define what sensor data defines our robots observations + To know which Variables we have acces to, we need to read the + droneEnv API DOCS + :return: + """ + rospy.logdebug("Start Get Observation ==>") + # We get the laser scan data + gt_pose = self.get_gt_pose() + + # We get the orientation of the cube in RPY + roll, pitch, yaw = self.get_orientation_euler(gt_pose.orientation) + + # We get the sonar value + sonar = self.get_sonar() + sonar_value = sonar.range + + """ + observations = [ round(gt_pose.position.x, 1), + round(gt_pose.position.y, 1), + round(gt_pose.position.z, 1), + round(roll, 1), + round(pitch, 1), + round(yaw, 1), + round(sonar_value,1)] + """ + # We simplify a bit the spatial grid to make learning faster + observations = [int(gt_pose.position.x), + int(gt_pose.position.y), + int(gt_pose.position.z), + round(roll, 1), + round(pitch, 1), + round(yaw, 1), + round(sonar_value, 1)] + + rospy.logdebug("Observations==>"+str(observations)) + rospy.logdebug("END Get Observation ==>") + return observations + + def _is_done(self, observations): + """ + The done can be done due to three reasons: + 1) It went outside the workspace + 2) It detected something with the sonar that is too close + 3) It flipped due to a crash or something + 4) It has reached the desired point + """ + + episode_done = False + + current_position = Point() + current_position.x = observations[0] + current_position.y = observations[1] + current_position.z = observations[2] + + current_orientation = Point() + current_orientation.x = observations[3] + current_orientation.y = observations[4] + current_orientation.z = observations[5] + + sonar_value = observations[6] + + is_inside_workspace_now = self.is_inside_workspace(current_position) + sonar_detected_something_too_close_now = self.sonar_detected_something_too_close( + sonar_value) + drone_flipped = self.drone_has_flipped(current_orientation) + has_reached_des_point = self.is_in_desired_position( + current_position, self.desired_point_epsilon) + + rospy.logwarn(">>>>>> DONE RESULTS <<<<<") + if not is_inside_workspace_now: + rospy.logerr("is_inside_workspace_now=" + + str(is_inside_workspace_now)) + else: + rospy.logwarn("is_inside_workspace_now=" + + str(is_inside_workspace_now)) + + if sonar_detected_something_too_close_now: + rospy.logerr("sonar_detected_something_too_close_now=" + + str(sonar_detected_something_too_close_now)) + else: + rospy.logwarn("sonar_detected_something_too_close_now=" + + str(sonar_detected_something_too_close_now)) + + if drone_flipped: + rospy.logerr("drone_flipped="+str(drone_flipped)) + else: + rospy.logwarn("drone_flipped="+str(drone_flipped)) + + if has_reached_des_point: + rospy.logerr("has_reached_des_point="+str(has_reached_des_point)) + else: + rospy.logwarn("has_reached_des_point="+str(has_reached_des_point)) + + # We see if we are outside the Learning Space + episode_done = not( + is_inside_workspace_now) or sonar_detected_something_too_close_now or drone_flipped or has_reached_des_point + + if episode_done: + rospy.logerr("episode_done====>"+str(episode_done)) + else: + rospy.logwarn("episode_done====>"+str(episode_done)) + + return episode_done + + def _compute_reward(self, observations, done): + + current_position = Point() + current_position.x = observations[0] + current_position.y = observations[1] + current_position.z = observations[2] + + distance_from_des_point = self.get_distance_from_desired_point( + current_position) + distance_difference = distance_from_des_point - \ + self.previous_distance_from_des_point + + if not done: + + # If there has been a decrease in the distance to the desired point, we reward it + if distance_difference < 0.0: + rospy.logwarn("DECREASE IN DISTANCE GOOD") + reward = self.closer_to_point_reward + else: + rospy.logerr("ENCREASE IN DISTANCE BAD") + reward = 0 + + else: + + if self.is_in_desired_position(current_position, epsilon=0.5): + reward = self.end_episode_points + else: + reward = -1*self.end_episode_points + + self.previous_distance_from_des_point = distance_from_des_point + + rospy.logdebug("reward=" + str(reward)) + self.cumulated_reward += reward + rospy.logdebug("Cumulated_reward=" + str(self.cumulated_reward)) + self.cumulated_steps += 1 + rospy.logdebug("Cumulated_steps=" + str(self.cumulated_steps)) + + return reward + + # Internal TaskEnv Methods + + def is_in_desired_position(self, current_position, epsilon=0.05): + """ + It return True if the current position is similar to the desired poistion + """ + + is_in_desired_pos = False + + x_pos_plus = self.desired_point.x + epsilon + x_pos_minus = self.desired_point.x - epsilon + y_pos_plus = self.desired_point.y + epsilon + y_pos_minus = self.desired_point.y - epsilon + + x_current = current_position.x + y_current = current_position.y + + x_pos_are_close = (x_current <= x_pos_plus) and ( + x_current > x_pos_minus) + y_pos_are_close = (y_current <= y_pos_plus) and ( + y_current > y_pos_minus) + + is_in_desired_pos = x_pos_are_close and y_pos_are_close + + rospy.logwarn("###### IS DESIRED POS ? ######") + rospy.logwarn("current_position"+str(current_position)) + rospy.logwarn("x_pos_plus"+str(x_pos_plus) + + ",x_pos_minus="+str(x_pos_minus)) + rospy.logwarn("y_pos_plus"+str(y_pos_plus) + + ",y_pos_minus="+str(y_pos_minus)) + rospy.logwarn("x_pos_are_close"+str(x_pos_are_close)) + rospy.logwarn("y_pos_are_close"+str(y_pos_are_close)) + rospy.logwarn("is_in_desired_pos"+str(is_in_desired_pos)) + rospy.logwarn("############") + + return is_in_desired_pos + + def is_inside_workspace(self, current_position): + """ + Check if the Drone is inside the Workspace defined + """ + is_inside = False + + rospy.logwarn("##### INSIDE WORK SPACE? #######") + rospy.logwarn("XYZ current_position"+str(current_position)) + rospy.logwarn("work_space_x_max"+str(self.work_space_x_max) + + ",work_space_x_min="+str(self.work_space_x_min)) + rospy.logwarn("work_space_y_max"+str(self.work_space_y_max) + + ",work_space_y_min="+str(self.work_space_y_min)) + rospy.logwarn("work_space_z_max"+str(self.work_space_z_max) + + ",work_space_z_min="+str(self.work_space_z_min)) + rospy.logwarn("############") + + if current_position.x > self.work_space_x_min and current_position.x <= self.work_space_x_max: + if current_position.y > self.work_space_y_min and current_position.y <= self.work_space_y_max: + if current_position.z > self.work_space_z_min and current_position.z <= self.work_space_z_max: + is_inside = True + + return is_inside + + def sonar_detected_something_too_close(self, sonar_value): + """ + Detects if there is something too close to the drone front + """ + rospy.logwarn("##### SONAR TOO CLOSE? #######") + rospy.logwarn("sonar_value"+str(sonar_value) + + ",min_sonar_value="+str(self.min_sonar_value)) + rospy.logwarn("############") + + too_close = sonar_value < self.min_sonar_value + + return too_close + + def drone_has_flipped(self, current_orientation): + """ + Based on the orientation RPY given states if the drone has flipped + """ + has_flipped = True + + self.max_roll = rospy.get_param("/drone/max_roll") + self.max_pitch = rospy.get_param("/drone/max_pitch") + + rospy.logwarn("#### HAS FLIPPED? ########") + rospy.logwarn("RPY current_orientation"+str(current_orientation)) + rospy.logwarn("max_roll"+str(self.max_roll) + + ",min_roll="+str(-1*self.max_roll)) + rospy.logwarn("max_pitch"+str(self.max_pitch) + + ",min_pitch="+str(-1*self.max_pitch)) + rospy.logwarn("############") + + if current_orientation.x > -1*self.max_roll and current_orientation.x <= self.max_roll: + if current_orientation.y > -1*self.max_pitch and current_orientation.y <= self.max_pitch: + has_flipped = False + + return has_flipped + + def get_distance_from_desired_point(self, current_position): + """ + Calculates the distance from the current position to the desired point + :param start_point: + :return: + """ + distance = self.get_distance_from_point(current_position, + self.desired_point) + + return distance + + def get_distance_from_point(self, pstart, p_end): + """ + Given a Vector3 Object, get distance from current position + :param p_end: + :return: + """ + a = numpy.array((pstart.x, pstart.y, pstart.z)) + b = numpy.array((p_end.x, p_end.y, p_end.z)) + + distance = numpy.linalg.norm(a - b) + + return distance + + def get_orientation_euler(self, quaternion_vector): + # We convert from quaternions to euler + orientation_list = [quaternion_vector.x, + quaternion_vector.y, + quaternion_vector.z, + quaternion_vector.w] + + roll, pitch, yaw = euler_from_quaternion(orientation_list) + return roll, pitch, yaw diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sawyer/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sawyer/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sawyer/config/learn_to_touch_cube.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sawyer/config/learn_to_touch_cube.yaml new file mode 100644 index 0000000..8dda63b --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sawyer/config/learn_to_touch_cube.yaml @@ -0,0 +1,17 @@ +sawyer: #namespace + n_actions: 14 # Increase and decrease 7 joints. + + work_space: # 3D cube in which Sawyers TCP ( right_electric_gripper_base frame) is allowed to move in + x_max: 1.1 + x_min: 0.0 + y_max: 1.0 + y_min: -1.0 + z_max: 1.3 + z_min: 0.3 + + max_effort: 50.0 # Maximum Effort , above which the episode is done. + number_decimals_precision_obs: 1 + acceptable_distance_to_cube: 0.16 # Distance to the clock that we consider that it reached the block. + tcp_z_position_min: 0.83 # Z value minimum to consider that the tcp is above the table, to avoid cheating the block distance done. + done_reward: 1000.0 # reward + closer_to_block_reward: 100.0 # reward \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sawyer/learn_to_touch_cube.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sawyer/learn_to_touch_cube.py new file mode 100644 index 0000000..b017f77 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sawyer/learn_to_touch_cube.py @@ -0,0 +1,476 @@ +import rospy +import numpy +from gym import spaces +from openai_ros.robot_envs import sawyer_env +from gym.envs.registration import register +from geometry_msgs.msg import Point +from geometry_msgs.msg import Vector3 +from tf.transformations import euler_from_quaternion +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +from openai_ros.openai_ros_common import ROSLauncher +import os + +class SawyerTouchCubeEnv(sawyer_env.SawyerEnv): + def __init__(self): + """ + Make sawyer learn how pick up a cube + """ + + # This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there + ros_ws_abspath = rospy.get_param("/sawyer/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name="sawyer_gazebo", + launch_file_name="start_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/sawyer/config", + yaml_file_name="learn_to_touch_cube.yaml") + + import time + time.sleep(15) + print("STARTING SPAWN ROBOT") + # We execute this one before because there are some functions that this + # TaskEnv uses that use variables from the parent class, like the effort limit fetch. + super(SawyerTouchCubeEnv, self).__init__(ros_ws_abspath) + + # Here we will add any init functions prior to starting the MyRobotEnv + + # Only variable needed to be set here + + rospy.logdebug("Start SawyerTouchCubeEnv INIT...") + number_actions = rospy.get_param('/sawyer/n_actions') + self.action_space = spaces.Discrete(number_actions) + + # We set the reward range, which is not compulsory but here we do it. + self.reward_range = (-numpy.inf, numpy.inf) + + self.work_space_x_max = rospy.get_param("/sawyer/work_space/x_max") + self.work_space_x_min = rospy.get_param("/sawyer/work_space/x_min") + self.work_space_y_max = rospy.get_param("/sawyer/work_space/y_max") + self.work_space_y_min = rospy.get_param("/sawyer/work_space/y_min") + self.work_space_z_max = rospy.get_param("/sawyer/work_space/z_max") + self.work_space_z_min = rospy.get_param("/sawyer/work_space/z_min") + + self.max_effort = rospy.get_param("/sawyer/max_effort") + + self.dec_obs = rospy.get_param("/sawyer/number_decimals_precision_obs") + + self.acceptable_distance_to_cube = rospy.get_param( + "/sawyer/acceptable_distance_to_cube") + + self.tcp_z_position_min = rospy.get_param("/sawyer/tcp_z_position_min") + + # We place the Maximum and minimum values of observations + # TODO: Fill when get_observations is done. + """ + We supose that its all these: + head_pan, right_gripper_l_finger_joint, right_gripper_r_finger_joint, right_j0, right_j1, + right_j2, right_j3, right_j4, right_j5, right_j6 + + Plus the first three are the block_to_tcp vector + """ + + # We fetch the limits of the joinst to get the effort and angle limits + self.joint_limits = self.init_joint_limits() + + high = numpy.array([self.work_space_x_max, + self.work_space_y_max, + self.work_space_z_max, + self.joint_limits.position_upper[0], + self.joint_limits.position_upper[1], + self.joint_limits.position_upper[2], + self.joint_limits.position_upper[3], + self.joint_limits.position_upper[4], + self.joint_limits.position_upper[5], + self.joint_limits.position_upper[6], + self.joint_limits.position_upper[7], + self.joint_limits.position_upper[8], + self.joint_limits.position_upper[9] + ]) + + low = numpy.array([self.work_space_x_min, + self.work_space_y_min, + self.work_space_z_min, + self.joint_limits.position_lower[0], + self.joint_limits.position_lower[1], + self.joint_limits.position_lower[2], + self.joint_limits.position_lower[3], + self.joint_limits.position_lower[4], + self.joint_limits.position_lower[5], + self.joint_limits.position_lower[6], + self.joint_limits.position_lower[7], + self.joint_limits.position_lower[8], + self.joint_limits.position_lower[9] + ]) + + self.observation_space = spaces.Box(low, high) + + rospy.logdebug("ACTION SPACES TYPE===>"+str(self.action_space)) + rospy.logdebug("OBSERVATION SPACES TYPE===>" + + str(self.observation_space)) + + # Rewards + + self.done_reward = rospy.get_param("/sawyer/done_reward") + self.closer_to_block_reward = rospy.get_param( + "/sawyer/closer_to_block_reward") + + self.cumulated_steps = 0.0 + + rospy.logdebug("END SawyerTouchCubeEnv INIT...") + + def _set_init_pose(self): + """ + Sets the two proppelers speed to 0.0 and waits for the time_sleep + to allow the action to be executed + """ + + # We set the angles to zero of the limb + self.joints = self.get_limb_joint_names_array() + join_values_array = [0.0]*len(self.joints) + joint_positions_dict_zero = dict(zip(self.joints, join_values_array)) + + actual_joint_angles_dict = self.get_all_limb_joint_angles() + # We generate the two step movement. Turn Right/Left where you are and then set all to zero + if "right_j0" in actual_joint_angles_dict: + # We turn to the left or to the right based on where the position is to avoid the table. + if actual_joint_angles_dict["right_j0"] >= 0.0: + actual_joint_angles_dict["right_j0"] = 1.57 + else: + actual_joint_angles_dict["right_j0"] = -1.57 + if "right_j1" in actual_joint_angles_dict: + actual_joint_angles_dict["right_j1"] = actual_joint_angles_dict["right_j1"] - 0.3 + + self.move_joints_to_angle_blocking( + actual_joint_angles_dict, timeout=15.0, threshold=0.008726646) + self.move_joints_to_angle_blocking( + joint_positions_dict_zero, timeout=15.0, threshold=0.008726646) + + # We Open the gripper + self.set_g(action="open") + + return True + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + + # For Info Purposes + self.cumulated_reward = 0.0 + # We get the initial pose to mesure the distance from the desired point. + translation_tcp_block, rotation_tcp_block = self.get_tf_start_to_end_frames(start_frame_name="block", + end_frame_name="right_electric_gripper_base") + tf_tcp_to_block_vector = Vector3() + tf_tcp_to_block_vector.x = translation_tcp_block[0] + tf_tcp_to_block_vector.y = translation_tcp_block[1] + tf_tcp_to_block_vector.z = translation_tcp_block[2] + + self.previous_distance_from_block = self.get_magnitud_tf_tcp_to_block( + tf_tcp_to_block_vector) + + self.translation_tcp_world, _ = self.get_tf_start_to_end_frames(start_frame_name="world", + end_frame_name="right_electric_gripper_base") + + def _set_action(self, action): + """ + It sets the joints of sawyer based on the action integer given + based on the action number given. + :param action: The action integer that sets what movement to do next. + """ + + rospy.logdebug("Start Set Action ==>"+str(action)) + + if action == 0: # Increase joint_0 + action_id = self.joints[0]+"_increase" + elif action == 1: # Decrease joint_0 + action_id = self.joints[0]+"_decrease" + elif action == 2: # Increase joint_1 + action_id = self.joints[1]+"_increase" + elif action == 3: # Decrease joint_1 + action_id = self.joints[1]+"_decrease" + elif action == 4: # Increase joint_2 + action_id = self.joints[2]+"_increase" + elif action == 5: # Decrease joint_2 + action_id = self.joints[2]+"_decrease" + elif action == 6: # Increase joint_3 + action_id = self.joints[3]+"_increase" + elif action == 7: # Decrease joint_3 + action_id = self.joints[3]+"_decrease" + elif action == 8: # Increase joint_4 + action_id = self.joints[4]+"_increase" + elif action == 9: # Decrease joint_4 + action_id = self.joints[4]+"_decrease" + elif action == 10: # Increase joint_5 + action_id = self.joints[5]+"_increase" + elif action == 11: # Decrease joint_5 + action_id = self.joints[5]+"_decrease" + elif action == 12: # Increase joint_6 + action_id = self.joints[6]+"_increase" + elif action == 13: # Decrease joint_6 + action_id = self.joints[6]+"_decrease" + + # We tell sawyer the action to perform + self.execute_movement(action_id) + + rospy.logdebug("END Set Action ==>"+str(action)+","+str(action_id)) + + def _get_obs(self): + """ + Here we define what sensor data defines our robots observations + To know which Variables we have access to, we need to read the + sawyerEnv API DOCS. + :return: observation + """ + rospy.logdebug("Start Get Observation ==>") + + # We get the translation of the base of the gripper to the block + translation_tcp_block, _ = self.get_tf_start_to_end_frames(start_frame_name="block", + end_frame_name="right_electric_gripper_base") + + translation_tcp_block_round = numpy.around( + translation_tcp_block, decimals=self.dec_obs) + + # We get this data but we dont put it in the observations because its somthing internal for evaluation. + # The order is cucial, get it upside down and it make no sense. + self.translation_tcp_world, _ = self.get_tf_start_to_end_frames(start_frame_name="world", + end_frame_name="right_electric_gripper_base") + + # Same here, the values are used internally for knowing if done, they wont define the state ( although these are left out for performance) + self.joints_efforts_dict = self.get_all_limb_joint_efforts() + rospy.logdebug("JOINTS EFFORTS DICT OBSERVATION METHOD==>" + + str(self.joints_efforts_dict)) + """ + We supose that its all these: + head_pan, right_gripper_l_finger_joint, right_gripper_r_finger_joint, right_j0, right_j1, + right_j2, right_j3, right_j4, right_j5, right_j6 + """ + + joints_angles_array = self.get_all_limb_joint_angles().values() + joints_angles_array_round = numpy.around( + joints_angles_array, decimals=self.dec_obs) + + # We concatenate the two rounded arrays and convert them to standard Python list + observation = numpy.concatenate( + (translation_tcp_block_round, joints_angles_array_round), axis=0).tolist() + + return observation + + def _is_done(self, observations): + """ + We consider the episode done if: + 1) The sawyer TCP is outside the workspace, with self.translation_tcp_world + 2) The Joints exeded a certain effort ( it got stuck somewhere ), self.joints_efforts_array + 3) The TCP to block distance is lower than a threshold ( it got to the place ) + """ + + is_stuck = self.is_arm_stuck(self.joints_efforts_dict) + + tcp_current_pos = Vector3() + tcp_current_pos.x = self.translation_tcp_world[0] + tcp_current_pos.y = self.translation_tcp_world[1] + tcp_current_pos.z = self.translation_tcp_world[2] + + is_inside_workspace = self.is_inside_workspace(tcp_current_pos) + + tcp_to_block_pos = Vector3() + tcp_to_block_pos.x = observations[0] + tcp_to_block_pos.y = observations[1] + tcp_to_block_pos.z = observations[2] + + has_reached_the_block = self.reached_block(tcp_to_block_pos, + self.acceptable_distance_to_cube, + self.translation_tcp_world[2], + self.tcp_z_position_min) + + done = is_stuck or not(is_inside_workspace) or has_reached_the_block + + rospy.logdebug("#### IS DONE ? ####") + rospy.logdebug("is_stuck ?="+str(is_stuck)) + rospy.logdebug("Not is_inside_workspace ?=" + + str(not(is_inside_workspace))) + rospy.logdebug("has_reached_the_block ?="+str(has_reached_the_block)) + rospy.logdebug("done ?="+str(done)) + rospy.logdebug("#### #### ####") + + return done + + def _compute_reward(self, observations, done): + """ + We Base the rewards in if its done or not and we base it on + if the distance to the block has increased or not. + :return: + """ + + tf_tcp_to_block_vector = Vector3() + tf_tcp_to_block_vector.x = observations[0] + tf_tcp_to_block_vector.y = observations[1] + tf_tcp_to_block_vector.z = observations[2] + + distance_block_to_tcp = self.get_magnitud_tf_tcp_to_block( + tf_tcp_to_block_vector) + distance_difference = distance_block_to_tcp - self.previous_distance_from_block + + if not done: + + # If there has been a decrease in the distance to the desired point, we reward it + if distance_difference < 0.0: + rospy.logdebug("DECREASE IN DISTANCE GOOD") + reward = self.closer_to_block_reward + else: + rospy.logerr("ENCREASE IN DISTANCE BAD") + #reward = -1*self.closer_to_block_reward + reward = 0.0 + + else: + + if self.reached_block(tf_tcp_to_block_vector, self.acceptable_distance_to_cube, self.translation_tcp_world[2], self.tcp_z_position_min): + reward = self.done_reward + else: + reward = -1*self.done_reward + + self.previous_distance_from_block = distance_block_to_tcp + + rospy.logdebug("reward=" + str(reward)) + self.cumulated_reward += reward + rospy.logdebug("Cumulated_reward=" + str(self.cumulated_reward)) + self.cumulated_steps += 1 + rospy.logdebug("Cumulated_steps=" + str(self.cumulated_steps)) + + return reward + + # Internal TaskEnv Methods + + def is_arm_stuck(self, joints_efforts_dict): + """ + Checks if the efforts in the arm joints exceed certain theshhold + We will only check the joints_0,1,2,3,4,5,6 + """ + is_arm_stuck = False + + for joint_name in self.joint_limits.joint_names: + if joint_name in joints_efforts_dict: + + effort_value = joints_efforts_dict[joint_name] + index = self.joint_limits.joint_names.index(joint_name) + effort_limit = self.joint_limits.effort[index] + + rospy.logdebug("Joint Effort ==>Name="+str(joint_name) + + ",Effort="+str(effort_value)+",Limit="+str(effort_limit)) + + if abs(effort_value) > effort_limit: + is_arm_stuck = True + rospy.logerr("Joint Effort TOO MUCH ==>" + + str(joint_name)+","+str(effort_value)) + break + else: + rospy.logdebug("Joint Effort is ok==>" + + str(joint_name)+","+str(effort_value)) + else: + rospy.logdebug( + "Joint Name is not in the effort dict==>"+str(joint_name)) + + return is_arm_stuck + + def reached_block(self, block_to_tcp_vector, minimum_distance, tcp_z_position, tcp_z_position_min): + """ + It return True if the transform TCP to block vector magnitude is smaller than + the minimum_distance. + tcp_z_position we use it to only consider that it has reached if its above the table. + """ + + reached_block_b = False + + distance_to_block = self.get_magnitud_tf_tcp_to_block( + block_to_tcp_vector) + + tcp_z_pos_ok = tcp_z_position >= tcp_z_position_min + distance_ok = distance_to_block <= minimum_distance + reached_block_b = distance_ok and tcp_z_pos_ok + + rospy.logdebug("###### REACHED BLOCK ? ######") + rospy.logdebug("tcp_z_pos_ok==>"+str(tcp_z_pos_ok)) + rospy.logdebug("distance_ok==>"+str(distance_ok)) + rospy.logdebug("reached_block_b==>"+str(reached_block_b)) + rospy.logdebug("############") + + return reached_block_b + + def get_distance_from_desired_point(self, current_position): + """ + Calculates the distance from the current position to the desired point + :param start_point: + :return: + """ + distance = self.get_distance_from_point(current_position, + self.desired_point) + + return distance + + def get_distance_from_point(self, pstart, p_end): + """ + Given a Vector3 Object, get distance from current position + :param p_end: + :return: + """ + a = numpy.array((pstart.x, pstart.y, pstart.z)) + b = numpy.array((p_end.x, p_end.y, p_end.z)) + + distance = numpy.linalg.norm(a - b) + + return distance + + def get_magnitud_tf_tcp_to_block(self, translation_vector): + """ + Given a Vector3 Object, get the magnitud + :param p_end: + :return: + """ + a = numpy.array((translation_vector.x, + translation_vector.y, + translation_vector.z)) + + distance = numpy.linalg.norm(a) + + return distance + + def get_orientation_euler(self, quaternion_vector): + # We convert from quaternions to euler + orientation_list = [quaternion_vector.x, + quaternion_vector.y, + quaternion_vector.z, + quaternion_vector.w] + + roll, pitch, yaw = euler_from_quaternion(orientation_list) + return roll, pitch, yaw + + def is_inside_workspace(self, current_position): + """ + Check if the sawyer is inside the Workspace defined + """ + is_inside = False + + rospy.logdebug("##### INSIDE WORK SPACE? #######") + rospy.logdebug("XYZ current_position"+str(current_position)) + rospy.logdebug("work_space_x_max"+str(self.work_space_x_max) + + ",work_space_x_min="+str(self.work_space_x_min)) + rospy.logdebug("work_space_y_max"+str(self.work_space_y_max) + + ",work_space_y_min="+str(self.work_space_y_min)) + rospy.logdebug("work_space_z_max"+str(self.work_space_z_max) + + ",work_space_z_min="+str(self.work_space_z_min)) + rospy.logdebug("############") + + if current_position.x > self.work_space_x_min and current_position.x <= self.work_space_x_max: + if current_position.y > self.work_space_y_min and current_position.y <= self.work_space_y_max: + if current_position.z > self.work_space_z_min and current_position.z <= self.work_space_z_max: + is_inside = True + + return is_inside diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/shadow_tc/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/shadow_tc/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/shadow_tc/config/learn_to_pick_ball.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/shadow_tc/config/learn_to_pick_ball.yaml new file mode 100644 index 0000000..f3bc0a5 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/shadow_tc/config/learn_to_pick_ball.yaml @@ -0,0 +1,19 @@ +shadow_tc: #namespace + + n_actions: 8 # Increase and decrease 7 joints. + + movement_delta: 0.1 # Movement Variation of the TCP position for each step in meters. + + work_space: # 3D cube in which Sawyers TCP ( right_electric_gripper_base frame) is allowed to move in + x_max: 0.3 + x_min: -0.5 + y_max: 0.19 + y_min: -0.45 + z_max: 1.7 + z_min: 1.45 + + number_decimals_precision_obs: 1 + acceptable_distance_to_ball: 0.05 # Distance to the ball that we consider that it reached the ball. + + done_reward: 1000.0 # reward + closer_to_block_reward: 100.0 # reward \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/shadow_tc/learn_to_pick_ball.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/shadow_tc/learn_to_pick_ball.py new file mode 100644 index 0000000..e6e4eb0 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/shadow_tc/learn_to_pick_ball.py @@ -0,0 +1,356 @@ +import rospy +import numpy +from gym import spaces +from openai_ros.robot_envs import shadow_tc_env +from gym.envs.registration import register +from geometry_msgs.msg import Point +from geometry_msgs.msg import Vector3 +from tf.transformations import euler_from_quaternion +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +from openai_ros.openai_ros_common import ROSLauncher +import os + +class ShadowTcGetBallEnv(shadow_tc_env.ShadowTcEnv): + def __init__(self): + """ + Make ShadowTc learn how pick up a ball + """ + # This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there + ros_ws_abspath = rospy.get_param("/shadow_tc/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name="shadow_gazebo", + launch_file_name="start_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/shadow_tc/config", + yaml_file_name="learn_to_pick_ball.yaml") + + + # We execute this one before because there are some functions that this + # TaskEnv uses that use variables from the parent class, like the effort limit fetch. + super(ShadowTcGetBallEnv, self).__init__(ros_ws_abspath) + + # Here we will add any init functions prior to starting the MyRobotEnv + + + # Only variable needed to be set here + + rospy.logdebug("Start ShadowTcGetBallEnv INIT...") + number_actions = rospy.get_param('/shadow_tc/n_actions') + self.action_space = spaces.Discrete(number_actions) + + # We set the reward range, which is not compulsory but here we do it. + self.reward_range = (-numpy.inf, numpy.inf) + + + self.movement_delta =rospy.get_param("/shadow_tc/movement_delta") + + self.work_space_x_max = rospy.get_param("/shadow_tc/work_space/x_max") + self.work_space_x_min = rospy.get_param("/shadow_tc/work_space/x_min") + self.work_space_y_max = rospy.get_param("/shadow_tc/work_space/y_max") + self.work_space_y_min = rospy.get_param("/shadow_tc/work_space/y_min") + self.work_space_z_max = rospy.get_param("/shadow_tc/work_space/z_max") + self.work_space_z_min = rospy.get_param("/shadow_tc/work_space/z_min") + + + self.dec_obs = rospy.get_param("/shadow_tc/number_decimals_precision_obs") + + self.acceptable_distance_to_ball = rospy.get_param("/shadow_tc/acceptable_distance_to_ball") + + + # We place the Maximum and minimum values of observations + # TODO: Fill when get_observations is done. + + + high = numpy.array([self.work_space_x_max, + self.work_space_y_max, + self.work_space_z_max, + 1,1,1]) + + low = numpy.array([ self.work_space_x_min, + self.work_space_y_min, + self.work_space_z_min, + 0,0,0]) + + + self.observation_space = spaces.Box(low, high) + + rospy.logdebug("ACTION SPACES TYPE===>"+str(self.action_space)) + rospy.logdebug("OBSERVATION SPACES TYPE===>"+str(self.observation_space)) + + # Rewards + + self.done_reward =rospy.get_param("/shadow_tc/done_reward") + self.closer_to_block_reward = rospy.get_param("/shadow_tc/closer_to_block_reward") + + self.cumulated_steps = 0.0 + + rospy.logdebug("END shadow_tcGetBallEnv INIT...") + + def _set_init_pose(self): + """ + Sets the UR5 arm to the initial position and the objects to the original position. + """ + rospy.logdebug("START _set_init_pose...") + # We set the angles to zero of the limb + self.reset_scene() + + rospy.logdebug("END _set_init_pose...") + return True + + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + rospy.logdebug("START TaskEnv _init_env_variables") + # For Info Purposes + self.cumulated_reward = 0.0 + + self.ball_pose = self.get_ball_pose() + tcp_pose = self.get_tip_pose() + rospy.logdebug("TCP POSE ===>"+str(tcp_pose)) + self.previous_distance_from_ball = self.get_distance_from_point(self.ball_pose.position, tcp_pose.position) + + rospy.logdebug("END TaskEnv _init_env_variables") + + + + def _set_action(self, action): + """ + It sets the joints of shadow_tc based on the action integer given + based on the action number given. + :param action: The action integer that sets what movement to do next. + """ + + rospy.logdebug("Start Set Action ==>"+str(action)) + + + increment_vector = Vector3() + action_id="move" + + if action == 0: # Increase X + increment_vector.x = self.movement_delta + elif action == 1: # Decrease X + increment_vector.x = -1*self.movement_delta + elif action == 2: # Increase Y + increment_vector.y = self.movement_delta + elif action == 3: # Decrease Y + increment_vector.y = -1*self.movement_delta + elif action == 4: # Increase Z + increment_vector.z = self.movement_delta + elif action == 5: # Decrease Z + increment_vector.z = -1*self.movement_delta + elif action == 6: # Open Claw + action_id = "open" + elif action == 7: # Close Claw + action_id = "close" + + rospy.logdebug("Action_id="+str(action_id)+",IncrementVector===>"+str(increment_vector)) + + if action_id == "move": + # We tell shadow_tc the action to perform + # We dont change the RPY, therefore it will always be zero + + self.move_tip( x=increment_vector.x, + y=increment_vector.y, + z=increment_vector.z) + + elif action_id == "open": + self.open_hand() + elif action_id == "close": + self.close_hand() + + rospy.logdebug("END Set Action ==>"+str(action)+",action_id="+str(action_id)+",IncrementVector===>"+str(increment_vector)) + + def _get_obs(self): + """ + Here we define what sensor data defines our robots observations + To know which Variables we have access to, we need to read the + shadow_tcEnv API DOCS. + :return: observation + """ + rospy.logdebug("Start Get Observation ==>") + + tcp_pose = self.get_tip_pose() + + # We dont add it to the observations because is not part of the robot + self.ball_pose = self.get_ball_pose() + + # We activate the Finguer collision detection + self.finger_collided_dict = self.get_fingers_colision(object_collision_name="cricket_ball__link") + f1_collided = self.finger_collided_dict["f1"] + f2_collided = self.finger_collided_dict["f2"] + f3_collided = self.finger_collided_dict["f3"] + + + observation = [ round(tcp_pose.position.x,self.dec_obs), + round(tcp_pose.position.y,self.dec_obs), + round(tcp_pose.position.z,self.dec_obs), + int(f1_collided), + int(f2_collided), + int(f3_collided) + ] + + rospy.logdebug("Observations ==>"+str(observation)) + rospy.logdebug("END Get Observation ==>") + + return observation + + + def _is_done(self, observations): + """ + We consider the episode done if: + 1) The shadow_tc TCP is outside the workspace. + 2) The TCP to block distance is lower than a threshold ( it got to the place ) + and the the collisions in the figuers are true. + """ + tcp_pos = Vector3() + tcp_pos.x = observations[0] + tcp_pos.y = observations[1] + tcp_pos.z = observations[2] + + # We check if all three finguers have collided with the ball + finguers_collided = observations[3] and observations[4] and observations[5] + + bool_is_inside_workspace = self.is_inside_workspace(tcp_pos) + + + has_reached_the_ball = self.reached_ball( tcp_pos, + self.ball_pose.position, + self.acceptable_distance_to_ball, + finguers_collided) + + done = has_reached_the_ball or not(bool_is_inside_workspace) + + rospy.logdebug("#### IS DONE ? ####") + rospy.logdebug("Not bool_is_inside_workspace ?="+str(not(bool_is_inside_workspace))) + rospy.logdebug("has_reached_the_ball ?="+str(has_reached_the_ball)) + rospy.logdebug("done ?="+str(done)) + rospy.logdebug("#### #### ####") + + return done + + def _compute_reward(self, observations, done): + """ + We Base the rewards in if its done or not and we base it on + if the distance to the ball has increased or not. + :return: + """ + + tcp_pos = Vector3() + tcp_pos.x = observations[0] + tcp_pos.y = observations[1] + tcp_pos.z = observations[2] + # We check if all three finguers have collided with the ball + finguers_collided = observations[3] and observations[4] and observations[5] + + distance_from_ball = self.get_distance_from_point(self.ball_pose.position, tcp_pos) + + distance_difference = distance_from_ball - self.previous_distance_from_ball + + + if not done: + + # If there has been a decrease in the distance to the desired point, we reward it + if distance_difference < 0.0: + rospy.logerr("NOT ERROR: DECREASE IN DISTANCE GOOD") + reward = self.closer_to_block_reward + else: + rospy.logerr("NOT ERROR: ENCREASE IN DISTANCE BAD") + #reward = -1*self.closer_to_block_reward + reward = 0.0 + + else: + + has_reached_the_ball = self.reached_ball( tcp_pos, + self.ball_pose.position, + self.acceptable_distance_to_ball, + finguers_collided) + + if has_reached_the_ball: + reward = self.done_reward + else: + reward = -1*self.done_reward + + + self.previous_distance_from_ball = distance_from_ball + + + rospy.logdebug("reward=" + str(reward)) + self.cumulated_reward += reward + rospy.logdebug("Cumulated_reward=" + str(self.cumulated_reward)) + self.cumulated_steps += 1 + rospy.logdebug("Cumulated_steps=" + str(self.cumulated_steps)) + + return reward + + + # Internal TaskEnv Methods + + def reached_ball(self,tcp_position, ball_position, minimum_distance, finguers_collided): + """ + Return true if the distance from TCP position to the ball position is + lower than the minimum_distance and all three finguers are touching the ball. + """ + + distance_from_ball = self.get_distance_from_point(tcp_position, ball_position) + + distance_to_ball_ok = distance_from_ball < minimum_distance + + reached_ball_b = distance_to_ball_ok and finguers_collided + + rospy.logdebug("###### REACHED BLOCK ? ######") + rospy.logdebug("distance_from_ball==>"+str(distance_from_ball)) + rospy.logdebug("distance_to_ball_ok==>"+str(distance_to_ball_ok)) + rospy.logdebug("reached_ball_b==>"+str(reached_ball_b)) + rospy.logdebug("finguers_collided==>"+str(finguers_collided)) + rospy.logdebug("############") + + return reached_ball_b + + + def get_distance_from_point(self, pstart, p_end): + """ + Given a Vector3 Object, get distance from current position + :param p_end: + :return: + """ + a = numpy.array((pstart.x, pstart.y, pstart.z)) + b = numpy.array((p_end.x, p_end.y, p_end.z)) + + distance = numpy.linalg.norm(a - b) + + return distance + + + def is_inside_workspace(self,current_position): + """ + Check if the shadow_tc is inside the Workspace defined + """ + is_inside = False + + rospy.logdebug("##### INSIDE WORK SPACE? #######") + rospy.logdebug("XYZ current_position"+str(current_position)) + rospy.logdebug("work_space_x_max"+str(self.work_space_x_max)+",work_space_x_min="+str(self.work_space_x_min)) + rospy.logdebug("work_space_y_max"+str(self.work_space_y_max)+",work_space_y_min="+str(self.work_space_y_min)) + rospy.logdebug("work_space_z_max"+str(self.work_space_z_max)+",work_space_z_min="+str(self.work_space_z_min)) + rospy.logdebug("############") + + if current_position.x > self.work_space_x_min and current_position.x <= self.work_space_x_max: + if current_position.y > self.work_space_y_min and current_position.y <= self.work_space_y_max: + if current_position.z > self.work_space_z_min and current_position.z <= self.work_space_z_max: + is_inside = True + + return is_inside + + + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sumit_xl/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sumit_xl/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sumit_xl/config/sumit_xl_room.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sumit_xl/config/sumit_xl_room.yaml new file mode 100755 index 0000000..7cd3513 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sumit_xl/config/sumit_xl_room.yaml @@ -0,0 +1,30 @@ +sumit_xl: #namespace + n_actions: 3 # We have 3 actions, Forwards,TurnLeft,TurnRight, STOP + n_observations: 6 # We have 6 different observations + + speed_step: 1.0 # Time to wait in the reset phases + + linear_forward_speed: 0.5 # Spwwed for ging fowards + linear_turn_speed: 0.05 # Lienare speed when turning + angular_speed: 0.3 # Angular speed when turning Left or Right + init_linear_forward_speed: 0.0 # Initial linear speed in which we start each episode + init_linear_turn_speed: 0.0 # Initial angular speed in shich we start each episode + + new_ranges: 5 # How many laser readings we jump in each observation reading, the bigger the less laser resolution + min_range: 0.5 # Minimum meters below wich we consider we have crashed + max_laser_value: 6 # Value considered Ok, no wall + min_laser_value: 0 # Value considered there is an obstacle or crashed + max_linear_aceleration: 20.0 # Linear acceleration value in which we consider Turtlebot 3 has crashed into something + max_distance: 4.0 # Distance value in which we base the space range of the odometry X and Y axis ( SQUARE RANGE ) + + desired_pose: + x: -2.94 + y: 3.92 + z: 0.0 + + + closer_to_point_reward: 10 # We give points for getting closer to the desired point + not_ending_point_reward: 1 # We give points for getting closer to the desired point + end_episode_points: 200 # Points given when ending an episode + + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sumit_xl/sumit_xl_room.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sumit_xl/sumit_xl_room.py new file mode 100755 index 0000000..0d36c94 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/sumit_xl/sumit_xl_room.py @@ -0,0 +1,414 @@ +import rospy +import numpy +from gym import spaces +from openai_ros.robot_envs import sumitxl_env +from gym.envs.registration import register +from geometry_msgs.msg import Vector3 +from geometry_msgs.msg import Point +from tf.transformations import euler_from_quaternion +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +from openai_ros.openai_ros_common import ROSLauncher +import os + +class SumitXlRoom(sumitxl_env.SumitXlEnv): + def __init__(self): + """ + This Task Env is designed for having the sumit_xl in the sumit_xl world + closed room with columns. + It will learn how to move around without crashing. + """ + + # This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there + ros_ws_abspath = rospy.get_param("/sumit_xl/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name="summit_xl_gazebo", + launch_file_name="start_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/sumit_xl/config", + yaml_file_name="sumit_xl_room.yaml") + + # Here we will add any init functions prior to starting the MyRobotEnv + super(SumitXlRoom, self).__init__(ros_ws_abspath) + + # Only variable needed to be set here + number_actions = rospy.get_param('/sumit_xl/n_actions') + self.action_space = spaces.Discrete(number_actions) + + # We set the reward range, which is not compulsory but here we do it. + self.reward_range = (-numpy.inf, numpy.inf) + + # Actions and Observations + self.linear_forward_speed = rospy.get_param( + '/sumit_xl/linear_forward_speed') + self.linear_turn_speed = rospy.get_param('/sumit_xl/linear_turn_speed') + self.angular_speed = rospy.get_param('/sumit_xl/angular_speed') + self.init_linear_forward_speed = rospy.get_param( + '/sumit_xl/init_linear_forward_speed') + self.init_linear_turn_speed = rospy.get_param( + '/sumit_xl/init_linear_turn_speed') + + self.new_ranges = rospy.get_param('/sumit_xl/new_ranges') + self.min_range = rospy.get_param('/sumit_xl/min_range') + self.max_laser_value = rospy.get_param('/sumit_xl/max_laser_value') + self.min_laser_value = rospy.get_param('/sumit_xl/min_laser_value') + self.max_linear_aceleration = rospy.get_param( + '/sumit_xl/max_linear_aceleration') + + self.max_distance = rospy.get_param('/sumit_xl/max_distance') + + # Get Desired Point to Get + self.desired_point = Point() + self.desired_point.x = rospy.get_param("/sumit_xl/desired_pose/x") + self.desired_point.y = rospy.get_param("/sumit_xl/desired_pose/y") + self.desired_point.z = rospy.get_param("/sumit_xl/desired_pose/z") + + # We create the arrays for the laser readings + # We also create the arrays for the odometry readings + # We join them toeguether. + laser_scan = self.get_laser_scan() + + num_laser_readings = int(len(laser_scan.ranges)/self.new_ranges) + + high_laser = numpy.full((num_laser_readings), self.max_laser_value) + low_laser = numpy.full((num_laser_readings), self.min_laser_value) + + # We place the Maximum and minimum values of the X,Y and YAW of the odometry + # The odometry yaw can be any value in the circunference. + high_odometry = numpy.array( + [self.max_distance, self.max_distance, 3.14]) + low_odometry = numpy.array( + [-1*self.max_distance, -1*self.max_distance, -1*3.14]) + + # We join both arrays + high = numpy.concatenate([high_laser, high_odometry]) + low = numpy.concatenate([low_laser, low_odometry]) + + self.observation_space = spaces.Box(low, high) + + rospy.logdebug("ACTION SPACES TYPE===>"+str(self.action_space)) + rospy.logdebug("OBSERVATION SPACES TYPE===>" + + str(self.observation_space)) + + # Rewards + self.closer_to_point_reward = rospy.get_param( + "/sumit_xl/closer_to_point_reward") + self.not_ending_point_reward = rospy.get_param( + "/sumit_xl/not_ending_point_reward") + + self.end_episode_points = rospy.get_param( + "/sumit_xl/end_episode_points") + + self.cumulated_steps = 0.0 + + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + self.move_base(self.init_linear_forward_speed, + self.init_linear_turn_speed, + epsilon=0.05, + update_rate=10) + + return True + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + # For Info Purposes + self.cumulated_reward = 0.0 + # Set to false Done, because its calculated asyncronously + self._episode_done = False + + odometry = self.get_odom() + self.previous_distance_from_des_point = self.get_distance_from_desired_point( + odometry.pose.pose.position) + + def _set_action(self, action): + """ + This set action will Set the linear and angular speed of the SumitXl + based on the action number given. + :param action: The action integer that set s what movement to do next. + """ + + rospy.logdebug("Start Set Action ==>"+str(action)) + # We convert the actions to speed movements to send to the parent class CubeSingleDiskEnv + if action == 0: # FORWARD + linear_speed = self.linear_forward_speed + angular_speed = 0.0 + self.last_action = "FORWARDS" + elif action == 1: # LEFT + linear_speed = self.linear_turn_speed + angular_speed = self.angular_speed + self.last_action = "TURN_LEFT" + elif action == 2: # RIGHT + linear_speed = self.linear_turn_speed + angular_speed = -1*self.angular_speed + self.last_action = "TURN_RIGHT" + """ + elif action == 3: #STOP + linear_speed = 0.0 + angular_speed = 0.0 + self.last_action = "STOP" + """ + + # We tell SumitXL the linear and angular speed to set to execute + self.move_base(linear_speed, angular_speed, + epsilon=0.05, update_rate=10) + + rospy.logdebug("END Set Action ==>"+str(action)) + + def _get_obs(self): + """ + Here we define what sensor data defines our robots observations + To know which Variables we have acces to, we need to read the + SumitXlEnv API DOCS + + WALL CLOSE LEFT [1, 1, 9, 0, 0, 0, -1.8, 0.46, 0.01] + WALL CLOSE RIGHT [0, 0, 0, 10, 1, 2, -1.8, -0.61, 0.01] + WALL BACK [0, 9, 1, 1, 6, 0, -1.8, -0.54, 1.59] + WALL FRONT [2, 9, 0, 0, 2, 2, -1.83, 0.51, 1.58] + + 0 in reality is arround front 0.4, back 0.5, sides 0.3 + + :return: + """ + rospy.logdebug("Start Get Observation ==>") + # We get the laser scan data + laser_scan = self.get_laser_scan() + + discretized_laser_scan = self.discretize_scan_observation(laser_scan, + self.new_ranges + ) + # We get the odometry so that SumitXL knows where it is. + odometry = self.get_odom() + x_position = odometry.pose.pose.position.x + y_position = odometry.pose.pose.position.y + + # We get the orientation of the cube in RPY + roll, pitch, yaw = self.get_orientation_euler() + # We round to only two decimals to avoid very big Observation space + odometry_array = [round(x_position, 2), + round(y_position, 2), + round(yaw, 2)] + + # We only want the X and Y position and the Yaw + + observations = discretized_laser_scan + odometry_array + + rospy.logdebug("Observations==>"+str(observations)) + rospy.logdebug("END Get Observation ==>") + + return observations + + def _is_done(self, observations): + + if self._episode_done: + rospy.logerr("SumitXl is Too Close to wall==>") + else: + rospy.logdebug("SumitXl is NOT close to a wall ==>") + + # Now we check if it has crashed based on the imu + imu_data = self.get_imu() + linear_acceleration_magnitude = self.get_vector_magnitude( + imu_data.linear_acceleration) + if linear_acceleration_magnitude > self.max_linear_aceleration: + rospy.logerr("SumitXl Crashed==>"+str(linear_acceleration_magnitude) + + ">"+str(self.max_linear_aceleration)) + self._episode_done = True + else: + rospy.logerr("DIDNT crash SumitXl ==>"+str(linear_acceleration_magnitude) + + ">"+str(self.max_linear_aceleration)) + + current_position = Point() + current_position.x = observations[-3] + current_position.y = observations[-2] + current_position.z = 0.0 + + if abs(current_position.x) <= self.max_distance: + if abs(current_position.y) <= self.max_distance: + rospy.logdebug( + "SummitXL Position is OK ==>["+str(current_position.x)+","+str(current_position.y)+"]") + else: + rospy.logerr("SummitXL to Far in Y Pos ==>" + + str(current_position.x)) + self._episode_done = True + else: + rospy.logerr("SummitXL to Far in X Pos ==>" + + str(current_position.x)) + self._episode_done = True + + if self.is_in_desired_position(current_position): + self._episode_done = True + + return self._episode_done + + def _compute_reward(self, observations, done): + """ + We give reward to the robot when it gets closer to the desired point. + We Dont give it contsnatly, but only if there is an improvement + """ + + # We get the current Position from the obervations + current_position = Point() + current_position.x = observations[-3] + current_position.y = observations[-2] + current_position.z = 0.0 + + distance_from_des_point = self.get_distance_from_desired_point( + current_position) + + distance_difference = distance_from_des_point - \ + self.previous_distance_from_des_point + + rospy.logwarn("current_position=" + str(current_position)) + rospy.logwarn("desired_point=" + str(self.desired_point)) + + rospy.logwarn("total_distance_from_des_point=" + + str(self.previous_distance_from_des_point)) + rospy.logwarn("distance_from_des_point=" + + str(distance_from_des_point)) + rospy.logwarn("distance_difference=" + str(distance_difference)) + + if not done: + # If there has been a decrease in the distance to the desired point, we reward it + if distance_difference < 0.0: + rospy.logwarn("DECREASE IN DISTANCE GOOD") + reward = self.closer_to_point_reward + else: + # If it didnt get closer, we give much less points in theory + # This should trigger the behaviour of moving towards the point + rospy.logwarn("NO DECREASE IN DISTANCE, so much less points") + reward = self.not_ending_point_reward + else: + if self.is_in_desired_position(current_position): + reward = self.end_episode_points + rospy.logwarn( + "GOT TO DESIRED POINT ; DONE, reward=" + str(reward)) + else: + reward = -1*self.end_episode_points + rospy.logerr( + "SOMETHING WENT WRONG ; DONE, reward=" + str(reward)) + + self.previous_distance_from_des_point = distance_from_des_point + + rospy.logwarn("reward=" + str(reward)) + self.cumulated_reward += reward + rospy.logdebug("Cumulated_reward=" + str(self.cumulated_reward)) + self.cumulated_steps += 1 + rospy.logdebug("Cumulated_steps=" + str(self.cumulated_steps)) + + return reward + + # Internal TaskEnv Methods + + def discretize_scan_observation(self, data, new_ranges): + """ + Discards all the laser readings that are not multiple in index of new_ranges + value. + """ + self._episode_done = False + + discretized_ranges = [] + mod = len(data.ranges)/new_ranges + + rospy.logdebug("data=" + str(data)) + rospy.logdebug("new_ranges=" + str(new_ranges)) + rospy.logdebug("mod=" + str(mod)) + + for i, item in enumerate(data.ranges): + if (i % mod == 0): + if item == float('Inf') or numpy.isinf(item): + discretized_ranges.append(self.max_laser_value) + elif numpy.isnan(item): + discretized_ranges.append(self.min_laser_value) + else: + discretized_ranges.append(int(item)) + + if (self.min_range > item > 0): + rospy.logerr("done Validation >>> item=" + + str(item)+"< "+str(self.min_range)) + self._episode_done = True + else: + rospy.logdebug("NOT done Validation >>> item=" + + str(item)+"< "+str(self.min_range)) + + return discretized_ranges + + def get_vector_magnitude(self, vector): + """ + It calculated the magnitude of the Vector3 given. + This is usefull for reading imu accelerations and knowing if there has been + a crash + :return: + """ + contact_force_np = numpy.array((vector.x, vector.y, vector.z)) + force_magnitude = numpy.linalg.norm(contact_force_np) + + return force_magnitude + + def get_orientation_euler(self): + # We convert from quaternions to euler + orientation_list = [self.odom.pose.pose.orientation.x, + self.odom.pose.pose.orientation.y, + self.odom.pose.pose.orientation.z, + self.odom.pose.pose.orientation.w] + + roll, pitch, yaw = euler_from_quaternion(orientation_list) + return roll, pitch, yaw + + def get_distance_from_desired_point(self, current_position): + """ + Calculates the distance from the current position to the desired point + :param start_point: + :return: + """ + distance = self.get_distance_from_point(current_position, + self.desired_point) + + return distance + + def get_distance_from_point(self, pstart, p_end): + """ + Given a Vector3 Object, get distance from current position + :param p_end: + :return: + """ + a = numpy.array((pstart.x, pstart.y, pstart.z)) + b = numpy.array((p_end.x, p_end.y, p_end.z)) + + distance = numpy.linalg.norm(a - b) + + return distance + + def is_in_desired_position(self, current_position, epsilon=0.05): + """ + It return True if the current position is similar to the desired poistion + """ + + is_in_desired_pos = False + + x_pos_plus = self.desired_point.x + epsilon + x_pos_minus = self.desired_point.x - epsilon + y_pos_plus = self.desired_point.y + epsilon + y_pos_minus = self.desired_point.y - epsilon + + x_current = current_position.x + y_current = current_position.y + + x_pos_are_close = (x_current <= x_pos_plus) and ( + x_current > x_pos_minus) + y_pos_are_close = (y_current <= y_pos_plus) and ( + y_current > y_pos_minus) + + is_in_desired_pos = x_pos_are_close and y_pos_are_close + + return is_in_desired_pos diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/task_commons.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/task_commons.py new file mode 100644 index 0000000..2eec742 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/task_commons.py @@ -0,0 +1,17 @@ +#!/usr/bin/env python +import rosparam +import rospkg +import os + +def LoadYamlFileParamsTest(rospackage_name, rel_path_from_package_to_file, yaml_file_name): + + rospack = rospkg.RosPack() + pkg_path = rospack.get_path(rospackage_name) + config_dir = os.path.join(pkg_path, rel_path_from_package_to_file) + path_config_file = os.path.join(config_dir, yaml_file_name) + + paramlist=rosparam.load_file(path_config_file) + + for params, ns in paramlist: + rosparam.upload_params(ns,params) + \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/task_envs_list.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/task_envs_list.py new file mode 100644 index 0000000..274d73e --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/task_envs_list.py @@ -0,0 +1,255 @@ +#!/usr/bin/env python +from gym.envs.registration import register +from gym import envs + + +def RegisterOpenAI_Ros_Env(task_env, max_episode_steps=10000): + """ + Registers all the ENVS supported in OpenAI ROS. This way we can load them + with variable limits. + Here is where you have to PLACE YOUR NEW TASK ENV, to be registered and accesible. + return: False if the Task_Env wasnt registered, True if it was. + """ + + ########################################################################### + # MovingCube Task-Robot Envs + + result = True + + # Cubli Moving Cube + if task_env == 'MovingCubeOneDiskWalk-v0': + print("Import module") + + # We have to import the Class that we registered so that it can be found afterwards in the Make + from openai_ros.task_envs.moving_cube import one_disk_walk + + print("Importing register env") + # We register the Class through the Gym system + register( + id=task_env, + #entry_point='openai_ros:task_envs.moving_cube.one_disk_walk.MovingCubeOneDiskWalkEnv', + entry_point='openai_ros.task_envs.moving_cube.one_disk_walk:MovingCubeOneDiskWalkEnv', + max_episode_steps=max_episode_steps, + ) + + + # Husarion Robot + elif task_env == 'HusarionGetToPosTurtleBotPlayGround-v0': + + register( + id=task_env, + entry_point='openai_ros.task_envs.husarion.husarion_get_to_position_turtlebot_playground:HusarionGetToPosTurtleBotPlayGroundEnv', + max_episode_steps=max_episode_steps, + ) + + # import our training environment + from openai_ros.task_envs.husarion import husarion_get_to_position_turtlebot_playground + + elif task_env == 'FetchTest-v0': + register( + id=task_env, + entry_point='openai_ros.task_envs.fetch.fetch_test_task:FetchTestEnv', + max_episode_steps=max_episode_steps, + ) + # 50 + # We have to import the Class that we registered so that it can be found afterwards in the Make + from openai_ros.task_envs.fetch import fetch_test_task + + elif task_env == 'FetchSimpleTest-v0': + register( + id=task_env, + # entry_point='openai_ros:task_envs.fetch.fetch_simple_task.FetchSimpleTestEnv', + entry_point='openai_ros.task_envs.fetch.fetch_simple_task:FetchSimpleTestEnv', + max_episode_steps=max_episode_steps, + ) + # We have to import the Class that we registered so that it can be found afterwards in the Make + from openai_ros.task_envs.fetch import fetch_simple_task + + elif task_env == 'FetchPickAndPlace-v0': + register( + id=task_env, + # entry_point='openai_ros:task_envs.fetch.fetch_pick_and_place_task.FetchPickAndPlaceEnv', + entry_point='openai_ros.task_envs.fetch.fetch_pick_and_place_task:FetchPickAndPlaceEnv', + max_episode_steps=max_episode_steps, + ) + # We have to import the Class that we registered so that it can be found afterwards in the Make + from openai_ros.task_envs.fetch import fetch_pick_and_place_task + + elif task_env == 'FetchPush-v0': + register( + id=task_env, + # entry_point='openai_ros:task_envs.fetch.fetch_pick_and_place_task.FetchPushEnv', + # entry_point='openai_ros:task_envs.fetch.fetch_push.FetchPushEnv', + entry_point='openai_ros.task_envs.fetch.fetch_push:FetchPushEnv', + max_episode_steps=max_episode_steps, + ) + # We have to import the Class that we registered so that it can be found afterwards in the Make + from openai_ros.task_envs.fetch import fetch_push + + elif task_env == 'CartPoleStayUp-v0': + register( + id=task_env, + entry_point='openai_ros.task_envs.cartpole_stay_up.stay_up:CartPoleStayUpEnv', + max_episode_steps=max_episode_steps, + ) + + # import our training environment + from openai_ros.task_envs.cartpole_stay_up import stay_up + + elif task_env == 'HopperStayUp-v0': + + register( + id=task_env, + entry_point='openai_ros.task_envs.hopper.hopper_stay_up:HopperStayUpEnv', + max_episode_steps=max_episode_steps, + ) + + # import our training environment + from openai_ros.task_envs.hopper import hopper_stay_up + + elif task_env == 'IriWamTcpToBowl-v0': + + register( + id=task_env, + entry_point='openai_ros.task_envs.iriwam.tcp_to_bowl:IriWamTcpToBowlEnv', + max_episode_steps=max_episode_steps, + ) + + # import our training environment + from openai_ros.task_envs.iriwam import tcp_to_bowl + + elif task_env == 'ParrotDroneGoto-v0': + + register( + id=task_env, + entry_point='openai_ros.task_envs.parrotdrone.parrotdrone_goto:ParrotDroneGotoEnv', + max_episode_steps=max_episode_steps, + ) + + # import our training environment + from openai_ros.task_envs.parrotdrone import parrotdrone_goto + + elif task_env == 'SawyerTouchCube-v0': + + register( + id=task_env, + entry_point='openai_ros.task_envs.sawyer.learn_to_touch_cube:SawyerTouchCubeEnv', + max_episode_steps=max_episode_steps, + ) + + # import our training environment + from openai_ros.task_envs.sawyer import learn_to_touch_cube + + elif task_env == 'ShadowTcGetBall-v0': + + register( + id=task_env, + entry_point='openai_ros.task_envs.shadow_tc.learn_to_pick_ball:ShadowTcGetBallEnv', + max_episode_steps=max_episode_steps, + ) + + # import our training environment + from openai_ros.task_envs.shadow_tc import learn_to_pick_ball + + elif task_env == 'SumitXlRoom-v0': + + register( + id='SumitXlRoom-v0', + entry_point='openai_ros.task_envs.sumit_xl.sumit_xl_room:SumitXlRoom', + max_episode_steps=max_episode_steps, + ) + + # import our training environment + from openai_ros.task_envs.sumit_xl import sumit_xl_room + + elif task_env == 'MyTurtleBot2Maze-v0': + + register( + id=task_env, + entry_point='openai_ros.task_envs.turtlebot2.turtlebot2_maze:TurtleBot2MazeEnv', + max_episode_steps=max_episode_steps, + ) + # import our training environment + from openai_ros.task_envs.turtlebot2 import turtlebot2_maze + + elif task_env == 'MyTurtleBot2Wall-v0': + + register( + id=task_env, + entry_point='openai_ros.task_envs.turtlebot2.turtlebot2_wall:TurtleBot2WallEnv', + max_episode_steps=max_episode_steps, + ) + # import our training environment + from openai_ros.task_envs.turtlebot2 import turtlebot2_wall + + elif task_env == 'TurtleBot3World-v0': + + register( + id=task_env, + entry_point='openai_ros.task_envs.turtlebot3.turtlebot3_world:TurtleBot3WorldEnv', + max_episode_steps=max_episode_steps, + ) + + # import our training environment + from openai_ros.task_envs.turtlebot3 import turtlebot3_world + + elif task_env == 'WamvNavTwoSetsBuoys-v0': + + register( + id=task_env, + entry_point='openai_ros.task_envs.wamv.wamv_nav_twosets_buoys:WamvNavTwoSetsBuoysEnv', + max_episode_steps=max_episode_steps, + ) + + # import our training environment + from openai_ros.task_envs.wamv import wamv_nav_twosets_buoys + + elif task_env == 'TurtleBot3WorldMazeTesting-v0': + + register( + id=task_env, + entry_point='openai_ros.task_envs.turtlebot3_custom.turtlebot3_world:TurtleBot3WorldEnv', + max_episode_steps=max_episode_steps, + ) + + # import our training environment + from openai_ros.task_envs.turtlebot3_custom import turtlebot3_world + + elif task_env == 'TurtleBot3A2CTesting-v0': + + register( + id=task_env, + entry_point='openai_ros.task_envs.turtlebot3_custom_a2c.turtlebot3_world:TurtleBot3WorldEnv', + max_episode_steps=max_episode_steps, + ) + + # import our training environment + from openai_ros.task_envs.turtlebot3_custom import turtlebot3_world + + + # Add here your Task Envs to be registered + else: + result = False + + ########################################################################### + + if result: + # We check that it was really registered + supported_gym_envs = GetAllRegisteredGymEnvs() + #print("REGISTERED GYM ENVS===>"+str(supported_gym_envs)) + assert (task_env in supported_gym_envs), "The Task_Robot_ENV given is not Registered ==>" + \ + str(task_env) + + return result + + +def GetAllRegisteredGymEnvs(): + """ + Returns a List of all the registered Envs in the system + return EX: ['Copy-v0', 'RepeatCopy-v0', 'ReversedAddition-v0', ... ] + """ + + all_envs = envs.registry.all() + env_ids = [env_spec.id for env_spec in all_envs] + + return env_ids diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot2/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot2/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot2/config/turtlebot2_maze.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot2/config/turtlebot2_maze.yaml new file mode 100644 index 0000000..33adb04 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot2/config/turtlebot2_maze.yaml @@ -0,0 +1,30 @@ +turtlebot2: #namespace + + n_actions: 3 # We have 3 actions, Forwards,TurnLeft,TurnRight + n_observations: 6 # We have 6 different observations + + speed_step: 1.0 # Time to wait in the reset phases + + linear_forward_speed: 0.5 # Spawned for ging fowards + linear_turn_speed: 0.1 # Lienare speed when turning + angular_speed: 0.3 # Angular speed when turning Left or Right + init_linear_forward_speed: 0.0 # Initial linear speed in which we start each episode + init_linear_turn_speed: 0.0 # Initial angular speed in shich we start each episode + + new_ranges: 5 # How many laser readings we jump in each observation reading, the bigger the less laser resolution + min_range: 0.5 # Minimum meters below wich we consider we have crashed + max_laser_value: 6 # Value considered Ok, no wall + min_laser_value: 0 # Value considered there is an obstacle or crashed + + number_of_sectors: 3 # How many sectors we have + min_range: 0.5 # Minimum meters below wich we consider we have crashed + middle_range: 1.0 # Minimum meters below wich we consider we have crashed + danger_laser_value: 2 # Value considered Ok, no wall + middle_laser_value: 1 # Middle value + safe_laser_value: 0 # Value considered there is an obstacle or crashed + + forwards_reward: 5 # Points Given to go forwards + turn_reward: 1 # Points Given to turn as action + end_episode_points: 200 # Points given when ending an episode + + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot2/config/turtlebot2_wall.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot2/config/turtlebot2_wall.yaml new file mode 100644 index 0000000..6087107 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot2/config/turtlebot2_wall.yaml @@ -0,0 +1,28 @@ +turtlebot2: #namespace + + n_actions: 3 # We have 3 actions, Forwards,TurnLeft,TurnRight,Backwards + n_observations: 6 # We have 6 different observations + + speed_step: 1.0 # Time to wait in the reset phases + + linear_forward_speed: 0.5 # Spwwed for ging fowards + linear_turn_speed: 0.05 # Lienare speed when turning + angular_speed: 0.3 # Angular speed when turning Left or Right + init_linear_forward_speed: 0.0 # Initial linear speed in which we start each episode + init_linear_turn_speed: 0.0 # Initial angular speed in shich we start each episode + + new_ranges: 5 # How many laser readings we jump in each observation reading, the bigger the less laser resolution + min_range: 0.5 # Minimum meters below wich we consider we have crashed + max_laser_value: 6 # Value considered Ok, no wall + min_laser_value: 0 # Value considered there is an obstacle or crashed + + desired_pose: + x: 5.0 + y: 0.0 + z: 0.0 + + forwards_reward: 5 # Points Given to go forwards + turn_reward: 1 # Points Given to turn as action + end_episode_points: 200 # Points given when ending an episode + + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot2/turtlebot2_maze.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot2/turtlebot2_maze.py new file mode 100644 index 0000000..cf512e6 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot2/turtlebot2_maze.py @@ -0,0 +1,324 @@ +import rospy +import numpy +import time +import math +from gym import spaces +from openai_ros.robot_envs import turtlebot2_env +from gym.envs.registration import register +from sensor_msgs.msg import LaserScan +from std_msgs.msg import Header +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +from openai_ros.openai_ros_common import ROSLauncher +import os + + +class TurtleBot2MazeEnv(turtlebot2_env.TurtleBot2Env): + def __init__(self): + """ + This Task Env is designed for having the TurtleBot2 in some kind of maze. + It will learn how to move around the maze without crashing. + """ + + # This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there + # This parameter HAS to be set up in the MAIN launch of the AI RL script + ros_ws_abspath = rospy.get_param("/turtlebot2/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path "+ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p "+ros_ws_abspath + \ + "/src;cd "+ros_ws_abspath+";catkin_make" + + ROSLauncher(rospackage_name="turtlebot_gazebo", + launch_file_name="start_world_maze_loop_brick.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/turtlebot2/config", + yaml_file_name="turtlebot2_maze.yaml") + + # Here we will add any init functions prior to starting the MyRobotEnv + super(TurtleBot2MazeEnv, self).__init__(ros_ws_abspath) + + # Only variable needed to be set here + number_actions = rospy.get_param('/turtlebot2/n_actions') + self.action_space = spaces.Discrete(number_actions) + + # We set the reward range, which is not compulsory but here we do it. + self.reward_range = (-numpy.inf, numpy.inf) + + #number_observations = rospy.get_param('/turtlebot2/n_observations') + """ + We set the Observation space for the 6 observations + cube_observations = [ + round(current_disk_roll_vel, 0), + round(y_distance, 1), + round(roll, 1), + round(pitch, 1), + round(y_linear_speed,1), + round(yaw, 1), + ] + """ + + # Actions and Observations + self.dec_obs = rospy.get_param( + "/turtlebot2/number_decimals_precision_obs", 1) + self.linear_forward_speed = rospy.get_param( + '/turtlebot2/linear_forward_speed') + self.linear_turn_speed = rospy.get_param( + '/turtlebot2/linear_turn_speed') + self.angular_speed = rospy.get_param('/turtlebot2/angular_speed') + self.init_linear_forward_speed = rospy.get_param( + '/turtlebot2/init_linear_forward_speed') + self.init_linear_turn_speed = rospy.get_param( + '/turtlebot2/init_linear_turn_speed') + + self.n_observations = rospy.get_param('/turtlebot2/n_observations') + self.min_range = rospy.get_param('/turtlebot2/min_range') + self.max_laser_value = rospy.get_param('/turtlebot2/max_laser_value') + self.min_laser_value = rospy.get_param('/turtlebot2/min_laser_value') + + # We create two arrays based on the binary values that will be assigned + # In the discretization method. + #laser_scan = self._check_laser_scan_ready() + laser_scan = self.get_laser_scan() + rospy.logdebug("laser_scan len===>"+str(len(laser_scan.ranges))) + + # Laser data + self.laser_scan_frame = laser_scan.header.frame_id + + # Number of laser reading jumped + self.new_ranges = int( + math.ceil(float(len(laser_scan.ranges)) / float(self.n_observations))) + + rospy.logdebug("n_observations===>"+str(self.n_observations)) + rospy.logdebug( + "new_ranges, jumping laser readings===>"+str(self.new_ranges)) + + high = numpy.full((self.n_observations), self.max_laser_value) + low = numpy.full((self.n_observations), self.min_laser_value) + + # We only use two integers + self.observation_space = spaces.Box(low, high) + + rospy.logdebug("ACTION SPACES TYPE===>"+str(self.action_space)) + rospy.logdebug("OBSERVATION SPACES TYPE===>" + + str(self.observation_space)) + + # Rewards + self.forwards_reward = rospy.get_param("/turtlebot2/forwards_reward") + self.turn_reward = rospy.get_param("/turtlebot2/turn_reward") + self.end_episode_points = rospy.get_param( + "/turtlebot2/end_episode_points") + + self.cumulated_steps = 0.0 + + self.laser_filtered_pub = rospy.Publisher( + '/turtlebot2/laser/scan_filtered', LaserScan, queue_size=1) + + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + self.move_base(self.init_linear_forward_speed, + self.init_linear_turn_speed, + epsilon=0.05, + update_rate=10, + min_laser_distance=-1) + + return True + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + # For Info Purposes + self.cumulated_reward = 0.0 + # Set to false Done, because its calculated asyncronously + self._episode_done = False + + # We wait a small ammount of time to start everything because in very fast resets, laser scan values are sluggish + # and sometimes still have values from the prior position that triguered the done. + time.sleep(1.0) + + # TODO: Add reset of published filtered laser readings + laser_scan = self.get_laser_scan() + discretized_ranges = laser_scan.ranges + self.publish_filtered_laser_scan(laser_original_data=laser_scan, + new_filtered_laser_range=discretized_ranges) + + def _set_action(self, action): + """ + This set action will Set the linear and angular speed of the turtlebot2 + based on the action number given. + :param action: The action integer that set s what movement to do next. + """ + + rospy.logdebug("Start Set Action ==>"+str(action)) + # We convert the actions to speed movements to send to the parent class CubeSingleDiskEnv + if action == 0: # FORWARD + linear_speed = self.linear_forward_speed + angular_speed = 0.0 + self.last_action = "FORWARDS" + elif action == 1: # LEFT + linear_speed = self.linear_turn_speed + angular_speed = self.angular_speed + self.last_action = "TURN_LEFT" + elif action == 2: # RIGHT + linear_speed = self.linear_turn_speed + angular_speed = -1*self.angular_speed + self.last_action = "TURN_RIGHT" + + # We tell TurtleBot2 the linear and angular speed to set to execute + self.move_base(linear_speed, + angular_speed, + epsilon=0.05, + update_rate=10, + min_laser_distance=self.min_range) + + rospy.logdebug("END Set Action ==>"+str(action) + + ", NAME="+str(self.last_action)) + + def _get_obs(self): + """ + Here we define what sensor data defines our robots observations + To know which Variables we have acces to, we need to read the + TurtleBot2Env API DOCS + :return: + """ + rospy.logdebug("Start Get Observation ==>") + # We get the laser scan data + laser_scan = self.get_laser_scan() + + rospy.logdebug("BEFORE DISCRET _episode_done==>" + + str(self._episode_done)) + + discretized_observations = self.discretize_observation(laser_scan, + self.new_ranges + ) + + rospy.logdebug("Observations==>"+str(discretized_observations)) + rospy.logdebug("AFTER DISCRET_episode_done==>"+str(self._episode_done)) + rospy.logdebug("END Get Observation ==>") + return discretized_observations + + def _is_done(self, observations): + + if self._episode_done: + rospy.logdebug("TurtleBot2 is Too Close to wall==>" + + str(self._episode_done)) + else: + rospy.logerr("TurtleBot2 is Ok ==>") + + return self._episode_done + + def _compute_reward(self, observations, done): + + if not done: + if self.last_action == "FORWARDS": + reward = self.forwards_reward + else: + reward = self.turn_reward + else: + reward = -1*self.end_episode_points + + rospy.logdebug("reward=" + str(reward)) + self.cumulated_reward += reward + rospy.logdebug("Cumulated_reward=" + str(self.cumulated_reward)) + self.cumulated_steps += 1 + rospy.logdebug("Cumulated_steps=" + str(self.cumulated_steps)) + + return reward + + # Internal TaskEnv Methods + + def discretize_observation(self, data, new_ranges): + """ + Discards all the laser readings that are not multiple in index of new_ranges + value. + """ + self._episode_done = False + + discretized_ranges = [] + filtered_range = [] + #mod = len(data.ranges)/new_ranges + mod = new_ranges + + max_laser_value = data.range_max + min_laser_value = data.range_min + + rospy.logdebug("data=" + str(data)) + rospy.logwarn("mod=" + str(mod)) + + for i, item in enumerate(data.ranges): + if (i % mod == 0): + if item == float('Inf') or numpy.isinf(item): + # discretized_ranges.append(self.max_laser_value) + discretized_ranges.append( + round(max_laser_value, self.dec_obs)) + elif numpy.isnan(item): + # discretized_ranges.append(self.min_laser_value) + discretized_ranges.append( + round(min_laser_value, self.dec_obs)) + else: + # discretized_ranges.append(int(item)) + discretized_ranges.append(round(item, self.dec_obs)) + + if (self.min_range > item > 0): + rospy.logerr("done Validation >>> item=" + + str(item)+"< "+str(self.min_range)) + self._episode_done = True + else: + rospy.logwarn("NOT done Validation >>> item=" + + str(item)+"< "+str(self.min_range)) + # We add last value appended + filtered_range.append(discretized_ranges[-1]) + else: + # We add value zero + filtered_range.append(0.1) + + rospy.logdebug( + "Size of observations, discretized_ranges==>"+str(len(discretized_ranges))) + + self.publish_filtered_laser_scan(laser_original_data=data, + new_filtered_laser_range=discretized_ranges) + + return discretized_ranges + + def publish_filtered_laser_scan(self, laser_original_data, new_filtered_laser_range): + + rospy.logdebug("new_filtered_laser_range==>" + + str(new_filtered_laser_range)) + + laser_filtered_object = LaserScan() + + h = Header() + # Note you need to call rospy.init_node() before this will work + h.stamp = rospy.Time.now() + h.frame_id = laser_original_data.header.frame_id + + laser_filtered_object.header = h + laser_filtered_object.angle_min = laser_original_data.angle_min + laser_filtered_object.angle_max = laser_original_data.angle_max + + new_angle_incr = abs(laser_original_data.angle_max - + laser_original_data.angle_min) / len(new_filtered_laser_range) + + #laser_filtered_object.angle_increment = laser_original_data.angle_increment + laser_filtered_object.angle_increment = new_angle_incr + laser_filtered_object.time_increment = laser_original_data.time_increment + laser_filtered_object.scan_time = laser_original_data.scan_time + laser_filtered_object.range_min = laser_original_data.range_min + laser_filtered_object.range_max = laser_original_data.range_max + + laser_filtered_object.ranges = [] + laser_filtered_object.intensities = [] + for item in new_filtered_laser_range: + if item == 0.0: + laser_distance = 0.1 + else: + laser_distance = item + laser_filtered_object.ranges.append(laser_distance) + laser_filtered_object.intensities.append(item) + + self.laser_filtered_pub.publish(laser_filtered_object) diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot2/turtlebot2_wall.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot2/turtlebot2_wall.py new file mode 100644 index 0000000..7809389 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot2/turtlebot2_wall.py @@ -0,0 +1,356 @@ +import rospy +import numpy +from gym import spaces +from openai_ros.robot_envs import turtlebot2_env +from gym.envs.registration import register +from geometry_msgs.msg import Point +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +from openai_ros.openai_ros_common import ROSLauncher +import os + +class TurtleBot2WallEnv(turtlebot2_env.TurtleBot2Env): + def __init__(self): + """ + This Task Env is designed for having the TurtleBot2 in some kind of maze. + It will learn how to move around the maze without crashing. + """ + + # This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there + ros_ws_abspath = rospy.get_param("/turtlebot2/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name="turtlebot_gazebo", + launch_file_name="start_world_wall.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/turtlebot2/config", + yaml_file_name="turtlebot2_wall.yaml") + + # Here we will add any init functions prior to starting the MyRobotEnv + super(TurtleBot2WallEnv, self).__init__(ros_ws_abspath) + + # Only variable needed to be set here + number_actions = rospy.get_param('/turtlebot2/n_actions') + self.action_space = spaces.Discrete(number_actions) + + # We set the reward range, which is not compulsory but here we do it. + self.reward_range = (-numpy.inf, numpy.inf) + + + #number_observations = rospy.get_param('/turtlebot2/n_observations') + """ + We set the Observation space for the 6 observations + cube_observations = [ + round(current_disk_roll_vel, 0), + round(y_distance, 1), + round(roll, 1), + round(pitch, 1), + round(y_linear_speed,1), + round(yaw, 1), + ] + """ + + # Actions and Observations + self.linear_forward_speed = rospy.get_param('/turtlebot2/linear_forward_speed') + self.linear_turn_speed = rospy.get_param('/turtlebot2/linear_turn_speed') + self.angular_speed = rospy.get_param('/turtlebot2/angular_speed') + self.init_linear_forward_speed = rospy.get_param('/turtlebot2/init_linear_forward_speed') + self.init_linear_turn_speed = rospy.get_param('/turtlebot2/init_linear_turn_speed') + + self.new_ranges = rospy.get_param('/turtlebot2/new_ranges') + self.min_range = rospy.get_param('/turtlebot2/min_range') + self.max_laser_value = rospy.get_param('/turtlebot2/max_laser_value') + self.min_laser_value = rospy.get_param('/turtlebot2/min_laser_value') + + # Get Desired Point to Get + self.desired_point = Point() + self.desired_point.x = rospy.get_param("/turtlebot2/desired_pose/x") + self.desired_point.y = rospy.get_param("/turtlebot2/desired_pose/y") + self.desired_point.z = rospy.get_param("/turtlebot2/desired_pose/z") + + # We create two arrays based on the binary values that will be assigned + # In the discretization method. + laser_scan = self.get_laser_scan() + rospy.logdebug("laser_scan len===>" + str(len(laser_scan.ranges))) + + num_laser_readings = int(len(laser_scan.ranges)/self.new_ranges) + high = numpy.full((num_laser_readings), self.max_laser_value) + low = numpy.full((num_laser_readings), self.min_laser_value) + + # We only use two integers + self.observation_space = spaces.Box(low, high) + + rospy.logdebug("ACTION SPACES TYPE===>"+str(self.action_space)) + rospy.logdebug("OBSERVATION SPACES TYPE===>"+str(self.observation_space)) + + # Rewards + self.forwards_reward = rospy.get_param("/turtlebot2/forwards_reward") + self.turn_reward = rospy.get_param("/turtlebot2/turn_reward") + self.end_episode_points = rospy.get_param("/turtlebot2/end_episode_points") + + self.cumulated_steps = 0.0 + + + + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + self.move_base( self.init_linear_forward_speed, + self.init_linear_turn_speed, + epsilon=0.05, + update_rate=10) + + return True + + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + # For Info Purposes + self.cumulated_reward = 0.0 + # Set to false Done, because its calculated asyncronously + self._episode_done = False + + odometry = self.get_odom() + self.previous_distance_from_des_point = self.get_distance_from_desired_point(odometry.pose.pose.position) + + + def _set_action(self, action): + """ + This set action will Set the linear and angular speed of the turtlebot2 + based on the action number given. + :param action: The action integer that set s what movement to do next. + """ + + rospy.logdebug("Start Set Action ==>"+str(action)) + # We convert the actions to speed movements to send to the parent class CubeSingleDiskEnv + if action == 0: #FORWARD + linear_speed = self.linear_forward_speed + angular_speed = 0.0 + self.last_action = "FORWARDS" + elif action == 1: #LEFT + linear_speed = self.linear_turn_speed + angular_speed = self.angular_speed + self.last_action = "TURN_LEFT" + elif action == 2: #RIGHT + linear_speed = self.linear_turn_speed + angular_speed = -1*self.angular_speed + self.last_action = "TURN_RIGHT" + + + # We tell TurtleBot2 the linear and angular speed to set to execute + self.move_base(linear_speed, angular_speed, epsilon=0.05, update_rate=10) + + rospy.logdebug("END Set Action ==>"+str(action)) + + def _get_obs(self): + """ + Here we define what sensor data defines our robots observations + To know which Variables we have acces to, we need to read the + TurtleBot2Env API DOCS + :return: + """ + rospy.logdebug("Start Get Observation ==>") + # We get the laser scan data + laser_scan = self.get_laser_scan() + + discretized_laser_scan = self.discretize_observation( laser_scan, + self.new_ranges + ) + + + # We get the odometry so that SumitXL knows where it is. + odometry = self.get_odom() + x_position = odometry.pose.pose.position.x + y_position = odometry.pose.pose.position.y + + # We round to only two decimals to avoid very big Observation space + odometry_array = [round(x_position, 2),round(y_position, 2)] + + # We only want the X and Y position and the Yaw + + observations = discretized_laser_scan + odometry_array + + rospy.logdebug("Observations==>"+str(observations)) + rospy.logdebug("END Get Observation ==>") + return observations + + + def _is_done(self, observations): + + if self._episode_done: + rospy.logerr("TurtleBot2 is Too Close to wall==>") + else: + rospy.logerr("TurtleBot2 didnt crash at least ==>") + + + current_position = Point() + current_position.x = observations[-2] + current_position.y = observations[-1] + current_position.z = 0.0 + + MAX_X = 6.0 + MIN_X = -1.0 + MAX_Y = 3.0 + MIN_Y = -3.0 + + # We see if we are outside the Learning Space + + if current_position.x <= MAX_X and current_position.x > MIN_X: + if current_position.y <= MAX_Y and current_position.y > MIN_Y: + rospy.logdebug("TurtleBot Position is OK ==>["+str(current_position.x)+","+str(current_position.y)+"]") + + # We see if it got to the desired point + if self.is_in_desired_position(current_position): + self._episode_done = True + + + else: + rospy.logerr("TurtleBot to Far in Y Pos ==>"+str(current_position.x)) + self._episode_done = True + else: + rospy.logerr("TurtleBot to Far in X Pos ==>"+str(current_position.x)) + self._episode_done = True + + + + + return self._episode_done + + def _compute_reward(self, observations, done): + + current_position = Point() + current_position.x = observations[-2] + current_position.y = observations[-1] + current_position.z = 0.0 + + distance_from_des_point = self.get_distance_from_desired_point(current_position) + distance_difference = distance_from_des_point - self.previous_distance_from_des_point + + + if not done: + + if self.last_action == "FORWARDS": + reward = self.forwards_reward + else: + reward = self.turn_reward + + # If there has been a decrease in the distance to the desired point, we reward it + if distance_difference < 0.0: + rospy.logwarn("DECREASE IN DISTANCE GOOD") + reward += self.forwards_reward + else: + rospy.logerr("ENCREASE IN DISTANCE BAD") + reward += 0 + + else: + + if self.is_in_desired_position(current_position): + reward = self.end_episode_points + else: + reward = -1*self.end_episode_points + + + self.previous_distance_from_des_point = distance_from_des_point + + + rospy.logdebug("reward=" + str(reward)) + self.cumulated_reward += reward + rospy.logdebug("Cumulated_reward=" + str(self.cumulated_reward)) + self.cumulated_steps += 1 + rospy.logdebug("Cumulated_steps=" + str(self.cumulated_steps)) + + return reward + + + # Internal TaskEnv Methods + + def discretize_observation(self,data,new_ranges): + """ + Discards all the laser readings that are not multiple in index of new_ranges + value. + """ + self._episode_done = False + + discretized_ranges = [] + mod = len(data.ranges)/new_ranges + + rospy.logdebug("data=" + str(data)) + rospy.logwarn("new_ranges=" + str(new_ranges)) + rospy.logwarn("mod=" + str(mod)) + + for i, item in enumerate(data.ranges): + if (i%mod==0): + if item == float ('Inf') or numpy.isinf(item): + discretized_ranges.append(self.max_laser_value) + elif numpy.isnan(item): + discretized_ranges.append(self.min_laser_value) + else: + discretized_ranges.append(int(item)) + + if (self.min_range > item > 0): + rospy.logerr("done Validation >>> item=" + str(item)+"< "+str(self.min_range)) + self._episode_done = True + else: + rospy.logwarn("NOT done Validation >>> item=" + str(item)+"< "+str(self.min_range)) + + + return discretized_ranges + + + def is_in_desired_position(self,current_position, epsilon=0.05): + """ + It return True if the current position is similar to the desired poistion + """ + + is_in_desired_pos = False + + + x_pos_plus = self.desired_point.x + epsilon + x_pos_minus = self.desired_point.x - epsilon + y_pos_plus = self.desired_point.y + epsilon + y_pos_minus = self.desired_point.y - epsilon + + x_current = current_position.x + y_current = current_position.y + + x_pos_are_close = (x_current <= x_pos_plus) and (x_current > x_pos_minus) + y_pos_are_close = (y_current <= y_pos_plus) and (y_current > y_pos_minus) + + is_in_desired_pos = x_pos_are_close and y_pos_are_close + + return is_in_desired_pos + + + def get_distance_from_desired_point(self, current_position): + """ + Calculates the distance from the current position to the desired point + :param start_point: + :return: + """ + distance = self.get_distance_from_point(current_position, + self.desired_point) + + return distance + + def get_distance_from_point(self, pstart, p_end): + """ + Given a Vector3 Object, get distance from current position + :param p_end: + :return: + """ + a = numpy.array((pstart.x, pstart.y, pstart.z)) + b = numpy.array((p_end.x, p_end.y, p_end.z)) + + distance = numpy.linalg.norm(a - b) + + return distance + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3/config/turtlebot3_world.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3/config/turtlebot3_world.yaml new file mode 100755 index 0000000..88745bb --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3/config/turtlebot3_world.yaml @@ -0,0 +1,24 @@ +turtlebot3: #namespace + + n_actions: 3 # We have 3 actions, Forwards,TurnLeft,TurnRight + n_observations: 6 # We have 6 different observations + + speed_step: 1.0 # Time to wait in the reset phases + + linear_forward_speed: 0.5 # Spwwed for ging fowards + linear_turn_speed: 0.05 # Lienare speed when turning + angular_speed: 0.3 # Angular speed when turning Left or Right + init_linear_forward_speed: 0.0 # Initial linear speed in which we start each episode + init_linear_turn_speed: 0.0 # Initial angular speed in shich we start each episode + + new_ranges: 5 # How many laser readings we jump in each observation reading, the bigger the less laser resolution + min_range: 0.2 # Minimum meters below wich we consider we have crashed + max_laser_value: 6 # Value considered Ok, no wall + min_laser_value: 0 # Value considered there is an obstacle or crashed + max_linear_aceleration: 5.0 # Linear acceleration value in which we consider Turtlebot 3 has crashed into something + + forwards_reward: 5 # Points Given to go forwards + turn_reward: 1 # Points Given to turn as action + end_episode_points: 200 # Points given when ending an episode + + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3/turtlebot3_world.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3/turtlebot3_world.py new file mode 100755 index 0000000..518ddf0 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3/turtlebot3_world.py @@ -0,0 +1,250 @@ +import rospy +import numpy +from gym import spaces +from openai_ros.robot_envs import turtlebot3_env +from gym.envs.registration import register +from geometry_msgs.msg import Vector3 +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +from openai_ros.openai_ros_common import ROSLauncher +import os + + +class TurtleBot3WorldEnv(turtlebot3_env.TurtleBot3Env): + def __init__(self): + """ + This Task Env is designed for having the TurtleBot3 in the turtlebot3 world + closed room with columns. + It will learn how to move around without crashing. + """ + # This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there + ros_ws_abspath = rospy.get_param("/turtlebot3/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name="turtlebot3_gazebo", + launch_file_name="start_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/turtlebot3/config", + yaml_file_name="turtlebot3_world.yaml") + + + # Here we will add any init functions prior to starting the MyRobotEnv + super(TurtleBot3WorldEnv, self).__init__(ros_ws_abspath) + + # Only variable needed to be set here + number_actions = rospy.get_param('/turtlebot3/n_actions') + self.action_space = spaces.Discrete(number_actions) + + # We set the reward range, which is not compulsory but here we do it. + self.reward_range = (-numpy.inf, numpy.inf) + + + #number_observations = rospy.get_param('/turtlebot3/n_observations') + """ + We set the Observation space for the 6 observations + cube_observations = [ + round(current_disk_roll_vel, 0), + round(y_distance, 1), + round(roll, 1), + round(pitch, 1), + round(y_linear_speed,1), + round(yaw, 1), + ] + """ + + # Actions and Observations + self.linear_forward_speed = rospy.get_param('/turtlebot3/linear_forward_speed') + self.linear_turn_speed = rospy.get_param('/turtlebot3/linear_turn_speed') + self.angular_speed = rospy.get_param('/turtlebot3/angular_speed') + self.init_linear_forward_speed = rospy.get_param('/turtlebot3/init_linear_forward_speed') + self.init_linear_turn_speed = rospy.get_param('/turtlebot3/init_linear_turn_speed') + + self.new_ranges = rospy.get_param('/turtlebot3/new_ranges') + self.min_range = rospy.get_param('/turtlebot3/min_range') + self.max_laser_value = rospy.get_param('/turtlebot3/max_laser_value') + self.min_laser_value = rospy.get_param('/turtlebot3/min_laser_value') + self.max_linear_aceleration = rospy.get_param('/turtlebot3/max_linear_aceleration') + + + # We create two arrays based on the binary values that will be assigned + # In the discretization method. + laser_scan = self.get_laser_scan() + + num_laser_readings = int(len(laser_scan.ranges)/self.new_ranges) + high = numpy.full((num_laser_readings), self.max_laser_value) + low = numpy.full((num_laser_readings), self.min_laser_value) + + # We only use two integers + self.observation_space = spaces.Box(low, high) + + rospy.logdebug("ACTION SPACES TYPE===>"+str(self.action_space)) + rospy.logdebug("OBSERVATION SPACES TYPE===>"+str(self.observation_space)) + + # Rewards + self.forwards_reward = rospy.get_param("/turtlebot3/forwards_reward") + self.turn_reward = rospy.get_param("/turtlebot3/turn_reward") + self.end_episode_points = rospy.get_param("/turtlebot3/end_episode_points") + + self.cumulated_steps = 0.0 + + + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + self.move_base( self.init_linear_forward_speed, + self.init_linear_turn_speed, + epsilon=0.05, + update_rate=10) + + return True + + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + # For Info Purposes + self.cumulated_reward = 0.0 + # Set to false Done, because its calculated asyncronously + self._episode_done = False + + + def _set_action(self, action): + """ + This set action will Set the linear and angular speed of the turtlebot2 + based on the action number given. + :param action: The action integer that set s what movement to do next. + """ + + rospy.logdebug("Start Set Action ==>"+str(action)) + # We convert the actions to speed movements to send to the parent class CubeSingleDiskEnv + if action == 0: #FORWARD + linear_speed = self.linear_forward_speed + angular_speed = 0.0 + self.last_action = "FORWARDS" + elif action == 1: #LEFT + linear_speed = self.linear_turn_speed + angular_speed = self.angular_speed + self.last_action = "TURN_LEFT" + elif action == 2: #RIGHT + linear_speed = self.linear_turn_speed + angular_speed = -1*self.angular_speed + self.last_action = "TURN_RIGHT" + + # We tell TurtleBot2 the linear and angular speed to set to execute + self.move_base(linear_speed, angular_speed, epsilon=0.05, update_rate=10) + + rospy.logdebug("END Set Action ==>"+str(action)) + + def _get_obs(self): + """ + Here we define what sensor data defines our robots observations + To know which Variables we have acces to, we need to read the + TurtleBot2Env API DOCS + :return: + """ + rospy.logdebug("Start Get Observation ==>") + # We get the laser scan data + laser_scan = self.get_laser_scan() + + discretized_observations = self.discretize_scan_observation( laser_scan, + self.new_ranges + ) + + rospy.logdebug("Observations==>"+str(discretized_observations)) + rospy.logdebug("END Get Observation ==>") + return discretized_observations + + + def _is_done(self, observations): + + if self._episode_done: + rospy.logerr("TurtleBot2 is Too Close to wall==>") + else: + rospy.logwarn("TurtleBot2 is NOT close to a wall ==>") + + # Now we check if it has crashed based on the imu + imu_data = self.get_imu() + linear_acceleration_magnitude = self.get_vector_magnitude(imu_data.linear_acceleration) + if linear_acceleration_magnitude > self.max_linear_aceleration: + rospy.logerr("TurtleBot2 Crashed==>"+str(linear_acceleration_magnitude)+">"+str(self.max_linear_aceleration)) + self._episode_done = True + else: + rospy.logerr("DIDNT crash TurtleBot2 ==>"+str(linear_acceleration_magnitude)+">"+str(self.max_linear_aceleration)) + + + return self._episode_done + + def _compute_reward(self, observations, done): + + if not done: + if self.last_action == "FORWARDS": + reward = self.forwards_reward + else: + reward = self.turn_reward + else: + reward = -1*self.end_episode_points + + + rospy.logdebug("reward=" + str(reward)) + self.cumulated_reward += reward + rospy.logdebug("Cumulated_reward=" + str(self.cumulated_reward)) + self.cumulated_steps += 1 + rospy.logdebug("Cumulated_steps=" + str(self.cumulated_steps)) + + return reward + + + # Internal TaskEnv Methods + + def discretize_scan_observation(self,data,new_ranges): + """ + Discards all the laser readings that are not multiple in index of new_ranges + value. + """ + self._episode_done = False + + discretized_ranges = [] + mod = len(data.ranges)/new_ranges + + rospy.logdebug("data=" + str(data)) + rospy.logdebug("new_ranges=" + str(new_ranges)) + rospy.logdebug("mod=" + str(mod)) + + for i, item in enumerate(data.ranges): + if (i%mod==0): + if item == float ('Inf') or numpy.isinf(item): + discretized_ranges.append(self.max_laser_value) + elif numpy.isnan(item): + discretized_ranges.append(self.min_laser_value) + else: + discretized_ranges.append(int(item)) + + if (self.min_range > item > 0): + rospy.logerr("done Validation >>> item=" + str(item)+"< "+str(self.min_range)) + self._episode_done = True + else: + rospy.logdebug("NOT done Validation >>> item=" + str(item)+"< "+str(self.min_range)) + + + return discretized_ranges + + + def get_vector_magnitude(self, vector): + """ + It calculated the magnitude of the Vector3 given. + This is usefull for reading imu accelerations and knowing if there has been + a crash + :return: + """ + contact_force_np = numpy.array((vector.x, vector.y, vector.z)) + force_magnitude = numpy.linalg.norm(contact_force_np) + + return force_magnitude diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom/config/turtlebot3_world.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom/config/turtlebot3_world.yaml new file mode 100755 index 0000000..88745bb --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom/config/turtlebot3_world.yaml @@ -0,0 +1,24 @@ +turtlebot3: #namespace + + n_actions: 3 # We have 3 actions, Forwards,TurnLeft,TurnRight + n_observations: 6 # We have 6 different observations + + speed_step: 1.0 # Time to wait in the reset phases + + linear_forward_speed: 0.5 # Spwwed for ging fowards + linear_turn_speed: 0.05 # Lienare speed when turning + angular_speed: 0.3 # Angular speed when turning Left or Right + init_linear_forward_speed: 0.0 # Initial linear speed in which we start each episode + init_linear_turn_speed: 0.0 # Initial angular speed in shich we start each episode + + new_ranges: 5 # How many laser readings we jump in each observation reading, the bigger the less laser resolution + min_range: 0.2 # Minimum meters below wich we consider we have crashed + max_laser_value: 6 # Value considered Ok, no wall + min_laser_value: 0 # Value considered there is an obstacle or crashed + max_linear_aceleration: 5.0 # Linear acceleration value in which we consider Turtlebot 3 has crashed into something + + forwards_reward: 5 # Points Given to go forwards + turn_reward: 1 # Points Given to turn as action + end_episode_points: 200 # Points given when ending an episode + + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom/turtlebot3_world.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom/turtlebot3_world.py new file mode 100755 index 0000000..a4a024a --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom/turtlebot3_world.py @@ -0,0 +1,251 @@ +import rospy +import numpy +from gym import spaces +from openai_ros.robot_envs import turtlebot3_env_custom +from gym.envs.registration import register +from geometry_msgs.msg import Vector3 +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +from openai_ros.openai_ros_common import ROSLauncher +import os + + +class TurtleBot3WorldEnv(turtlebot3_env_custom.TurtleBot3Env): + def __init__(self): + """ + This Task Env is designed for having the TurtleBot3 in the turtlebot3 world + closed room with columns. + It will learn how to move around without crashing. + """ + # This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there + ros_ws_abspath = rospy.get_param("/turtlebot3/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + # ROSLauncher(rospackage_name="turtlebot3_gazebo", + # launch_file_name="start_world.launch", + # ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/turtlebot3_custom/config", + yaml_file_name="turtlebot3_world.yaml") + + + # Here we will add any init functions prior to starting the MyRobotEnv + super(TurtleBot3WorldEnv, self).__init__(ros_ws_abspath) + + # Only variable needed to be set here + number_actions = rospy.get_param('/turtlebot3/n_actions') + self.action_space = spaces.Discrete(number_actions) + + # We set the reward range, which is not compulsory but here we do it. + self.reward_range = (-numpy.inf, numpy.inf) + + + #number_observations = rospy.get_param('/turtlebot3/n_observations') + """ + We set the Observation space for the 6 observations + cube_observations = [ + round(current_disk_roll_vel, 0), + round(y_distance, 1), + round(roll, 1), + round(pitch, 1), + round(y_linear_speed,1), + round(yaw, 1), + ] + """ + + # Actions and Observations + self.linear_forward_speed = rospy.get_param('/turtlebot3/linear_forward_speed') + self.linear_turn_speed = rospy.get_param('/turtlebot3/linear_turn_speed') + self.angular_speed = rospy.get_param('/turtlebot3/angular_speed') + self.init_linear_forward_speed = rospy.get_param('/turtlebot3/init_linear_forward_speed') + self.init_linear_turn_speed = rospy.get_param('/turtlebot3/init_linear_turn_speed') + + self.new_ranges = rospy.get_param('/turtlebot3/new_ranges') + self.min_range = rospy.get_param('/turtlebot3/min_range') + self.max_laser_value = rospy.get_param('/turtlebot3/max_laser_value') + self.min_laser_value = rospy.get_param('/turtlebot3/min_laser_value') + self.max_linear_aceleration = rospy.get_param('/turtlebot3/max_linear_aceleration') + + + # We create two arrays based on the binary values that will be assigned + # In the discretization method. + laser_scan = self.get_laser_scan() + rospy.logwarn("LASER SCAN " + str(laser_scan)) + num_laser_readings = int(len(laser_scan.ranges)/self.new_ranges) + high = numpy.full((num_laser_readings), self.max_laser_value) + low = numpy.full((num_laser_readings), self.min_laser_value) + + # We only use two integers + self.observation_space = spaces.Box(low, high) + + rospy.logdebug("ACTION SPACES TYPE===>"+str(self.action_space)) + rospy.logdebug("OBSERVATION SPACES TYPE===>"+str(self.observation_space)) + + # Rewards + self.forwards_reward = rospy.get_param("/turtlebot3/forwards_reward") + self.turn_reward = rospy.get_param("/turtlebot3/turn_reward") + self.end_episode_points = rospy.get_param("/turtlebot3/end_episode_points") + + self.cumulated_steps = 0.0 + + + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + self.move_base( self.init_linear_forward_speed, + self.init_linear_turn_speed, + epsilon=0.05, + update_rate=10) + + return True + + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + # For Info Purposes + self.cumulated_reward = 0.0 + # Set to false Done, because its calculated asyncronously + self._episode_done = False + + + def _set_action(self, action): + """ + This set action will Set the linear and angular speed of the turtlebot2 + based on the action number given. + :param action: The action integer that set s what movement to do next. + """ + + rospy.logdebug("Start Set Action ==>"+str(action)) + # We convert the actions to speed movements to send to the parent class CubeSingleDiskEnv + if action == 0: #FORWARD + linear_speed = self.linear_forward_speed + angular_speed = 0.0 + self.last_action = "FORWARDS" + elif action == 1: #LEFT + linear_speed = self.linear_turn_speed + angular_speed = self.angular_speed + self.last_action = "TURN_LEFT" + elif action == 2: #RIGHT + linear_speed = self.linear_turn_speed + angular_speed = -1*self.angular_speed + self.last_action = "TURN_RIGHT" + + # We tell TurtleBot2 the linear and angular speed to set to execute + self.move_base(linear_speed, angular_speed, epsilon=0.05, update_rate=10) + + rospy.logdebug("END Set Action ==>"+str(action)) + + def _get_obs(self): + """ + Here we define what sensor data defines our robots observations + To know which Variables we have acces to, we need to read the + TurtleBot2Env API DOCS + :return: + """ + rospy.logdebug("Start Get Observation ==>") + # We get the laser scan data + laser_scan = self.get_laser_scan() + + + discretized_observations = self.discretize_scan_observation( laser_scan, + self.new_ranges + ) + + rospy.logdebug("Observations==>"+str(discretized_observations)) + rospy.logdebug("END Get Observation ==>") + return discretized_observations + + + def _is_done(self, observations): + + if self._episode_done: + rospy.logerr("TurtleBot2 is Too Close to wall==>") + else: + rospy.logwarn("TurtleBot2 is NOT close to a wall ==>") + + # Now we check if it has crashed based on the imu + imu_data = self.get_imu() + linear_acceleration_magnitude = self.get_vector_magnitude(imu_data.linear_acceleration) + if linear_acceleration_magnitude > self.max_linear_aceleration: + rospy.logerr("TurtleBot2 Crashed==>"+str(linear_acceleration_magnitude)+">"+str(self.max_linear_aceleration)) + self._episode_done = True + else: + rospy.logerr("DIDNT crash TurtleBot2 ==>"+str(linear_acceleration_magnitude)+">"+str(self.max_linear_aceleration)) + + + return self._episode_done + + def _compute_reward(self, observations, done): + + if not done: + if self.last_action == "FORWARDS": + reward = self.forwards_reward + else: + reward = self.turn_reward + else: + reward = -1*self.end_episode_points + + + rospy.logdebug("reward=" + str(reward)) + self.cumulated_reward += reward + rospy.logdebug("Cumulated_reward=" + str(self.cumulated_reward)) + self.cumulated_steps += 1 + rospy.logdebug("Cumulated_steps=" + str(self.cumulated_steps)) + + return reward + + + # Internal TaskEnv Methods + + def discretize_scan_observation(self,data,new_ranges): + """ + Discards all the laser readings that are not multiple in index of new_ranges + value. + """ + self._episode_done = False + + discretized_ranges = [] + mod = len(data.ranges)/new_ranges + + rospy.logdebug("data=" + str(data)) + rospy.logdebug("new_ranges=" + str(new_ranges)) + rospy.logdebug("mod=" + str(mod)) + + for i, item in enumerate(data.ranges): + if (i%mod==0): + if item == float ('Inf') or numpy.isinf(item): + discretized_ranges.append(self.max_laser_value) + elif numpy.isnan(item): + discretized_ranges.append(self.min_laser_value) + else: + discretized_ranges.append(int(item)) + + if (self.min_range > item > 0): + rospy.logerr("done Validation >>> item=" + str(item)+"< "+str(self.min_range)) + self._episode_done = True + else: + rospy.logdebug("NOT done Validation >>> item=" + str(item)+"< "+str(self.min_range)) + + + return discretized_ranges + + + def get_vector_magnitude(self, vector): + """ + It calculated the magnitude of the Vector3 given. + This is usefull for reading imu accelerations and knowing if there has been + a crash + :return: + """ + contact_force_np = numpy.array((vector.x, vector.y, vector.z)) + force_magnitude = numpy.linalg.norm(contact_force_np) + + return force_magnitude diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom_a2c/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom_a2c/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom_a2c/config/turtlebot3_world.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom_a2c/config/turtlebot3_world.yaml new file mode 100755 index 0000000..53663a7 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom_a2c/config/turtlebot3_world.yaml @@ -0,0 +1,35 @@ +turtlebot3: #namespace + +# n_actions: 3 # We have 3 actions, Forwards,TurnLeft,TurnRight +# n_observations: 6 # We have 6 different observations + + n_actions: 78848 #46080 + n_observations: 236544 #138240 + + speed_step: 1.0 # Time to wait in the reset phases + + linear_forward_speed: 0.5 # Spwwed for ging fowards + linear_turn_speed: 0.05 # Lienare speed when turning + angular_speed: 0.3 # Angular speed when turning Left or Right + init_linear_forward_speed: 0.0 # Initial linear speed in which we start each episode + init_linear_turn_speed: 0.0 # Initial angular speed in shich we start each episode + + new_ranges: 5 # How many laser readings we jump in each observation reading, the bigger the less laser resolution + min_range: 0.2 # Minimum meters below wich we consider we have crashed + max_laser_value: 6 # Value considered Ok, no wall + min_laser_value: 0 # Value considered there is an obstacle or crashed + max_linear_aceleration: 5.0 # Linear acceleration value in which we consider Turtlebot 3 has crashed into something + +# forwards_reward: 5 # Points Given to go forwards +# turn_reward: 1 # Points Given to turn as action +# end_episode_points: 200 # Points given when ending an episode + unoccupied_reward: 400 + unknown_reward: 5 + occupied_reward: 200 + end_episode_points: 500 + open_cell_found_points: 50 + occupied_cell_found_points: 25 + travel_time_points: 1 + near_wall_points: 50 + + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom_a2c/turtlebot3_world.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom_a2c/turtlebot3_world.py new file mode 100755 index 0000000..295f3d3 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/turtlebot3_custom_a2c/turtlebot3_world.py @@ -0,0 +1,503 @@ +import rospy +import numpy +from gym import spaces +from openai_ros.robot_envs import turtlebot3_env_custom_a2c +from gym.envs.registration import register +from std_msgs.msg import Header +from geometry_msgs.msg import Vector3, Point, PoseStamped, Pose +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +from nav_msgs.msg import Odometry, OccupancyGrid, MapMetaData +from openai_ros.openai_ros_common import ROSLauncher +import rosnode +import os +import time + + +class TurtleBot3WorldEnv(turtlebot3_env_custom_a2c.TurtleBot3Env): + def __init__(self): + """ + This Task Env is designed for having the TurtleBot3 in the turtlebot3 world + closed room with columns. + It will learn how to move around without crashing. + """ + # This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there + ros_ws_abspath = rospy.get_param("/turtlebot3/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + # ROSLauncher(rospackage_name="turtlebot3_gazebo", + # launch_file_name="start_world.launch", + # ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/turtlebot3_custom_a2c/config", + yaml_file_name="turtlebot3_world.yaml") + + + # Here we will add any init functions prior to starting the MyRobotEnv + super(TurtleBot3WorldEnv, self).__init__(ros_ws_abspath) + + # Only variable needed to be set here + number_actions = rospy.get_param('/turtlebot3/n_actions') + self.action_space = spaces.Discrete(number_actions) + + # We set the reward range, which is not compulsory but here we do it. + self.reward_range = (-numpy.inf, numpy.inf) + + + #number_observations = rospy.get_param('/turtlebot3/n_observations') + """ + We set the Observation space for the 6 observations + cube_observations = [ + round(current_disk_roll_vel, 0), + round(y_distance, 1), + round(roll, 1), + round(pitch, 1), + round(y_linear_speed,1), + round(yaw, 1), + ] + """ + + # Actions and Observations + self.linear_forward_speed = rospy.get_param('/turtlebot3/linear_forward_speed') + self.linear_turn_speed = rospy.get_param('/turtlebot3/linear_turn_speed') + self.angular_speed = rospy.get_param('/turtlebot3/angular_speed') + self.init_linear_forward_speed = rospy.get_param('/turtlebot3/init_linear_forward_speed') + self.init_linear_turn_speed = rospy.get_param('/turtlebot3/init_linear_turn_speed') + + self.new_ranges = rospy.get_param('/turtlebot3/new_ranges') + self.min_range = rospy.get_param('/turtlebot3/min_range') + self.max_laser_value = rospy.get_param('/turtlebot3/max_laser_value') + self.min_laser_value = rospy.get_param('/turtlebot3/min_laser_value') + self.max_linear_aceleration = rospy.get_param('/turtlebot3/max_linear_aceleration') + + + # We create two arrays based on the binary values that will be assigned + # In the discretization method. + # laser_scan = self.get_laser_scan() + # rospy.logwarn("LASER SCAN " + str(laser_scan)) + # num_laser_readings = int(len(laser_scan.ranges)/self.new_ranges) + # high = numpy.full((num_laser_readings), self.max_laser_value) + # low = numpy.full((num_laser_readings), self.min_laser_value) + # + # # We only use two integers + # self.observation_space = spaces.Box(low, high) + number_observations = rospy.get_param('/turtlebot3/n_observations') + self.observation_space = spaces.Discrete(number_observations) + + + print("ACTION SPACES TYPE===>"+str(self.action_space)) + print("OBSERVATION SPACES TYPE===>"+str(self.observation_space)) + + # Rewards + # self.forwards_reward = 5 #rospy.get_param("/turtlebot3/forwards_reward") + # self.turn_reward = 5 #rospy.get_param("/turtlebot3/turn_reward") + # self.end_episode_points = 200 #rospy.get_param("/turtlebot3/end_episode_points") + + self.unoccupied_reward = rospy.get_param("/turtlebot3/unoccupied_reward") + self.unknown_reward = rospy.get_param("/turtlebot3/unknown_reward") + self.occupied_reward = rospy.get_param("/turtlebot3/occupied_reward") + self.end_episode_points = rospy.get_param("/turtlebot3/end_episode_points") + self.open_cell_found_points = rospy.get_param("/turtlebot3/open_cell_found_points") + self.occupied_cell_found_points = rospy.get_param("/turtlebot3/occupied_cell_found_points") + self.travel_time_points = rospy.get_param("/turtlebot3/travel_time_points") + self.near_wall_points = rospy.get_param("/turtlebot3/near_wall_points") + + self.cumulated_steps = 0.0 + + self.first_ep = True + + # self.episode_start_time = rospy.get_time() + self.episode_start_time = time.perf_counter() + self.max_ep_time = 2700 #5 #1000 # 1800 * (3/60) + + + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + self.move_base( self.init_linear_forward_speed, + self.init_linear_turn_speed, + epsilon=0.05, + update_rate=10) + + return True + + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + # For Info Purposes + self.cumulated_reward = 0.0 + # Set to false Done, because its calculated asyncronously + self._episode_done = False + + + def _set_action(self, action): + """ + This set action will Set the linear and angular speed of the turtlebot2 + based on the action number given. + :param action: The action integer that set s what movement to do next. + """ + + print("ACTION: " + str(action)) + + while not self.get_map_set(): + rospy.logwarn("Map not set yet - waiting one second") + rospy.sleep(1) + print("Map set - continuing with choosing action") + + map = self.get_map() + + if self.is_valid_point(action, map): + pt = self.index_to_point(action, map) + + if self.get_occupied_neighbors((pt[0],pt[1]), map): + rospy.logwarn("NOT SENDING TO POINT BECAUSE NEXT TO WALL") + + else: + real_point = self.map_to_world(pt[0], pt[1], map) + + rospy.logwarn("SENDING TO POINT: " + str(real_point)) + + self.send_to_location(real_point) + + else: + rospy.logwarn("ACTION INDEX POINT IS NOT VALID - MOVING ON") + + + rospy.loginfo("END Set Action ==> "+ str(action)) + self.last_action = action + self.last_action_start_time = rospy.get_time() + + # rospy.logdebug("Start Set Action ==>"+str(action)) + # action = 0 + # # We convert the actions to speed movements to send to the parent class CubeSingleDiskEnv + # if action == 0: #FORWARD + # linear_speed = self.linear_forward_speed + # angular_speed = 0.0 + # self.last_action = "FORWARDS" + # elif action == 1: #LEFT + # linear_speed = self.linear_turn_speed + # angular_speed = self.angular_speed + # self.last_action = "TURN_LEFT" + # elif action == 2: #RIGHT + # linear_speed = self.linear_turn_speed + # angular_speed = -1*self.angular_speed + # self.last_action = "TURN_RIGHT" + # + # # We tell TurtleBot2 the linear and angular speed to set to execute + # self.move_base(linear_speed, angular_speed, epsilon=0.05, update_rate=10) + # + # rospy.logdebug("END Set Action ==>"+str(action)) + + def _get_obs(self): + """ + Here we define what sensor data defines our robots observations + To know which Variables we have acces to, we need to read the + TurtleBot2Env API DOCS + :return: + """ + # rospy.logdebug("Start Get Observation ==>") + # # We get the laser scan data + # laser_scan = self.get_laser_scan() + # + # discretized_observations = self.discretize_scan_observation(laser_scan, self.new_ranges) + # + # print("Observations==>"+str(discretized_observations)) + # print("END Get Observation ==>") + # + # return discretized_observations + + if not self.get_map_set() or not self.get_frontier_map_set() or not self.get_pose_map_set(): + + rospy.logwarn("Observations all made occupied out because not set yet") + # obs = numpy.zeros(self.observation_space.n) + obs = numpy.full(self.observation_space.n, 100) + return obs + + else: + + print("Observation maps set - continuing with creating observations") + + observation1 = self.get_frontier_map() + observation2 = self.get_map() + observation3 = self.get_pose_map() + + full_obs = numpy.array(observation1.data) + full_obs = numpy.append(full_obs, numpy.array(observation2.data)) + full_obs = numpy.append(full_obs, numpy.array(observation3.data)) + + return full_obs + + + def get_obs(self): + return self._get_obs() + + + def _is_done(self, observations): + + if self._episode_done: + rospy.logerr("Turtlebot is ending episode") + else: + rospy.logwarn("TurtleBot is continuing with episode") + + # Now we check if it has crashed based on the imu + imu_data = self.get_imu() + linear_acceleration_magnitude = self.get_vector_magnitude(imu_data.linear_acceleration) + if linear_acceleration_magnitude > self.max_linear_aceleration: + rospy.logerr("TurtleBot2 Crashed==>"+str(linear_acceleration_magnitude)+">"+str(self.max_linear_aceleration)) + self._episode_done = True + else: + rospy.logerr("DIDNT crash TurtleBot2 ==>"+str(linear_acceleration_magnitude)+">"+str(self.max_linear_aceleration)) + + if self.get_frontier_map_set(): + fringe_data = numpy.array(self.get_frontier_map().data) + if 0 not in fringe_data: + rospy.logerr("Turtlebot has no frontiers left ====>") + self._episode_done = True + + # print("Time left in episode: " + str(self.max_ep_time - (rospy.get_time() - self.episode_start_time)) + " seconds") + # if rospy.get_time() - self.episode_start_time > self.max_ep_time: + # rospy.logerr("Turtlebot is ending episode due to max time being reached") + # self._episode_done = True + + print("Time left in episode: " + str(self.max_ep_time - (time.perf_counter() - self.episode_start_time)) + " seconds") + if time.perf_counter() - self.episode_start_time > self.max_ep_time: + rospy.logerr("Turtlebot is ending episode due to max time being reached") + self._episode_done = True + + return self._episode_done + + + def _compute_reward(self, observations, done): + + if self.first_ep: + reward = -1 * self.end_episode_points + if self.get_map_set(): + self.last_computed_reward_map = numpy.array(self.get_map().data) + else: + self.last_computed_reward_map = numpy.full(self.observation_space.n/3, 100) + self.first_ep = False + + else: + if not done and self.get_map_set(): + map = self.get_map() + data = map.data + + occupancy = data[int(self.last_action)] + if occupancy == -1: + reward = self.unknown_reward + elif occupancy == 0: + reward = self.unoccupied_reward + elif occupancy > 0: + # penalty + reward = -1 * self.occupied_reward + + n_open_prev_map = numpy.count_nonzero(self.last_computed_reward_map == 0) + n_open_this_map = numpy.count_nonzero(numpy.array(data) == 0) + + n_occupied_prev_map = numpy.count_nonzero(self.last_computed_reward_map == 100) + n_occupied_this_map = numpy.count_nonzero(numpy.array(data) == 100) + + amount_of_open_cells_discovered = n_open_this_map - n_open_prev_map + amount_of_occupied_cells_discovered = n_occupied_this_map - n_occupied_prev_map + + reward += amount_of_open_cells_discovered*self.open_cell_found_points + amount_of_occupied_cells_discovered*self.occupied_cell_found_points + + # penalty + time_reward = -1 * (rospy.get_time() - self.last_action_start_time) * self.travel_time_points + reward += time_reward + + print("time reward: " + str(time_reward)) + + # penalty + pt = self.index_to_point(self.last_action, map) + if self.get_occupied_neighbors((pt[0], pt[1]), map): + near_wall_points = -1 * self.near_wall_points + reward += near_wall_points + print("near_wall reward: " + str(near_wall_points)) + + self.last_computed_reward_map = numpy.array(data) + + else: + reward = -1*self.end_episode_points + + # if not done: + # if self.last_action == "FORWARDS": + # reward = self.forwards_reward + # else: + # reward = self.turn_reward + # else: + # reward = -1 * self.end_episode_points + + + rospy.loginfo("reward=" + str(reward)) + print("reward=" + str(reward)) + self.cumulated_reward += reward + rospy.loginfo("Cumulated_reward=" + str(self.cumulated_reward)) + print("Cumulated_reward=" + str(self.cumulated_reward)) + self.cumulated_steps += 1 + rospy.loginfo("Cumulated_steps=" + str(self.cumulated_steps)) + print("Cumulated_steps=" + str(self.cumulated_steps)) + + return reward + + + # Internal TaskEnv Methods + + def discretize_scan_observation(self,data,new_ranges): + """ + Discards all the laser readings that are not multiple in index of new_ranges + value. + """ + self._episode_done = False + + discretized_ranges = [] + mod = len(data.ranges)/new_ranges + + rospy.logdebug("data=" + str(data)) + rospy.logdebug("new_ranges=" + str(new_ranges)) + rospy.logdebug("mod=" + str(mod)) + + for i, item in enumerate(data.ranges): + if (i%mod==0): + if item == float ('Inf') or numpy.isinf(item): + discretized_ranges.append(self.max_laser_value) + elif numpy.isnan(item): + discretized_ranges.append(self.min_laser_value) + else: + discretized_ranges.append(int(item)) + + if (self.min_range > item > 0): + rospy.logerr("done Validation >>> item=" + str(item)+"< "+str(self.min_range)) + self._episode_done = True + else: + rospy.logdebug("NOT done Validation >>> item=" + str(item)+"< "+str(self.min_range)) + + + return discretized_ranges + + + def get_vector_magnitude(self, vector): + """ + It calculated the magnitude of the Vector3 given. + This is usefull for reading imu accelerations and knowing if there has been + a crash + :return: + """ + contact_force_np = numpy.array((vector.x, vector.y, vector.z)) + force_magnitude = numpy.linalg.norm(contact_force_np) + + return force_magnitude + + + def index_to_point(self, index, my_map): + x = index % int(my_map.info.width) + y = (index - x)/my_map.info.width + return (x,y) + + + def map_to_world(self, x, y, my_map): + """ + converts a point from the map to the world + :param x: float of x position + :param y: float of y position + :return: tuple of converted point + """ + resolution = my_map.info.resolution + + originX = my_map.info.origin.position.x + originY = my_map.info.origin.position.y + + # multiply by resolution, then move by origin offset + x = x * resolution + originX + resolution / 2 + y = y * resolution + originY + resolution / 2 + return (x, y) + + + def is_valid_point(self, index, map): + data = map.data + if data is not None and len(data) >= index: + if data[index] > 95 or data[index] < 0: + # print("occupied or unknown space") + return False + else: + return True + else: + # print("data is none") + return False + + def is_occupied_point(self, index, map): + data = map.data + if data is not None and len(data) >= index: + if data[index] > 95: + print("occupied space") + return True + else: + return False + else: + # print("data is none") + return False + + def get_occupied_neighbors(self, loc, my_map): + + # my_map: http://docs.ros.org/melodic/api/nav_msgs/html/msg/OccupancyGrid.html + """ + returns the legal neighbors of loc + :param loc: tuple of location + :return: list of tuples + """ + # get x and y coordinates + Xloc = loc[0] + Yloc = loc[1] + + # relative locations to look at + neighbors = list() + potentNeighbors = [] + case1 = (Xloc + 1, Yloc) + case2 = (Xloc - 1, Yloc) + case3 = (Xloc, Yloc + 1) + case4 = (Xloc, Yloc - 1) + + # these are the nodes we want to check + potentNeighbors.append(case1) + potentNeighbors.append(case2) + potentNeighbors.append(case3) + potentNeighbors.append(case4) + + # only return valid locations (i.e. not walls) + for neighbor in potentNeighbors: + neighbor_index = int(neighbor[1] * my_map.info.width + neighbor[0]) + if neighbor_index >= my_map.info.width * my_map.info.height: + continue + elif self.is_occupied_point(neighbor_index, my_map): + neighbors.append(neighbor) + + return neighbors + + + def reset_gmapping(self): + print("RESETTING GMAPPING") + self.pub_reset_gmapping() + self.first_ep = True + + # while not self.gmapping_fully_reset: + # pass + # print("RESETTING GMAPPING FINALIZED") + # self.gmapping_fully_reset = False + + def set_start_time(self): + # self.episode_start_time = rospy.get_time() + self.episode_start_time = time.perf_counter() + + + + + + diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/wamv/__init__.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/wamv/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/wamv/config/wamv_nav_twosets_buoys.yaml b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/wamv/config/wamv_nav_twosets_buoys.yaml new file mode 100644 index 0000000..1a4a424 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/wamv/config/wamv_nav_twosets_buoys.yaml @@ -0,0 +1,24 @@ +wamv: #namespace + n_actions: 4 # Forwards, Backwards, TurnLeft and TurnRight + + propeller_high_speed: 1.0 # High Propeller Speed + propeller_low_speed: 0.2 # Low Propeller Speed + max_angular_speed: 1.0 # Maximum Base Turn Angular speed + + work_space: # 3D cube in which Drone is allowed to move + x_max: 30.0 + x_min: -1.0 + y_max: 5.0 + y_min: -5.0 + + desired_point: + x: 30.0 + y: 0.0 + z: 1.0 + desired_point_epsilon: 0.5 + max_distance_from_des_point: 30.0 # Maximum distance regitered in observations, has to be bigger than the largest distance of the workspace. + + number_decimals_precision_obs: 1 + + done_reward: 1000.0 # reward + closer_to_point_reward: 100.0 # reward \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/wamv/wamv_nav_twosets_buoys.py b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/wamv/wamv_nav_twosets_buoys.py new file mode 100644 index 0000000..18e4ac9 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/src/openai_ros/task_envs/wamv/wamv_nav_twosets_buoys.py @@ -0,0 +1,366 @@ +import rospy +import numpy +from gym import spaces +from openai_ros.robot_envs import wamv_env +from gym.envs.registration import register +from geometry_msgs.msg import Point +from geometry_msgs.msg import Vector3 +from tf.transformations import euler_from_quaternion +from openai_ros.task_envs.task_commons import LoadYamlFileParamsTest +from openai_ros.openai_ros_common import ROSLauncher +import os + +class WamvNavTwoSetsBuoysEnv(wamv_env.WamvEnv): + def __init__(self): + """ + Make Wamv learn how to move straight from The starting point + to a desired point inside the designed corridor. + http://robotx.org/images/files/RobotX_2018_Task_Summary.pdf + Demonstrate Navigation Control + """ + + # This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there + ros_ws_abspath = rospy.get_param("/wamv/ros_ws_abspath", None) + assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'" + assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \ + " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \ + "/src;cd " + ros_ws_abspath + ";catkin_make" + + ROSLauncher(rospackage_name="robotx_gazebo", + launch_file_name="start_world.launch", + ros_ws_abspath=ros_ws_abspath) + + # Load Params from the desired Yaml file + LoadYamlFileParamsTest(rospackage_name="openai_ros", + rel_path_from_package_to_file="src/openai_ros/task_envs/wamv/config", + yaml_file_name="wamv_nav_twosets_buoys.yaml") + + # Here we will add any init functions prior to starting the MyRobotEnv + super(WamvNavTwoSetsBuoysEnv, self).__init__(ros_ws_abspath) + + # Only variable needed to be set here + + rospy.logdebug("Start WamvNavTwoSetsBuoysEnv INIT...") + number_actions = rospy.get_param('/wamv/n_actions') + self.action_space = spaces.Discrete(number_actions) + + # We set the reward range, which is not compulsory but here we do it. + self.reward_range = (-numpy.inf, numpy.inf) + + + # Actions and Observations + self.propeller_high_speed = rospy.get_param('/wamv/propeller_high_speed') + self.propeller_low_speed = rospy.get_param('/wamv/propeller_low_speed') + self.max_angular_speed = rospy.get_param('/wamv/max_angular_speed') + self.max_distance_from_des_point = rospy.get_param('/wamv/max_distance_from_des_point') + + # Get Desired Point to Get + self.desired_point = Point() + self.desired_point.x = rospy.get_param("/wamv/desired_point/x") + self.desired_point.y = rospy.get_param("/wamv/desired_point/y") + self.desired_point.z = rospy.get_param("/wamv/desired_point/z") + self.desired_point_epsilon = rospy.get_param("/wamv/desired_point_epsilon") + + self.work_space_x_max = rospy.get_param("/wamv/work_space/x_max") + self.work_space_x_min = rospy.get_param("/wamv/work_space/x_min") + self.work_space_y_max = rospy.get_param("/wamv/work_space/y_max") + self.work_space_y_min = rospy.get_param("/wamv/work_space/y_min") + + self.dec_obs = rospy.get_param("/wamv/number_decimals_precision_obs") + + + # We place the Maximum and minimum values of observations + + high = numpy.array([self.work_space_x_max, + self.work_space_y_max, + 1.57, + 1.57, + 3.14, + self.propeller_high_speed, + self.propeller_high_speed, + self.max_angular_speed, + self.max_distance_from_des_point + ]) + + low = numpy.array([ self.work_space_x_min, + self.work_space_y_min, + -1*1.57, + -1*1.57, + -1*3.14, + -1*self.propeller_high_speed, + -1*self.propeller_high_speed, + -1*self.max_angular_speed, + 0.0 + ]) + + + self.observation_space = spaces.Box(low, high) + + rospy.logdebug("ACTION SPACES TYPE===>"+str(self.action_space)) + rospy.logdebug("OBSERVATION SPACES TYPE===>"+str(self.observation_space)) + + # Rewards + + self.done_reward =rospy.get_param("/wamv/done_reward") + self.closer_to_point_reward = rospy.get_param("/wamv/closer_to_point_reward") + + self.cumulated_steps = 0.0 + + rospy.logdebug("END WamvNavTwoSetsBuoysEnv INIT...") + + def _set_init_pose(self): + """ + Sets the two proppelers speed to 0.0 and waits for the time_sleep + to allow the action to be executed + """ + + right_propeller_speed = 0.0 + left_propeller_speed = 0.0 + self.set_propellers_speed( right_propeller_speed, + left_propeller_speed, + time_sleep=1.0) + + return True + + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + + # For Info Purposes + self.cumulated_reward = 0.0 + # We get the initial pose to mesure the distance from the desired point. + odom = self.get_odom() + current_position = Vector3() + current_position.x = odom.pose.pose.position.x + current_position.y = odom.pose.pose.position.y + self.previous_distance_from_des_point = self.get_distance_from_desired_point(current_position) + + + + def _set_action(self, action): + """ + It sets the joints of wamv based on the action integer given + based on the action number given. + :param action: The action integer that sets what movement to do next. + """ + + rospy.logdebug("Start Set Action ==>"+str(action)) + + + right_propeller_speed = 0.0 + left_propeller_speed = 0.0 + + if action == 0: # Go Forwards + right_propeller_speed = self.propeller_high_speed + left_propeller_speed = self.propeller_high_speed + elif action == 1: # Go BackWards + right_propeller_speed = -1*self.propeller_high_speed + left_propeller_speed = -1*self.propeller_high_speed + elif action == 2: # Turn Left + right_propeller_speed = self.propeller_high_speed + left_propeller_speed = -1*self.propeller_high_speed + elif action == 3: # Turn Right + right_propeller_speed = -1*self.propeller_high_speed + left_propeller_speed = self.propeller_high_speed + + + # We tell wamv the propeller speeds + self.set_propellers_speed( right_propeller_speed, + left_propeller_speed, + time_sleep=1.0) + + rospy.logdebug("END Set Action ==>"+str(action)) + + def _get_obs(self): + """ + Here we define what sensor data defines our robots observations + To know which Variables we have access to, we need to read the + WamvEnv API DOCS. + :return: observation + """ + rospy.logdebug("Start Get Observation ==>") + + odom = self.get_odom() + base_position = odom.pose.pose.position + base_orientation_quat = odom.pose.pose.orientation + base_roll, base_pitch, base_yaw = self.get_orientation_euler(base_orientation_quat) + base_speed_linear = odom.twist.twist.linear + base_speed_angular_yaw = odom.twist.twist.angular.z + + distance_from_desired_point = self.get_distance_from_desired_point(base_position) + + observation = [] + observation.append(round(base_position.x,self.dec_obs)) + observation.append(round(base_position.y,self.dec_obs)) + + observation.append(round(base_roll,self.dec_obs)) + observation.append(round(base_pitch,self.dec_obs)) + observation.append(round(base_yaw,self.dec_obs)) + + observation.append(round(base_speed_linear.x,self.dec_obs)) + observation.append(round(base_speed_linear.y,self.dec_obs)) + + observation.append(round(base_speed_angular_yaw,self.dec_obs)) + + observation.append(round(distance_from_desired_point,self.dec_obs)) + + return observation + + + def _is_done(self, observations): + """ + We consider the episode done if: + 1) The wamvs is ouside the workspace + 2) It got to the desired point + """ + distance_from_desired_point = observations[8] + + current_position = Vector3() + current_position.x = observations[0] + current_position.y = observations[1] + + is_inside_corridor = self.is_inside_workspace(current_position) + has_reached_des_point = self.is_in_desired_position(current_position, self.desired_point_epsilon) + + done = not(is_inside_corridor) or has_reached_des_point + + return done + + def _compute_reward(self, observations, done): + """ + We Base the rewards in if its done or not and we base it on + if the distance to the desired point has increased or not + :return: + """ + + # We only consider the plane, the fluctuation in z is due mainly to wave + current_position = Point() + current_position.x = observations[0] + current_position.y = observations[1] + + distance_from_des_point = self.get_distance_from_desired_point(current_position) + distance_difference = distance_from_des_point - self.previous_distance_from_des_point + + + if not done: + + # If there has been a decrease in the distance to the desired point, we reward it + if distance_difference < 0.0: + rospy.logwarn("DECREASE IN DISTANCE GOOD") + reward = self.closer_to_point_reward + else: + rospy.logerr("ENCREASE IN DISTANCE BAD") + reward = -1*self.closer_to_point_reward + + else: + + if self.is_in_desired_position(current_position, self.desired_point_epsilon): + reward = self.done_reward + else: + reward = -1*self.done_reward + + + self.previous_distance_from_des_point = distance_from_des_point + + + rospy.logdebug("reward=" + str(reward)) + self.cumulated_reward += reward + rospy.logdebug("Cumulated_reward=" + str(self.cumulated_reward)) + self.cumulated_steps += 1 + rospy.logdebug("Cumulated_steps=" + str(self.cumulated_steps)) + + return reward + + + # Internal TaskEnv Methods + + def is_in_desired_position(self,current_position, epsilon=0.05): + """ + It return True if the current position is similar to the desired poistion + """ + + is_in_desired_pos = False + + + x_pos_plus = self.desired_point.x + epsilon + x_pos_minus = self.desired_point.x - epsilon + y_pos_plus = self.desired_point.y + epsilon + y_pos_minus = self.desired_point.y - epsilon + + x_current = current_position.x + y_current = current_position.y + + x_pos_are_close = (x_current <= x_pos_plus) and (x_current > x_pos_minus) + y_pos_are_close = (y_current <= y_pos_plus) and (y_current > y_pos_minus) + + is_in_desired_pos = x_pos_are_close and y_pos_are_close + + rospy.logdebug("###### IS DESIRED POS ? ######") + rospy.logdebug("current_position"+str(current_position)) + rospy.logdebug("x_pos_plus"+str(x_pos_plus)+",x_pos_minus="+str(x_pos_minus)) + rospy.logdebug("y_pos_plus"+str(y_pos_plus)+",y_pos_minus="+str(y_pos_minus)) + rospy.logdebug("x_pos_are_close"+str(x_pos_are_close)) + rospy.logdebug("y_pos_are_close"+str(y_pos_are_close)) + rospy.logdebug("is_in_desired_pos"+str(is_in_desired_pos)) + rospy.logdebug("############") + + return is_in_desired_pos + + def get_distance_from_desired_point(self, current_position): + """ + Calculates the distance from the current position to the desired point + :param start_point: + :return: + """ + distance = self.get_distance_from_point(current_position, + self.desired_point) + + return distance + + def get_distance_from_point(self, pstart, p_end): + """ + Given a Vector3 Object, get distance from current position + :param p_end: + :return: + """ + a = numpy.array((pstart.x, pstart.y, pstart.z)) + b = numpy.array((p_end.x, p_end.y, p_end.z)) + + distance = numpy.linalg.norm(a - b) + + return distance + + def get_orientation_euler(self, quaternion_vector): + # We convert from quaternions to euler + orientation_list = [quaternion_vector.x, + quaternion_vector.y, + quaternion_vector.z, + quaternion_vector.w] + + roll, pitch, yaw = euler_from_quaternion(orientation_list) + return roll, pitch, yaw + + def is_inside_workspace(self,current_position): + """ + Check if the Wamv is inside the Workspace defined + """ + is_inside = False + + rospy.logwarn("##### INSIDE WORK SPACE? #######") + rospy.logwarn("XYZ current_position"+str(current_position)) + rospy.logwarn("work_space_x_max"+str(self.work_space_x_max)+",work_space_x_min="+str(self.work_space_x_min)) + rospy.logwarn("work_space_y_max"+str(self.work_space_y_max)+",work_space_y_min="+str(self.work_space_y_min)) + rospy.logwarn("############") + + if current_position.x > self.work_space_x_min and current_position.x <= self.work_space_x_max: + if current_position.y > self.work_space_y_min and current_position.y <= self.work_space_y_max: + is_inside = True + + return is_inside + + + diff --git a/frontier_rl/openai_ros/openai_ros/templates/template_my_robot_env.py b/frontier_rl/openai_ros/openai_ros/templates/template_my_robot_env.py new file mode 100755 index 0000000..960104f --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/templates/template_my_robot_env.py @@ -0,0 +1,74 @@ +from openai_ros import robot_gazebo_env + + +class MyRobotEnv(robot_gazebo_env.RobotGazeboEnv): + """Superclass for all Robot environments. + """ + + def __init__(self): + """Initializes a new Robot environment. + """ + # Variables that we give through the constructor. + + # Internal Vars + self.controllers_list = ['my_robot_controller1','my_robot_controller2', ..., 'my_robot_controllerX'] + + self.robot_name_space = "my_robot_namespace" + + reset_controls_bool = True or False + + # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv + + super(MyRobotEnv, self).__init__(controllers_list=self.controllers_list, + robot_name_space=self.robot_name_space, + reset_controls=reset_controls_bool) + + # Methods needed by the RobotGazeboEnv + # ---------------------------- + + + + def _check_all_systems_ready(self): + """ + Checks that all the sensors, publishers and other simulation systems are + operational. + """ + # TODO + return True + + # Methods that the TrainingEnvironment will need to define here as virtual + # because they will be used in RobotGazeboEnv GrandParentClass and defined in the + # TrainingEnvironment. + # ---------------------------- + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + raise NotImplementedError() + + + def _init_env_variables(self): + """Inits variables needed to be initialised each time we reset at the start + of an episode. + """ + raise NotImplementedError() + + def _compute_reward(self, observations, done): + """Calculates the reward to give based on the observations given. + """ + raise NotImplementedError() + + def _set_action(self, action): + """Applies the given action to the simulation. + """ + raise NotImplementedError() + + def _get_obs(self): + raise NotImplementedError() + + def _is_done(self, observations): + """Checks if episode done based on observations given. + """ + raise NotImplementedError() + + # Methods that the TrainingEnvironment will need. + # ---------------------------- \ No newline at end of file diff --git a/frontier_rl/openai_ros/openai_ros/templates/template_my_training_env.py b/frontier_rl/openai_ros/openai_ros/templates/template_my_training_env.py new file mode 100644 index 0000000..cb53ae4 --- /dev/null +++ b/frontier_rl/openai_ros/openai_ros/templates/template_my_training_env.py @@ -0,0 +1,88 @@ +import numpy +from gym import spaces +import my_robot_env +from gym.envs.registration import register +import rospy + +# The path is __init__.py of openai_ros, where we import the MovingCubeOneDiskWalkEnv directly +from src.Frontier_ML.openai_ros.openai_ros.src.openai_ros.robot_envs import cube_single_disk_env + +timestep_limit_per_episode = 1000 # Can be any Value + +register( + id='MyTrainingEnv-v0', + entry_point='template_my_training_env:MovingCubeOneDiskWalkEnv', + timestep_limit=timestep_limit_per_episode, + ) + +class MyTrainingEnv(cube_single_disk_env.MyRobotEnv): + def __init__(self): + + # Only variable needed to be set here + number_actions = rospy.get_param('/my_robot_namespace/n_actions') + self.action_space = spaces.Discrete(number_actions) + + # This is the most common case of Box observation type + high = numpy.array([ + # obs1_max_value, + # obs12_max_value, + # ... + # obsN_max_value + ]) + + self.observation_space = spaces.Box(-high, high) + + # Variables that we retrieve through the param server, loded when launch training launch. + + + + # Here we will add any init functions prior to starting the MyRobotEnv + super(MyTrainingEnv, self).__init__() + + + def _set_init_pose(self): + """Sets the Robot in its init pose + """ + # TODO + + def _init_env_variables(self): + """ + Inits variables needed to be initialised each time we reset at the start + of an episode. + :return: + """ + # TODO + + + def _set_action(self, action): + """ + Move the robot based on the action variable given + """ + # TODO: Move robot + + def _get_obs(self): + """ + Here we define what sensor data of our robots observations + To know which Variables we have acces to, we need to read the + MyRobotEnv API DOCS + :return: observations + """ + # TODO + return #observations + + def _is_done(self, observations): + """ + Decide if episode is done based on the observations + """ + # TODO + return #done + + def _compute_reward(self, observations, done): + """ + Return the reward based on the observations given + """ + # TODO + return #reward + + # Internal TaskEnv Methods + diff --git a/mdis_state_machine/CMakeLists.txt b/mdis_state_machine/CMakeLists.txt index b9dac59..7b65eb0 100644 --- a/mdis_state_machine/CMakeLists.txt +++ b/mdis_state_machine/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation octomap_msgs nav_msgs + coms tf ) @@ -25,6 +26,9 @@ add_message_files( FILES Connection.msg DataCommunication.msg + Location.msg + LocationAck.msg + Interest.msg RobotsState.msg ) diff --git a/mdis_state_machine/include/robot_state_machine.h b/mdis_state_machine/include/robot_state_machine.h index 7a5d4d9..27aea92 100644 --- a/mdis_state_machine/include/robot_state_machine.h +++ b/mdis_state_machine/include/robot_state_machine.h @@ -2,10 +2,21 @@ #include #include -#include -#include +#include #include #include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + enum ROLE{ RELAY, EXPLORER, @@ -33,36 +44,22 @@ class RobotState { } uint64_t getId() const { return m_unId; } - const std::string& getName() const { return m_strName; } - virtual bool entryPoint() = 0; - virtual void exitPoint() = 0; - virtual void step() = 0; - virtual bool isDone() = 0; - virtual TEAM_STATES transition() = 0; - - void setParent(const std::string& name) - { - parent_robot_name = name; - } - - void setChild(const std::string& name) - { - child_robot_name = name; - } + void setParent(const std::string& name){parent_robot_name = name;} + void setChild(const std::string& name){child_robot_name = name;} + void setRobotRole(ROLE role){robot_role = role;} + std::vector robot_locations_during_meeting; - void setRobotRole(ROLE role) - { - robot_role = role; - } + protected: uint64_t m_unId; + // ros::Subscriber interest_sub; std::string m_strName; std::string robot_name; static float curr_meet_x, curr_meet_y, next_meet_x, next_meet_y; @@ -75,6 +72,7 @@ class RobotState { geometry_msgs::Point data_dump_location; + bool interested; bool is_explorer; bool meeting_started, go_for_exploration; @@ -87,7 +85,7 @@ class RobotState { curr_meet_x = meeting.x; curr_meet_y = meeting.y; } - void setNextMeetingPoint(const geometry_msgs::Point& meeting) + void setNextMeetingPoint(const geometry_msgs::Point& meeting)//not being used now. This was to set meeting point as the one from which explorer returned { next_meet_x = meeting.x; next_meet_y = meeting.y; @@ -99,6 +97,39 @@ class RobotState { point.y = curr_meet_y; return point; } + geometry_msgs::Point getNextFurthestMeetingPoint(const std::vector& frontiers, const std::vector& curr_location_f) + // geometry_msgs::Point getNextFurthestMeetingPoint(const std::vector& frontiers, const geometry_msgs::Point& curr_location_f) + { + float max_dis = 0.0; + geometry_msgs::Point point; + geometry_msgs::Point temp_pos; + // temp_pos.x=curr_location_f.x; + // temp_pos.y=curr_location_f.y; + + for (auto& frontier : frontiers){//euclidean distance to frontier from explorer meeting point + geometry_msgs::Point temp; + // geometry_msgs::Point temp_pos; + temp.x=frontier.x; + temp.y=frontier.y; + float total_distance_from_frontier = 0; + for (int i=0;igetDistancePrediction(temp, temp_pos); + total_distance_from_frontier += distance_from_frontier; + } + // float total_distance_from_frontier = explore_interface->getDistancePrediction(temp, temp_pos); + if (total_distance_from_frontier>max_dis){ + max_dis = total_distance_from_frontier; + point.x=frontier.x; + point.y=frontier.y; + setNextMeetingPoint(point); + + } + + } + return point; + } geometry_msgs::Point getNextMeetingPoint() { geometry_msgs::Point point; @@ -106,6 +137,24 @@ class RobotState { point.y = next_meet_y; return point; } + geometry_msgs::Point getNextClosestMeetingPoint(const std::vector& frontiers, const geometry_msgs::Point& curr_location_f) + { + float min_dis = LONG_MAX; + geometry_msgs::Point point; + for (auto& frontier : frontiers){ + geometry_msgs::Point temp; + temp.x=frontier.x; + temp.y=frontier.y; + float distance_from_frontier = explore_interface->getDistancePrediction(temp); + // float distance_from_frontier = sqrt(((curr_location_f.x-frontier.x)*(curr_location_f.x-frontier.x))+((curr_location_f.y-frontier.y)*(curr_location_f.y-frontier.y))); + if (distance_from_frontier(nh.getNamespace() + "/robots_state", 1000); + interest_sub = nh.subscribe("/interest_check", 1000, &GoToMeet::interestCB, this); + interest_pub = nh.advertise("/interest_check", 1000); } bool isDone() override ; @@ -193,77 +244,107 @@ class GoToMeet: public RobotState{ private: bool connected; std::string conn_robot; - ros::Subscriber conn_sub; + ros::Publisher interest_pub; + ros::Subscriber interest_sub; ros::Publisher robot_state_pub; + ros::Subscriber conn_sub; mdis_state_machine::RobotsState state_pub_data; - void connCB(const mdis_state_machine::Connection::ConstPtr msg); -}; - + void interestCB(const mdis_state_machine::Interest::ConstPtr msg); + ros::Time time_of_last_conn; + ros::Duration wait_time_for_conn = ros::Duration(2.0); +}; class Meet: public RobotState{ public: Meet(ros::NodeHandle &nh, bool testing):RobotState(MEET, "Meet", nh, testing){ - meeting_data_pub = nh.advertise(nh.getNamespace() + "/data_communication", 1000); - meeting_data_sub = nh.subscribe(nh.getNamespace() + "/data_communication", 1000, &Meet::nextMeetingLocationCB, this); - robot_state_pub = nh.advertise(nh.getNamespace() + "/robots_state", 1000); + meeting_data_pub = nh.advertise("/data_communication", 1000); + meeting_data_sub = nh.subscribe("/data_communication", 1000, &Meet::nextMeetingLocationCB, this); + robot_state_pub = nh.advertise(nh.getNamespace() + "/robots_state", 1000); frontier_data_sub = nh.subscribe("/tb3_0/frontier_list", 1000, &Meet::getBestFrontiersCB, this); + location_data_pub = nh.advertise("/robot_location", 1000); + location_data_sub = nh.subscribe("/robot_location", 1000, &Meet::getLocationCB, this); + location_data_ack_pub = nh.advertise("/robot_location_ack", 1000); + location_data_ack_sub = nh.subscribe("/robot_location_ack", 1000, &Meet::getLocationAckCB, this); + frontier_req_pub = nh.advertise("/frontier_request", 1000); + frontier_data_sub = nh.subscribe("/tb3_0/frontier_list", 1000, &Meet::getBestFrontiersCB, this); + + mergeRequestClient = nh.serviceClient(srv_name); } - bool isDone() override ; + std::string srv_name = "trigger_merge"; + bool isDone() override ; TEAM_STATES transition() override; - bool entryPoint() override; void step() override; void exitPoint() override; private: ros::Publisher meeting_data_pub; + ros::Publisher frontier_req_pub; + ros::Subscriber meeting_data_sub; + ros::Subscriber frontier_data_sub; + // frontier_exploration::FrontierSearch search_; + ros::Publisher location_data_pub; + ros::Subscriber location_data_sub; + ros::Publisher location_data_ack_pub; + ros::Subscriber location_data_ack_sub; ros::Publisher robot_state_pub; + ros::ServiceClient mergeRequestClient; mdis_state_machine::RobotsState state_pub_data; - ros::Subscriber meeting_data_sub; - bool data_received; + bool data_received; + bool frontier_data_received; + bool location_data_received; + bool location_data_ack; + void getFrontiersCB(const explore_lite::FrontiersArray::ConstPtr msg); + void getBestFrontiersCB(const geometry_msgs::PoseArray::ConstPtr msg); + void requestMerge(std::string conn_robot); void publishNextMeetingLocation(); void nextMeetingLocationCB(const mdis_state_machine::DataCommunication::ConstPtr msg); + void getLocationCB(const mdis_state_machine::Location::ConstPtr msg); + void getLocationAckCB(const mdis_state_machine::LocationAck::ConstPtr msg); void getNextMeetingLocationFromCallback(); void setExplorationTime(); + void getLocations(); geometry_msgs::Point buffer_next_location; -}; - + std::vector frontier_msg; + geometry_msgs::Point location_msg; + bool is_merge_complete; +}; class GoToDumpData: public RobotState{ public: GoToDumpData(ros::NodeHandle &nh, bool testing):RobotState(GO_TO_DUMP_DATA, "GoToDumpData", nh, testing){ - robot_state_pub = nh.advertise(nh.getNamespace() + "/robots_state", 1000); + robot_state_pub = nh.advertise(nh.getNamespace() + "/robots_state", 1000); + conn_sub = nh.subscribe(nh.getNamespace() + "/connection_check", 1000, &GoToDumpData::connCB, this); } bool isDone() override ; - TEAM_STATES transition() override; - bool entryPoint() override; void step() override; void exitPoint() override; - private: ros::Publisher robot_state_pub; + ros::Subscriber conn_sub; + std::string conn_robot; mdis_state_machine::RobotsState state_pub_data; -}; - + bool connected; + void connCB(const mdis_state_machine::Connection::ConstPtr msg); + ros::Time time_of_last_conn; + ros::Duration wait_time_for_conn = ros::Duration(2.0); +}; class DumpData: public RobotState{ public: DumpData(ros::NodeHandle &nh, bool testing):RobotState(DUMP_DATA, "DumpData", nh, testing){ robot_state_pub = nh.advertise(nh.getNamespace() + "/robots_state", 1000); } bool isDone() override ; - TEAM_STATES transition() override; - bool entryPoint() override; void step() override; void exitPoint() override; - private: ros::Publisher robot_state_pub; mdis_state_machine::RobotsState state_pub_data; diff --git a/mdis_state_machine/launch/test_argos.launch b/mdis_state_machine/launch/test_argos.launch new file mode 100644 index 0000000..cfb4785 --- /dev/null +++ b/mdis_state_machine/launch/test_argos.launch @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/mdis_state_machine/launch/test_merge.launch b/mdis_state_machine/launch/test_merge.launch new file mode 100644 index 0000000..58ecaba --- /dev/null +++ b/mdis_state_machine/launch/test_merge.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/mdis_state_machine/launch/test_merge_SLAM.launch b/mdis_state_machine/launch/test_merge_SLAM.launch new file mode 100644 index 0000000..d324c13 --- /dev/null +++ b/mdis_state_machine/launch/test_merge_SLAM.launch @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/mdis_state_machine/launch/test_state_machine.launch b/mdis_state_machine/launch/test_state_machine.launch new file mode 100644 index 0000000..2864bb2 --- /dev/null +++ b/mdis_state_machine/launch/test_state_machine.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/mdis_state_machine/msg/Connection.msg b/mdis_state_machine/msg/Connection.msg index 9f65580..4ddef32 100644 --- a/mdis_state_machine/msg/Connection.msg +++ b/mdis_state_machine/msg/Connection.msg @@ -1,2 +1,2 @@ -std_msgs/String[] connection_between +std_msgs/String connection_to float32 distance \ No newline at end of file diff --git a/mdis_state_machine/msg/Interest.msg b/mdis_state_machine/msg/Interest.msg new file mode 100644 index 0000000..70af35f --- /dev/null +++ b/mdis_state_machine/msg/Interest.msg @@ -0,0 +1 @@ +std_msgs/String[] req \ No newline at end of file diff --git a/mdis_state_machine/msg/Location.msg b/mdis_state_machine/msg/Location.msg new file mode 100644 index 0000000..bd784a9 --- /dev/null +++ b/mdis_state_machine/msg/Location.msg @@ -0,0 +1 @@ +geometry_msgs/Point robot_location \ No newline at end of file diff --git a/mdis_state_machine/msg/LocationAck.msg b/mdis_state_machine/msg/LocationAck.msg new file mode 100644 index 0000000..f5ef14c --- /dev/null +++ b/mdis_state_machine/msg/LocationAck.msg @@ -0,0 +1 @@ +std_msgs/String[] ack \ No newline at end of file diff --git a/mdis_state_machine/package.xml b/mdis_state_machine/package.xml index 9c0ee52..5d95cbe 100644 --- a/mdis_state_machine/package.xml +++ b/mdis_state_machine/package.xml @@ -28,6 +28,7 @@ geometry_msgs nav_msgs tf + coms roscpp rospy @@ -40,6 +41,7 @@ geometry_msgs nav_msgs tf + coms roscpp rospy @@ -52,6 +54,7 @@ geometry_msgs nav_msgs tf + coms diff --git a/mdis_state_machine/src/connection_check.cpp b/mdis_state_machine/src/connection_check.cpp index f5705f2..96f65f6 100644 --- a/mdis_state_machine/src/connection_check.cpp +++ b/mdis_state_machine/src/connection_check.cpp @@ -7,7 +7,7 @@ #include const int MAX_ATTEMPTS=5; -const float DIST_THRESHOLD=3.0; +const float DIST_THRESHOLD=1.414; geometry_msgs::Point getRobotCurrentPose(const std::string& robot_name, tf::TransformListener& tf_listener) { @@ -60,27 +60,35 @@ int main(int argc, char** argv) tf::TransformListener tf_listener; ros::Publisher conn_pub = nh.advertise(nh.getNamespace() + "/connection_check", 1000); + geometry_msgs::Point data_center_location; + data_center_location.x = -6; + data_center_location.y = -5; while(ros::ok()) { mdis_state_machine::Connection conn_msg; - conn_msg.connection_between.resize(2); - for (int i=0; i str: result = "" @@ -54,7 +58,7 @@ def verifyTimedStateChange(init_state, change_state, robot_name): return False -def verifyConnStateChange(init_state, change_state, robot_name): +def verifyConnStateChange(init_state, change_state, robot_name, robot_partner): rospy.sleep(ideal_state_change_duration - headstart_to_check) init_state_sucs = False # while rospy.get_rostime().secs < time_start+max_time_to_wait_to_change_state: @@ -68,12 +72,19 @@ def verifyConnStateChange(init_state, change_state, robot_name): robot_conn_msg.connection_between = [] robot_conn_msg.connection_between.append(String(data=robot_name)) robot_conn_msg.connection_between.append(String(data="dummy_parent")) + robot_interest_msg = Interest() + robot_interest_msg.req = [] + robot_interest_msg.req.append(String(data="Request for connection")) + robot_interest_msg.req.append(String(data=robot_partner)) connection_topic_name = gen_topic_name([robot_name, connection_check_topic]) + # interest_topic_name = gen_topic_name([robot_partner, interest_check_topic]) pub = rospy.Publisher(connection_topic_name, Connection, queue_size=10) + int_pub = rospy.Publisher('/interest_check', Interest, queue_size=10) time_start = rospy.get_rostime().secs while rospy.get_rostime().secs < time_start + max_time_to_wait_to_change_state: pub.publish(robot_conn_msg) + int_pub.publish(robot_interest_msg) rospy.sleep(0.5) msg = rospy.wait_for_message(state_topic_name, RobotsState, timeout=message_wait_timeout) if msg.robot_name.data == robot_name: diff --git a/mdis_state_machine/src/robot_state_machine.cpp b/mdis_state_machine/src/robot_state_machine.cpp index 63d699a..24c83c6 100644 --- a/mdis_state_machine/src/robot_state_machine.cpp +++ b/mdis_state_machine/src/robot_state_machine.cpp @@ -4,16 +4,17 @@ float RobotState::curr_meet_x = 0.0; float RobotState::curr_meet_y = 0.0; float RobotState::next_meet_x = 0.0; float RobotState::next_meet_y = 0.0; -float RobotState::time_for_exploration = 30.0; +float RobotState::time_for_exploration = 40.0; std::string RobotState::parent_robot_name = ""; std::string RobotState::child_robot_name = ""; +std::string connected_robot_name = ""; bool RobotState::testing_mode = false; ROLE RobotState::robot_role; RobotState::RobotState(uint64_t un_id, const std::string& str_name, ros::NodeHandle &nh, bool testing) : - // m_pcTeam(nullptr), + // m_pcTeam(nullptr), m_unId(un_id), m_strName(str_name) { if(!testing) @@ -22,20 +23,21 @@ RobotState::RobotState(uint64_t un_id, const std::string& str_name, ros::NodeHan } else { - ROS_INFO("Testing mode, skipping initializations"); + ROS_INFO_STREAM("["<getRobotCurrentPose().pose.position; + curr_meet_x = current_pose.x; + curr_meet_y = current_pose.y; + next_meet_x = current_pose.x; + next_meet_y = current_pose.y; - curr_meet_x = -31; - curr_meet_y = -6; - next_meet_x = -28; - next_meet_y = -6; - - data_dump_location.x = -31; - data_dump_location.y = -10; + data_dump_location.x = -6; + data_dump_location.y = -5; } @@ -55,17 +57,17 @@ bool Idle::isDone() } -TEAM_STATES Idle::transition() +TEAM_STATES Idle::transition() { return IDLE; } void Idle::step() { - ROS_INFO("Step for Idle"); + ROS_INFO_STREAM("["<goToPoint(temp_point, true); return true; } -TEAM_STATES GoToExplore::transition() +TEAM_STATES GoToExplore::transition() { if(isDone()) return EXPLORE; @@ -117,12 +120,12 @@ TEAM_STATES GoToExplore::transition() void GoToExplore::step() { - ROS_INFO("Executing step for GO_TO_EXPLORE"); + ROS_INFO_STREAM("["< time_until_next_meeting; } -TEAM_STATES Explore::transition() +TEAM_STATES Explore::transition() { if(isDone()) return GO_TO_MEET; @@ -175,10 +178,11 @@ void Explore::step() std_msgs::Bool msg; msg.data = false; pause_exploration_pub.publish(msg); + robot_state_pub.publish(state_pub_data); ROS_INFO_THROTTLE(10,"Executing step for EXPLORE"); } -void Explore::exitPoint() +void Explore::exitPoint() { std_msgs::Bool msg; msg.data = true; @@ -194,7 +198,7 @@ void Explore::exitPoint() explore_interface->stopRobot(); } - ROS_INFO_STREAM("Exiting the state EXPLORE\nNext meeting "<goToPoint(temp_point, false); + interested = false; + connected = false; if(!RobotState::testing_mode) explore_interface->goToPoint(temp_point, false); - // Giving it some time to reflect - ros::Duration(1).sleep(); + // explore_interface->goToPoint(temp_point, false); + ros::Duration(1).sleep(); // Giving it some time to reflect return true; } @@ -220,23 +226,29 @@ bool GoToMeet::isDone() { if(connected) { - if(isConnDirectRelated(conn_robot)) - { - ROS_INFO("Robot is connected to the party of interest"); - return true; - } + mdis_state_machine::Interest data; + data.req.resize(2); + data.req.at(0).data = "Request for connection. Interested?"; + data.req.at(1).data = robot_name; + interest_pub.publish(data); + ros::Duration(1).sleep(); + if(interested){ + ROS_INFO_STREAM("["<wait_time_for_conn) + connected = false; + ROS_INFO_THROTTLE(10,"Executing the step for GO_TO_MEET"); } -void GoToMeet::exitPoint() +void GoToMeet::exitPoint() { - ROS_INFO("Exiting the state GO_TO_MEET"); + ROS_INFO_STREAM("["<stopRobot(); } @@ -259,25 +275,40 @@ void GoToMeet::exitPoint() void GoToMeet::connCB(const mdis_state_machine::Connection::ConstPtr msg) { - connected = false; - for(int i = 0; iconnection_between.size(); i++) + if(msg->connection_to.data == parent_robot_name || msg->connection_to.data == child_robot_name) { - if(robot_name == msg->connection_between.at(i).data) - { - int j = 1 ? i==0 : 0; - connected = true; - conn_robot = msg->connection_between.at(j).data; - } + connected = true; + time_of_last_conn = ros::Time::now(); + conn_robot = msg->connection_to.data; + } +} + +void GoToMeet::interestCB(const mdis_state_machine::Interest::ConstPtr msg){ + std::string r_name = msg->req.at(1).data; + ROS_INFO_STREAM("["<getRobotCurrentPose().pose.position; + location_data_pub.publish(temp_location); + ros::Duration(0.5).sleep(); + } + + } + if (robot_role == EXPLORER){ + while(ros::ok() && !location_data_received) + { + ROS_INFO_STREAM("["<getRobotCurrentPose().pose.position; + robot_locations_during_meeting.push_back(temp_location); + robot_locations_during_meeting.push_back(location_msg); + // Set this only if the meeting was successful if(robot_role == EXPLORER) { @@ -337,12 +398,39 @@ void Meet::exitPoint() } else getNextMeetingLocationFromCallback(); - + + robot_locations_during_meeting.clear(); // get time estimate from move_base_interface // set exploration_duration accordingly } +void Meet::requestMerge(std::string conn_robot) +{ + coms::TriggerMerge mergeRequest; + mergeRequest.request.robot_id = conn_robot; + bool success = false; + + ROS_INFO_STREAM("["<getRobotCurrentPose().pose.position; @@ -351,18 +439,56 @@ void Meet::setExplorationTime() float time_to_meet_after = explore_interface->getTimePredictionForTravel(data_dump_location, temp_next_location); float time_to_exploration_site = explore_interface->getTimePredictionForTravel(curr_location, temp_next_location); float total_time = time_to_dump+time_to_meet_after; - - time_for_exploration = (total_time-time_to_exploration_site)/2; + + time_for_exploration = (total_time-time_to_exploration_site)/2; +} + +void Meet::getFrontiersCB(const explore_lite::FrontiersArray::ConstPtr msg) +{ + for (int i=0;ifrontiers.size();i++){ + frontier_msg.push_back(msg->frontiers.at(i)); + } + frontier_data_received = true; +} + +void Meet::getBestFrontiersCB(const geometry_msgs::PoseArray::ConstPtr msg) +{ + for (int i=0;iposes.size();i++){ + frontier_msg.push_back(msg->poses.at(i).position); + } + frontier_data_received = true; +} + +void Meet::getLocationCB(const mdis_state_machine::Location::ConstPtr msg){ + // geometry_msgs::Point location_msg; + location_msg=msg->robot_location; + location_data_received = true; +} + +void Meet::getLocationAckCB(const mdis_state_machine::LocationAck::ConstPtr msg){ + location_data_ack = true; } void Meet::publishNextMeetingLocation() { + while(ros::ok() && !frontier_data_received) + { + ROS_INFO_STREAM("["<connection_between.size(); i++) { @@ -409,17 +537,22 @@ void Meet::nextMeetingLocationCB(const mdis_state_machine::DataCommunication::Co bool GoToDumpData::entryPoint() { - ROS_INFO("Entering the state GO_TO_DUMP"); + ROS_INFO_STREAM("["<goToPoint(data_dump_location, false); + ros::Duration(1).sleep(); return true; } bool GoToDumpData::isDone() { - ROS_INFO_STREAM("Going to Dump Data at"<goToPoint(data_dump_location, true); - return true; + // explore_interface->goToPoint(data_dump_location, true); + // return true; } -TEAM_STATES GoToDumpData::transition() +TEAM_STATES GoToDumpData::transition() { if(isDone()) return DUMP_DATA; @@ -450,22 +588,34 @@ TEAM_STATES GoToDumpData::transition() void GoToDumpData::step() { ROS_INFO_THROTTLE(10, "Going to dump data"); + ros::Duration time_since_connection = ros::Time::now() - time_of_last_conn; + if(time_since_connection>wait_time_for_conn) + connected = false; + } -void GoToDumpData::exitPoint() +void GoToDumpData::exitPoint() { - ROS_INFO("Exiting the state GO_TO_DUMP"); + ROS_INFO_STREAM("["<stopRobot(); } +void GoToDumpData::connCB(const mdis_state_machine::Connection::ConstPtr msg) +{ + if(msg->connection_to.data == "data_center") + { + connected = true; + time_of_last_conn = ros::Time::now(); + } +} //////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////// D U M P S T A T E C L A S S //////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool DumpData::entryPoint() { - ROS_INFO("Entering the state DUMP_DATA"); + ROS_INFO_STREAM("["<setParent(parent_name); current_state_ptr->setChild(child_name); current_state_ptr->setRobotRole(role); } TeamScheduler::~TeamScheduler() -{ +{ std::for_each( MACRO_STATE_PTR_MAP.begin(), MACRO_STATE_PTR_MAP.end(), @@ -47,7 +48,7 @@ void TeamScheduler::addState(RobotState* pc_state) { // return m_pcTeam->getState(un_state); // } -RobotState& TeamScheduler::getStatePtr(uint64_t un_id) +RobotState& TeamScheduler::getStatePtr(uint64_t un_id) { auto pcState = MACRO_STATE_PTR_MAP.find(un_id); if(pcState != MACRO_STATE_PTR_MAP.end()) { @@ -64,7 +65,7 @@ void TeamScheduler::setInitialState(uint64_t un_state) { if(pcState != MACRO_STATE_PTR_MAP.end()) { // acquire value of the state (every map has a key(first) and a value(second)) current_state_ptr = pcState->second; - + // completes entry point of the initial state current_state_ptr->entryPoint(); setTeamMacroState((TEAM_STATES) un_state); @@ -94,7 +95,7 @@ void TeamScheduler::step() { current_state_ptr->exitPoint(); // cNewState->setResetRobot(reset_robot_odometry); bool entry = cNewState->entryPoint(); - + setTeamMacroState((TEAM_STATES) cNewState->getId()); if(!entry) return; @@ -127,7 +128,7 @@ int main(int argc, char** argv) { ros::init(argc, argv, "state_machine"); ros::NodeHandle nh; - + // if (argc != 4 && argc != 2) // { // ROS_ERROR("This node must be launched with number of robots as argument"); @@ -149,11 +150,11 @@ int main(int argc, char** argv) testing = true; role = std::stoi(argv[1]) == 5 ? EXPLORER : RELAY; } - + parent_name = argv[2]; if(role != EXPLORER) child_name = argv[3]; - + if(role == RELAY && !testing) ros::Duration(20).sleep(); diff --git a/mdis_state_machine/tests/test_state_shift_explorer.py b/mdis_state_machine/tests/test_state_shift_explorer.py index 2b3780c..45dd79d 100755 --- a/mdis_state_machine/tests/test_state_shift_explorer.py +++ b/mdis_state_machine/tests/test_state_shift_explorer.py @@ -10,6 +10,7 @@ LAUNCH_DIR = os.path.join(TEST_DIR.parent.absolute(), 'launch') EXPLORER_LAUNCH_FILE = os.path.join(LAUNCH_DIR, 'test_state_machine_explorer.launch') ROBOT_NAME = "test_robot_1" +ROBOT_PARTNER = "test_robot_2" class TestStateShiftExplorer(unittest.TestCase): @@ -25,7 +26,7 @@ def test_init_state(self: unittest.TestCase) -> None: self.assertEqual(verifyTimedStateChange(EXPLORE, GO_TO_MEET, ROBOT_NAME), True, "State shift check for explorer state 2-3 failed") # def test_triggered_states(self: unittest.TestCase) -> None: - self.assertEqual(verifyConnStateChange(GO_TO_MEET, MEET, ROBOT_NAME), True, "State shift check for explorer state 3-4 failed") + self.assertEqual(verifyConnStateChange(GO_TO_MEET, MEET, ROBOT_NAME, ROBOT_PARTNER), True, "State shift check for explorer state 3-4 failed") # def test_second_loop(self: unittest.TestCase) -> None: self.assertEqual(verifyTimedStateChange(MEET, GO_TO_EXPLORE, ROBOT_NAME), True, "State shift check for explorer state 4-1 failed") diff --git a/multi_map_ros/rviz/multi_robot.rviz b/multi_map_ros/rviz/multi_robot.rviz index 8d67d7d..03f9d62 100644 --- a/multi_map_ros/rviz/multi_robot.rviz +++ b/multi_map_ros/rviz/multi_robot.rviz @@ -7,6 +7,9 @@ Panels: - /RobotModel1 - /TF1/Frames1 - /Map1 + - /GridCells1 + - /GridCells2 + - /Marker1 Splitter Ratio: 0.594406008720398 Tree Height: 719 - Class: rviz/Selection @@ -167,6 +170,32 @@ Visualization Manager: Unreliable: false Use Timestamp: false Value: true + - Alpha: 1 + Class: rviz/GridCells + Color: 25; 255; 0 + Enabled: true + Name: GridCells + Queue Size: 10 + Topic: /selected_frontier + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/GridCells + Color: 246; 22; 226 + Enabled: true + Name: GridCells + Queue Size: 10 + Topic: /selected_center + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /visualization_marker + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/multi_map_ros/src/multi_map_ros/Evaluation/Exploration/src/exploration_eval.py b/multi_map_ros/src/multi_map_ros/Evaluation/Exploration/src/exploration_eval.py new file mode 100755 index 0000000..f08c55b --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/Evaluation/Exploration/src/exploration_eval.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python3.8 +import math + +import rospy +import numpy as np +import copy +from scipy import signal as sig + +from std_msgs.msg import Header +from nav_msgs.msg import OccupancyGrid, GridCells, Odometry +from geometry_msgs.msg import Point, PoseStamped +from tf import TransformListener + +import matplotlib.pyplot as plt + +class exploration_eval: + def __init__(self): + rospy.init_node('exploration_eval') + self.robot_namespace = '/tb3_0' + self.mapSub = rospy.Subscriber(self.robot_namespace+'/map', OccupancyGrid, self.map_callback) + self.odomSub = rospy.Subscriber(self.robot_namespace+'/odom', Odometry, self.odom_callback) + # self.odomPub = rospy.Publisher('/distance_travelled', Odometry, queue_size=10) + self.timer = rospy.Timer(rospy.Duration(5), self.timer_callback) + + self.start_time = rospy.get_time() #rospy.get_rostime() + + self.total_distance = 0.0 + self.previous_x = 0 + self.previous_y = 0 + self.first_run = True + + self.time_record = [] + self.percent_of_map_covered_record = [] + self.total_distance_travelled_record = [] + self.percent_of_map_covered_time = [] + self.total_distance_travelled_time = [] + + self.map = OccupancyGrid() + self.odom = Odometry() + + self.files_written = False + + + def map_callback(self, msg): + + self.map = msg + + # data = np.asarray(msg.data) + # values = [0, 101] + # freq = np.histogram(data, bins=[-np.inf] + values + [np.inf])[0] / data.size + # # print(freq) + # + # # uniques, counts = np.unique(data, return_counts=True) + # # percentages = dict(zip(uniques, counts / len(data))) + # # print(percentages) + # + # # total_percent_explored = percentages.get(1) + percentages.get(0) + # + # total_percent_explored = freq[1] + # + # print("total Percent explored: " + str(total_percent_explored)) + # + # self.percent_of_map_covered_record.append(total_percent_explored) + # self.percent_of_map_covered_time.append(rospy.get_time() - self.start_time) + + + def odom_callback(self, msg): + + self.odom = msg + + # if (self.first_run): + # self.previous_x = msg.pose.pose.position.x + # self.previous_y = msg.pose.pose.position.y + # x = msg.pose.pose.position.x + # y = msg.pose.pose.position.y + # print(msg) + # d_increment = math.sqrt(((x - self.previous_x) * (x - self.previous_x)) + ((y - self.previous_y) * (y - self.previous_y))) + # self.total_distance = self.total_distance + d_increment + # print("Total distance traveled is: " + str(self.total_distance)) + # # self.odomPub.publish(data) + # self.previous_x = msg.pose.pose.position.x + # self.previous_y = msg.pose.pose.position.y + # self.first_run = False + # + # self.total_distance_travelled_record.append(self.total_distance) + # self.total_distance_travelled_time.append(rospy.get_time() - self.start_time) + + + def timer_callback(self, timer_info): + + data = np.asarray(self.map.data) + odom = self.odom + + values = [0, 101] + freq = np.histogram(data, bins=[-np.inf] + values + [np.inf])[0] / data.size + + # print(freq) + # + # uniques, counts = np.unique(data, return_counts=True) + # percentages = dict(zip(uniques, counts / len(data))) + # print(percentages) + + total_percent_explored = freq[1]*100 + + print("Total Percent explored: " + str(total_percent_explored)) + + self.percent_of_map_covered_record.append(total_percent_explored) + + if (self.first_run): + self.start_time = rospy.get_time() + self.previous_x = odom.pose.pose.position.x + self.previous_y = odom.pose.pose.position.y + x = odom.pose.pose.position.x + y = odom.pose.pose.position.y + d_increment = math.sqrt(((x - self.previous_x) * (x - self.previous_x)) + ((y - self.previous_y) * (y - self.previous_y))) + self.total_distance = self.total_distance + d_increment + print("Total distance traveled is: " + str(self.total_distance)) + self.previous_x = odom.pose.pose.position.x + self.previous_y = odom.pose.pose.position.y + self.first_run = False + + self.total_distance_travelled_record.append(self.total_distance) + + time_diff = rospy.get_time() - self.start_time + print("Time: " + str(time_diff)) + print("----------------------------------------------------------") + self.time_record.append(time_diff) + + if time_diff > 1200 and not self.files_written: + rospy.loginfo("Writing Eval Graphs to Files") + plt.plot(self.time_record,self.total_distance_travelled_record) + plt.xlabel('Time (s)') + plt.ylabel('Total Distance Travelled (m)') + + plt.savefig('Distance_vs_Time_For_%s.png' % self.robot_namespace.replace('/','')) + + plt.clf() + + plt.plot(self.time_record, self.percent_of_map_covered_record) + plt.xlabel('Time (s)') + plt.ylabel('Percent of Map Explored') + + plt.savefig('Percent_Explored_vs_Time_For_%s.png' % self.robot_namespace.replace('/','')) + + plt.clf() + + plt.plot(self.total_distance_travelled_record, self.percent_of_map_covered_record) + plt.xlabel('Total Distance Travelled (m)') + plt.ylabel('Percent of Map Explored') + + plt.savefig('Percent_Explored_vs_Distance_For_%s.png' % self.robot_namespace.replace('/','')) + + self.files_written = True + + rospy.signal_shutdown("Finished Evaluation") + + +if __name__ == '__main__': + node = exploration_eval() + + while not rospy.is_shutdown(): + pass \ No newline at end of file diff --git a/multi_map_ros/src/multi_map_ros/FrontierSelection/launch/start_fringe.launch b/multi_map_ros/src/multi_map_ros/FrontierSelection/launch/start_fringe.launch new file mode 100644 index 0000000..5bdfb13 --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/FrontierSelection/launch/start_fringe.launch @@ -0,0 +1,3 @@ + + + diff --git a/multi_map_ros/src/multi_map_ros/FrontierSelection/scripts/alg_testing.py b/multi_map_ros/src/multi_map_ros/FrontierSelection/scripts/alg_testing.py new file mode 100755 index 0000000..c2d3e8e --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/FrontierSelection/scripts/alg_testing.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3.8 +import copy + +import numpy as np + +if __name__ == '__main__': + gridCells = list() + cell_threshold = 3 + gridCells.append((0, 0, 0)) + gridCells.append((1, 0, 0)) + gridCells.append((2, 0, 0)) + gridCells.append((3, 0, 0)) + gridCells.append((4, 0, 0)) + gridCells.append((5, 0, 0)) + gridCells.append((6, 0, 0)) + gridCells.append((7, 0, 0)) + gridCells.append((100, 100, 100)) + gridCells.append((30, 20, 3)) + + all_frontier_clusters = dict() # keeps track of clusters and the parent + remaining_cells = copy.deepcopy(gridCells) # cells remaining to be assigned to a group + + for a_cell in gridCells: + assigned = False + if all_frontier_clusters: + for key in all_frontier_clusters: + cells_in_cluster = all_frontier_clusters.get(key) + iterable_cells = copy.deepcopy(cells_in_cluster) + for other_cell in iterable_cells: + #if True in (abs(np.subtract(a_cell, other_cell)) <= cell_threshold): + if np.linalg.norm(np.asarray(a_cell)-np.asarray(other_cell)) <= cell_threshold: + if a_cell not in cells_in_cluster: + assigned = True + cells_in_cluster.append(a_cell) + remaining_cells.remove(a_cell) + all_frontier_clusters[key] = cells_in_cluster + + else: + assigned = True + + for potential_cell in remaining_cells: # in gridCells + # if True in (abs(np.subtract(a_cell, potential_cell)) <= cell_threshold): + if np.linalg.norm(np.asarray(a_cell) - np.asarray(potential_cell)) <= cell_threshold: + if potential_cell not in cells_in_cluster: + cells_in_cluster.append(potential_cell) + remaining_cells.remove(potential_cell) + all_frontier_clusters[key] = cells_in_cluster + + if not assigned: + first_list = list() + first_list.append(a_cell) + remaining_cells.remove(a_cell) + all_frontier_clusters[a_cell] = first_list + + + else: + first_list = list() + first_list.append(a_cell) + remaining_cells.remove(a_cell) + all_frontier_clusters[a_cell] = first_list + + print(str(all_frontier_clusters)) + + all_values = all_frontier_clusters.values() + + max_value = max(all_values) + print(max_value) + + max_key, max_value = max(all_frontier_clusters.items(), key=lambda x: len(set(x[1]))) + print(max_key) + print(max_value) \ No newline at end of file diff --git a/multi_map_ros/src/multi_map_ros/FrontierSelection/scripts/cluster_sorting_testing.py b/multi_map_ros/src/multi_map_ros/FrontierSelection/scripts/cluster_sorting_testing.py new file mode 100644 index 0000000..f6cec2a --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/FrontierSelection/scripts/cluster_sorting_testing.py @@ -0,0 +1,30 @@ + +if __name__ == '__main__': + + gridCells1 = list() + gridCells2 = list() + gridCells3 = list() + gridCells4 = list() + + + gridCells1.append((0, 0, 0)) + gridCells1.append((1, 0, 0)) + gridCells2.append((2, 0, 0)) + gridCells2.append((3, 0, 0)) + gridCells3.append((4, 0, 0)) + gridCells3.append((5, 0, 0)) + gridCells3.append((6, 0, 0)) + gridCells3.append((7, 0, 0)) + gridCells4.append((100, 100, 100)) + gridCells4.append((30, 20, 3)) + + all_frontier_clusters = dict() + all_frontier_clusters["one"] = gridCells1 + all_frontier_clusters["two"] = gridCells2 + all_frontier_clusters["three"] = gridCells3 + all_frontier_clusters["four"] = gridCells4 + + + length_dict = {key: len(value) for key, value in all_frontier_clusters.items()} + new_list = sorted(length_dict, key=length_dict.get, reverse=True) + print(new_list) \ No newline at end of file diff --git a/multi_map_ros/src/multi_map_ros/FrontierSelection/src/frontier_clustering.py b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/frontier_clustering.py new file mode 100755 index 0000000..b6fb9b4 --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/frontier_clustering.py @@ -0,0 +1,526 @@ +#!/usr/bin/env python3.8 + +import rospy + +import numpy as np + +import copy + +from scipy import signal as sig + +from std_msgs.msg import Header, String + +from nav_msgs.msg import OccupancyGrid, GridCells + +from geometry_msgs.msg import Point, PoseStamped, Pose, PoseArray + +from tf import TransformListener + +from rospy.exceptions import ROSTimeMovedBackwardsException, ROSInterruptException + +SEND_TO_FRONTIERS = False + +SEND_STATE_MACHINE_FRONTIERS = True + +SM_LIMIT = 10 + +class find_frontier: + + def __init__(self): + + rospy.init_node('find_fontier') + + self.cell_threshold = 0 + + self.threshold_multiplier = 2 + + # self.tf_listener_ = TransformListener() + + self.weight_output = .75 + + self.clusters = list() + + self.max_msg_count_before_start = 10 + + self.curr_msg_count = 0 + + self.robot_namespace = '/tb3_0' + + self.publish_next_frontier = False + + self.publish_ns = "" + + # self.timer = rospy.Timer(rospy.Duration(5), self.frontier_callback) + + self.init_subscribers_and_publishers(self.robot_namespace) + + rospy.loginfo("FINISHED INIT") + + def init_subscribers_and_publishers(self, ns): + + self.mapSub = rospy.Subscriber(ns + '/map', OccupancyGrid, self.map_callback) + + self.fringe = rospy.Publisher('/map_fringe', GridCells, queue_size=10) + + self.final_frontier_pub = rospy.Publisher('/selected_frontier', GridCells, queue_size=10) + + self.center_fringe_pub = rospy.Publisher('/selected_center', GridCells, queue_size=10) + + self.move_pub = rospy.Publisher(ns + '/move_base_simple/goal', PoseStamped, queue_size=10) + + #self.move_helper_sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.helper) + + #self.move_helper_pub = rospy.Publisher(ns + '/move_base_simple/goal', PoseStamped, + + queue_size=10) + + self.frontier_center_pub = rospy.Publisher(ns + '/frontier_list', PoseArray, queue_size=10) + + self.frontier_center_request = rospy.Subscriber('/frontier_request', String, self.request_callback) + + def request_callback(self, msg): + + ns = str(msg.data).replace("\"","") + + if self.robot_namespace != ns: + + self.robot_namespace = ns + + self.init_subscribers_and_publishers(ns) + + self.publish_ns = ns + + self.publish_next_frontier = True + + def helper(self, msg): + + self.move_helper_pub.publish(msg) + + def map_callback(self, msg): + + self.map = msg + + # if self.curr_msg_count > self.max_msg_count_before_start: + + # self.frontier_callback(0) + + # self.curr_msg_count = 0 + + # self.curr_msg_count += 1 + + # rospy.loginfo("curr message count is: " + str(self.curr_msg_count)) + + if SEND_STATE_MACHINE_FRONTIERS: + + if self.publish_next_frontier and self.publish_ns in str(msg.header.frame_id): + + rospy.loginfo("Publishing frontiers for: " + str(self.publish_ns)) + + self.frontier_callback(0) + + self.publish_next_frontier = False + + else: + + self.frontier_callback(0) + + def frontier_callback(self, timer_var): + + msg = self.map + + fringe_points = self.find_fringe(msg) + + if SEND_TO_FRONTIERS: + + all_frontier_clusters = self.cluster_cells(fringe_points) + + best_frontier_cells = self.calculate_least_cost_frontier(all_frontier_clusters) + + self.send_to_frontier(best_frontier_cells, msg) + + elif SEND_STATE_MACHINE_FRONTIERS: + + all_frontier_clusters = self.cluster_cells(fringe_points) + + centers = self.get_centers(all_frontier_clusters) + + self.send_centers_to_pub(centers) + + def find_fringe(self, msg): + + res = msg.info.resolution + + width = msg.info.width + + height = msg.info.height + + data = np.asarray(msg.data).reshape((width,height), order='F') + + expl = (data > -1) + + # Takes in a list of (x,y) locations to paint as explored nodes + + gridCells = GridCells() + + gridCells.cell_width = res + + gridCells.cell_height = res + + self.cell_threshold = self.threshold_multiplier*res + + header = Header() + + header.frame_id = msg.header.frame_id + + gridCells.header = header + + filter = np.asarray([-1,0,1,-2,0,2,-1,0,1]).reshape((3,3)) + + edgeX = sig.convolve2d(expl, filter, mode='same', fillvalue=-1) + + edgeY = sig.convolve2d(expl, filter.T, mode='same', fillvalue=-1) + + edge = np.sqrt(np.add(np.power(edgeX, 2), np.power(edgeY, 2))) + + open_cells = np.logical_and((data > -1), (data < 10)) + + fringe = np.logical_and((edge >= 2), open_cells) + + fringe_list = np.transpose(np.where(fringe)) + + pts = [] + + raw_pts = [] + + for node in fringe_list: + + tup = self.map_to_world(node[0], node[1], msg) + + pt = Point(tup[0], tup[1], 0) + + raw_pt = (tup[0], tup[1], 0) + + pts.append(pt) + + raw_pts.append(raw_pt) + + gridCells.cells = pts + + self.fringe.publish(gridCells) + + return raw_pts + + def map_to_world(self, x, y, my_map): + + """ + + converts a point from the map to the world + + :param x: float of x position + + :param y: float of y position + + :return: tuple of converted point + + @Author tberg1234 (from rbe3002) + + """ + + resolution = my_map.info.resolution + + originX = my_map.info.origin.position.x + + originY = my_map.info.origin.position.y + + # multiply by resolution, then move by origin offset + + x = x * resolution + originX + resolution / 2 + + y = y * resolution + originY + resolution / 2 + + return (x, y) + + def cluster_cells(self, gridCells): + + all_frontier_clusters = dict() # keeps track of clusters and the parent + + remaining_cells = copy.deepcopy(gridCells) # cells remaining to be assigned to a group + + for a_cell in gridCells: # in remaining_cells worked kinda but it was faster sooo + + if remaining_cells: + + assigned = False + + if all_frontier_clusters: + + for key in all_frontier_clusters: + + cells_in_cluster = all_frontier_clusters.get(key) + + iterable_cells = copy.deepcopy(cells_in_cluster) + + for other_cell in iterable_cells: + + if np.linalg.norm(np.asarray(a_cell) - np.asarray(other_cell)) <= self.cell_threshold: + + if a_cell not in cells_in_cluster: + + assigned = True + + cells_in_cluster.append(a_cell) + + if a_cell in remaining_cells: + + remaining_cells.remove(a_cell) + + # all_frontier_clusters[key] = cells_in_cluster + + else: + + assigned = True + + for potential_cell in remaining_cells: # in gridCells + + if np.linalg.norm(np.asarray(a_cell) - np.asarray(potential_cell)) <= self.cell_threshold: + + if potential_cell not in cells_in_cluster: + + cells_in_cluster.append(potential_cell) + + if potential_cell in remaining_cells: + + remaining_cells.remove(potential_cell) + + # all_frontier_clusters[key] = cells_in_cluster + + all_frontier_clusters[key] = cells_in_cluster + + if not assigned: + + first_list = list() + + first_list.append(a_cell) + + remaining_cells.remove(a_cell) + + all_frontier_clusters[a_cell] = first_list + + else: + + first_list = list() + + first_list.append(a_cell) + + remaining_cells.remove(a_cell) + + all_frontier_clusters[a_cell] = first_list + + else: + + pass + + return all_frontier_clusters + + def calculate_least_cost_frontier(self, all_frontier_clusters): + + # max_key, max_value = max(all_frontier_clusters.items(), key=lambda x: len(set(x[1]))) + + # return max_key, max_value + + cost_dict = dict() + + length_dict = {key: len(value) for key, value in all_frontier_clusters.items()} + + robot_position = self.get_current_position_in_map() + + for key in all_frontier_clusters.keys(): + + cells = all_frontier_clusters.get(key) + + size = length_dict.get(key) + + center = np.average(cells, axis=0) + + distance = np.linalg.norm(np.asarray(center) - np.asarray(robot_position)) + + cost = self.weight_output*distance + (1-self.weight_output)*(1-size) + + cost_dict[key] = cost + + sorted_costs = sorted(cost_dict, key=cost_dict.get, reverse=False) + + return all_frontier_clusters.get(sorted_costs[0]) + + def get_centers(self, all_frontier_clusters): + + cost_dict = dict() + + length_dict = {key: len(value) for key, value in all_frontier_clusters.items()} + + robot_position = self.get_current_position_in_map() + + for key in all_frontier_clusters.keys(): + + cells = all_frontier_clusters.get(key) + + size = length_dict.get(key) + + center = np.average(cells, axis=0) + + distance = np.linalg.norm(np.asarray(center) - np.asarray(robot_position)) + + cost = self.weight_output * distance + (1 - self.weight_output) * (1 - size) + + cost_dict[key] = cost + + sorted_costs = sorted(cost_dict, key=cost_dict.get, reverse=False) + + centers = list() + + for cost in sorted_costs: + + fr = all_frontier_clusters.get(cost) + + c = np.average(fr, axis=0) + + centers.append(c) + + return centers + + def send_centers_to_pub(self, centers): + + centers_for_pub = [] + + counter = 0 + + for c in centers: + + if counter > SM_LIMIT: + + break + + move_msg = Pose() + + move_msg.position.x = c[0] + + move_msg.position.y = c[1] + + move_msg.position.z = c[2] + + move_msg.orientation.x = 0 + + move_msg.orientation.y = 0 + + move_msg.orientation.z = 0 + + move_msg.orientation.w = 1 + + centers_for_pub.append(move_msg) + + counter += 1 + + full_msg = PoseArray() + + full_msg.poses = centers_for_pub + + full_msg.header.frame_id = self.robot_namespace + '/map' + + self.frontier_center_pub.publish(full_msg) + + rospy.loginfo("sent frontier list") + + def send_to_frontier(self, best_frontier_cells, msg): + + final_frontier = list() + + for cell in best_frontier_cells: + + final_frontier.append(Point(cell[0], cell[1], cell[2])) + + res = msg.info.resolution + + gridCells = GridCells() + + gridCells.cell_width = res + + gridCells.cell_height = res + + header = Header() + + header.frame_id = msg.header.frame_id + + gridCells.header = header + + gridCells.cells = final_frontier + + self.final_frontier_pub.publish(gridCells) + + center = np.average(best_frontier_cells, axis=0) + + center_lst = list() + + center_lst.append(Point(center[0], center[1], center[2])) + + gridCells.cells = center_lst + + self.center_fringe_pub.publish(gridCells) + + move_msg = PoseStamped() + + move_msg.header.frame_id = 'map' + + move_msg.pose.position.x = center[0] + + move_msg.pose.position.y = center[1] + + move_msg.pose.position.z = center[2] + + rospy.loginfo("SENDING TO FRONTIER: " + str(center)) + + move_msg.pose.orientation.x = 0 + + move_msg.pose.orientation.y = 0 + + move_msg.pose.orientation.z = 0 + + move_msg.pose.orientation.w = 1 + + self.move_pub.publish(move_msg) + + def get_current_position_in_map(self): + + # Position of the robot base in the map + + p1 = PoseStamped() + + p1.header.frame_id = self.robot_namespace+"/base_link" + + p1.pose.orientation.w = 1.0 # Neutral orientation + + # try: + + # tf_listener_ = TransformListener() + + # p_in_base = tf_listener_.transformPose(self.robot_namespace + "/map", p1) + + # except ROSTimeMovedBackwardsException: + + # p_in_base = PoseStamped() + + # print("ROS Interrupt Exception! Just ignore the exception!") + + p_in_base = PoseStamped() + + # p_in_base = self.tf_listener_.transformPose(self.robot_namespace+"/map", p1) + + return (p_in_base.pose.position.x, p_in_base.pose.position.y, p_in_base.pose.position.z) + +if __name__ == '__main__': + + node = find_frontier() + + while not rospy.is_shutdown(): + + pass + diff --git a/multi_map_ros/src/multi_map_ros/FrontierSelection/src/move_base_pub.py b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/move_base_pub.py new file mode 100755 index 0000000..cd86908 --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/move_base_pub.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python3.8 + +import rospy +import numpy as np +import copy +from scipy import signal as sig + +from std_msgs.msg import Header +from nav_msgs.msg import OccupancyGrid, GridCells +from geometry_msgs.msg import Point, PoseStamped +from tf import TransformListener + +class move_base_msg_forwarder: + def __init__(self): + rospy.init_node('move_base_msg_forwarder') + self.robot_namespace = '/tb3_0' + self.move_helper_sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.helper) + self.move_helper_pub = rospy.Publisher(self.robot_namespace+'/move_base_simple/goal', PoseStamped, queue_size=10) + + def helper(self, msg): + self.move_helper_pub.publish(msg) + + +if __name__ == '__main__': + node = move_base_msg_forwarder() + + while not rospy.is_shutdown(): + pass \ No newline at end of file diff --git a/multi_map_ros/src/multi_map_ros/FrontierSelection/src/move_base_reset.py b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/move_base_reset.py new file mode 100755 index 0000000..acc4fee --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/move_base_reset.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3.8 + +import rospy +import roslaunch +import numpy as np +import copy +from scipy import signal as sig +import subprocess +import signal + +from std_msgs.msg import Header, String +from nav_msgs.msg import OccupancyGrid, GridCells +from geometry_msgs.msg import Point, PoseStamped +from tf import TransformListener + +class move_base_reset: + + def __init__(self): + rospy.init_node('mapping_reset_node') + self.robot_namespace = '/tb3_0' + self.stop_signal = rospy.Subscriber('/stop_move_base', String, self.stop) + self.start_signal = rospy.Subscriber('/start_move_base', String, self.start) + + + def stop(self, msg): + # self.launch.shutdown() + print("STOPPING") + self.child.send_signal(signal.SIGINT) + + + def start(self, msg): + # uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + # roslaunch.configure_logging(uuid) + # self.launch = roslaunch.parent.ROSLaunchParent(uuid, [ + # "/home/taylor/multidrone_slam/src/2122_MultiDroneIndoorSLAM/turtlebot3_simulations/turtlebot3_gazebo/launch/multi_move_base.launch"]) + # self.launch.start() + # rospy.loginfo("move base started") + + print("STARTING") + self.child = subprocess.Popen(["roslaunch", "turtlebot3_gazebo", "multi_move_base.launch"]) + # child.wait() #You can use this line to block the parent process until the child process finished. + # print("parent process") + # print(self.child.poll()) + + rospy.loginfo('The PID of child: %d', self.child.pid) + # print("The PID of child:", self.child.pid) + + +if __name__ == '__main__': + node = move_base_reset() + + while not rospy.is_shutdown(): + pass \ No newline at end of file diff --git a/multi_map_ros/src/multi_map_ros/FrontierSelection/src/publish_inputs_for_learning.py b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/publish_inputs_for_learning.py new file mode 100755 index 0000000..4690226 --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/publish_inputs_for_learning.py @@ -0,0 +1,200 @@ +#!/usr/bin/env python3.8 + +import rospy +import numpy as np +import copy + +from matplotlib import pyplot, colors +from scipy import signal as sig + +from std_msgs.msg import Header +from nav_msgs.msg import OccupancyGrid, GridCells, MapMetaData, Odometry +from geometry_msgs.msg import Point, PoseStamped +from tf import TransformListener + +class publish_inputs_for_learning: + + def __init__(self): + rospy.init_node('publish_inputs_rl') + self.robot_namespace = '/tb3_0' + # self.desired_dimension_x = 640 + # self.desired_dimension_y = 640 + # self.desired_dimension_x = 288 + # self.desired_dimension_y = 160 + self.desired_dimension_x = 352 + self.desired_dimension_y = 224 + self.map = OccupancyGrid() + self.map_cells = np.array([]) + self.fringe = GridCells() + self.pub_msg = OccupancyGrid() + self.odom = Odometry() + self.resized_map_set = False + self.pose_map_sent = False + self.fringe_map_sent = False + self.full_map_sent = False + + #below commented because size of origional map + self.mapSub = rospy.Subscriber(self.robot_namespace+'/map', OccupancyGrid, self.map_callback) + self.mapPub = rospy.Publisher(self.robot_namespace+'/rl_map', OccupancyGrid, queue_size=10) + self.posePub = rospy.Publisher(self.robot_namespace+'/rl_pose', OccupancyGrid, queue_size=10) + self.frontierPub = rospy.Publisher(self.robot_namespace+'/rl_frontiers', OccupancyGrid, queue_size=10) + self.fringeSub = rospy.Subscriber('/map_fringe', GridCells, self.fringe_callback) + self.poseSub = rospy.Subscriber(self.robot_namespace+'/odom', Odometry, self.odom_callback) + + self.last_map = OccupancyGrid() + self.last_fringe = GridCells() + self.last_odom = Odometry() + + + def map_callback(self, msg): + self.map = msg + map_height = self.map.info.height + map_width = self.map.info.width + self.map_cells = np.array(self.map.data) + resized_map = np.reshape(self.map_cells, (map_height, map_width)) + resized_map = np.pad(resized_map, [(0, self.desired_dimension_y-map_height), (0, self.desired_dimension_x-map_width)], mode='constant', constant_values=-1) + final_resized_map = resized_map.reshape(-1) + + self.pub_msg = OccupancyGrid() + pub_msg_info = MapMetaData() + pub_msg_header = Header() + pub_msg_info.height = self.desired_dimension_y + pub_msg_info.width = self.desired_dimension_x + pub_msg_info.origin = self.map.info.origin + pub_msg_info.resolution = self.map.info.resolution + self.pub_msg.info = pub_msg_info + pub_msg_header.frame_id = self.map.header.frame_id + self.pub_msg.header = pub_msg_header + self.pub_msg.data = final_resized_map + + self.resized_map_set = True + + self.mapPub.publish(self.pub_msg) + + # if not self.full_map_sent: + # rospy.loginfo("FULL MAP SENT") + # self.full_map_sent = True + + self.last_map = msg + + + + def fringe_callback(self, msg): + self.fringe = msg + + if self.resized_map_set: + cells = msg.cells + + fringe_map = OccupancyGrid() + fringe_arr = np.full((self.desired_dimension_y, self.desired_dimension_x), -1) + fringe_resized_map = fringe_arr.reshape(-1) + index_arr = [] + for c in cells: + index_arr.append(int(self.point_to_index((c.x, c.y), self.pub_msg))) + + for i in index_arr: + fringe_resized_map[i] = 0 + + int_fringe_resized_map = fringe_resized_map.astype(np.int8) + + fringe_map_info = MapMetaData() + fringe_map_header = Header() + fringe_map_info.height = self.desired_dimension_y + fringe_map_info.width = self.desired_dimension_x + fringe_map_info.origin = self.map.info.origin + fringe_map_info.resolution = self.map.info.resolution + fringe_map.info = fringe_map_info + fringe_map_header.frame_id = self.map.header.frame_id + fringe_map.header = fringe_map_header + fringe_map.data = int_fringe_resized_map + self.frontierPub.publish(fringe_map) + + # if not self.fringe_map_sent: + # rospy.loginfo("FRINGE MAP SENT") + # self.fringe_map_sent = True + + self.last_fringe = msg + + + def odom_callback(self, msg): + self.odom = msg + + if self.resized_map_set: + odom_map = OccupancyGrid() + odom_arr = np.full((self.desired_dimension_y, self.desired_dimension_x), -1) + odom_resized_map = odom_arr.reshape(-1) + + index_of_robot = int(self.point_to_index((msg.pose.pose.position.x, msg.pose.pose.position.y), self.pub_msg)) + + odom_resized_map[index_of_robot] = 0 + + int_odom_resized_map = odom_resized_map.astype(np.int8) + + odom_map_info = MapMetaData() + odom_map_header = Header() + odom_map_info.height = self.desired_dimension_y + odom_map_info.width = self.desired_dimension_x + odom_map_info.origin = self.map.info.origin + odom_map_info.resolution = self.map.info.resolution + odom_map.info = odom_map_info + odom_map_header.frame_id = self.map.header.frame_id + odom_map.header = odom_map_header + odom_map.data = int_odom_resized_map + self.posePub.publish(odom_map) + + # if not self.pose_map_sent: + # rospy.loginfo("POSE MAP SENT") + # self.pose_map_sent = True + + self.last_odom = msg + + + def world_to_map(self, x, y, my_map): + """ + converts a point from the world to the map + :param x: float of x position + :param y: float of y position + :return: tuple of converted point + """ + + return self.convert_location((x, y), my_map) + + + def convert_location(self, loc, my_map): + """converts points to the grid""" + res = my_map.info.resolution + Xorigin = my_map.info.origin.position.x + Yorigin = my_map.info.origin.position.y + + # print("loc: " + str(loc)) + # print("Xorigin: " + str(Xorigin)) + # print("res: "+ str(res)) + + # offset from origin and then divide by resolution + Xcell = int((loc[0] - Xorigin - (res / 2)) / res) + Ycell = int((loc[1] - Yorigin - (res / 2)) / res) + return (Xcell, Ycell) + + + def point_to_index(self, point, my_map): + """convert a index to a point""" + pt = self.world_to_map(point[0], point[1], my_map) + return pt[1] * my_map.info.width + pt[0] + + + def world_to_map(self, x, y, my_map): + """ + converts a point from the world to the map + :param x: float of x position + :param y: float of y position + :return: tuple of converted point + """ + + return self.convert_location((x,y), my_map) + + +if __name__ == '__main__': + node = publish_inputs_for_learning() + + while not rospy.is_shutdown(): + pass \ No newline at end of file diff --git a/multi_map_ros/src/multi_map_ros/FrontierSelection/src/resize_map.py b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/resize_map.py new file mode 100755 index 0000000..6b124c1 --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/resize_map.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3.8 + +import rospy +import numpy as np +import copy + +from matplotlib import pyplot, colors +from scipy import signal as sig + +from std_msgs.msg import Header +from nav_msgs.msg import OccupancyGrid, GridCells, MapMetaData +from geometry_msgs.msg import Point, PoseStamped +from tf import TransformListener + +class resize_map: + + def __init__(self): + rospy.init_node('resize_map') + self.robot_namespace = '/tb3_0' + self.mapSub = rospy.Subscriber(self.robot_namespace+'/map', OccupancyGrid, self.map_callback) + self.mapPub = rospy.Publisher(self.robot_namespace+'/resized_map', OccupancyGrid, queue_size=10) + self.map = OccupancyGrid() + self.map_cells = np.array([]) + self.desired_dimension = 640 + + def map_callback(self, msg): + self.map = msg + map_height = self.map.info.height + map_width = self.map.info.width + self.map_cells = np.array(self.map.data) + resized_map = np.reshape(self.map_cells, (map_height, map_width)) + resized_map = np.pad(resized_map, [(0, self.desired_dimension-map_height), (0, self.desired_dimension-map_width)], mode='constant') + final_resized_map = resized_map.reshape(-1) + + pub_msg = OccupancyGrid() + pub_msg_info = MapMetaData() + pub_msg_header = Header() + pub_msg_info.height = self.desired_dimension + pub_msg_info.width = self.desired_dimension + pub_msg_info.origin = self.map.info.origin + pub_msg_info.resolution = self.map.info.resolution + pub_msg.info = pub_msg_info + pub_msg_header.frame_id = self.map.header.frame_id + pub_msg.header = pub_msg_header + pub_msg.data = final_resized_map + self.mapPub.publish(pub_msg) + + +if __name__ == '__main__': + node = resize_map() + + while not rospy.is_shutdown(): + pass \ No newline at end of file diff --git a/slam_gmapping/.gitignore b/slam_gmapping/.gitignore new file mode 100644 index 0000000..23255e5 --- /dev/null +++ b/slam_gmapping/.gitignore @@ -0,0 +1,11 @@ +.cproject +.project +.pydevproject +cmake_install.cmake +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +*.pyc +*~ diff --git a/slam_gmapping/README.md b/slam_gmapping/README.md new file mode 100644 index 0000000..04be513 --- /dev/null +++ b/slam_gmapping/README.md @@ -0,0 +1,2 @@ +slam_gmapping [![Build Status](https://travis-ci.com/ros-perception/slam_gmapping.svg?branch=melodic-devel)](https://travis-ci.org/ros-perception/slam_gmapping) +================================================================================================================================================================ diff --git a/slam_gmapping/gmapping/CHANGELOG.rst b/slam_gmapping/gmapping/CHANGELOG.rst new file mode 100644 index 0000000..0a8348e --- /dev/null +++ b/slam_gmapping/gmapping/CHANGELOG.rst @@ -0,0 +1,202 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gmapping +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.4.2 (2020-10-02) +------------------ +* change find_package(Boost REQUIRED signales) to find_package(Boost REQUIRED) for noteic (`#84 `_) +* Contributors: Kei Okada + +1.4.1 (2020-03-16) +------------------ +* Merge pull request `#85 `_ from k-okada/install_nodelet + install slam_gmapping_nodelet +* Merge pull request `#87 `_ from acxz/patch-1 + remove signals dep +* remove signals dep + signals is included in boost > 1.70 +* install slam_gmapping_nodelet +* Contributors: Kei Okada, Michael Ferguson, acxz + +1.4.0 (2019-07-12) +------------------ +* update license to BSD and maintainer to ros-orphaned-packages@googlegroups.com + since original gmapping source and ROS openslam_gmapping package has been updated to the BSD-3 license, I think we have no reason to use CC for slam_gmapping package +* Contributors: Kei Okada + +1.3.10 (2018-01-23) +------------------- +* Install nodelet plugin descriptor file. (`#56 `_) +* Contributors: Mikael Arguedas, gavanderhoorn + +1.3.9 (2017-10-22) +------------------ +* remove unused file +* add missing nodelet dependency to find_package +* make rostest in CMakeLists optional (`ros/rosdistro#3010 `_) +* Add nodelet implementation. (`#41 `_) + * Add nodelet implementation. + Add additional nodelet layer to mirror the node + implementation. This allows the Slam GMapping + library to be run as a nodelet instead. This + would allow you to, for example, run it under + the same nodelet manager as the nodelet producing + the /scan output for greater efficiency. + * Remove superfluous semicolons + Removed superfluous semicolons and + mildly clarified info stream output. +* fix comment, change type from double to int (`#40 `_) + * fix comment, change type from double to int + * fix comment, iterations param is not double but int +* Contributors: David Hodo, Kevin Wells, Lukas Bulwahn, Oscar Lima, Vincent Rabaud + +1.3.8 (2015-07-31) +------------------ +* fix a test that would take too long sometimes +* better verbosity +* add a test for upside down lasers +* add a test for symmetry +* make sure the laser sent to gmapping is always centered +* do not display warning message if scan is processed at some point +* Contributors: Vincent Rabaud + +1.3.7 (2015-07-04) +------------------ +* get replay to behave like live processing +* Contributors: Vincent Rabaud + +1.3.6 (2015-06-26) +------------------ +* Don't crash on exit from replay. +* replay: Add "on_done" command line parameter. + Example usage: + ros run gmapping slam_gmapping_replay --scan_topic=/scan --bag_filename=/tmp/in.bag --on_done "rosrun map_server map_saver -f foo" _particles:=100 _maxUrange:=10 +* replay: Handle order-of-data-in-bag-file issues better at startup. + Silently discard scans at startup that arrive prior to the TF frames + required by those scans. + TF can't interpolate from non-existant data, so discard scans that + arrive with timestamps prior to the first TF frame. We do this + expediently by discarding laser scans that throw + ExtrapolationException's until/unless we successfully process our first + scan. + Similarly, ignore LookupException's at startup. +* Fix indexing error for inverting scan. +* add test dependencies +* get replay test to pass +* rename test +* fixing include order + removing pointer for Gmapping object +* Minor Fix thanks to vrabaud comments +* [new feature] replay on bag file + The aim is to provide a way to get exactly the same map after running + gmapping multiple times on the same rosbag file. It wasn't possible with the + tool 'rosbag play', indeed one missing laser scan could provide really + different results. + Moreover, this modification allow to process rosbag offline at the maximum + speed with the guarantee that all lasers scans are processed. It is + useful in automatic tests and when finding optimal gmapping parameters with a script. + Example usage: + rosrun gmapping slam_gmapping_replay --scan_topic=/scan --bag_filename=/tmp/in.bag _particles:=100 _maxUrange:=10 + more options: + rosrun gmapping slam_gmapping_replay --help +* spliting init and constructor + The objective is to allow future handling of replay offline rosbag files. + This commit also add a variable for the seed, with the objective is to allow to + have repeateable run (for future offline rosbag replay) +* Added cfloat include +* Change arbitrary constant to FLT_EPSILON +* Added check that scan goes from -x to x +* Contributors: Laurent GEORGE, Patrick Doyle, Qiao Huang, Vincent Rabaud, Windel Bouwman + +1.3.5 (2014-08-28) +------------------ +* Fixed typo in slam_gmapping_pr2.launch + Fixed a typo in the launchfile in the parameter "map_update_interval". +* Contributors: DaMalo + +1.3.4 (2014-08-07) +------------------ +* Reenabled temporal update in slam_gmapping.cpp +* Contributors: DaMalo + +1.3.3 (2014-06-23) +------------------ +* Adding the ability to set openslam_gmapping miminumScore through the ros parameter minimumScore +* Contributors: Koen Lekkerkerker, William Woodall + +1.3.2 (2014-01-14) +------------------ +* Contributors: Vincent Rabaud + +1.3.1 (2014-01-13) +------------------ +* Fix usage of scoped locks so that they are not immediately destroyed. + fixes `#11 `_ +* check for CATKIN_ENABLE_TESTING +* Contributors: Lukas Bulwahn, Stefan Kohlbrecher, William Woodall + +1.3.0 (2013-06-28) +------------------ +* Renamed to gmapping, adding metapackage for slam_gmapping +* catkinize slam_gmapping +* Changed reference frame from base to laser to account for upside down and/or back facing laserscanners. + - Added a check if the scanner is facing down + - Added a safety check if the scanner is aligned planar + - Made laserscan min- and max-angles global as they are needed later for scanners with a negative angle-increment + - Replaced the base->laser pose for gmapping with the identity transform and included the base->laser part into the gmap_pose + - Removed a transform-lookup from the map->odom transformation process as it is not needed anymore + These changes should make gmapping more robust against laserscanners that are mounted upside down, facing backwards or are rotating counter-clockwise. + It will also allow gmapping to work with panning laserscanners, since the transform base->laser is no longer considered fixed. +* Fix poorly formed paths in patches + These patches won't apply in Fedora because they contain "..", which is considered "unsafe" +* Fixed test files to use the new rosbag command layout. +* Respect tf_prefix when sending maps +* Fixed tf expiration +* Added tf_delay param +* Add gcc 4.7 patch and Precise support by removing wiped during installed +* Oneiric linker fixes, bump version to 1.2.6 +* Convert to not use bullet datatypes directly +* Rejiggered linker lines to accommodate Oneiric's stricter linker behavior. +* Now uses angle_increment provided in laser scan message, instead of computing it myself (not sure why I was doing that, anyway), `#4730 `_ +* Applied patch to avoid assert when laser gives varying number of beams per + scan, `#4856 `_. Added the bag from that ticket as a test case. +* Applied patch from `#4984 `_ to fix occ grid generation with lasers that provide scans in reverse order +* Applied patch from `#4583 `_ with misc fixes to our patch against gmapping +* Excluded test program from all build +* Applied typo fix from Maurice Fallon +* Added Ubuntu platform tags to manifest +* Removed unused inverted_laser parameter +* Added transform logic necessary to account for non-horizontal lasers. This + change is intended to handle upside-down lasers, but should also work for + non-planar lasers (as long as the vertical structure of the environment is + continuous), `#3052 `_. I tested minimally with a hacked version of Stage, but + this functionality still needs to be validated on data from a real robot + with an upside-down laser. +* Reindexed bag used in testing +* Added publication of entropy +* add entropy computation method +* Added occ_thresh parameter +* Turning time based updates off by default +* Updating so that gmapping updates on a timer when not moving. Added the temporalUpdate parameter and updated docs. +* Updated md5sums for new bags +* Threading publishing of transforms so that they are published regularly regardless of how long map updates take. +* Updated patch to fix gcc 4.4 warning, and made top-level Makefile call through to Makefile.gmapping on clean +* Updating to work with the navigation stack. Now publishes header information on map messages. +* Applied patch to update tf usage, `#3457 `_ +* Remove use of deprecated rosbuild macros +* Removed unused parameter +* Fix the position gmapping gives to the map's info. Was trying to center the map on the origin, when it should just have been using the world positiong of the map's origin (`#3037 `_) +* Added doc cleared to manifest +* Switched sleep to WallDuration, to avoid getting stuck after rosplay has run out of time data to publish +* Converted from tf::MessageNotifier to tf::MessageFilter. +* Reverted accidental change to CMakeLists.txt +* Added advertisement and publication of MapMetaData (docs are updated to + match). Added minimal test for the resulting map. Updated local params to use + NodeHandle("~"). +* Added latched topic version of map, API cleared +* Updated manifest to explain version that we're using +* Remove ros/node.h inclusion +* tf publishes on new topic: \tf. See ticket `#2381 `_ +* Merging in changes from reorgnization of laser pipeline. +* removed redundant code (getOdomPose) that could result in unnecessary warnings +* Contributors: Ben Struss, Dave Hershberger, Dereck Wonnacott, Mike Ferguson, Scott K Logan, Vincent Rabaud, William Woodall, duhadway-bosch, eitan, gerkey, jfaust, jleibs, kwc, meeussen, vrabaud, wheeler diff --git a/slam_gmapping/gmapping/CMakeLists.txt b/slam_gmapping/gmapping/CMakeLists.txt new file mode 100644 index 0000000..04f3c8d --- /dev/null +++ b/slam_gmapping/gmapping/CMakeLists.txt @@ -0,0 +1,94 @@ +cmake_minimum_required(VERSION 2.8) +project(gmapping) + +find_package(catkin REQUIRED nav_msgs nodelet openslam_gmapping roscpp tf rosbag_storage) + +find_package(Boost REQUIRED) + +include_directories(${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) + +include_directories(src) + +catkin_package() + +add_executable(slam_gmapping src/slam_gmapping.cpp src/main.cpp) +target_link_libraries(slam_gmapping ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +if(catkin_EXPORTED_TARGETS) + add_dependencies(slam_gmapping ${catkin_EXPORTED_TARGETS}) +endif() + +add_library(slam_gmapping_nodelet src/slam_gmapping.cpp src/nodelet.cpp) +target_link_libraries(slam_gmapping_nodelet ${catkin_LIBRARIES}) + +add_executable(slam_gmapping_replay src/slam_gmapping.cpp src/replay.cpp) +target_link_libraries(slam_gmapping_replay ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +if(catkin_EXPORTED_TARGETS) +add_dependencies(slam_gmapping_replay ${catkin_EXPORTED_TARGETS}) +endif() + +install(TARGETS slam_gmapping slam_gmapping_nodelet slam_gmapping_replay + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + if(TARGET tests) + add_executable(gmapping-rtest EXCLUDE_FROM_ALL test/rtest.cpp) + target_link_libraries(gmapping-rtest ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) + add_dependencies(tests gmapping-rtest) + endif() + + # Need to make the tests more robust; currently the output map can differ + # substantially between runs. + catkin_download_test_data( + ${PROJECT_NAME}_basic_localization_stage_indexed.bag + http://download.ros.org/data/gmapping/basic_localization_stage_indexed.bag + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 322a0014f47bcfbb0ad16a317738b0dc) + catkin_download_test_data( + ${PROJECT_NAME}_hallway_slow_2011-03-04-21-41-33.bag + http://download.ros.org/data/gmapping/hallway_slow_2011-03-04-21-41-33.bag + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 e772b89713693adc610f4c5b96f5dc03) + catkin_download_test_data( + ${PROJECT_NAME}_basic_localization_stage_groundtruth.pgm + http://download.ros.org/data/gmapping/basic_localization_stage_groundtruth.pgm + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 abf208f721053915145215b18c98f9b3) + catkin_download_test_data( + ${PROJECT_NAME}_test_replay_crash.bag + https://github.com/ros-perception/slam_gmapping_test_data/raw/master/test_replay_crash.bag + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 bb0e086207eb4fccf0b13d3406f610a1) + catkin_download_test_data( + ${PROJECT_NAME}_test_turtlebot.bag + https://github.com/ros-perception/slam_gmapping_test_data/raw/master/test_turtlebot.bag + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 402e1e5f7c00445d2a446e58e3151830) + catkin_download_test_data( + ${PROJECT_NAME}_test_upside_down.bag + https://github.com/ros-perception/slam_gmapping_test_data/raw/master/test_upside_down.bag + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 3b026a2144ec14f3fdf218b5c7077d54) + set(LOCAL_DEPENDENCIES gmapping-rtest ${PROJECT_NAME}_basic_localization_stage_indexed.bag + ${PROJECT_NAME}_test_replay_crash.bag + ${PROJECT_NAME}_test_turtlebot.bag + ${PROJECT_NAME}_test_upside_down.bag + ${PROJECT_NAME}_hallway_slow_2011-03-04-21-41-33.bag + ${PROJECT_NAME}_basic_localization_stage_groundtruth.pgm + slam_gmapping + slam_gmapping_replay + ) + add_rostest(test/basic_localization_stage.launch DEPENDENCIES ${LOCAL_DEPENDENCIES}) + add_rostest(test/basic_localization_stage_replay.launch DEPENDENCIES ${LOCAL_DEPENDENCIES}) + add_rostest(test/basic_localization_stage_replay2.launch DEPENDENCIES ${LOCAL_DEPENDENCIES}) + add_rostest(test/basic_localization_symmetry.launch DEPENDENCIES ${LOCAL_DEPENDENCIES}) + add_rostest(test/basic_localization_upside_down.launch DEPENDENCIES ${LOCAL_DEPENDENCIES}) + add_rostest(test/basic_localization_laser_different_beamcount.test DEPENDENCIES ${LOCAL_DEPENDENCIES}) +endif() diff --git a/slam_gmapping/gmapping/cmake-build-debug/.cmake/api/v1/query/cache-v2 b/slam_gmapping/gmapping/cmake-build-debug/.cmake/api/v1/query/cache-v2 new file mode 100644 index 0000000..e69de29 diff --git a/slam_gmapping/gmapping/cmake-build-debug/.cmake/api/v1/query/cmakeFiles-v1 b/slam_gmapping/gmapping/cmake-build-debug/.cmake/api/v1/query/cmakeFiles-v1 new file mode 100644 index 0000000..e69de29 diff --git a/slam_gmapping/gmapping/cmake-build-debug/.cmake/api/v1/query/codemodel-v2 b/slam_gmapping/gmapping/cmake-build-debug/.cmake/api/v1/query/codemodel-v2 new file mode 100644 index 0000000..e69de29 diff --git a/slam_gmapping/gmapping/cmake-build-debug/.cmake/api/v1/query/toolchains-v1 b/slam_gmapping/gmapping/cmake-build-debug/.cmake/api/v1/query/toolchains-v1 new file mode 100644 index 0000000..e69de29 diff --git a/slam_gmapping/gmapping/cmake-build-debug/CMakeCache.txt b/slam_gmapping/gmapping/cmake-build-debug/CMakeCache.txt new file mode 100644 index 0000000..d7d79a5 --- /dev/null +++ b/slam_gmapping/gmapping/cmake-build-debug/CMakeCache.txt @@ -0,0 +1,401 @@ +# This is the CMakeCache file. +# For build in directory: /home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug +# It was generated by CMake: /home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/bin/cmake +# You can edit this file to change values found and used by cmake. +# If you do not want to change any of the values, simply exit the editor. +# If you do want to change a value, simply edit, save, and exit the editor. +# The syntax for the file is as follows: +# KEY:TYPE=VALUE +# KEY is the name of a variable in the cache. +# TYPE is a hint to GUIs for the type of VALUE, DO NOT EDIT TYPE!. +# VALUE is the current value for the KEY. + +######################## +# EXTERNAL cache entries +######################## + +//Path to a program. +CMAKE_ADDR2LINE:FILEPATH=/usr/bin/addr2line + +//Path to a program. +CMAKE_AR:FILEPATH=/usr/bin/ar + +//Choose the type of build, options are: None Debug Release RelWithDebInfo +// MinSizeRel ... +CMAKE_BUILD_TYPE:STRING=Debug + +//Id string of the compiler for the CodeBlocks IDE. Automatically +// detected when left empty +CMAKE_CODEBLOCKS_COMPILER_ID:STRING= + +//The CodeBlocks executable +CMAKE_CODEBLOCKS_EXECUTABLE:FILEPATH=CMAKE_CODEBLOCKS_EXECUTABLE-NOTFOUND + +//Additional command line arguments when CodeBlocks invokes make. +// Enter e.g. -j to get parallel builds +CMAKE_CODEBLOCKS_MAKE_ARGUMENTS:STRING=-j12 + +//Enable/Disable color output during build. +CMAKE_COLOR_MAKEFILE:BOOL=ON + +//CXX compiler +CMAKE_CXX_COMPILER:FILEPATH=/usr/bin/c++ + +//A wrapper around 'ar' adding the appropriate '--plugin' option +// for the GCC compiler +CMAKE_CXX_COMPILER_AR:FILEPATH=/usr/bin/gcc-ar-9 + +//A wrapper around 'ranlib' adding the appropriate '--plugin' option +// for the GCC compiler +CMAKE_CXX_COMPILER_RANLIB:FILEPATH=/usr/bin/gcc-ranlib-9 + +//Flags used by the CXX compiler during all build types. +CMAKE_CXX_FLAGS:STRING= + +//Flags used by the CXX compiler during DEBUG builds. +CMAKE_CXX_FLAGS_DEBUG:STRING=-g + +//Flags used by the CXX compiler during MINSIZEREL builds. +CMAKE_CXX_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG + +//Flags used by the CXX compiler during RELEASE builds. +CMAKE_CXX_FLAGS_RELEASE:STRING=-O3 -DNDEBUG + +//Flags used by the CXX compiler during RELWITHDEBINFO builds. +CMAKE_CXX_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG + +//C compiler +CMAKE_C_COMPILER:FILEPATH=/usr/bin/cc + +//A wrapper around 'ar' adding the appropriate '--plugin' option +// for the GCC compiler +CMAKE_C_COMPILER_AR:FILEPATH=/usr/bin/gcc-ar-9 + +//A wrapper around 'ranlib' adding the appropriate '--plugin' option +// for the GCC compiler +CMAKE_C_COMPILER_RANLIB:FILEPATH=/usr/bin/gcc-ranlib-9 + +//Flags used by the C compiler during all build types. +CMAKE_C_FLAGS:STRING= + +//Flags used by the C compiler during DEBUG builds. +CMAKE_C_FLAGS_DEBUG:STRING=-g + +//Flags used by the C compiler during MINSIZEREL builds. +CMAKE_C_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG + +//Flags used by the C compiler during RELEASE builds. +CMAKE_C_FLAGS_RELEASE:STRING=-O3 -DNDEBUG + +//Flags used by the C compiler during RELWITHDEBINFO builds. +CMAKE_C_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG + +//Path to a program. +CMAKE_DLLTOOL:FILEPATH=CMAKE_DLLTOOL-NOTFOUND + +//Flags used by the linker during all build types. +CMAKE_EXE_LINKER_FLAGS:STRING= + +//Flags used by the linker during DEBUG builds. +CMAKE_EXE_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during MINSIZEREL builds. +CMAKE_EXE_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during RELEASE builds. +CMAKE_EXE_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during RELWITHDEBINFO builds. +CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//Enable/Disable output of compile commands during generation. +CMAKE_EXPORT_COMPILE_COMMANDS:BOOL= + +//Install path prefix, prepended onto install directories. +CMAKE_INSTALL_PREFIX:PATH=/usr/local + +//Path to a program. +CMAKE_LINKER:FILEPATH=/usr/bin/ld + +//Path to a program. +CMAKE_MAKE_PROGRAM:FILEPATH=/usr/bin/make + +//Flags used by the linker during the creation of modules during +// all build types. +CMAKE_MODULE_LINKER_FLAGS:STRING= + +//Flags used by the linker during the creation of modules during +// DEBUG builds. +CMAKE_MODULE_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during the creation of modules during +// MINSIZEREL builds. +CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during the creation of modules during +// RELEASE builds. +CMAKE_MODULE_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during the creation of modules during +// RELWITHDEBINFO builds. +CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//Path to a program. +CMAKE_NM:FILEPATH=/usr/bin/nm + +//Path to a program. +CMAKE_OBJCOPY:FILEPATH=/usr/bin/objcopy + +//Path to a program. +CMAKE_OBJDUMP:FILEPATH=/usr/bin/objdump + +//Value Computed by CMake +CMAKE_PROJECT_DESCRIPTION:STATIC= + +//Value Computed by CMake +CMAKE_PROJECT_HOMEPAGE_URL:STATIC= + +//Value Computed by CMake +CMAKE_PROJECT_NAME:STATIC=gmapping + +//Path to a program. +CMAKE_RANLIB:FILEPATH=/usr/bin/ranlib + +//Path to a program. +CMAKE_READELF:FILEPATH=/usr/bin/readelf + +//Flags used by the linker during the creation of shared libraries +// during all build types. +CMAKE_SHARED_LINKER_FLAGS:STRING= + +//Flags used by the linker during the creation of shared libraries +// during DEBUG builds. +CMAKE_SHARED_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during the creation of shared libraries +// during MINSIZEREL builds. +CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during the creation of shared libraries +// during RELEASE builds. +CMAKE_SHARED_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during the creation of shared libraries +// during RELWITHDEBINFO builds. +CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//If set, runtime paths are not added when installing shared libraries, +// but are added when building. +CMAKE_SKIP_INSTALL_RPATH:BOOL=NO + +//If set, runtime paths are not added when using shared libraries. +CMAKE_SKIP_RPATH:BOOL=NO + +//Flags used by the linker during the creation of static libraries +// during all build types. +CMAKE_STATIC_LINKER_FLAGS:STRING= + +//Flags used by the linker during the creation of static libraries +// during DEBUG builds. +CMAKE_STATIC_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during the creation of static libraries +// during MINSIZEREL builds. +CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during the creation of static libraries +// during RELEASE builds. +CMAKE_STATIC_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during the creation of static libraries +// during RELWITHDEBINFO builds. +CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//Path to a program. +CMAKE_STRIP:FILEPATH=/usr/bin/strip + +//If this value is on, makefiles will be generated without the +// .SILENT directive, and all commands will be echoed to the console +// during the make. This is useful for debugging only. With Visual +// Studio IDE projects all commands are done without /nologo. +CMAKE_VERBOSE_MAKEFILE:BOOL=FALSE + +//Path to a program. +ProcessorCount_cmd_nproc:FILEPATH=/usr/bin/nproc + +//Path to a program. +ProcessorCount_cmd_sysctl:FILEPATH=/usr/sbin/sysctl + +//The directory containing a CMake configuration file for catkin. +catkin_DIR:PATH=catkin_DIR-NOTFOUND + +//Value Computed by CMake +gmapping_BINARY_DIR:STATIC=/home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug + +//Value Computed by CMake +gmapping_IS_TOP_LEVEL:STATIC=ON + +//Value Computed by CMake +gmapping_SOURCE_DIR:STATIC=/home/taylor/multidrone_slam/src/slam_gmapping/gmapping + + +######################## +# INTERNAL cache entries +######################## + +//ADVANCED property for variable: CMAKE_ADDR2LINE +CMAKE_ADDR2LINE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_AR +CMAKE_AR-ADVANCED:INTERNAL=1 +//This is the directory where this CMakeCache.txt was created +CMAKE_CACHEFILE_DIR:INTERNAL=/home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug +//Major version of cmake used to create the current loaded cache +CMAKE_CACHE_MAJOR_VERSION:INTERNAL=3 +//Minor version of cmake used to create the current loaded cache +CMAKE_CACHE_MINOR_VERSION:INTERNAL=21 +//Patch version of cmake used to create the current loaded cache +CMAKE_CACHE_PATCH_VERSION:INTERNAL=1 +//ADVANCED property for variable: CMAKE_COLOR_MAKEFILE +CMAKE_COLOR_MAKEFILE-ADVANCED:INTERNAL=1 +//Path to CMake executable. +CMAKE_COMMAND:INTERNAL=/home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/bin/cmake +//Path to cpack program executable. +CMAKE_CPACK_COMMAND:INTERNAL=/home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/bin/cpack +//Path to ctest program executable. +CMAKE_CTEST_COMMAND:INTERNAL=/home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/bin/ctest +//ADVANCED property for variable: CMAKE_CXX_COMPILER +CMAKE_CXX_COMPILER-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_COMPILER_AR +CMAKE_CXX_COMPILER_AR-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_COMPILER_RANLIB +CMAKE_CXX_COMPILER_RANLIB-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS +CMAKE_CXX_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_DEBUG +CMAKE_CXX_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_MINSIZEREL +CMAKE_CXX_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELEASE +CMAKE_CXX_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELWITHDEBINFO +CMAKE_CXX_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_COMPILER +CMAKE_C_COMPILER-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_COMPILER_AR +CMAKE_C_COMPILER_AR-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_COMPILER_RANLIB +CMAKE_C_COMPILER_RANLIB-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS +CMAKE_C_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_DEBUG +CMAKE_C_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_MINSIZEREL +CMAKE_C_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_RELEASE +CMAKE_C_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_RELWITHDEBINFO +CMAKE_C_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_DLLTOOL +CMAKE_DLLTOOL-ADVANCED:INTERNAL=1 +//Executable file format +CMAKE_EXECUTABLE_FORMAT:INTERNAL=ELF +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS +CMAKE_EXE_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_DEBUG +CMAKE_EXE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_MINSIZEREL +CMAKE_EXE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELEASE +CMAKE_EXE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXPORT_COMPILE_COMMANDS +CMAKE_EXPORT_COMPILE_COMMANDS-ADVANCED:INTERNAL=1 +//Name of external makefile project generator. +CMAKE_EXTRA_GENERATOR:INTERNAL=CodeBlocks +//CXX compiler system defined macros +CMAKE_EXTRA_GENERATOR_CXX_SYSTEM_DEFINED_MACROS:INTERNAL=__STDC__;1;__STDC_VERSION__;201710L;__STDC_UTF_16__;1;__STDC_UTF_32__;1;__STDC_HOSTED__;1;__GNUC__;9;__GNUC_MINOR__;3;__GNUC_PATCHLEVEL__;0;__VERSION__;"9.3.0";__ATOMIC_RELAXED;0;__ATOMIC_SEQ_CST;5;__ATOMIC_ACQUIRE;2;__ATOMIC_RELEASE;3;__ATOMIC_ACQ_REL;4;__ATOMIC_CONSUME;1;__pic__;2;__PIC__;2;__pie__;2;__PIE__;2;__FINITE_MATH_ONLY__;0;_LP64;1;__LP64__;1;__SIZEOF_INT__;4;__SIZEOF_LONG__;8;__SIZEOF_LONG_LONG__;8;__SIZEOF_SHORT__;2;__SIZEOF_FLOAT__;4;__SIZEOF_DOUBLE__;8;__SIZEOF_LONG_DOUBLE__;16;__SIZEOF_SIZE_T__;8;__CHAR_BIT__;8;__BIGGEST_ALIGNMENT__;16;__ORDER_LITTLE_ENDIAN__;1234;__ORDER_BIG_ENDIAN__;4321;__ORDER_PDP_ENDIAN__;3412;__BYTE_ORDER__;__ORDER_LITTLE_ENDIAN__;__FLOAT_WORD_ORDER__;__ORDER_LITTLE_ENDIAN__;__SIZEOF_POINTER__;8;__SIZE_TYPE__;long unsigned int;__PTRDIFF_TYPE__;long int;__WCHAR_TYPE__;int;__WINT_TYPE__;unsigned int;__INTMAX_TYPE__;long int;__UINTMAX_TYPE__;long unsigned int;__CHAR16_TYPE__;short unsigned int;__CHAR32_TYPE__;unsigned int;__SIG_ATOMIC_TYPE__;int;__INT8_TYPE__;signed char;__INT16_TYPE__;short int;__INT32_TYPE__;int;__INT64_TYPE__;long int;__UINT8_TYPE__;unsigned char;__UINT16_TYPE__;short unsigned int;__UINT32_TYPE__;unsigned int;__UINT64_TYPE__;long unsigned int;__INT_LEAST8_TYPE__;signed char;__INT_LEAST16_TYPE__;short int;__INT_LEAST32_TYPE__;int;__INT_LEAST64_TYPE__;long int;__UINT_LEAST8_TYPE__;unsigned char;__UINT_LEAST16_TYPE__;short unsigned int;__UINT_LEAST32_TYPE__;unsigned int;__UINT_LEAST64_TYPE__;long unsigned int;__INT_FAST8_TYPE__;signed char;__INT_FAST16_TYPE__;long int;__INT_FAST32_TYPE__;long int;__INT_FAST64_TYPE__;long int;__UINT_FAST8_TYPE__;unsigned char;__UINT_FAST16_TYPE__;long unsigned int;__UINT_FAST32_TYPE__;long unsigned int;__UINT_FAST64_TYPE__;long unsigned int;__INTPTR_TYPE__;long int;__UINTPTR_TYPE__;long unsigned int;__has_include(STR);__has_include__(STR);__has_include_next(STR);__has_include_next__(STR);__GXX_ABI_VERSION;1013;__SCHAR_MAX__;0x7f;__SHRT_MAX__;0x7fff;__INT_MAX__;0x7fffffff;__LONG_MAX__;0x7fffffffffffffffL;__LONG_LONG_MAX__;0x7fffffffffffffffLL;__WCHAR_MAX__;0x7fffffff;__WCHAR_MIN__;(-__WCHAR_MAX__ - 1);__WINT_MAX__;0xffffffffU;__WINT_MIN__;0U;__PTRDIFF_MAX__;0x7fffffffffffffffL;__SIZE_MAX__;0xffffffffffffffffUL;__SCHAR_WIDTH__;8;__SHRT_WIDTH__;16;__INT_WIDTH__;32;__LONG_WIDTH__;64;__LONG_LONG_WIDTH__;64;__WCHAR_WIDTH__;32;__WINT_WIDTH__;32;__PTRDIFF_WIDTH__;64;__SIZE_WIDTH__;64;__INTMAX_MAX__;0x7fffffffffffffffL;__INTMAX_C(c);c ## L;__UINTMAX_MAX__;0xffffffffffffffffUL;__UINTMAX_C(c);c ## UL;__INTMAX_WIDTH__;64;__SIG_ATOMIC_MAX__;0x7fffffff;__SIG_ATOMIC_MIN__;(-__SIG_ATOMIC_MAX__ - 1);__SIG_ATOMIC_WIDTH__;32;__INT8_MAX__;0x7f;__INT16_MAX__;0x7fff;__INT32_MAX__;0x7fffffff;__INT64_MAX__;0x7fffffffffffffffL;__UINT8_MAX__;0xff;__UINT16_MAX__;0xffff;__UINT32_MAX__;0xffffffffU;__UINT64_MAX__;0xffffffffffffffffUL;__INT_LEAST8_MAX__;0x7f;__INT8_C(c);c;__INT_LEAST8_WIDTH__;8;__INT_LEAST16_MAX__;0x7fff;__INT16_C(c);c;__INT_LEAST16_WIDTH__;16;__INT_LEAST32_MAX__;0x7fffffff;__INT32_C(c);c;__INT_LEAST32_WIDTH__;32;__INT_LEAST64_MAX__;0x7fffffffffffffffL;__INT64_C(c);c ## L;__INT_LEAST64_WIDTH__;64;__UINT_LEAST8_MAX__;0xff;__UINT8_C(c);c;__UINT_LEAST16_MAX__;0xffff;__UINT16_C(c);c;__UINT_LEAST32_MAX__;0xffffffffU;__UINT32_C(c);c ## U;__UINT_LEAST64_MAX__;0xffffffffffffffffUL;__UINT64_C(c);c ## UL;__INT_FAST8_MAX__;0x7f;__INT_FAST8_WIDTH__;8;__INT_FAST16_MAX__;0x7fffffffffffffffL;__INT_FAST16_WIDTH__;64;__INT_FAST32_MAX__;0x7fffffffffffffffL;__INT_FAST32_WIDTH__;64;__INT_FAST64_MAX__;0x7fffffffffffffffL;__INT_FAST64_WIDTH__;64;__UINT_FAST8_MAX__;0xff;__UINT_FAST16_MAX__;0xffffffffffffffffUL;__UINT_FAST32_MAX__;0xffffffffffffffffUL;__UINT_FAST64_MAX__;0xffffffffffffffffUL;__INTPTR_MAX__;0x7fffffffffffffffL;__INTPTR_WIDTH__;64;__UINTPTR_MAX__;0xffffffffffffffffUL;__GCC_IEC_559;2;__GCC_IEC_559_COMPLEX;2;__FLT_EVAL_METHOD__;0;__FLT_EVAL_METHOD_TS_18661_3__;0;__DEC_EVAL_METHOD__;2;__FLT_RADIX__;2;__FLT_MANT_DIG__;24;__FLT_DIG__;6;__FLT_MIN_EXP__;(-125);__FLT_MIN_10_EXP__;(-37);__FLT_MAX_EXP__;128;__FLT_MAX_10_EXP__;38;__FLT_DECIMAL_DIG__;9;__FLT_MAX__;3.40282346638528859811704183484516925e+38F;__FLT_MIN__;1.17549435082228750796873653722224568e-38F;__FLT_EPSILON__;1.19209289550781250000000000000000000e-7F;__FLT_DENORM_MIN__;1.40129846432481707092372958328991613e-45F;__FLT_HAS_DENORM__;1;__FLT_HAS_INFINITY__;1;__FLT_HAS_QUIET_NAN__;1;__DBL_MANT_DIG__;53;__DBL_DIG__;15;__DBL_MIN_EXP__;(-1021);__DBL_MIN_10_EXP__;(-307);__DBL_MAX_EXP__;1024;__DBL_MAX_10_EXP__;308;__DBL_DECIMAL_DIG__;17;__DBL_MAX__;((double)1.79769313486231570814527423731704357e+308L);__DBL_MIN__;((double)2.22507385850720138309023271733240406e-308L);__DBL_EPSILON__;((double)2.22044604925031308084726333618164062e-16L);__DBL_DENORM_MIN__;((double)4.94065645841246544176568792868221372e-324L);__DBL_HAS_DENORM__;1;__DBL_HAS_INFINITY__;1;__DBL_HAS_QUIET_NAN__;1;__LDBL_MANT_DIG__;64;__LDBL_DIG__;18;__LDBL_MIN_EXP__;(-16381);__LDBL_MIN_10_EXP__;(-4931);__LDBL_MAX_EXP__;16384;__LDBL_MAX_10_EXP__;4932;__DECIMAL_DIG__;21;__LDBL_DECIMAL_DIG__;21;__LDBL_MAX__;1.18973149535723176502126385303097021e+4932L;__LDBL_MIN__;3.36210314311209350626267781732175260e-4932L;__LDBL_EPSILON__;1.08420217248550443400745280086994171e-19L;__LDBL_DENORM_MIN__;3.64519953188247460252840593361941982e-4951L;__LDBL_HAS_DENORM__;1;__LDBL_HAS_INFINITY__;1;__LDBL_HAS_QUIET_NAN__;1;__FLT32_MANT_DIG__;24;__FLT32_DIG__;6;__FLT32_MIN_EXP__;(-125);__FLT32_MIN_10_EXP__;(-37);__FLT32_MAX_EXP__;128;__FLT32_MAX_10_EXP__;38;__FLT32_DECIMAL_DIG__;9;__FLT32_MAX__;3.40282346638528859811704183484516925e+38F32;__FLT32_MIN__;1.17549435082228750796873653722224568e-38F32;__FLT32_EPSILON__;1.19209289550781250000000000000000000e-7F32;__FLT32_DENORM_MIN__;1.40129846432481707092372958328991613e-45F32;__FLT32_HAS_DENORM__;1;__FLT32_HAS_INFINITY__;1;__FLT32_HAS_QUIET_NAN__;1;__FLT64_MANT_DIG__;53;__FLT64_DIG__;15;__FLT64_MIN_EXP__;(-1021);__FLT64_MIN_10_EXP__;(-307);__FLT64_MAX_EXP__;1024;__FLT64_MAX_10_EXP__;308;__FLT64_DECIMAL_DIG__;17;__FLT64_MAX__;1.79769313486231570814527423731704357e+308F64;__FLT64_MIN__;2.22507385850720138309023271733240406e-308F64;__FLT64_EPSILON__;2.22044604925031308084726333618164062e-16F64;__FLT64_DENORM_MIN__;4.94065645841246544176568792868221372e-324F64;__FLT64_HAS_DENORM__;1;__FLT64_HAS_INFINITY__;1;__FLT64_HAS_QUIET_NAN__;1;__FLT128_MANT_DIG__;113;__FLT128_DIG__;33;__FLT128_MIN_EXP__;(-16381);__FLT128_MIN_10_EXP__;(-4931);__FLT128_MAX_EXP__;16384;__FLT128_MAX_10_EXP__;4932;__FLT128_DECIMAL_DIG__;36;__FLT128_MAX__;1.18973149535723176508575932662800702e+4932F128;__FLT128_MIN__;3.36210314311209350626267781732175260e-4932F128;__FLT128_EPSILON__;1.92592994438723585305597794258492732e-34F128;__FLT128_DENORM_MIN__;6.47517511943802511092443895822764655e-4966F128;__FLT128_HAS_DENORM__;1;__FLT128_HAS_INFINITY__;1;__FLT128_HAS_QUIET_NAN__;1;__FLT32X_MANT_DIG__;53;__FLT32X_DIG__;15;__FLT32X_MIN_EXP__;(-1021);__FLT32X_MIN_10_EXP__;(-307);__FLT32X_MAX_EXP__;1024;__FLT32X_MAX_10_EXP__;308;__FLT32X_DECIMAL_DIG__;17;__FLT32X_MAX__;1.79769313486231570814527423731704357e+308F32x;__FLT32X_MIN__;2.22507385850720138309023271733240406e-308F32x;__FLT32X_EPSILON__;2.22044604925031308084726333618164062e-16F32x;__FLT32X_DENORM_MIN__;4.94065645841246544176568792868221372e-324F32x;__FLT32X_HAS_DENORM__;1;__FLT32X_HAS_INFINITY__;1;__FLT32X_HAS_QUIET_NAN__;1;__FLT64X_MANT_DIG__;64;__FLT64X_DIG__;18;__FLT64X_MIN_EXP__;(-16381);__FLT64X_MIN_10_EXP__;(-4931);__FLT64X_MAX_EXP__;16384;__FLT64X_MAX_10_EXP__;4932;__FLT64X_DECIMAL_DIG__;21;__FLT64X_MAX__;1.18973149535723176502126385303097021e+4932F64x;__FLT64X_MIN__;3.36210314311209350626267781732175260e-4932F64x;__FLT64X_EPSILON__;1.08420217248550443400745280086994171e-19F64x;__FLT64X_DENORM_MIN__;3.64519953188247460252840593361941982e-4951F64x;__FLT64X_HAS_DENORM__;1;__FLT64X_HAS_INFINITY__;1;__FLT64X_HAS_QUIET_NAN__;1;__DEC32_MANT_DIG__;7;__DEC32_MIN_EXP__;(-94);__DEC32_MAX_EXP__;97;__DEC32_MIN__;1E-95DF;__DEC32_MAX__;9.999999E96DF;__DEC32_EPSILON__;1E-6DF;__DEC32_SUBNORMAL_MIN__;0.000001E-95DF;__DEC64_MANT_DIG__;16;__DEC64_MIN_EXP__;(-382);__DEC64_MAX_EXP__;385;__DEC64_MIN__;1E-383DD;__DEC64_MAX__;9.999999999999999E384DD;__DEC64_EPSILON__;1E-15DD;__DEC64_SUBNORMAL_MIN__;0.000000000000001E-383DD;__DEC128_MANT_DIG__;34;__DEC128_MIN_EXP__;(-6142);__DEC128_MAX_EXP__;6145;__DEC128_MIN__;1E-6143DL;__DEC128_MAX__;9.999999999999999999999999999999999E6144DL;__DEC128_EPSILON__;1E-33DL;__DEC128_SUBNORMAL_MIN__;0.000000000000000000000000000000001E-6143DL;__REGISTER_PREFIX__; ;__USER_LABEL_PREFIX__; ;__GNUC_STDC_INLINE__;1;__NO_INLINE__;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_1;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_2;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_4;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_8;1;__GCC_ATOMIC_BOOL_LOCK_FREE;2;__GCC_ATOMIC_CHAR_LOCK_FREE;2;__GCC_ATOMIC_CHAR16_T_LOCK_FREE;2;__GCC_ATOMIC_CHAR32_T_LOCK_FREE;2;__GCC_ATOMIC_WCHAR_T_LOCK_FREE;2;__GCC_ATOMIC_SHORT_LOCK_FREE;2;__GCC_ATOMIC_INT_LOCK_FREE;2;__GCC_ATOMIC_LONG_LOCK_FREE;2;__GCC_ATOMIC_LLONG_LOCK_FREE;2;__GCC_ATOMIC_TEST_AND_SET_TRUEVAL;1;__GCC_ATOMIC_POINTER_LOCK_FREE;2;__HAVE_SPECULATION_SAFE_VALUE;1;__GCC_HAVE_DWARF2_CFI_ASM;1;__PRAGMA_REDEFINE_EXTNAME;1;__SSP_STRONG__;3;__SIZEOF_INT128__;16;__SIZEOF_WCHAR_T__;4;__SIZEOF_WINT_T__;4;__SIZEOF_PTRDIFF_T__;8;__amd64;1;__amd64__;1;__x86_64;1;__x86_64__;1;__SIZEOF_FLOAT80__;16;__SIZEOF_FLOAT128__;16;__ATOMIC_HLE_ACQUIRE;65536;__ATOMIC_HLE_RELEASE;131072;__GCC_ASM_FLAG_OUTPUTS__;1;__k8;1;__k8__;1;__code_model_small__;1;__MMX__;1;__SSE__;1;__SSE2__;1;__FXSR__;1;__SSE_MATH__;1;__SSE2_MATH__;1;__SEG_FS;1;__SEG_GS;1;__CET__;3;__gnu_linux__;1;__linux;1;__linux__;1;linux;1;__unix;1;__unix__;1;unix;1;__ELF__;1;__DECIMAL_BID_FORMAT__;1;_STDC_PREDEF_H;1;__STDC_IEC_559__;1;__STDC_IEC_559_COMPLEX__;1;__STDC_ISO_10646__;201706L;__STDC__;1;__cplusplus;201402L;__STDC_UTF_16__;1;__STDC_UTF_32__;1;__STDC_HOSTED__;1;__GNUC__;9;__GNUC_MINOR__;3;__GNUC_PATCHLEVEL__;0;__VERSION__;"9.3.0";__ATOMIC_RELAXED;0;__ATOMIC_SEQ_CST;5;__ATOMIC_ACQUIRE;2;__ATOMIC_RELEASE;3;__ATOMIC_ACQ_REL;4;__ATOMIC_CONSUME;1;__pic__;2;__PIC__;2;__pie__;2;__PIE__;2;__FINITE_MATH_ONLY__;0;_LP64;1;__LP64__;1;__SIZEOF_INT__;4;__SIZEOF_LONG__;8;__SIZEOF_LONG_LONG__;8;__SIZEOF_SHORT__;2;__SIZEOF_FLOAT__;4;__SIZEOF_DOUBLE__;8;__SIZEOF_LONG_DOUBLE__;16;__SIZEOF_SIZE_T__;8;__CHAR_BIT__;8;__BIGGEST_ALIGNMENT__;16;__ORDER_LITTLE_ENDIAN__;1234;__ORDER_BIG_ENDIAN__;4321;__ORDER_PDP_ENDIAN__;3412;__BYTE_ORDER__;__ORDER_LITTLE_ENDIAN__;__FLOAT_WORD_ORDER__;__ORDER_LITTLE_ENDIAN__;__SIZEOF_POINTER__;8;__GNUG__;9;__SIZE_TYPE__;long unsigned int;__PTRDIFF_TYPE__;long int;__WCHAR_TYPE__;int;__WINT_TYPE__;unsigned int;__INTMAX_TYPE__;long int;__UINTMAX_TYPE__;long unsigned int;__CHAR16_TYPE__;short unsigned int;__CHAR32_TYPE__;unsigned int;__SIG_ATOMIC_TYPE__;int;__INT8_TYPE__;signed char;__INT16_TYPE__;short int;__INT32_TYPE__;int;__INT64_TYPE__;long int;__UINT8_TYPE__;unsigned char;__UINT16_TYPE__;short unsigned int;__UINT32_TYPE__;unsigned int;__UINT64_TYPE__;long unsigned int;__INT_LEAST8_TYPE__;signed char;__INT_LEAST16_TYPE__;short int;__INT_LEAST32_TYPE__;int;__INT_LEAST64_TYPE__;long int;__UINT_LEAST8_TYPE__;unsigned char;__UINT_LEAST16_TYPE__;short unsigned int;__UINT_LEAST32_TYPE__;unsigned int;__UINT_LEAST64_TYPE__;long unsigned int;__INT_FAST8_TYPE__;signed char;__INT_FAST16_TYPE__;long int;__INT_FAST32_TYPE__;long int;__INT_FAST64_TYPE__;long int;__UINT_FAST8_TYPE__;unsigned char;__UINT_FAST16_TYPE__;long unsigned int;__UINT_FAST32_TYPE__;long unsigned int;__UINT_FAST64_TYPE__;long unsigned int;__INTPTR_TYPE__;long int;__UINTPTR_TYPE__;long unsigned int;__has_include(STR);__has_include__(STR);__has_include_next(STR);__has_include_next__(STR);__GXX_WEAK__;1;__DEPRECATED;1;__GXX_RTTI;1;__cpp_rtti;199711;__GXX_EXPERIMENTAL_CXX0X__;1;__cpp_binary_literals;201304;__cpp_hex_float;201603;__cpp_runtime_arrays;198712;__cpp_unicode_characters;200704;__cpp_raw_strings;200710;__cpp_unicode_literals;200710;__cpp_user_defined_literals;200809;__cpp_lambdas;200907;__cpp_range_based_for;200907;__cpp_static_assert;200410;__cpp_decltype;200707;__cpp_attributes;200809;__cpp_rvalue_reference;200610;__cpp_rvalue_references;200610;__cpp_variadic_templates;200704;__cpp_initializer_lists;200806;__cpp_delegating_constructors;200604;__cpp_nsdmi;200809;__cpp_inheriting_constructors;201511;__cpp_ref_qualifiers;200710;__cpp_alias_templates;200704;__cpp_return_type_deduction;201304;__cpp_init_captures;201304;__cpp_generic_lambdas;201304;__cpp_constexpr;201304;__cpp_decltype_auto;201304;__cpp_aggregate_nsdmi;201304;__cpp_variable_templates;201304;__cpp_digit_separators;201309;__cpp_sized_deallocation;201309;__cpp_threadsafe_static_init;200806;__EXCEPTIONS;1;__cpp_exceptions;199711;__GXX_ABI_VERSION;1013;__SCHAR_MAX__;0x7f;__SHRT_MAX__;0x7fff;__INT_MAX__;0x7fffffff;__LONG_MAX__;0x7fffffffffffffffL;__LONG_LONG_MAX__;0x7fffffffffffffffLL;__WCHAR_MAX__;0x7fffffff;__WCHAR_MIN__;(-__WCHAR_MAX__ - 1);__WINT_MAX__;0xffffffffU;__WINT_MIN__;0U;__PTRDIFF_MAX__;0x7fffffffffffffffL;__SIZE_MAX__;0xffffffffffffffffUL;__SCHAR_WIDTH__;8;__SHRT_WIDTH__;16;__INT_WIDTH__;32;__LONG_WIDTH__;64;__LONG_LONG_WIDTH__;64;__WCHAR_WIDTH__;32;__WINT_WIDTH__;32;__PTRDIFF_WIDTH__;64;__SIZE_WIDTH__;64;__GLIBCXX_TYPE_INT_N_0;__int128;__GLIBCXX_BITSIZE_INT_N_0;128;__INTMAX_MAX__;0x7fffffffffffffffL;__INTMAX_C(c);c ## L;__UINTMAX_MAX__;0xffffffffffffffffUL;__UINTMAX_C(c);c ## UL;__INTMAX_WIDTH__;64;__SIG_ATOMIC_MAX__;0x7fffffff;__SIG_ATOMIC_MIN__;(-__SIG_ATOMIC_MAX__ - 1);__SIG_ATOMIC_WIDTH__;32;__INT8_MAX__;0x7f;__INT16_MAX__;0x7fff;__INT32_MAX__;0x7fffffff;__INT64_MAX__;0x7fffffffffffffffL;__UINT8_MAX__;0xff;__UINT16_MAX__;0xffff;__UINT32_MAX__;0xffffffffU;__UINT64_MAX__;0xffffffffffffffffUL;__INT_LEAST8_MAX__;0x7f;__INT8_C(c);c;__INT_LEAST8_WIDTH__;8;__INT_LEAST16_MAX__;0x7fff;__INT16_C(c);c;__INT_LEAST16_WIDTH__;16;__INT_LEAST32_MAX__;0x7fffffff;__INT32_C(c);c;__INT_LEAST32_WIDTH__;32;__INT_LEAST64_MAX__;0x7fffffffffffffffL;__INT64_C(c);c ## L;__INT_LEAST64_WIDTH__;64;__UINT_LEAST8_MAX__;0xff;__UINT8_C(c);c;__UINT_LEAST16_MAX__;0xffff;__UINT16_C(c);c;__UINT_LEAST32_MAX__;0xffffffffU;__UINT32_C(c);c ## U;__UINT_LEAST64_MAX__;0xffffffffffffffffUL;__UINT64_C(c);c ## UL;__INT_FAST8_MAX__;0x7f;__INT_FAST8_WIDTH__;8;__INT_FAST16_MAX__;0x7fffffffffffffffL;__INT_FAST16_WIDTH__;64;__INT_FAST32_MAX__;0x7fffffffffffffffL;__INT_FAST32_WIDTH__;64;__INT_FAST64_MAX__;0x7fffffffffffffffL;__INT_FAST64_WIDTH__;64;__UINT_FAST8_MAX__;0xff;__UINT_FAST16_MAX__;0xffffffffffffffffUL;__UINT_FAST32_MAX__;0xffffffffffffffffUL;__UINT_FAST64_MAX__;0xffffffffffffffffUL;__INTPTR_MAX__;0x7fffffffffffffffL;__INTPTR_WIDTH__;64;__UINTPTR_MAX__;0xffffffffffffffffUL;__GCC_IEC_559;2;__GCC_IEC_559_COMPLEX;2;__FLT_EVAL_METHOD__;0;__FLT_EVAL_METHOD_TS_18661_3__;0;__DEC_EVAL_METHOD__;2;__FLT_RADIX__;2;__FLT_MANT_DIG__;24;__FLT_DIG__;6;__FLT_MIN_EXP__;(-125);__FLT_MIN_10_EXP__;(-37);__FLT_MAX_EXP__;128;__FLT_MAX_10_EXP__;38;__FLT_DECIMAL_DIG__;9;__FLT_MAX__;3.40282346638528859811704183484516925e+38F;__FLT_MIN__;1.17549435082228750796873653722224568e-38F;__FLT_EPSILON__;1.19209289550781250000000000000000000e-7F;__FLT_DENORM_MIN__;1.40129846432481707092372958328991613e-45F;__FLT_HAS_DENORM__;1;__FLT_HAS_INFINITY__;1;__FLT_HAS_QUIET_NAN__;1;__DBL_MANT_DIG__;53;__DBL_DIG__;15;__DBL_MIN_EXP__;(-1021);__DBL_MIN_10_EXP__;(-307);__DBL_MAX_EXP__;1024;__DBL_MAX_10_EXP__;308;__DBL_DECIMAL_DIG__;17;__DBL_MAX__;double(1.79769313486231570814527423731704357e+308L);__DBL_MIN__;double(2.22507385850720138309023271733240406e-308L);__DBL_EPSILON__;double(2.22044604925031308084726333618164062e-16L);__DBL_DENORM_MIN__;double(4.94065645841246544176568792868221372e-324L);__DBL_HAS_DENORM__;1;__DBL_HAS_INFINITY__;1;__DBL_HAS_QUIET_NAN__;1;__LDBL_MANT_DIG__;64;__LDBL_DIG__;18;__LDBL_MIN_EXP__;(-16381);__LDBL_MIN_10_EXP__;(-4931);__LDBL_MAX_EXP__;16384;__LDBL_MAX_10_EXP__;4932;__DECIMAL_DIG__;21;__LDBL_DECIMAL_DIG__;21;__LDBL_MAX__;1.18973149535723176502126385303097021e+4932L;__LDBL_MIN__;3.36210314311209350626267781732175260e-4932L;__LDBL_EPSILON__;1.08420217248550443400745280086994171e-19L;__LDBL_DENORM_MIN__;3.64519953188247460252840593361941982e-4951L;__LDBL_HAS_DENORM__;1;__LDBL_HAS_INFINITY__;1;__LDBL_HAS_QUIET_NAN__;1;__FLT32_MANT_DIG__;24;__FLT32_DIG__;6;__FLT32_MIN_EXP__;(-125);__FLT32_MIN_10_EXP__;(-37);__FLT32_MAX_EXP__;128;__FLT32_MAX_10_EXP__;38;__FLT32_DECIMAL_DIG__;9;__FLT32_MAX__;3.40282346638528859811704183484516925e+38F32;__FLT32_MIN__;1.17549435082228750796873653722224568e-38F32;__FLT32_EPSILON__;1.19209289550781250000000000000000000e-7F32;__FLT32_DENORM_MIN__;1.40129846432481707092372958328991613e-45F32;__FLT32_HAS_DENORM__;1;__FLT32_HAS_INFINITY__;1;__FLT32_HAS_QUIET_NAN__;1;__FLT64_MANT_DIG__;53;__FLT64_DIG__;15;__FLT64_MIN_EXP__;(-1021);__FLT64_MIN_10_EXP__;(-307);__FLT64_MAX_EXP__;1024;__FLT64_MAX_10_EXP__;308;__FLT64_DECIMAL_DIG__;17;__FLT64_MAX__;1.79769313486231570814527423731704357e+308F64;__FLT64_MIN__;2.22507385850720138309023271733240406e-308F64;__FLT64_EPSILON__;2.22044604925031308084726333618164062e-16F64;__FLT64_DENORM_MIN__;4.94065645841246544176568792868221372e-324F64;__FLT64_HAS_DENORM__;1;__FLT64_HAS_INFINITY__;1;__FLT64_HAS_QUIET_NAN__;1;__FLT128_MANT_DIG__;113;__FLT128_DIG__;33;__FLT128_MIN_EXP__;(-16381);__FLT128_MIN_10_EXP__;(-4931);__FLT128_MAX_EXP__;16384;__FLT128_MAX_10_EXP__;4932;__FLT128_DECIMAL_DIG__;36;__FLT128_MAX__;1.18973149535723176508575932662800702e+4932F128;__FLT128_MIN__;3.36210314311209350626267781732175260e-4932F128;__FLT128_EPSILON__;1.92592994438723585305597794258492732e-34F128;__FLT128_DENORM_MIN__;6.47517511943802511092443895822764655e-4966F128;__FLT128_HAS_DENORM__;1;__FLT128_HAS_INFINITY__;1;__FLT128_HAS_QUIET_NAN__;1;__FLT32X_MANT_DIG__;53;__FLT32X_DIG__;15;__FLT32X_MIN_EXP__;(-1021);__FLT32X_MIN_10_EXP__;(-307);__FLT32X_MAX_EXP__;1024;__FLT32X_MAX_10_EXP__;308;__FLT32X_DECIMAL_DIG__;17;__FLT32X_MAX__;1.79769313486231570814527423731704357e+308F32x;__FLT32X_MIN__;2.22507385850720138309023271733240406e-308F32x;__FLT32X_EPSILON__;2.22044604925031308084726333618164062e-16F32x;__FLT32X_DENORM_MIN__;4.94065645841246544176568792868221372e-324F32x;__FLT32X_HAS_DENORM__;1;__FLT32X_HAS_INFINITY__;1;__FLT32X_HAS_QUIET_NAN__;1;__FLT64X_MANT_DIG__;64;__FLT64X_DIG__;18;__FLT64X_MIN_EXP__;(-16381);__FLT64X_MIN_10_EXP__;(-4931);__FLT64X_MAX_EXP__;16384;__FLT64X_MAX_10_EXP__;4932;__FLT64X_DECIMAL_DIG__;21;__FLT64X_MAX__;1.18973149535723176502126385303097021e+4932F64x;__FLT64X_MIN__;3.36210314311209350626267781732175260e-4932F64x;__FLT64X_EPSILON__;1.08420217248550443400745280086994171e-19F64x;__FLT64X_DENORM_MIN__;3.64519953188247460252840593361941982e-4951F64x;__FLT64X_HAS_DENORM__;1;__FLT64X_HAS_INFINITY__;1;__FLT64X_HAS_QUIET_NAN__;1;__DEC32_MANT_DIG__;7;__DEC32_MIN_EXP__;(-94);__DEC32_MAX_EXP__;97;__DEC32_MIN__;1E-95DF;__DEC32_MAX__;9.999999E96DF;__DEC32_EPSILON__;1E-6DF;__DEC32_SUBNORMAL_MIN__;0.000001E-95DF;__DEC64_MANT_DIG__;16;__DEC64_MIN_EXP__;(-382);__DEC64_MAX_EXP__;385;__DEC64_MIN__;1E-383DD;__DEC64_MAX__;9.999999999999999E384DD;__DEC64_EPSILON__;1E-15DD;__DEC64_SUBNORMAL_MIN__;0.000000000000001E-383DD;__DEC128_MANT_DIG__;34;__DEC128_MIN_EXP__;(-6142);__DEC128_MAX_EXP__;6145;__DEC128_MIN__;1E-6143DL;__DEC128_MAX__;9.999999999999999999999999999999999E6144DL;__DEC128_EPSILON__;1E-33DL;__DEC128_SUBNORMAL_MIN__;0.000000000000000000000000000000001E-6143DL;__REGISTER_PREFIX__; ;__USER_LABEL_PREFIX__; ;__GNUC_STDC_INLINE__;1;__NO_INLINE__;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_1;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_2;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_4;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_8;1;__GCC_ATOMIC_BOOL_LOCK_FREE;2;__GCC_ATOMIC_CHAR_LOCK_FREE;2;__GCC_ATOMIC_CHAR16_T_LOCK_FREE;2;__GCC_ATOMIC_CHAR32_T_LOCK_FREE;2;__GCC_ATOMIC_WCHAR_T_LOCK_FREE;2;__GCC_ATOMIC_SHORT_LOCK_FREE;2;__GCC_ATOMIC_INT_LOCK_FREE;2;__GCC_ATOMIC_LONG_LOCK_FREE;2;__GCC_ATOMIC_LLONG_LOCK_FREE;2;__GCC_ATOMIC_TEST_AND_SET_TRUEVAL;1;__GCC_ATOMIC_POINTER_LOCK_FREE;2;__HAVE_SPECULATION_SAFE_VALUE;1;__GCC_HAVE_DWARF2_CFI_ASM;1;__PRAGMA_REDEFINE_EXTNAME;1;__SSP_STRONG__;3;__SIZEOF_INT128__;16;__SIZEOF_WCHAR_T__;4;__SIZEOF_WINT_T__;4;__SIZEOF_PTRDIFF_T__;8;__amd64;1;__amd64__;1;__x86_64;1;__x86_64__;1;__SIZEOF_FLOAT80__;16;__SIZEOF_FLOAT128__;16;__ATOMIC_HLE_ACQUIRE;65536;__ATOMIC_HLE_RELEASE;131072;__GCC_ASM_FLAG_OUTPUTS__;1;__k8;1;__k8__;1;__code_model_small__;1;__MMX__;1;__SSE__;1;__SSE2__;1;__FXSR__;1;__SSE_MATH__;1;__SSE2_MATH__;1;__SEG_FS;1;__SEG_GS;1;__CET__;3;__gnu_linux__;1;__linux;1;__linux__;1;linux;1;__unix;1;__unix__;1;unix;1;__ELF__;1;__DECIMAL_BID_FORMAT__;1;_GNU_SOURCE;1;_STDC_PREDEF_H;1;__STDC_IEC_559__;1;__STDC_IEC_559_COMPLEX__;1;__STDC_ISO_10646__;201706L +//CXX compiler system include directories +CMAKE_EXTRA_GENERATOR_CXX_SYSTEM_INCLUDE_DIRS:INTERNAL=/usr/include/c++/9;/usr/include/x86_64-linux-gnu/c++/9;/usr/include/c++/9/backward;/usr/lib/gcc/x86_64-linux-gnu/9/include;/usr/local/include;/usr/include/x86_64-linux-gnu;/usr/include +//C compiler system defined macros +CMAKE_EXTRA_GENERATOR_C_SYSTEM_DEFINED_MACROS:INTERNAL=__STDC__;1;__STDC_VERSION__;201710L;__STDC_UTF_16__;1;__STDC_UTF_32__;1;__STDC_HOSTED__;1;__GNUC__;9;__GNUC_MINOR__;3;__GNUC_PATCHLEVEL__;0;__VERSION__;"9.3.0";__ATOMIC_RELAXED;0;__ATOMIC_SEQ_CST;5;__ATOMIC_ACQUIRE;2;__ATOMIC_RELEASE;3;__ATOMIC_ACQ_REL;4;__ATOMIC_CONSUME;1;__pic__;2;__PIC__;2;__pie__;2;__PIE__;2;__FINITE_MATH_ONLY__;0;_LP64;1;__LP64__;1;__SIZEOF_INT__;4;__SIZEOF_LONG__;8;__SIZEOF_LONG_LONG__;8;__SIZEOF_SHORT__;2;__SIZEOF_FLOAT__;4;__SIZEOF_DOUBLE__;8;__SIZEOF_LONG_DOUBLE__;16;__SIZEOF_SIZE_T__;8;__CHAR_BIT__;8;__BIGGEST_ALIGNMENT__;16;__ORDER_LITTLE_ENDIAN__;1234;__ORDER_BIG_ENDIAN__;4321;__ORDER_PDP_ENDIAN__;3412;__BYTE_ORDER__;__ORDER_LITTLE_ENDIAN__;__FLOAT_WORD_ORDER__;__ORDER_LITTLE_ENDIAN__;__SIZEOF_POINTER__;8;__SIZE_TYPE__;long unsigned int;__PTRDIFF_TYPE__;long int;__WCHAR_TYPE__;int;__WINT_TYPE__;unsigned int;__INTMAX_TYPE__;long int;__UINTMAX_TYPE__;long unsigned int;__CHAR16_TYPE__;short unsigned int;__CHAR32_TYPE__;unsigned int;__SIG_ATOMIC_TYPE__;int;__INT8_TYPE__;signed char;__INT16_TYPE__;short int;__INT32_TYPE__;int;__INT64_TYPE__;long int;__UINT8_TYPE__;unsigned char;__UINT16_TYPE__;short unsigned int;__UINT32_TYPE__;unsigned int;__UINT64_TYPE__;long unsigned int;__INT_LEAST8_TYPE__;signed char;__INT_LEAST16_TYPE__;short int;__INT_LEAST32_TYPE__;int;__INT_LEAST64_TYPE__;long int;__UINT_LEAST8_TYPE__;unsigned char;__UINT_LEAST16_TYPE__;short unsigned int;__UINT_LEAST32_TYPE__;unsigned int;__UINT_LEAST64_TYPE__;long unsigned int;__INT_FAST8_TYPE__;signed char;__INT_FAST16_TYPE__;long int;__INT_FAST32_TYPE__;long int;__INT_FAST64_TYPE__;long int;__UINT_FAST8_TYPE__;unsigned char;__UINT_FAST16_TYPE__;long unsigned int;__UINT_FAST32_TYPE__;long unsigned int;__UINT_FAST64_TYPE__;long unsigned int;__INTPTR_TYPE__;long int;__UINTPTR_TYPE__;long unsigned int;__has_include(STR);__has_include__(STR);__has_include_next(STR);__has_include_next__(STR);__GXX_ABI_VERSION;1013;__SCHAR_MAX__;0x7f;__SHRT_MAX__;0x7fff;__INT_MAX__;0x7fffffff;__LONG_MAX__;0x7fffffffffffffffL;__LONG_LONG_MAX__;0x7fffffffffffffffLL;__WCHAR_MAX__;0x7fffffff;__WCHAR_MIN__;(-__WCHAR_MAX__ - 1);__WINT_MAX__;0xffffffffU;__WINT_MIN__;0U;__PTRDIFF_MAX__;0x7fffffffffffffffL;__SIZE_MAX__;0xffffffffffffffffUL;__SCHAR_WIDTH__;8;__SHRT_WIDTH__;16;__INT_WIDTH__;32;__LONG_WIDTH__;64;__LONG_LONG_WIDTH__;64;__WCHAR_WIDTH__;32;__WINT_WIDTH__;32;__PTRDIFF_WIDTH__;64;__SIZE_WIDTH__;64;__INTMAX_MAX__;0x7fffffffffffffffL;__INTMAX_C(c);c ## L;__UINTMAX_MAX__;0xffffffffffffffffUL;__UINTMAX_C(c);c ## UL;__INTMAX_WIDTH__;64;__SIG_ATOMIC_MAX__;0x7fffffff;__SIG_ATOMIC_MIN__;(-__SIG_ATOMIC_MAX__ - 1);__SIG_ATOMIC_WIDTH__;32;__INT8_MAX__;0x7f;__INT16_MAX__;0x7fff;__INT32_MAX__;0x7fffffff;__INT64_MAX__;0x7fffffffffffffffL;__UINT8_MAX__;0xff;__UINT16_MAX__;0xffff;__UINT32_MAX__;0xffffffffU;__UINT64_MAX__;0xffffffffffffffffUL;__INT_LEAST8_MAX__;0x7f;__INT8_C(c);c;__INT_LEAST8_WIDTH__;8;__INT_LEAST16_MAX__;0x7fff;__INT16_C(c);c;__INT_LEAST16_WIDTH__;16;__INT_LEAST32_MAX__;0x7fffffff;__INT32_C(c);c;__INT_LEAST32_WIDTH__;32;__INT_LEAST64_MAX__;0x7fffffffffffffffL;__INT64_C(c);c ## L;__INT_LEAST64_WIDTH__;64;__UINT_LEAST8_MAX__;0xff;__UINT8_C(c);c;__UINT_LEAST16_MAX__;0xffff;__UINT16_C(c);c;__UINT_LEAST32_MAX__;0xffffffffU;__UINT32_C(c);c ## U;__UINT_LEAST64_MAX__;0xffffffffffffffffUL;__UINT64_C(c);c ## UL;__INT_FAST8_MAX__;0x7f;__INT_FAST8_WIDTH__;8;__INT_FAST16_MAX__;0x7fffffffffffffffL;__INT_FAST16_WIDTH__;64;__INT_FAST32_MAX__;0x7fffffffffffffffL;__INT_FAST32_WIDTH__;64;__INT_FAST64_MAX__;0x7fffffffffffffffL;__INT_FAST64_WIDTH__;64;__UINT_FAST8_MAX__;0xff;__UINT_FAST16_MAX__;0xffffffffffffffffUL;__UINT_FAST32_MAX__;0xffffffffffffffffUL;__UINT_FAST64_MAX__;0xffffffffffffffffUL;__INTPTR_MAX__;0x7fffffffffffffffL;__INTPTR_WIDTH__;64;__UINTPTR_MAX__;0xffffffffffffffffUL;__GCC_IEC_559;2;__GCC_IEC_559_COMPLEX;2;__FLT_EVAL_METHOD__;0;__FLT_EVAL_METHOD_TS_18661_3__;0;__DEC_EVAL_METHOD__;2;__FLT_RADIX__;2;__FLT_MANT_DIG__;24;__FLT_DIG__;6;__FLT_MIN_EXP__;(-125);__FLT_MIN_10_EXP__;(-37);__FLT_MAX_EXP__;128;__FLT_MAX_10_EXP__;38;__FLT_DECIMAL_DIG__;9;__FLT_MAX__;3.40282346638528859811704183484516925e+38F;__FLT_MIN__;1.17549435082228750796873653722224568e-38F;__FLT_EPSILON__;1.19209289550781250000000000000000000e-7F;__FLT_DENORM_MIN__;1.40129846432481707092372958328991613e-45F;__FLT_HAS_DENORM__;1;__FLT_HAS_INFINITY__;1;__FLT_HAS_QUIET_NAN__;1;__DBL_MANT_DIG__;53;__DBL_DIG__;15;__DBL_MIN_EXP__;(-1021);__DBL_MIN_10_EXP__;(-307);__DBL_MAX_EXP__;1024;__DBL_MAX_10_EXP__;308;__DBL_DECIMAL_DIG__;17;__DBL_MAX__;((double)1.79769313486231570814527423731704357e+308L);__DBL_MIN__;((double)2.22507385850720138309023271733240406e-308L);__DBL_EPSILON__;((double)2.22044604925031308084726333618164062e-16L);__DBL_DENORM_MIN__;((double)4.94065645841246544176568792868221372e-324L);__DBL_HAS_DENORM__;1;__DBL_HAS_INFINITY__;1;__DBL_HAS_QUIET_NAN__;1;__LDBL_MANT_DIG__;64;__LDBL_DIG__;18;__LDBL_MIN_EXP__;(-16381);__LDBL_MIN_10_EXP__;(-4931);__LDBL_MAX_EXP__;16384;__LDBL_MAX_10_EXP__;4932;__DECIMAL_DIG__;21;__LDBL_DECIMAL_DIG__;21;__LDBL_MAX__;1.18973149535723176502126385303097021e+4932L;__LDBL_MIN__;3.36210314311209350626267781732175260e-4932L;__LDBL_EPSILON__;1.08420217248550443400745280086994171e-19L;__LDBL_DENORM_MIN__;3.64519953188247460252840593361941982e-4951L;__LDBL_HAS_DENORM__;1;__LDBL_HAS_INFINITY__;1;__LDBL_HAS_QUIET_NAN__;1;__FLT32_MANT_DIG__;24;__FLT32_DIG__;6;__FLT32_MIN_EXP__;(-125);__FLT32_MIN_10_EXP__;(-37);__FLT32_MAX_EXP__;128;__FLT32_MAX_10_EXP__;38;__FLT32_DECIMAL_DIG__;9;__FLT32_MAX__;3.40282346638528859811704183484516925e+38F32;__FLT32_MIN__;1.17549435082228750796873653722224568e-38F32;__FLT32_EPSILON__;1.19209289550781250000000000000000000e-7F32;__FLT32_DENORM_MIN__;1.40129846432481707092372958328991613e-45F32;__FLT32_HAS_DENORM__;1;__FLT32_HAS_INFINITY__;1;__FLT32_HAS_QUIET_NAN__;1;__FLT64_MANT_DIG__;53;__FLT64_DIG__;15;__FLT64_MIN_EXP__;(-1021);__FLT64_MIN_10_EXP__;(-307);__FLT64_MAX_EXP__;1024;__FLT64_MAX_10_EXP__;308;__FLT64_DECIMAL_DIG__;17;__FLT64_MAX__;1.79769313486231570814527423731704357e+308F64;__FLT64_MIN__;2.22507385850720138309023271733240406e-308F64;__FLT64_EPSILON__;2.22044604925031308084726333618164062e-16F64;__FLT64_DENORM_MIN__;4.94065645841246544176568792868221372e-324F64;__FLT64_HAS_DENORM__;1;__FLT64_HAS_INFINITY__;1;__FLT64_HAS_QUIET_NAN__;1;__FLT128_MANT_DIG__;113;__FLT128_DIG__;33;__FLT128_MIN_EXP__;(-16381);__FLT128_MIN_10_EXP__;(-4931);__FLT128_MAX_EXP__;16384;__FLT128_MAX_10_EXP__;4932;__FLT128_DECIMAL_DIG__;36;__FLT128_MAX__;1.18973149535723176508575932662800702e+4932F128;__FLT128_MIN__;3.36210314311209350626267781732175260e-4932F128;__FLT128_EPSILON__;1.92592994438723585305597794258492732e-34F128;__FLT128_DENORM_MIN__;6.47517511943802511092443895822764655e-4966F128;__FLT128_HAS_DENORM__;1;__FLT128_HAS_INFINITY__;1;__FLT128_HAS_QUIET_NAN__;1;__FLT32X_MANT_DIG__;53;__FLT32X_DIG__;15;__FLT32X_MIN_EXP__;(-1021);__FLT32X_MIN_10_EXP__;(-307);__FLT32X_MAX_EXP__;1024;__FLT32X_MAX_10_EXP__;308;__FLT32X_DECIMAL_DIG__;17;__FLT32X_MAX__;1.79769313486231570814527423731704357e+308F32x;__FLT32X_MIN__;2.22507385850720138309023271733240406e-308F32x;__FLT32X_EPSILON__;2.22044604925031308084726333618164062e-16F32x;__FLT32X_DENORM_MIN__;4.94065645841246544176568792868221372e-324F32x;__FLT32X_HAS_DENORM__;1;__FLT32X_HAS_INFINITY__;1;__FLT32X_HAS_QUIET_NAN__;1;__FLT64X_MANT_DIG__;64;__FLT64X_DIG__;18;__FLT64X_MIN_EXP__;(-16381);__FLT64X_MIN_10_EXP__;(-4931);__FLT64X_MAX_EXP__;16384;__FLT64X_MAX_10_EXP__;4932;__FLT64X_DECIMAL_DIG__;21;__FLT64X_MAX__;1.18973149535723176502126385303097021e+4932F64x;__FLT64X_MIN__;3.36210314311209350626267781732175260e-4932F64x;__FLT64X_EPSILON__;1.08420217248550443400745280086994171e-19F64x;__FLT64X_DENORM_MIN__;3.64519953188247460252840593361941982e-4951F64x;__FLT64X_HAS_DENORM__;1;__FLT64X_HAS_INFINITY__;1;__FLT64X_HAS_QUIET_NAN__;1;__DEC32_MANT_DIG__;7;__DEC32_MIN_EXP__;(-94);__DEC32_MAX_EXP__;97;__DEC32_MIN__;1E-95DF;__DEC32_MAX__;9.999999E96DF;__DEC32_EPSILON__;1E-6DF;__DEC32_SUBNORMAL_MIN__;0.000001E-95DF;__DEC64_MANT_DIG__;16;__DEC64_MIN_EXP__;(-382);__DEC64_MAX_EXP__;385;__DEC64_MIN__;1E-383DD;__DEC64_MAX__;9.999999999999999E384DD;__DEC64_EPSILON__;1E-15DD;__DEC64_SUBNORMAL_MIN__;0.000000000000001E-383DD;__DEC128_MANT_DIG__;34;__DEC128_MIN_EXP__;(-6142);__DEC128_MAX_EXP__;6145;__DEC128_MIN__;1E-6143DL;__DEC128_MAX__;9.999999999999999999999999999999999E6144DL;__DEC128_EPSILON__;1E-33DL;__DEC128_SUBNORMAL_MIN__;0.000000000000000000000000000000001E-6143DL;__REGISTER_PREFIX__; ;__USER_LABEL_PREFIX__; ;__GNUC_STDC_INLINE__;1;__NO_INLINE__;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_1;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_2;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_4;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_8;1;__GCC_ATOMIC_BOOL_LOCK_FREE;2;__GCC_ATOMIC_CHAR_LOCK_FREE;2;__GCC_ATOMIC_CHAR16_T_LOCK_FREE;2;__GCC_ATOMIC_CHAR32_T_LOCK_FREE;2;__GCC_ATOMIC_WCHAR_T_LOCK_FREE;2;__GCC_ATOMIC_SHORT_LOCK_FREE;2;__GCC_ATOMIC_INT_LOCK_FREE;2;__GCC_ATOMIC_LONG_LOCK_FREE;2;__GCC_ATOMIC_LLONG_LOCK_FREE;2;__GCC_ATOMIC_TEST_AND_SET_TRUEVAL;1;__GCC_ATOMIC_POINTER_LOCK_FREE;2;__HAVE_SPECULATION_SAFE_VALUE;1;__GCC_HAVE_DWARF2_CFI_ASM;1;__PRAGMA_REDEFINE_EXTNAME;1;__SSP_STRONG__;3;__SIZEOF_INT128__;16;__SIZEOF_WCHAR_T__;4;__SIZEOF_WINT_T__;4;__SIZEOF_PTRDIFF_T__;8;__amd64;1;__amd64__;1;__x86_64;1;__x86_64__;1;__SIZEOF_FLOAT80__;16;__SIZEOF_FLOAT128__;16;__ATOMIC_HLE_ACQUIRE;65536;__ATOMIC_HLE_RELEASE;131072;__GCC_ASM_FLAG_OUTPUTS__;1;__k8;1;__k8__;1;__code_model_small__;1;__MMX__;1;__SSE__;1;__SSE2__;1;__FXSR__;1;__SSE_MATH__;1;__SSE2_MATH__;1;__SEG_FS;1;__SEG_GS;1;__CET__;3;__gnu_linux__;1;__linux;1;__linux__;1;linux;1;__unix;1;__unix__;1;unix;1;__ELF__;1;__DECIMAL_BID_FORMAT__;1;_STDC_PREDEF_H;1;__STDC_IEC_559__;1;__STDC_IEC_559_COMPLEX__;1;__STDC_ISO_10646__;201706L +//C compiler system include directories +CMAKE_EXTRA_GENERATOR_C_SYSTEM_INCLUDE_DIRS:INTERNAL=/usr/lib/gcc/x86_64-linux-gnu/9/include;/usr/local/include;/usr/include/x86_64-linux-gnu;/usr/include +//Name of generator. +CMAKE_GENERATOR:INTERNAL=Unix Makefiles +//Generator instance identifier. +CMAKE_GENERATOR_INSTANCE:INTERNAL= +//Name of generator platform. +CMAKE_GENERATOR_PLATFORM:INTERNAL= +//Name of generator toolset. +CMAKE_GENERATOR_TOOLSET:INTERNAL= +//Source directory with the top level CMakeLists.txt file for this +// project +CMAKE_HOME_DIRECTORY:INTERNAL=/home/taylor/multidrone_slam/src/slam_gmapping/gmapping +//Install .so files without execute permission. +CMAKE_INSTALL_SO_NO_EXE:INTERNAL=1 +//ADVANCED property for variable: CMAKE_LINKER +CMAKE_LINKER-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MAKE_PROGRAM +CMAKE_MAKE_PROGRAM-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS +CMAKE_MODULE_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_DEBUG +CMAKE_MODULE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL +CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELEASE +CMAKE_MODULE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_NM +CMAKE_NM-ADVANCED:INTERNAL=1 +//number of local generators +CMAKE_NUMBER_OF_MAKEFILES:INTERNAL=1 +//ADVANCED property for variable: CMAKE_OBJCOPY +CMAKE_OBJCOPY-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_OBJDUMP +CMAKE_OBJDUMP-ADVANCED:INTERNAL=1 +//Platform information initialized +CMAKE_PLATFORM_INFO_INITIALIZED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_RANLIB +CMAKE_RANLIB-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_READELF +CMAKE_READELF-ADVANCED:INTERNAL=1 +//Path to CMake installation. +CMAKE_ROOT:INTERNAL=/home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/share/cmake-3.21 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS +CMAKE_SHARED_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_DEBUG +CMAKE_SHARED_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL +CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELEASE +CMAKE_SHARED_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SKIP_INSTALL_RPATH +CMAKE_SKIP_INSTALL_RPATH-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SKIP_RPATH +CMAKE_SKIP_RPATH-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS +CMAKE_STATIC_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_DEBUG +CMAKE_STATIC_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL +CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELEASE +CMAKE_STATIC_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STRIP +CMAKE_STRIP-ADVANCED:INTERNAL=1 +//uname command +CMAKE_UNAME:INTERNAL=/usr/bin/uname +//ADVANCED property for variable: CMAKE_VERBOSE_MAKEFILE +CMAKE_VERBOSE_MAKEFILE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: ProcessorCount_cmd_nproc +ProcessorCount_cmd_nproc-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: ProcessorCount_cmd_sysctl +ProcessorCount_cmd_sysctl-ADVANCED:INTERNAL=1 + diff --git a/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CMakeCCompiler.cmake b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CMakeCCompiler.cmake new file mode 100644 index 0000000..58997cb --- /dev/null +++ b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CMakeCCompiler.cmake @@ -0,0 +1,80 @@ +set(CMAKE_C_COMPILER "/usr/bin/cc") +set(CMAKE_C_COMPILER_ARG1 "") +set(CMAKE_C_COMPILER_ID "GNU") +set(CMAKE_C_COMPILER_VERSION "9.3.0") +set(CMAKE_C_COMPILER_VERSION_INTERNAL "") +set(CMAKE_C_COMPILER_WRAPPER "") +set(CMAKE_C_STANDARD_COMPUTED_DEFAULT "17") +set(CMAKE_C_COMPILE_FEATURES "c_std_90;c_function_prototypes;c_std_99;c_restrict;c_variadic_macros;c_std_11;c_static_assert;c_std_17;c_std_23") +set(CMAKE_C90_COMPILE_FEATURES "c_std_90;c_function_prototypes") +set(CMAKE_C99_COMPILE_FEATURES "c_std_99;c_restrict;c_variadic_macros") +set(CMAKE_C11_COMPILE_FEATURES "c_std_11;c_static_assert") +set(CMAKE_C17_COMPILE_FEATURES "c_std_17") +set(CMAKE_C23_COMPILE_FEATURES "c_std_23") + +set(CMAKE_C_PLATFORM_ID "Linux") +set(CMAKE_C_SIMULATE_ID "") +set(CMAKE_C_COMPILER_FRONTEND_VARIANT "") +set(CMAKE_C_SIMULATE_VERSION "") + + + + +set(CMAKE_AR "/usr/bin/ar") +set(CMAKE_C_COMPILER_AR "/usr/bin/gcc-ar-9") +set(CMAKE_RANLIB "/usr/bin/ranlib") +set(CMAKE_C_COMPILER_RANLIB "/usr/bin/gcc-ranlib-9") +set(CMAKE_LINKER "/usr/bin/ld") +set(CMAKE_MT "") +set(CMAKE_COMPILER_IS_GNUCC 1) +set(CMAKE_C_COMPILER_LOADED 1) +set(CMAKE_C_COMPILER_WORKS TRUE) +set(CMAKE_C_ABI_COMPILED TRUE) +set(CMAKE_COMPILER_IS_MINGW ) +set(CMAKE_COMPILER_IS_CYGWIN ) +if(CMAKE_COMPILER_IS_CYGWIN) + set(CYGWIN 1) + set(UNIX 1) +endif() + +set(CMAKE_C_COMPILER_ENV_VAR "CC") + +if(CMAKE_COMPILER_IS_MINGW) + set(MINGW 1) +endif() +set(CMAKE_C_COMPILER_ID_RUN 1) +set(CMAKE_C_SOURCE_FILE_EXTENSIONS c;m) +set(CMAKE_C_IGNORE_EXTENSIONS h;H;o;O;obj;OBJ;def;DEF;rc;RC) +set(CMAKE_C_LINKER_PREFERENCE 10) + +# Save compiler ABI information. +set(CMAKE_C_SIZEOF_DATA_PTR "8") +set(CMAKE_C_COMPILER_ABI "ELF") +set(CMAKE_C_BYTE_ORDER "LITTLE_ENDIAN") +set(CMAKE_C_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") + +if(CMAKE_C_SIZEOF_DATA_PTR) + set(CMAKE_SIZEOF_VOID_P "${CMAKE_C_SIZEOF_DATA_PTR}") +endif() + +if(CMAKE_C_COMPILER_ABI) + set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_C_COMPILER_ABI}") +endif() + +if(CMAKE_C_LIBRARY_ARCHITECTURE) + set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") +endif() + +set(CMAKE_C_CL_SHOWINCLUDES_PREFIX "") +if(CMAKE_C_CL_SHOWINCLUDES_PREFIX) + set(CMAKE_CL_SHOWINCLUDES_PREFIX "${CMAKE_C_CL_SHOWINCLUDES_PREFIX}") +endif() + + + + + +set(CMAKE_C_IMPLICIT_INCLUDE_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/9/include;/usr/local/include;/usr/include/x86_64-linux-gnu;/usr/include") +set(CMAKE_C_IMPLICIT_LINK_LIBRARIES "gcc;gcc_s;c;gcc;gcc_s") +set(CMAKE_C_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/9;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") +set(CMAKE_C_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") diff --git a/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CMakeCXXCompiler.cmake b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CMakeCXXCompiler.cmake new file mode 100644 index 0000000..cd9d282 --- /dev/null +++ b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CMakeCXXCompiler.cmake @@ -0,0 +1,91 @@ +set(CMAKE_CXX_COMPILER "/usr/bin/c++") +set(CMAKE_CXX_COMPILER_ARG1 "") +set(CMAKE_CXX_COMPILER_ID "GNU") +set(CMAKE_CXX_COMPILER_VERSION "9.3.0") +set(CMAKE_CXX_COMPILER_VERSION_INTERNAL "") +set(CMAKE_CXX_COMPILER_WRAPPER "") +set(CMAKE_CXX_STANDARD_COMPUTED_DEFAULT "14") +set(CMAKE_CXX_COMPILE_FEATURES "cxx_std_98;cxx_template_template_parameters;cxx_std_11;cxx_alias_templates;cxx_alignas;cxx_alignof;cxx_attributes;cxx_auto_type;cxx_constexpr;cxx_decltype;cxx_decltype_incomplete_return_types;cxx_default_function_template_args;cxx_defaulted_functions;cxx_defaulted_move_initializers;cxx_delegating_constructors;cxx_deleted_functions;cxx_enum_forward_declarations;cxx_explicit_conversions;cxx_extended_friend_declarations;cxx_extern_templates;cxx_final;cxx_func_identifier;cxx_generalized_initializers;cxx_inheriting_constructors;cxx_inline_namespaces;cxx_lambdas;cxx_local_type_template_args;cxx_long_long_type;cxx_noexcept;cxx_nonstatic_member_init;cxx_nullptr;cxx_override;cxx_range_for;cxx_raw_string_literals;cxx_reference_qualified_functions;cxx_right_angle_brackets;cxx_rvalue_references;cxx_sizeof_member;cxx_static_assert;cxx_strong_enums;cxx_thread_local;cxx_trailing_return_types;cxx_unicode_literals;cxx_uniform_initialization;cxx_unrestricted_unions;cxx_user_literals;cxx_variadic_macros;cxx_variadic_templates;cxx_std_14;cxx_aggregate_default_initializers;cxx_attribute_deprecated;cxx_binary_literals;cxx_contextual_conversions;cxx_decltype_auto;cxx_digit_separators;cxx_generic_lambdas;cxx_lambda_init_captures;cxx_relaxed_constexpr;cxx_return_type_deduction;cxx_variable_templates;cxx_std_17;cxx_std_20") +set(CMAKE_CXX98_COMPILE_FEATURES "cxx_std_98;cxx_template_template_parameters") +set(CMAKE_CXX11_COMPILE_FEATURES "cxx_std_11;cxx_alias_templates;cxx_alignas;cxx_alignof;cxx_attributes;cxx_auto_type;cxx_constexpr;cxx_decltype;cxx_decltype_incomplete_return_types;cxx_default_function_template_args;cxx_defaulted_functions;cxx_defaulted_move_initializers;cxx_delegating_constructors;cxx_deleted_functions;cxx_enum_forward_declarations;cxx_explicit_conversions;cxx_extended_friend_declarations;cxx_extern_templates;cxx_final;cxx_func_identifier;cxx_generalized_initializers;cxx_inheriting_constructors;cxx_inline_namespaces;cxx_lambdas;cxx_local_type_template_args;cxx_long_long_type;cxx_noexcept;cxx_nonstatic_member_init;cxx_nullptr;cxx_override;cxx_range_for;cxx_raw_string_literals;cxx_reference_qualified_functions;cxx_right_angle_brackets;cxx_rvalue_references;cxx_sizeof_member;cxx_static_assert;cxx_strong_enums;cxx_thread_local;cxx_trailing_return_types;cxx_unicode_literals;cxx_uniform_initialization;cxx_unrestricted_unions;cxx_user_literals;cxx_variadic_macros;cxx_variadic_templates") +set(CMAKE_CXX14_COMPILE_FEATURES "cxx_std_14;cxx_aggregate_default_initializers;cxx_attribute_deprecated;cxx_binary_literals;cxx_contextual_conversions;cxx_decltype_auto;cxx_digit_separators;cxx_generic_lambdas;cxx_lambda_init_captures;cxx_relaxed_constexpr;cxx_return_type_deduction;cxx_variable_templates") +set(CMAKE_CXX17_COMPILE_FEATURES "cxx_std_17") +set(CMAKE_CXX20_COMPILE_FEATURES "cxx_std_20") +set(CMAKE_CXX23_COMPILE_FEATURES "") + +set(CMAKE_CXX_PLATFORM_ID "Linux") +set(CMAKE_CXX_SIMULATE_ID "") +set(CMAKE_CXX_COMPILER_FRONTEND_VARIANT "") +set(CMAKE_CXX_SIMULATE_VERSION "") + + + + +set(CMAKE_AR "/usr/bin/ar") +set(CMAKE_CXX_COMPILER_AR "/usr/bin/gcc-ar-9") +set(CMAKE_RANLIB "/usr/bin/ranlib") +set(CMAKE_CXX_COMPILER_RANLIB "/usr/bin/gcc-ranlib-9") +set(CMAKE_LINKER "/usr/bin/ld") +set(CMAKE_MT "") +set(CMAKE_COMPILER_IS_GNUCXX 1) +set(CMAKE_CXX_COMPILER_LOADED 1) +set(CMAKE_CXX_COMPILER_WORKS TRUE) +set(CMAKE_CXX_ABI_COMPILED TRUE) +set(CMAKE_COMPILER_IS_MINGW ) +set(CMAKE_COMPILER_IS_CYGWIN ) +if(CMAKE_COMPILER_IS_CYGWIN) + set(CYGWIN 1) + set(UNIX 1) +endif() + +set(CMAKE_CXX_COMPILER_ENV_VAR "CXX") + +if(CMAKE_COMPILER_IS_MINGW) + set(MINGW 1) +endif() +set(CMAKE_CXX_COMPILER_ID_RUN 1) +set(CMAKE_CXX_SOURCE_FILE_EXTENSIONS C;M;c++;cc;cpp;cxx;m;mm;mpp;CPP;ixx;cppm) +set(CMAKE_CXX_IGNORE_EXTENSIONS inl;h;hpp;HPP;H;o;O;obj;OBJ;def;DEF;rc;RC) + +foreach (lang C OBJC OBJCXX) + if (CMAKE_${lang}_COMPILER_ID_RUN) + foreach(extension IN LISTS CMAKE_${lang}_SOURCE_FILE_EXTENSIONS) + list(REMOVE_ITEM CMAKE_CXX_SOURCE_FILE_EXTENSIONS ${extension}) + endforeach() + endif() +endforeach() + +set(CMAKE_CXX_LINKER_PREFERENCE 30) +set(CMAKE_CXX_LINKER_PREFERENCE_PROPAGATES 1) + +# Save compiler ABI information. +set(CMAKE_CXX_SIZEOF_DATA_PTR "8") +set(CMAKE_CXX_COMPILER_ABI "ELF") +set(CMAKE_CXX_BYTE_ORDER "LITTLE_ENDIAN") +set(CMAKE_CXX_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") + +if(CMAKE_CXX_SIZEOF_DATA_PTR) + set(CMAKE_SIZEOF_VOID_P "${CMAKE_CXX_SIZEOF_DATA_PTR}") +endif() + +if(CMAKE_CXX_COMPILER_ABI) + set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_CXX_COMPILER_ABI}") +endif() + +if(CMAKE_CXX_LIBRARY_ARCHITECTURE) + set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") +endif() + +set(CMAKE_CXX_CL_SHOWINCLUDES_PREFIX "") +if(CMAKE_CXX_CL_SHOWINCLUDES_PREFIX) + set(CMAKE_CL_SHOWINCLUDES_PREFIX "${CMAKE_CXX_CL_SHOWINCLUDES_PREFIX}") +endif() + + + + + +set(CMAKE_CXX_IMPLICIT_INCLUDE_DIRECTORIES "/usr/include/c++/9;/usr/include/x86_64-linux-gnu/c++/9;/usr/include/c++/9/backward;/usr/lib/gcc/x86_64-linux-gnu/9/include;/usr/local/include;/usr/include/x86_64-linux-gnu;/usr/include") +set(CMAKE_CXX_IMPLICIT_LINK_LIBRARIES "stdc++;m;gcc_s;gcc;c;gcc_s;gcc") +set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/9;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") +set(CMAKE_CXX_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") diff --git a/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CMakeDetermineCompilerABI_C.bin b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CMakeDetermineCompilerABI_C.bin new file mode 100755 index 0000000..a0cb2d4 Binary files /dev/null and b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CMakeDetermineCompilerABI_C.bin differ diff --git a/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CMakeDetermineCompilerABI_CXX.bin b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CMakeDetermineCompilerABI_CXX.bin new file mode 100755 index 0000000..582f071 Binary files /dev/null and b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CMakeDetermineCompilerABI_CXX.bin differ diff --git a/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CMakeSystem.cmake b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CMakeSystem.cmake new file mode 100644 index 0000000..b2cbc0b --- /dev/null +++ b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CMakeSystem.cmake @@ -0,0 +1,15 @@ +set(CMAKE_HOST_SYSTEM "Linux-5.13.0-27-generic") +set(CMAKE_HOST_SYSTEM_NAME "Linux") +set(CMAKE_HOST_SYSTEM_VERSION "5.13.0-27-generic") +set(CMAKE_HOST_SYSTEM_PROCESSOR "x86_64") + + + +set(CMAKE_SYSTEM "Linux-5.13.0-27-generic") +set(CMAKE_SYSTEM_NAME "Linux") +set(CMAKE_SYSTEM_VERSION "5.13.0-27-generic") +set(CMAKE_SYSTEM_PROCESSOR "x86_64") + +set(CMAKE_CROSSCOMPILING "FALSE") + +set(CMAKE_SYSTEM_LOADED 1) diff --git a/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CompilerIdC/CMakeCCompilerId.c b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CompilerIdC/CMakeCCompilerId.c new file mode 100644 index 0000000..051f748 --- /dev/null +++ b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CompilerIdC/CMakeCCompilerId.c @@ -0,0 +1,807 @@ +#ifdef __cplusplus +# error "A C++ compiler has been selected for C." +#endif + +#if defined(__18CXX) +# define ID_VOID_MAIN +#endif +#if defined(__CLASSIC_C__) +/* cv-qualifiers did not exist in K&R C */ +# define const +# define volatile +#endif + +#if !defined(__has_include) +/* If the compiler does not have __has_include, pretend the answer is + always no. */ +# define __has_include(x) 0 +#endif + + +/* Version number components: V=Version, R=Revision, P=Patch + Version date components: YYYY=Year, MM=Month, DD=Day */ + +#if defined(__INTEL_COMPILER) || defined(__ICC) +# define COMPILER_ID "Intel" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# if defined(__GNUC__) +# define SIMULATE_ID "GNU" +# endif + /* __INTEL_COMPILER = VRP prior to 2021, and then VVVV for 2021 and later, + except that a few beta releases use the old format with V=2021. */ +# if __INTEL_COMPILER < 2021 || __INTEL_COMPILER == 202110 || __INTEL_COMPILER == 202111 +# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100) +# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10) +# if defined(__INTEL_COMPILER_UPDATE) +# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER_UPDATE) +# else +# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER % 10) +# endif +# else +# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER) +# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER_UPDATE) + /* The third version component from --version is an update index, + but no macro is provided for it. */ +# define COMPILER_VERSION_PATCH DEC(0) +# endif +# if defined(__INTEL_COMPILER_BUILD_DATE) + /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */ +# define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE) +# endif +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif +# if defined(__GNUC__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUC__) +# elif defined(__GNUG__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUG__) +# endif +# if defined(__GNUC_MINOR__) +# define SIMULATE_VERSION_MINOR DEC(__GNUC_MINOR__) +# endif +# if defined(__GNUC_PATCHLEVEL__) +# define SIMULATE_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif + +#elif (defined(__clang__) && defined(__INTEL_CLANG_COMPILER)) || defined(__INTEL_LLVM_COMPILER) +# define COMPILER_ID "IntelLLVM" +#if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +#endif +#if defined(__GNUC__) +# define SIMULATE_ID "GNU" +#endif +/* __INTEL_LLVM_COMPILER = VVVVRP prior to 2021.2.0, VVVVRRPP for 2021.2.0 and + * later. Look for 6 digit vs. 8 digit version number to decide encoding. + * VVVV is no smaller than the current year when a version is released. + */ +#if __INTEL_LLVM_COMPILER < 1000000L +# define COMPILER_VERSION_MAJOR DEC(__INTEL_LLVM_COMPILER/100) +# define COMPILER_VERSION_MINOR DEC(__INTEL_LLVM_COMPILER/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__INTEL_LLVM_COMPILER % 10) +#else +# define COMPILER_VERSION_MAJOR DEC(__INTEL_LLVM_COMPILER/10000) +# define COMPILER_VERSION_MINOR DEC(__INTEL_LLVM_COMPILER/100 % 100) +# define COMPILER_VERSION_PATCH DEC(__INTEL_LLVM_COMPILER % 100) +#endif +#if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +#endif +#if defined(__GNUC__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUC__) +#elif defined(__GNUG__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUG__) +#endif +#if defined(__GNUC_MINOR__) +# define SIMULATE_VERSION_MINOR DEC(__GNUC_MINOR__) +#endif +#if defined(__GNUC_PATCHLEVEL__) +# define SIMULATE_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +#endif + +#elif defined(__PATHCC__) +# define COMPILER_ID "PathScale" +# define COMPILER_VERSION_MAJOR DEC(__PATHCC__) +# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__) +# if defined(__PATHCC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__) +# endif + +#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__) +# define COMPILER_ID "Embarcadero" +# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF) +# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF) +# define COMPILER_VERSION_PATCH DEC(__CODEGEARC_VERSION__ & 0xFFFF) + +#elif defined(__BORLANDC__) +# define COMPILER_ID "Borland" + /* __BORLANDC__ = 0xVRR */ +# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8) +# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF) + +#elif defined(__WATCOMC__) && __WATCOMC__ < 1200 +# define COMPILER_ID "Watcom" + /* __WATCOMC__ = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100) +# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10) +# if (__WATCOMC__ % 10) > 0 +# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10) +# endif + +#elif defined(__WATCOMC__) +# define COMPILER_ID "OpenWatcom" + /* __WATCOMC__ = VVRP + 1100 */ +# define COMPILER_VERSION_MAJOR DEC((__WATCOMC__ - 1100) / 100) +# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10) +# if (__WATCOMC__ % 10) > 0 +# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10) +# endif + +#elif defined(__SUNPRO_C) +# define COMPILER_ID "SunPro" +# if __SUNPRO_C >= 0x5100 + /* __SUNPRO_C = 0xVRRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>12) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF) +# else + /* __SUNPRO_CC = 0xVRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>8) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF) +# endif + +#elif defined(__HP_cc) +# define COMPILER_ID "HP" + /* __HP_cc = VVRRPP */ +# define COMPILER_VERSION_MAJOR DEC(__HP_cc/10000) +# define COMPILER_VERSION_MINOR DEC(__HP_cc/100 % 100) +# define COMPILER_VERSION_PATCH DEC(__HP_cc % 100) + +#elif defined(__DECC) +# define COMPILER_ID "Compaq" + /* __DECC_VER = VVRRTPPPP */ +# define COMPILER_VERSION_MAJOR DEC(__DECC_VER/10000000) +# define COMPILER_VERSION_MINOR DEC(__DECC_VER/100000 % 100) +# define COMPILER_VERSION_PATCH DEC(__DECC_VER % 10000) + +#elif defined(__IBMC__) && defined(__COMPILER_VER__) +# define COMPILER_ID "zOS" + /* __IBMC__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10) + +#elif defined(__ibmxl__) && defined(__clang__) +# define COMPILER_ID "XLClang" +# define COMPILER_VERSION_MAJOR DEC(__ibmxl_version__) +# define COMPILER_VERSION_MINOR DEC(__ibmxl_release__) +# define COMPILER_VERSION_PATCH DEC(__ibmxl_modification__) +# define COMPILER_VERSION_TWEAK DEC(__ibmxl_ptf_fix_level__) + + +#elif defined(__IBMC__) && !defined(__COMPILER_VER__) && __IBMC__ >= 800 +# define COMPILER_ID "XL" + /* __IBMC__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10) + +#elif defined(__IBMC__) && !defined(__COMPILER_VER__) && __IBMC__ < 800 +# define COMPILER_ID "VisualAge" + /* __IBMC__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10) + +#elif defined(__NVCOMPILER) +# define COMPILER_ID "NVHPC" +# define COMPILER_VERSION_MAJOR DEC(__NVCOMPILER_MAJOR__) +# define COMPILER_VERSION_MINOR DEC(__NVCOMPILER_MINOR__) +# if defined(__NVCOMPILER_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__NVCOMPILER_PATCHLEVEL__) +# endif + +#elif defined(__PGI) +# define COMPILER_ID "PGI" +# define COMPILER_VERSION_MAJOR DEC(__PGIC__) +# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__) +# if defined(__PGIC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__) +# endif + +#elif defined(_CRAYC) +# define COMPILER_ID "Cray" +# define COMPILER_VERSION_MAJOR DEC(_RELEASE_MAJOR) +# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR) + +#elif defined(__TI_COMPILER_VERSION__) +# define COMPILER_ID "TI" + /* __TI_COMPILER_VERSION__ = VVVRRRPPP */ +# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000) +# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000) +# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000) + +#elif defined(__CLANG_FUJITSU) +# define COMPILER_ID "FujitsuClang" +# define COMPILER_VERSION_MAJOR DEC(__FCC_major__) +# define COMPILER_VERSION_MINOR DEC(__FCC_minor__) +# define COMPILER_VERSION_PATCH DEC(__FCC_patchlevel__) +# define COMPILER_VERSION_INTERNAL_STR __clang_version__ + + +#elif defined(__FUJITSU) +# define COMPILER_ID "Fujitsu" +# if defined(__FCC_version__) +# define COMPILER_VERSION __FCC_version__ +# elif defined(__FCC_major__) +# define COMPILER_VERSION_MAJOR DEC(__FCC_major__) +# define COMPILER_VERSION_MINOR DEC(__FCC_minor__) +# define COMPILER_VERSION_PATCH DEC(__FCC_patchlevel__) +# endif +# if defined(__fcc_version) +# define COMPILER_VERSION_INTERNAL DEC(__fcc_version) +# elif defined(__FCC_VERSION) +# define COMPILER_VERSION_INTERNAL DEC(__FCC_VERSION) +# endif + + +#elif defined(__ghs__) +# define COMPILER_ID "GHS" +/* __GHS_VERSION_NUMBER = VVVVRP */ +# ifdef __GHS_VERSION_NUMBER +# define COMPILER_VERSION_MAJOR DEC(__GHS_VERSION_NUMBER / 100) +# define COMPILER_VERSION_MINOR DEC(__GHS_VERSION_NUMBER / 10 % 10) +# define COMPILER_VERSION_PATCH DEC(__GHS_VERSION_NUMBER % 10) +# endif + +#elif defined(__TINYC__) +# define COMPILER_ID "TinyCC" + +#elif defined(__BCC__) +# define COMPILER_ID "Bruce" + +#elif defined(__SCO_VERSION__) +# define COMPILER_ID "SCO" + +#elif defined(__ARMCC_VERSION) && !defined(__clang__) +# define COMPILER_ID "ARMCC" +#if __ARMCC_VERSION >= 1000000 + /* __ARMCC_VERSION = VRRPPPP */ + # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/1000000) + # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 100) + # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000) +#else + /* __ARMCC_VERSION = VRPPPP */ + # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/100000) + # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 10) + # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000) +#endif + + +#elif defined(__clang__) && defined(__apple_build_version__) +# define COMPILER_ID "AppleClang" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# define COMPILER_VERSION_MAJOR DEC(__clang_major__) +# define COMPILER_VERSION_MINOR DEC(__clang_minor__) +# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif +# define COMPILER_VERSION_TWEAK DEC(__apple_build_version__) + +#elif defined(__clang__) && defined(__ARMCOMPILER_VERSION) +# define COMPILER_ID "ARMClang" + # define COMPILER_VERSION_MAJOR DEC(__ARMCOMPILER_VERSION/1000000) + # define COMPILER_VERSION_MINOR DEC(__ARMCOMPILER_VERSION/10000 % 100) + # define COMPILER_VERSION_PATCH DEC(__ARMCOMPILER_VERSION % 10000) +# define COMPILER_VERSION_INTERNAL DEC(__ARMCOMPILER_VERSION) + +#elif defined(__clang__) && __has_include() +# define COMPILER_ID "ROCMClang" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# elif defined(__clang__) +# define SIMULATE_ID "Clang" +# elif defined(__GNUC__) +# define SIMULATE_ID "GNU" +# endif +# if defined(__clang__) && __has_include() +# include +# define COMPILER_VERSION_MAJOR DEC(HIP_VERSION_MAJOR) +# define COMPILER_VERSION_MINOR DEC(HIP_VERSION_MINOR) +# define COMPILER_VERSION_PATCH DEC(HIP_VERSION_PATCH) +# endif + +#elif defined(__clang__) +# define COMPILER_ID "Clang" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# define COMPILER_VERSION_MAJOR DEC(__clang_major__) +# define COMPILER_VERSION_MINOR DEC(__clang_minor__) +# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif + +#elif defined(__GNUC__) +# define COMPILER_ID "GNU" +# define COMPILER_VERSION_MAJOR DEC(__GNUC__) +# if defined(__GNUC_MINOR__) +# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__) +# endif +# if defined(__GNUC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif + +#elif defined(_MSC_VER) +# define COMPILER_ID "MSVC" + /* _MSC_VER = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100) +# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100) +# if defined(_MSC_FULL_VER) +# if _MSC_VER >= 1400 + /* _MSC_FULL_VER = VVRRPPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000) +# else + /* _MSC_FULL_VER = VVRRPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000) +# endif +# endif +# if defined(_MSC_BUILD) +# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD) +# endif + +#elif defined(__VISUALDSPVERSION__) || defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__) +# define COMPILER_ID "ADSP" +#if defined(__VISUALDSPVERSION__) + /* __VISUALDSPVERSION__ = 0xVVRRPP00 */ +# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24) +# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8 & 0xFF) +#endif + +#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC) +# define COMPILER_ID "IAR" +# if defined(__VER__) && defined(__ICCARM__) +# define COMPILER_VERSION_MAJOR DEC((__VER__) / 1000000) +# define COMPILER_VERSION_MINOR DEC(((__VER__) / 1000) % 1000) +# define COMPILER_VERSION_PATCH DEC((__VER__) % 1000) +# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__) +# elif defined(__VER__) && (defined(__ICCAVR__) || defined(__ICCRX__) || defined(__ICCRH850__) || defined(__ICCRL78__) || defined(__ICC430__) || defined(__ICCRISCV__) || defined(__ICCV850__) || defined(__ICC8051__) || defined(__ICCSTM8__)) +# define COMPILER_VERSION_MAJOR DEC((__VER__) / 100) +# define COMPILER_VERSION_MINOR DEC((__VER__) - (((__VER__) / 100)*100)) +# define COMPILER_VERSION_PATCH DEC(__SUBVERSION__) +# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__) +# endif + +#elif defined(__SDCC_VERSION_MAJOR) || defined(SDCC) +# define COMPILER_ID "SDCC" +# if defined(__SDCC_VERSION_MAJOR) +# define COMPILER_VERSION_MAJOR DEC(__SDCC_VERSION_MAJOR) +# define COMPILER_VERSION_MINOR DEC(__SDCC_VERSION_MINOR) +# define COMPILER_VERSION_PATCH DEC(__SDCC_VERSION_PATCH) +# else + /* SDCC = VRP */ +# define COMPILER_VERSION_MAJOR DEC(SDCC/100) +# define COMPILER_VERSION_MINOR DEC(SDCC/10 % 10) +# define COMPILER_VERSION_PATCH DEC(SDCC % 10) +# endif + + +/* These compilers are either not known or too old to define an + identification macro. Try to identify the platform and guess that + it is the native compiler. */ +#elif defined(__hpux) || defined(__hpua) +# define COMPILER_ID "HP" + +#else /* unknown compiler */ +# define COMPILER_ID "" +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]"; +#ifdef SIMULATE_ID +char const* info_simulate = "INFO" ":" "simulate[" SIMULATE_ID "]"; +#endif + +#ifdef __QNXNTO__ +char const* qnxnto = "INFO" ":" "qnxnto[]"; +#endif + +#if defined(__CRAYXT_COMPUTE_LINUX_TARGET) +char const *info_cray = "INFO" ":" "compiler_wrapper[CrayPrgEnv]"; +#endif + +#define STRINGIFY_HELPER(X) #X +#define STRINGIFY(X) STRINGIFY_HELPER(X) + +/* Identify known platforms by name. */ +#if defined(__linux) || defined(__linux__) || defined(linux) +# define PLATFORM_ID "Linux" + +#elif defined(__MSYS__) +# define PLATFORM_ID "MSYS" + +#elif defined(__CYGWIN__) +# define PLATFORM_ID "Cygwin" + +#elif defined(__MINGW32__) +# define PLATFORM_ID "MinGW" + +#elif defined(__APPLE__) +# define PLATFORM_ID "Darwin" + +#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32) +# define PLATFORM_ID "Windows" + +#elif defined(__FreeBSD__) || defined(__FreeBSD) +# define PLATFORM_ID "FreeBSD" + +#elif defined(__NetBSD__) || defined(__NetBSD) +# define PLATFORM_ID "NetBSD" + +#elif defined(__OpenBSD__) || defined(__OPENBSD) +# define PLATFORM_ID "OpenBSD" + +#elif defined(__sun) || defined(sun) +# define PLATFORM_ID "SunOS" + +#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__) +# define PLATFORM_ID "AIX" + +#elif defined(__hpux) || defined(__hpux__) +# define PLATFORM_ID "HP-UX" + +#elif defined(__HAIKU__) +# define PLATFORM_ID "Haiku" + +#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS) +# define PLATFORM_ID "BeOS" + +#elif defined(__QNX__) || defined(__QNXNTO__) +# define PLATFORM_ID "QNX" + +#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__) +# define PLATFORM_ID "Tru64" + +#elif defined(__riscos) || defined(__riscos__) +# define PLATFORM_ID "RISCos" + +#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__) +# define PLATFORM_ID "SINIX" + +#elif defined(__UNIX_SV__) +# define PLATFORM_ID "UNIX_SV" + +#elif defined(__bsdos__) +# define PLATFORM_ID "BSDOS" + +#elif defined(_MPRAS) || defined(MPRAS) +# define PLATFORM_ID "MP-RAS" + +#elif defined(__osf) || defined(__osf__) +# define PLATFORM_ID "OSF1" + +#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv) +# define PLATFORM_ID "SCO_SV" + +#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX) +# define PLATFORM_ID "ULTRIX" + +#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX) +# define PLATFORM_ID "Xenix" + +#elif defined(__WATCOMC__) +# if defined(__LINUX__) +# define PLATFORM_ID "Linux" + +# elif defined(__DOS__) +# define PLATFORM_ID "DOS" + +# elif defined(__OS2__) +# define PLATFORM_ID "OS2" + +# elif defined(__WINDOWS__) +# define PLATFORM_ID "Windows3x" + +# elif defined(__VXWORKS__) +# define PLATFORM_ID "VxWorks" + +# else /* unknown platform */ +# define PLATFORM_ID +# endif + +#elif defined(__INTEGRITY) +# if defined(INT_178B) +# define PLATFORM_ID "Integrity178" + +# else /* regular Integrity */ +# define PLATFORM_ID "Integrity" +# endif + +#else /* unknown platform */ +# define PLATFORM_ID + +#endif + +/* For windows compilers MSVC and Intel we can determine + the architecture of the compiler being used. This is because + the compilers do not have flags that can change the architecture, + but rather depend on which compiler is being used +*/ +#if defined(_WIN32) && defined(_MSC_VER) +# if defined(_M_IA64) +# define ARCHITECTURE_ID "IA64" + +# elif defined(_M_ARM64EC) +# define ARCHITECTURE_ID "ARM64EC" + +# elif defined(_M_X64) || defined(_M_AMD64) +# define ARCHITECTURE_ID "x64" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# elif defined(_M_ARM64) +# define ARCHITECTURE_ID "ARM64" + +# elif defined(_M_ARM) +# if _M_ARM == 4 +# define ARCHITECTURE_ID "ARMV4I" +# elif _M_ARM == 5 +# define ARCHITECTURE_ID "ARMV5I" +# else +# define ARCHITECTURE_ID "ARMV" STRINGIFY(_M_ARM) +# endif + +# elif defined(_M_MIPS) +# define ARCHITECTURE_ID "MIPS" + +# elif defined(_M_SH) +# define ARCHITECTURE_ID "SHx" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__WATCOMC__) +# if defined(_M_I86) +# define ARCHITECTURE_ID "I86" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC) +# if defined(__ICCARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__ICCRX__) +# define ARCHITECTURE_ID "RX" + +# elif defined(__ICCRH850__) +# define ARCHITECTURE_ID "RH850" + +# elif defined(__ICCRL78__) +# define ARCHITECTURE_ID "RL78" + +# elif defined(__ICCRISCV__) +# define ARCHITECTURE_ID "RISCV" + +# elif defined(__ICCAVR__) +# define ARCHITECTURE_ID "AVR" + +# elif defined(__ICC430__) +# define ARCHITECTURE_ID "MSP430" + +# elif defined(__ICCV850__) +# define ARCHITECTURE_ID "V850" + +# elif defined(__ICC8051__) +# define ARCHITECTURE_ID "8051" + +# elif defined(__ICCSTM8__) +# define ARCHITECTURE_ID "STM8" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__ghs__) +# if defined(__PPC64__) +# define ARCHITECTURE_ID "PPC64" + +# elif defined(__ppc__) +# define ARCHITECTURE_ID "PPC" + +# elif defined(__ARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__x86_64__) +# define ARCHITECTURE_ID "x64" + +# elif defined(__i386__) +# define ARCHITECTURE_ID "X86" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__TI_COMPILER_VERSION__) +# if defined(__TI_ARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__MSP430__) +# define ARCHITECTURE_ID "MSP430" + +# elif defined(__TMS320C28XX__) +# define ARCHITECTURE_ID "TMS320C28x" + +# elif defined(__TMS320C6X__) || defined(_TMS320C6X) +# define ARCHITECTURE_ID "TMS320C6x" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#else +# define ARCHITECTURE_ID +#endif + +/* Convert integer to decimal digit literals. */ +#define DEC(n) \ + ('0' + (((n) / 10000000)%10)), \ + ('0' + (((n) / 1000000)%10)), \ + ('0' + (((n) / 100000)%10)), \ + ('0' + (((n) / 10000)%10)), \ + ('0' + (((n) / 1000)%10)), \ + ('0' + (((n) / 100)%10)), \ + ('0' + (((n) / 10)%10)), \ + ('0' + ((n) % 10)) + +/* Convert integer to hex digit literals. */ +#define HEX(n) \ + ('0' + ((n)>>28 & 0xF)), \ + ('0' + ((n)>>24 & 0xF)), \ + ('0' + ((n)>>20 & 0xF)), \ + ('0' + ((n)>>16 & 0xF)), \ + ('0' + ((n)>>12 & 0xF)), \ + ('0' + ((n)>>8 & 0xF)), \ + ('0' + ((n)>>4 & 0xF)), \ + ('0' + ((n) & 0xF)) + +/* Construct a string literal encoding the version number. */ +#ifdef COMPILER_VERSION +char const* info_version = "INFO" ":" "compiler_version[" COMPILER_VERSION "]"; + +/* Construct a string literal encoding the version number components. */ +#elif defined(COMPILER_VERSION_MAJOR) +char const info_version[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[', + COMPILER_VERSION_MAJOR, +# ifdef COMPILER_VERSION_MINOR + '.', COMPILER_VERSION_MINOR, +# ifdef COMPILER_VERSION_PATCH + '.', COMPILER_VERSION_PATCH, +# ifdef COMPILER_VERSION_TWEAK + '.', COMPILER_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct a string literal encoding the internal version number. */ +#ifdef COMPILER_VERSION_INTERNAL +char const info_version_internal[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','_', + 'i','n','t','e','r','n','a','l','[', + COMPILER_VERSION_INTERNAL,']','\0'}; +#elif defined(COMPILER_VERSION_INTERNAL_STR) +char const* info_version_internal = "INFO" ":" "compiler_version_internal[" COMPILER_VERSION_INTERNAL_STR "]"; +#endif + +/* Construct a string literal encoding the version number components. */ +#ifdef SIMULATE_VERSION_MAJOR +char const info_simulate_version[] = { + 'I', 'N', 'F', 'O', ':', + 's','i','m','u','l','a','t','e','_','v','e','r','s','i','o','n','[', + SIMULATE_VERSION_MAJOR, +# ifdef SIMULATE_VERSION_MINOR + '.', SIMULATE_VERSION_MINOR, +# ifdef SIMULATE_VERSION_PATCH + '.', SIMULATE_VERSION_PATCH, +# ifdef SIMULATE_VERSION_TWEAK + '.', SIMULATE_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]"; +char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]"; + + + +#if !defined(__STDC__) && !defined(__clang__) +# if defined(_MSC_VER) || defined(__ibmxl__) || defined(__IBMC__) +# define C_DIALECT "90" +# else +# define C_DIALECT +# endif +#elif __STDC_VERSION__ > 201710L +# define C_DIALECT "23" +#elif __STDC_VERSION__ >= 201710L +# define C_DIALECT "17" +#elif __STDC_VERSION__ >= 201000L +# define C_DIALECT "11" +#elif __STDC_VERSION__ >= 199901L +# define C_DIALECT "99" +#else +# define C_DIALECT "90" +#endif +const char* info_language_dialect_default = + "INFO" ":" "dialect_default[" C_DIALECT "]"; + +/*--------------------------------------------------------------------------*/ + +#ifdef ID_VOID_MAIN +void main() {} +#else +# if defined(__CLASSIC_C__) +int main(argc, argv) int argc; char *argv[]; +# else +int main(int argc, char* argv[]) +# endif +{ + int require = 0; + require += info_compiler[argc]; + require += info_platform[argc]; + require += info_arch[argc]; +#ifdef COMPILER_VERSION_MAJOR + require += info_version[argc]; +#endif +#ifdef COMPILER_VERSION_INTERNAL + require += info_version_internal[argc]; +#endif +#ifdef SIMULATE_ID + require += info_simulate[argc]; +#endif +#ifdef SIMULATE_VERSION_MAJOR + require += info_simulate_version[argc]; +#endif +#if defined(__CRAYXT_COMPUTE_LINUX_TARGET) + require += info_cray[argc]; +#endif + require += info_language_dialect_default[argc]; + (void)argv; + return require; +} +#endif diff --git a/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CompilerIdC/a.out b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CompilerIdC/a.out new file mode 100755 index 0000000..2d45b28 Binary files /dev/null and b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CompilerIdC/a.out differ diff --git a/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CompilerIdCXX/CMakeCXXCompilerId.cpp b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CompilerIdCXX/CMakeCXXCompilerId.cpp new file mode 100644 index 0000000..f675021 --- /dev/null +++ b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CompilerIdCXX/CMakeCXXCompilerId.cpp @@ -0,0 +1,795 @@ +/* This source file must have a .cpp extension so that all C++ compilers + recognize the extension without flags. Borland does not know .cxx for + example. */ +#ifndef __cplusplus +# error "A C compiler has been selected for C++." +#endif + +#if !defined(__has_include) +/* If the compiler does not have __has_include, pretend the answer is + always no. */ +# define __has_include(x) 0 +#endif + + +/* Version number components: V=Version, R=Revision, P=Patch + Version date components: YYYY=Year, MM=Month, DD=Day */ + +#if defined(__COMO__) +# define COMPILER_ID "Comeau" + /* __COMO_VERSION__ = VRR */ +# define COMPILER_VERSION_MAJOR DEC(__COMO_VERSION__ / 100) +# define COMPILER_VERSION_MINOR DEC(__COMO_VERSION__ % 100) + +#elif defined(__INTEL_COMPILER) || defined(__ICC) +# define COMPILER_ID "Intel" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# if defined(__GNUC__) +# define SIMULATE_ID "GNU" +# endif + /* __INTEL_COMPILER = VRP prior to 2021, and then VVVV for 2021 and later, + except that a few beta releases use the old format with V=2021. */ +# if __INTEL_COMPILER < 2021 || __INTEL_COMPILER == 202110 || __INTEL_COMPILER == 202111 +# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100) +# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10) +# if defined(__INTEL_COMPILER_UPDATE) +# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER_UPDATE) +# else +# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER % 10) +# endif +# else +# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER) +# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER_UPDATE) + /* The third version component from --version is an update index, + but no macro is provided for it. */ +# define COMPILER_VERSION_PATCH DEC(0) +# endif +# if defined(__INTEL_COMPILER_BUILD_DATE) + /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */ +# define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE) +# endif +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif +# if defined(__GNUC__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUC__) +# elif defined(__GNUG__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUG__) +# endif +# if defined(__GNUC_MINOR__) +# define SIMULATE_VERSION_MINOR DEC(__GNUC_MINOR__) +# endif +# if defined(__GNUC_PATCHLEVEL__) +# define SIMULATE_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif + +#elif (defined(__clang__) && defined(__INTEL_CLANG_COMPILER)) || defined(__INTEL_LLVM_COMPILER) +# define COMPILER_ID "IntelLLVM" +#if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +#endif +#if defined(__GNUC__) +# define SIMULATE_ID "GNU" +#endif +/* __INTEL_LLVM_COMPILER = VVVVRP prior to 2021.2.0, VVVVRRPP for 2021.2.0 and + * later. Look for 6 digit vs. 8 digit version number to decide encoding. + * VVVV is no smaller than the current year when a version is released. + */ +#if __INTEL_LLVM_COMPILER < 1000000L +# define COMPILER_VERSION_MAJOR DEC(__INTEL_LLVM_COMPILER/100) +# define COMPILER_VERSION_MINOR DEC(__INTEL_LLVM_COMPILER/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__INTEL_LLVM_COMPILER % 10) +#else +# define COMPILER_VERSION_MAJOR DEC(__INTEL_LLVM_COMPILER/10000) +# define COMPILER_VERSION_MINOR DEC(__INTEL_LLVM_COMPILER/100 % 100) +# define COMPILER_VERSION_PATCH DEC(__INTEL_LLVM_COMPILER % 100) +#endif +#if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +#endif +#if defined(__GNUC__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUC__) +#elif defined(__GNUG__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUG__) +#endif +#if defined(__GNUC_MINOR__) +# define SIMULATE_VERSION_MINOR DEC(__GNUC_MINOR__) +#endif +#if defined(__GNUC_PATCHLEVEL__) +# define SIMULATE_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +#endif + +#elif defined(__PATHCC__) +# define COMPILER_ID "PathScale" +# define COMPILER_VERSION_MAJOR DEC(__PATHCC__) +# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__) +# if defined(__PATHCC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__) +# endif + +#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__) +# define COMPILER_ID "Embarcadero" +# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF) +# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF) +# define COMPILER_VERSION_PATCH DEC(__CODEGEARC_VERSION__ & 0xFFFF) + +#elif defined(__BORLANDC__) +# define COMPILER_ID "Borland" + /* __BORLANDC__ = 0xVRR */ +# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8) +# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF) + +#elif defined(__WATCOMC__) && __WATCOMC__ < 1200 +# define COMPILER_ID "Watcom" + /* __WATCOMC__ = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100) +# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10) +# if (__WATCOMC__ % 10) > 0 +# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10) +# endif + +#elif defined(__WATCOMC__) +# define COMPILER_ID "OpenWatcom" + /* __WATCOMC__ = VVRP + 1100 */ +# define COMPILER_VERSION_MAJOR DEC((__WATCOMC__ - 1100) / 100) +# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10) +# if (__WATCOMC__ % 10) > 0 +# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10) +# endif + +#elif defined(__SUNPRO_CC) +# define COMPILER_ID "SunPro" +# if __SUNPRO_CC >= 0x5100 + /* __SUNPRO_CC = 0xVRRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>12) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF) +# else + /* __SUNPRO_CC = 0xVRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>8) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF) +# endif + +#elif defined(__HP_aCC) +# define COMPILER_ID "HP" + /* __HP_aCC = VVRRPP */ +# define COMPILER_VERSION_MAJOR DEC(__HP_aCC/10000) +# define COMPILER_VERSION_MINOR DEC(__HP_aCC/100 % 100) +# define COMPILER_VERSION_PATCH DEC(__HP_aCC % 100) + +#elif defined(__DECCXX) +# define COMPILER_ID "Compaq" + /* __DECCXX_VER = VVRRTPPPP */ +# define COMPILER_VERSION_MAJOR DEC(__DECCXX_VER/10000000) +# define COMPILER_VERSION_MINOR DEC(__DECCXX_VER/100000 % 100) +# define COMPILER_VERSION_PATCH DEC(__DECCXX_VER % 10000) + +#elif defined(__IBMCPP__) && defined(__COMPILER_VER__) +# define COMPILER_ID "zOS" + /* __IBMCPP__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10) + +#elif defined(__ibmxl__) && defined(__clang__) +# define COMPILER_ID "XLClang" +# define COMPILER_VERSION_MAJOR DEC(__ibmxl_version__) +# define COMPILER_VERSION_MINOR DEC(__ibmxl_release__) +# define COMPILER_VERSION_PATCH DEC(__ibmxl_modification__) +# define COMPILER_VERSION_TWEAK DEC(__ibmxl_ptf_fix_level__) + + +#elif defined(__IBMCPP__) && !defined(__COMPILER_VER__) && __IBMCPP__ >= 800 +# define COMPILER_ID "XL" + /* __IBMCPP__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10) + +#elif defined(__IBMCPP__) && !defined(__COMPILER_VER__) && __IBMCPP__ < 800 +# define COMPILER_ID "VisualAge" + /* __IBMCPP__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10) + +#elif defined(__NVCOMPILER) +# define COMPILER_ID "NVHPC" +# define COMPILER_VERSION_MAJOR DEC(__NVCOMPILER_MAJOR__) +# define COMPILER_VERSION_MINOR DEC(__NVCOMPILER_MINOR__) +# if defined(__NVCOMPILER_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__NVCOMPILER_PATCHLEVEL__) +# endif + +#elif defined(__PGI) +# define COMPILER_ID "PGI" +# define COMPILER_VERSION_MAJOR DEC(__PGIC__) +# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__) +# if defined(__PGIC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__) +# endif + +#elif defined(_CRAYC) +# define COMPILER_ID "Cray" +# define COMPILER_VERSION_MAJOR DEC(_RELEASE_MAJOR) +# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR) + +#elif defined(__TI_COMPILER_VERSION__) +# define COMPILER_ID "TI" + /* __TI_COMPILER_VERSION__ = VVVRRRPPP */ +# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000) +# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000) +# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000) + +#elif defined(__CLANG_FUJITSU) +# define COMPILER_ID "FujitsuClang" +# define COMPILER_VERSION_MAJOR DEC(__FCC_major__) +# define COMPILER_VERSION_MINOR DEC(__FCC_minor__) +# define COMPILER_VERSION_PATCH DEC(__FCC_patchlevel__) +# define COMPILER_VERSION_INTERNAL_STR __clang_version__ + + +#elif defined(__FUJITSU) +# define COMPILER_ID "Fujitsu" +# if defined(__FCC_version__) +# define COMPILER_VERSION __FCC_version__ +# elif defined(__FCC_major__) +# define COMPILER_VERSION_MAJOR DEC(__FCC_major__) +# define COMPILER_VERSION_MINOR DEC(__FCC_minor__) +# define COMPILER_VERSION_PATCH DEC(__FCC_patchlevel__) +# endif +# if defined(__fcc_version) +# define COMPILER_VERSION_INTERNAL DEC(__fcc_version) +# elif defined(__FCC_VERSION) +# define COMPILER_VERSION_INTERNAL DEC(__FCC_VERSION) +# endif + + +#elif defined(__ghs__) +# define COMPILER_ID "GHS" +/* __GHS_VERSION_NUMBER = VVVVRP */ +# ifdef __GHS_VERSION_NUMBER +# define COMPILER_VERSION_MAJOR DEC(__GHS_VERSION_NUMBER / 100) +# define COMPILER_VERSION_MINOR DEC(__GHS_VERSION_NUMBER / 10 % 10) +# define COMPILER_VERSION_PATCH DEC(__GHS_VERSION_NUMBER % 10) +# endif + +#elif defined(__SCO_VERSION__) +# define COMPILER_ID "SCO" + +#elif defined(__ARMCC_VERSION) && !defined(__clang__) +# define COMPILER_ID "ARMCC" +#if __ARMCC_VERSION >= 1000000 + /* __ARMCC_VERSION = VRRPPPP */ + # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/1000000) + # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 100) + # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000) +#else + /* __ARMCC_VERSION = VRPPPP */ + # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/100000) + # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 10) + # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000) +#endif + + +#elif defined(__clang__) && defined(__apple_build_version__) +# define COMPILER_ID "AppleClang" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# define COMPILER_VERSION_MAJOR DEC(__clang_major__) +# define COMPILER_VERSION_MINOR DEC(__clang_minor__) +# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif +# define COMPILER_VERSION_TWEAK DEC(__apple_build_version__) + +#elif defined(__clang__) && defined(__ARMCOMPILER_VERSION) +# define COMPILER_ID "ARMClang" + # define COMPILER_VERSION_MAJOR DEC(__ARMCOMPILER_VERSION/1000000) + # define COMPILER_VERSION_MINOR DEC(__ARMCOMPILER_VERSION/10000 % 100) + # define COMPILER_VERSION_PATCH DEC(__ARMCOMPILER_VERSION % 10000) +# define COMPILER_VERSION_INTERNAL DEC(__ARMCOMPILER_VERSION) + +#elif defined(__clang__) && __has_include() +# define COMPILER_ID "ROCMClang" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# elif defined(__clang__) +# define SIMULATE_ID "Clang" +# elif defined(__GNUC__) +# define SIMULATE_ID "GNU" +# endif +# if defined(__clang__) && __has_include() +# include +# define COMPILER_VERSION_MAJOR DEC(HIP_VERSION_MAJOR) +# define COMPILER_VERSION_MINOR DEC(HIP_VERSION_MINOR) +# define COMPILER_VERSION_PATCH DEC(HIP_VERSION_PATCH) +# endif + +#elif defined(__clang__) +# define COMPILER_ID "Clang" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# define COMPILER_VERSION_MAJOR DEC(__clang_major__) +# define COMPILER_VERSION_MINOR DEC(__clang_minor__) +# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif + +#elif defined(__GNUC__) || defined(__GNUG__) +# define COMPILER_ID "GNU" +# if defined(__GNUC__) +# define COMPILER_VERSION_MAJOR DEC(__GNUC__) +# else +# define COMPILER_VERSION_MAJOR DEC(__GNUG__) +# endif +# if defined(__GNUC_MINOR__) +# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__) +# endif +# if defined(__GNUC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif + +#elif defined(_MSC_VER) +# define COMPILER_ID "MSVC" + /* _MSC_VER = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100) +# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100) +# if defined(_MSC_FULL_VER) +# if _MSC_VER >= 1400 + /* _MSC_FULL_VER = VVRRPPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000) +# else + /* _MSC_FULL_VER = VVRRPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000) +# endif +# endif +# if defined(_MSC_BUILD) +# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD) +# endif + +#elif defined(__VISUALDSPVERSION__) || defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__) +# define COMPILER_ID "ADSP" +#if defined(__VISUALDSPVERSION__) + /* __VISUALDSPVERSION__ = 0xVVRRPP00 */ +# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24) +# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8 & 0xFF) +#endif + +#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC) +# define COMPILER_ID "IAR" +# if defined(__VER__) && defined(__ICCARM__) +# define COMPILER_VERSION_MAJOR DEC((__VER__) / 1000000) +# define COMPILER_VERSION_MINOR DEC(((__VER__) / 1000) % 1000) +# define COMPILER_VERSION_PATCH DEC((__VER__) % 1000) +# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__) +# elif defined(__VER__) && (defined(__ICCAVR__) || defined(__ICCRX__) || defined(__ICCRH850__) || defined(__ICCRL78__) || defined(__ICC430__) || defined(__ICCRISCV__) || defined(__ICCV850__) || defined(__ICC8051__) || defined(__ICCSTM8__)) +# define COMPILER_VERSION_MAJOR DEC((__VER__) / 100) +# define COMPILER_VERSION_MINOR DEC((__VER__) - (((__VER__) / 100)*100)) +# define COMPILER_VERSION_PATCH DEC(__SUBVERSION__) +# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__) +# endif + + +/* These compilers are either not known or too old to define an + identification macro. Try to identify the platform and guess that + it is the native compiler. */ +#elif defined(__hpux) || defined(__hpua) +# define COMPILER_ID "HP" + +#else /* unknown compiler */ +# define COMPILER_ID "" +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]"; +#ifdef SIMULATE_ID +char const* info_simulate = "INFO" ":" "simulate[" SIMULATE_ID "]"; +#endif + +#ifdef __QNXNTO__ +char const* qnxnto = "INFO" ":" "qnxnto[]"; +#endif + +#if defined(__CRAYXT_COMPUTE_LINUX_TARGET) +char const *info_cray = "INFO" ":" "compiler_wrapper[CrayPrgEnv]"; +#endif + +#define STRINGIFY_HELPER(X) #X +#define STRINGIFY(X) STRINGIFY_HELPER(X) + +/* Identify known platforms by name. */ +#if defined(__linux) || defined(__linux__) || defined(linux) +# define PLATFORM_ID "Linux" + +#elif defined(__MSYS__) +# define PLATFORM_ID "MSYS" + +#elif defined(__CYGWIN__) +# define PLATFORM_ID "Cygwin" + +#elif defined(__MINGW32__) +# define PLATFORM_ID "MinGW" + +#elif defined(__APPLE__) +# define PLATFORM_ID "Darwin" + +#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32) +# define PLATFORM_ID "Windows" + +#elif defined(__FreeBSD__) || defined(__FreeBSD) +# define PLATFORM_ID "FreeBSD" + +#elif defined(__NetBSD__) || defined(__NetBSD) +# define PLATFORM_ID "NetBSD" + +#elif defined(__OpenBSD__) || defined(__OPENBSD) +# define PLATFORM_ID "OpenBSD" + +#elif defined(__sun) || defined(sun) +# define PLATFORM_ID "SunOS" + +#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__) +# define PLATFORM_ID "AIX" + +#elif defined(__hpux) || defined(__hpux__) +# define PLATFORM_ID "HP-UX" + +#elif defined(__HAIKU__) +# define PLATFORM_ID "Haiku" + +#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS) +# define PLATFORM_ID "BeOS" + +#elif defined(__QNX__) || defined(__QNXNTO__) +# define PLATFORM_ID "QNX" + +#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__) +# define PLATFORM_ID "Tru64" + +#elif defined(__riscos) || defined(__riscos__) +# define PLATFORM_ID "RISCos" + +#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__) +# define PLATFORM_ID "SINIX" + +#elif defined(__UNIX_SV__) +# define PLATFORM_ID "UNIX_SV" + +#elif defined(__bsdos__) +# define PLATFORM_ID "BSDOS" + +#elif defined(_MPRAS) || defined(MPRAS) +# define PLATFORM_ID "MP-RAS" + +#elif defined(__osf) || defined(__osf__) +# define PLATFORM_ID "OSF1" + +#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv) +# define PLATFORM_ID "SCO_SV" + +#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX) +# define PLATFORM_ID "ULTRIX" + +#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX) +# define PLATFORM_ID "Xenix" + +#elif defined(__WATCOMC__) +# if defined(__LINUX__) +# define PLATFORM_ID "Linux" + +# elif defined(__DOS__) +# define PLATFORM_ID "DOS" + +# elif defined(__OS2__) +# define PLATFORM_ID "OS2" + +# elif defined(__WINDOWS__) +# define PLATFORM_ID "Windows3x" + +# elif defined(__VXWORKS__) +# define PLATFORM_ID "VxWorks" + +# else /* unknown platform */ +# define PLATFORM_ID +# endif + +#elif defined(__INTEGRITY) +# if defined(INT_178B) +# define PLATFORM_ID "Integrity178" + +# else /* regular Integrity */ +# define PLATFORM_ID "Integrity" +# endif + +#else /* unknown platform */ +# define PLATFORM_ID + +#endif + +/* For windows compilers MSVC and Intel we can determine + the architecture of the compiler being used. This is because + the compilers do not have flags that can change the architecture, + but rather depend on which compiler is being used +*/ +#if defined(_WIN32) && defined(_MSC_VER) +# if defined(_M_IA64) +# define ARCHITECTURE_ID "IA64" + +# elif defined(_M_ARM64EC) +# define ARCHITECTURE_ID "ARM64EC" + +# elif defined(_M_X64) || defined(_M_AMD64) +# define ARCHITECTURE_ID "x64" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# elif defined(_M_ARM64) +# define ARCHITECTURE_ID "ARM64" + +# elif defined(_M_ARM) +# if _M_ARM == 4 +# define ARCHITECTURE_ID "ARMV4I" +# elif _M_ARM == 5 +# define ARCHITECTURE_ID "ARMV5I" +# else +# define ARCHITECTURE_ID "ARMV" STRINGIFY(_M_ARM) +# endif + +# elif defined(_M_MIPS) +# define ARCHITECTURE_ID "MIPS" + +# elif defined(_M_SH) +# define ARCHITECTURE_ID "SHx" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__WATCOMC__) +# if defined(_M_I86) +# define ARCHITECTURE_ID "I86" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC) +# if defined(__ICCARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__ICCRX__) +# define ARCHITECTURE_ID "RX" + +# elif defined(__ICCRH850__) +# define ARCHITECTURE_ID "RH850" + +# elif defined(__ICCRL78__) +# define ARCHITECTURE_ID "RL78" + +# elif defined(__ICCRISCV__) +# define ARCHITECTURE_ID "RISCV" + +# elif defined(__ICCAVR__) +# define ARCHITECTURE_ID "AVR" + +# elif defined(__ICC430__) +# define ARCHITECTURE_ID "MSP430" + +# elif defined(__ICCV850__) +# define ARCHITECTURE_ID "V850" + +# elif defined(__ICC8051__) +# define ARCHITECTURE_ID "8051" + +# elif defined(__ICCSTM8__) +# define ARCHITECTURE_ID "STM8" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__ghs__) +# if defined(__PPC64__) +# define ARCHITECTURE_ID "PPC64" + +# elif defined(__ppc__) +# define ARCHITECTURE_ID "PPC" + +# elif defined(__ARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__x86_64__) +# define ARCHITECTURE_ID "x64" + +# elif defined(__i386__) +# define ARCHITECTURE_ID "X86" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__TI_COMPILER_VERSION__) +# if defined(__TI_ARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__MSP430__) +# define ARCHITECTURE_ID "MSP430" + +# elif defined(__TMS320C28XX__) +# define ARCHITECTURE_ID "TMS320C28x" + +# elif defined(__TMS320C6X__) || defined(_TMS320C6X) +# define ARCHITECTURE_ID "TMS320C6x" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#else +# define ARCHITECTURE_ID +#endif + +/* Convert integer to decimal digit literals. */ +#define DEC(n) \ + ('0' + (((n) / 10000000)%10)), \ + ('0' + (((n) / 1000000)%10)), \ + ('0' + (((n) / 100000)%10)), \ + ('0' + (((n) / 10000)%10)), \ + ('0' + (((n) / 1000)%10)), \ + ('0' + (((n) / 100)%10)), \ + ('0' + (((n) / 10)%10)), \ + ('0' + ((n) % 10)) + +/* Convert integer to hex digit literals. */ +#define HEX(n) \ + ('0' + ((n)>>28 & 0xF)), \ + ('0' + ((n)>>24 & 0xF)), \ + ('0' + ((n)>>20 & 0xF)), \ + ('0' + ((n)>>16 & 0xF)), \ + ('0' + ((n)>>12 & 0xF)), \ + ('0' + ((n)>>8 & 0xF)), \ + ('0' + ((n)>>4 & 0xF)), \ + ('0' + ((n) & 0xF)) + +/* Construct a string literal encoding the version number. */ +#ifdef COMPILER_VERSION +char const* info_version = "INFO" ":" "compiler_version[" COMPILER_VERSION "]"; + +/* Construct a string literal encoding the version number components. */ +#elif defined(COMPILER_VERSION_MAJOR) +char const info_version[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[', + COMPILER_VERSION_MAJOR, +# ifdef COMPILER_VERSION_MINOR + '.', COMPILER_VERSION_MINOR, +# ifdef COMPILER_VERSION_PATCH + '.', COMPILER_VERSION_PATCH, +# ifdef COMPILER_VERSION_TWEAK + '.', COMPILER_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct a string literal encoding the internal version number. */ +#ifdef COMPILER_VERSION_INTERNAL +char const info_version_internal[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','_', + 'i','n','t','e','r','n','a','l','[', + COMPILER_VERSION_INTERNAL,']','\0'}; +#elif defined(COMPILER_VERSION_INTERNAL_STR) +char const* info_version_internal = "INFO" ":" "compiler_version_internal[" COMPILER_VERSION_INTERNAL_STR "]"; +#endif + +/* Construct a string literal encoding the version number components. */ +#ifdef SIMULATE_VERSION_MAJOR +char const info_simulate_version[] = { + 'I', 'N', 'F', 'O', ':', + 's','i','m','u','l','a','t','e','_','v','e','r','s','i','o','n','[', + SIMULATE_VERSION_MAJOR, +# ifdef SIMULATE_VERSION_MINOR + '.', SIMULATE_VERSION_MINOR, +# ifdef SIMULATE_VERSION_PATCH + '.', SIMULATE_VERSION_PATCH, +# ifdef SIMULATE_VERSION_TWEAK + '.', SIMULATE_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]"; +char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]"; + + + +#if defined(__INTEL_COMPILER) && defined(_MSVC_LANG) && _MSVC_LANG < 201403L +# if defined(__INTEL_CXX11_MODE__) +# if defined(__cpp_aggregate_nsdmi) +# define CXX_STD 201402L +# else +# define CXX_STD 201103L +# endif +# else +# define CXX_STD 199711L +# endif +#elif defined(_MSC_VER) && defined(_MSVC_LANG) +# define CXX_STD _MSVC_LANG +#else +# define CXX_STD __cplusplus +#endif + +const char* info_language_dialect_default = "INFO" ":" "dialect_default[" +#if CXX_STD > 202002L + "23" +#elif CXX_STD > 201703L + "20" +#elif CXX_STD >= 201703L + "17" +#elif CXX_STD >= 201402L + "14" +#elif CXX_STD >= 201103L + "11" +#else + "98" +#endif +"]"; + +/*--------------------------------------------------------------------------*/ + +int main(int argc, char* argv[]) +{ + int require = 0; + require += info_compiler[argc]; + require += info_platform[argc]; +#ifdef COMPILER_VERSION_MAJOR + require += info_version[argc]; +#endif +#ifdef COMPILER_VERSION_INTERNAL + require += info_version_internal[argc]; +#endif +#ifdef SIMULATE_ID + require += info_simulate[argc]; +#endif +#ifdef SIMULATE_VERSION_MAJOR + require += info_simulate_version[argc]; +#endif +#if defined(__CRAYXT_COMPUTE_LINUX_TARGET) + require += info_cray[argc]; +#endif + require += info_language_dialect_default[argc]; + (void)argv; + return require; +} diff --git a/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CompilerIdCXX/a.out b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CompilerIdCXX/a.out new file mode 100755 index 0000000..c868426 Binary files /dev/null and b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CompilerIdCXX/a.out differ diff --git a/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/CMakeOutput.log b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/CMakeOutput.log new file mode 100644 index 0000000..5f1457e --- /dev/null +++ b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/CMakeOutput.log @@ -0,0 +1,443 @@ +The system is: Linux - 5.13.0-27-generic - x86_64 +Compiling the C compiler identification source file "CMakeCCompilerId.c" succeeded. +Compiler: /usr/bin/cc +Build flags: +Id flags: + +The output was: +0 + + +Compilation of the C compiler identification source "CMakeCCompilerId.c" produced "a.out" + +The C compiler identification is GNU, found in "/home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CompilerIdC/a.out" + +Compiling the CXX compiler identification source file "CMakeCXXCompilerId.cpp" succeeded. +Compiler: /usr/bin/c++ +Build flags: +Id flags: + +The output was: +0 + + +Compilation of the CXX compiler identification source "CMakeCXXCompilerId.cpp" produced "a.out" + +The CXX compiler identification is GNU, found in "/home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/3.21.1/CompilerIdCXX/a.out" + +Detecting C compiler ABI info compiled with the following output: +Change Dir: /home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/CMakeTmp + +Run Build Command(s):/usr/bin/make -f Makefile cmTC_0e213/fast && /usr/bin/make -f CMakeFiles/cmTC_0e213.dir/build.make CMakeFiles/cmTC_0e213.dir/build +make[1]: Entering directory '/home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/CMakeTmp' +Building C object CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o +/usr/bin/cc -v -o CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o -c /home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/share/cmake-3.21/Modules/CMakeCCompilerABI.c +Using built-in specs. +COLLECT_GCC=/usr/bin/cc +OFFLOAD_TARGET_NAMES=nvptx-none:hsa +OFFLOAD_TARGET_DEFAULT=1 +Target: x86_64-linux-gnu +Configured with: ../src/configure -v --with-pkgversion='Ubuntu 9.3.0-17ubuntu1~20.04' --with-bugurl=file:///usr/share/doc/gcc-9/README.Bugs --enable-languages=c,ada,c++,go,brig,d,fortran,objc,obj-c++,gm2 --prefix=/usr --with-gcc-major-version-only --program-suffix=-9 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib=auto --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none=/build/gcc-9-HskZEa/gcc-9-9.3.0/debian/tmp-nvptx/usr,hsa --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu +Thread model: posix +gcc version 9.3.0 (Ubuntu 9.3.0-17ubuntu1~20.04) +COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64' + /usr/lib/gcc/x86_64-linux-gnu/9/cc1 -quiet -v -imultiarch x86_64-linux-gnu /home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/share/cmake-3.21/Modules/CMakeCCompilerABI.c -quiet -dumpbase CMakeCCompilerABI.c -mtune=generic -march=x86-64 -auxbase-strip CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o -version -fasynchronous-unwind-tables -fstack-protector-strong -Wformat -Wformat-security -fstack-clash-protection -fcf-protection -o /tmp/ccZVfRnw.s +GNU C17 (Ubuntu 9.3.0-17ubuntu1~20.04) version 9.3.0 (x86_64-linux-gnu) + compiled by GNU C version 9.3.0, GMP version 6.2.0, MPFR version 4.0.2, MPC version 1.1.0, isl version isl-0.22.1-GMP + +GGC heuristics: --param ggc-min-expand=100 --param ggc-min-heapsize=131072 +ignoring nonexistent directory "/usr/local/include/x86_64-linux-gnu" +ignoring nonexistent directory "/usr/lib/gcc/x86_64-linux-gnu/9/include-fixed" +ignoring nonexistent directory "/usr/lib/gcc/x86_64-linux-gnu/9/../../../../x86_64-linux-gnu/include" +#include "..." search starts here: +#include <...> search starts here: + /usr/lib/gcc/x86_64-linux-gnu/9/include + /usr/local/include + /usr/include/x86_64-linux-gnu + /usr/include +End of search list. +GNU C17 (Ubuntu 9.3.0-17ubuntu1~20.04) version 9.3.0 (x86_64-linux-gnu) + compiled by GNU C version 9.3.0, GMP version 6.2.0, MPFR version 4.0.2, MPC version 1.1.0, isl version isl-0.22.1-GMP + +GGC heuristics: --param ggc-min-expand=100 --param ggc-min-heapsize=131072 +Compiler executable checksum: bbf13931d8de1abe14040c9909cb6969 +COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64' + as -v --64 -o CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o /tmp/ccZVfRnw.s +GNU assembler version 2.34 (x86_64-linux-gnu) using BFD version (GNU Binutils for Ubuntu) 2.34 +COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/ +LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/ +COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64' +Linking C executable cmTC_0e213 +/home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/bin/cmake -E cmake_link_script CMakeFiles/cmTC_0e213.dir/link.txt --verbose=1 +/usr/bin/cc -v -rdynamic CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o -o cmTC_0e213 +Using built-in specs. +COLLECT_GCC=/usr/bin/cc +COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper +OFFLOAD_TARGET_NAMES=nvptx-none:hsa +OFFLOAD_TARGET_DEFAULT=1 +Target: x86_64-linux-gnu +Configured with: ../src/configure -v --with-pkgversion='Ubuntu 9.3.0-17ubuntu1~20.04' --with-bugurl=file:///usr/share/doc/gcc-9/README.Bugs --enable-languages=c,ada,c++,go,brig,d,fortran,objc,obj-c++,gm2 --prefix=/usr --with-gcc-major-version-only --program-suffix=-9 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib=auto --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none=/build/gcc-9-HskZEa/gcc-9-9.3.0/debian/tmp-nvptx/usr,hsa --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu +Thread model: posix +gcc version 9.3.0 (Ubuntu 9.3.0-17ubuntu1~20.04) +COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/ +LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/ +COLLECT_GCC_OPTIONS='-v' '-rdynamic' '-o' 'cmTC_0e213' '-mtune=generic' '-march=x86-64' + /usr/lib/gcc/x86_64-linux-gnu/9/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper -plugin-opt=-fresolution=/tmp/cc4r9rAX.res -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_0e213 /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/9 -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/9/../../.. CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o -lgcc --push-state --as-needed -lgcc_s --pop-state -lc -lgcc --push-state --as-needed -lgcc_s --pop-state /usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o +COLLECT_GCC_OPTIONS='-v' '-rdynamic' '-o' 'cmTC_0e213' '-mtune=generic' '-march=x86-64' +make[1]: Leaving directory '/home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/CMakeTmp' + + + +Parsed C implicit include dir info from above output: rv=done + found start of include info + found start of implicit include info + add: [/usr/lib/gcc/x86_64-linux-gnu/9/include] + add: [/usr/local/include] + add: [/usr/include/x86_64-linux-gnu] + add: [/usr/include] + end of search list found + collapse include dir [/usr/lib/gcc/x86_64-linux-gnu/9/include] ==> [/usr/lib/gcc/x86_64-linux-gnu/9/include] + collapse include dir [/usr/local/include] ==> [/usr/local/include] + collapse include dir [/usr/include/x86_64-linux-gnu] ==> [/usr/include/x86_64-linux-gnu] + collapse include dir [/usr/include] ==> [/usr/include] + implicit include dirs: [/usr/lib/gcc/x86_64-linux-gnu/9/include;/usr/local/include;/usr/include/x86_64-linux-gnu;/usr/include] + + +Parsed C implicit link information from above output: + link line regex: [^( *|.*[/\])(ld|CMAKE_LINK_STARTFILE-NOTFOUND|([^/\]+-)?ld|collect2)[^/\]*( |$)] + ignore line: [Change Dir: /home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/CMakeTmp] + ignore line: [] + ignore line: [Run Build Command(s):/usr/bin/make -f Makefile cmTC_0e213/fast && /usr/bin/make -f CMakeFiles/cmTC_0e213.dir/build.make CMakeFiles/cmTC_0e213.dir/build] + ignore line: [make[1]: Entering directory '/home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/CMakeTmp'] + ignore line: [Building C object CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o] + ignore line: [/usr/bin/cc -v -o CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o -c /home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/share/cmake-3.21/Modules/CMakeCCompilerABI.c] + ignore line: [Using built-in specs.] + ignore line: [COLLECT_GCC=/usr/bin/cc] + ignore line: [OFFLOAD_TARGET_NAMES=nvptx-none:hsa] + ignore line: [OFFLOAD_TARGET_DEFAULT=1] + ignore line: [Target: x86_64-linux-gnu] + ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 9.3.0-17ubuntu1~20.04' --with-bugurl=file:///usr/share/doc/gcc-9/README.Bugs --enable-languages=c ada c++ go brig d fortran objc obj-c++ gm2 --prefix=/usr --with-gcc-major-version-only --program-suffix=-9 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib=auto --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32 m64 mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none=/build/gcc-9-HskZEa/gcc-9-9.3.0/debian/tmp-nvptx/usr hsa --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu] + ignore line: [Thread model: posix] + ignore line: [gcc version 9.3.0 (Ubuntu 9.3.0-17ubuntu1~20.04) ] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64'] + ignore line: [ /usr/lib/gcc/x86_64-linux-gnu/9/cc1 -quiet -v -imultiarch x86_64-linux-gnu /home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/share/cmake-3.21/Modules/CMakeCCompilerABI.c -quiet -dumpbase CMakeCCompilerABI.c -mtune=generic -march=x86-64 -auxbase-strip CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o -version -fasynchronous-unwind-tables -fstack-protector-strong -Wformat -Wformat-security -fstack-clash-protection -fcf-protection -o /tmp/ccZVfRnw.s] + ignore line: [GNU C17 (Ubuntu 9.3.0-17ubuntu1~20.04) version 9.3.0 (x86_64-linux-gnu)] + ignore line: [ compiled by GNU C version 9.3.0 GMP version 6.2.0 MPFR version 4.0.2 MPC version 1.1.0 isl version isl-0.22.1-GMP] + ignore line: [] + ignore line: [GGC heuristics: --param ggc-min-expand=100 --param ggc-min-heapsize=131072] + ignore line: [ignoring nonexistent directory "/usr/local/include/x86_64-linux-gnu"] + ignore line: [ignoring nonexistent directory "/usr/lib/gcc/x86_64-linux-gnu/9/include-fixed"] + ignore line: [ignoring nonexistent directory "/usr/lib/gcc/x86_64-linux-gnu/9/../../../../x86_64-linux-gnu/include"] + ignore line: [#include "..." search starts here:] + ignore line: [#include <...> search starts here:] + ignore line: [ /usr/lib/gcc/x86_64-linux-gnu/9/include] + ignore line: [ /usr/local/include] + ignore line: [ /usr/include/x86_64-linux-gnu] + ignore line: [ /usr/include] + ignore line: [End of search list.] + ignore line: [GNU C17 (Ubuntu 9.3.0-17ubuntu1~20.04) version 9.3.0 (x86_64-linux-gnu)] + ignore line: [ compiled by GNU C version 9.3.0 GMP version 6.2.0 MPFR version 4.0.2 MPC version 1.1.0 isl version isl-0.22.1-GMP] + ignore line: [] + ignore line: [GGC heuristics: --param ggc-min-expand=100 --param ggc-min-heapsize=131072] + ignore line: [Compiler executable checksum: bbf13931d8de1abe14040c9909cb6969] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64'] + ignore line: [ as -v --64 -o CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o /tmp/ccZVfRnw.s] + ignore line: [GNU assembler version 2.34 (x86_64-linux-gnu) using BFD version (GNU Binutils for Ubuntu) 2.34] + ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/] + ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64'] + ignore line: [Linking C executable cmTC_0e213] + ignore line: [/home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/bin/cmake -E cmake_link_script CMakeFiles/cmTC_0e213.dir/link.txt --verbose=1] + ignore line: [/usr/bin/cc -v -rdynamic CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o -o cmTC_0e213 ] + ignore line: [Using built-in specs.] + ignore line: [COLLECT_GCC=/usr/bin/cc] + ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper] + ignore line: [OFFLOAD_TARGET_NAMES=nvptx-none:hsa] + ignore line: [OFFLOAD_TARGET_DEFAULT=1] + ignore line: [Target: x86_64-linux-gnu] + ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 9.3.0-17ubuntu1~20.04' --with-bugurl=file:///usr/share/doc/gcc-9/README.Bugs --enable-languages=c ada c++ go brig d fortran objc obj-c++ gm2 --prefix=/usr --with-gcc-major-version-only --program-suffix=-9 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib=auto --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32 m64 mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none=/build/gcc-9-HskZEa/gcc-9-9.3.0/debian/tmp-nvptx/usr hsa --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu] + ignore line: [Thread model: posix] + ignore line: [gcc version 9.3.0 (Ubuntu 9.3.0-17ubuntu1~20.04) ] + ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/] + ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-rdynamic' '-o' 'cmTC_0e213' '-mtune=generic' '-march=x86-64'] + link line: [ /usr/lib/gcc/x86_64-linux-gnu/9/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper -plugin-opt=-fresolution=/tmp/cc4r9rAX.res -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_0e213 /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/9 -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/9/../../.. CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o -lgcc --push-state --as-needed -lgcc_s --pop-state -lc -lgcc --push-state --as-needed -lgcc_s --pop-state /usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o] + arg [/usr/lib/gcc/x86_64-linux-gnu/9/collect2] ==> ignore + arg [-plugin] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so] ==> ignore + arg [-plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper] ==> ignore + arg [-plugin-opt=-fresolution=/tmp/cc4r9rAX.res] ==> ignore + arg [-plugin-opt=-pass-through=-lgcc] ==> ignore + arg [-plugin-opt=-pass-through=-lgcc_s] ==> ignore + arg [-plugin-opt=-pass-through=-lc] ==> ignore + arg [-plugin-opt=-pass-through=-lgcc] ==> ignore + arg [-plugin-opt=-pass-through=-lgcc_s] ==> ignore + arg [--build-id] ==> ignore + arg [--eh-frame-hdr] ==> ignore + arg [-m] ==> ignore + arg [elf_x86_64] ==> ignore + arg [--hash-style=gnu] ==> ignore + arg [--as-needed] ==> ignore + arg [-export-dynamic] ==> ignore + arg [-dynamic-linker] ==> ignore + arg [/lib64/ld-linux-x86-64.so.2] ==> ignore + arg [-pie] ==> ignore + arg [-znow] ==> ignore + arg [-zrelro] ==> ignore + arg [-o] ==> ignore + arg [cmTC_0e213] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o] ==> obj [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o] + arg [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o] ==> obj [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o] + arg [/usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o] ==> obj [/usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/9] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/9] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib] + arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu] + arg [-L/lib/../lib] ==> dir [/lib/../lib] + arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu] + arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/9/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/9/../../..] + arg [CMakeFiles/cmTC_0e213.dir/CMakeCCompilerABI.c.o] ==> ignore + arg [-lgcc] ==> lib [gcc] + arg [--push-state] ==> ignore + arg [--as-needed] ==> ignore + arg [-lgcc_s] ==> lib [gcc_s] + arg [--pop-state] ==> ignore + arg [-lc] ==> lib [c] + arg [-lgcc] ==> lib [gcc] + arg [--push-state] ==> ignore + arg [--as-needed] ==> ignore + arg [-lgcc_s] ==> lib [gcc_s] + arg [--pop-state] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o] ==> obj [/usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o] + arg [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o] ==> obj [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o] + collapse obj [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o] ==> [/usr/lib/x86_64-linux-gnu/Scrt1.o] + collapse obj [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o] ==> [/usr/lib/x86_64-linux-gnu/crti.o] + collapse obj [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o] ==> [/usr/lib/x86_64-linux-gnu/crtn.o] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/9] ==> [/usr/lib/gcc/x86_64-linux-gnu/9] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib] ==> [/usr/lib] + collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu] + collapse library dir [/lib/../lib] ==> [/lib] + collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] + collapse library dir [/usr/lib/../lib] ==> [/usr/lib] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/9/../../..] ==> [/usr/lib] + implicit libs: [gcc;gcc_s;c;gcc;gcc_s] + implicit objs: [/usr/lib/x86_64-linux-gnu/Scrt1.o;/usr/lib/x86_64-linux-gnu/crti.o;/usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o;/usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o;/usr/lib/x86_64-linux-gnu/crtn.o] + implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/9;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib] + implicit fwks: [] + + +Detecting CXX compiler ABI info compiled with the following output: +Change Dir: /home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/CMakeTmp + +Run Build Command(s):/usr/bin/make -f Makefile cmTC_f5071/fast && /usr/bin/make -f CMakeFiles/cmTC_f5071.dir/build.make CMakeFiles/cmTC_f5071.dir/build +make[1]: Entering directory '/home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/CMakeTmp' +Building CXX object CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o +/usr/bin/c++ -v -o CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o -c /home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/share/cmake-3.21/Modules/CMakeCXXCompilerABI.cpp +Using built-in specs. +COLLECT_GCC=/usr/bin/c++ +OFFLOAD_TARGET_NAMES=nvptx-none:hsa +OFFLOAD_TARGET_DEFAULT=1 +Target: x86_64-linux-gnu +Configured with: ../src/configure -v --with-pkgversion='Ubuntu 9.3.0-17ubuntu1~20.04' --with-bugurl=file:///usr/share/doc/gcc-9/README.Bugs --enable-languages=c,ada,c++,go,brig,d,fortran,objc,obj-c++,gm2 --prefix=/usr --with-gcc-major-version-only --program-suffix=-9 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib=auto --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none=/build/gcc-9-HskZEa/gcc-9-9.3.0/debian/tmp-nvptx/usr,hsa --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu +Thread model: posix +gcc version 9.3.0 (Ubuntu 9.3.0-17ubuntu1~20.04) +COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64' + /usr/lib/gcc/x86_64-linux-gnu/9/cc1plus -quiet -v -imultiarch x86_64-linux-gnu -D_GNU_SOURCE /home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/share/cmake-3.21/Modules/CMakeCXXCompilerABI.cpp -quiet -dumpbase CMakeCXXCompilerABI.cpp -mtune=generic -march=x86-64 -auxbase-strip CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o -version -fasynchronous-unwind-tables -fstack-protector-strong -Wformat -Wformat-security -fstack-clash-protection -fcf-protection -o /tmp/cced4Kit.s +GNU C++14 (Ubuntu 9.3.0-17ubuntu1~20.04) version 9.3.0 (x86_64-linux-gnu) + compiled by GNU C version 9.3.0, GMP version 6.2.0, MPFR version 4.0.2, MPC version 1.1.0, isl version isl-0.22.1-GMP + +GGC heuristics: --param ggc-min-expand=100 --param ggc-min-heapsize=131072 +ignoring duplicate directory "/usr/include/x86_64-linux-gnu/c++/9" +ignoring nonexistent directory "/usr/local/include/x86_64-linux-gnu" +ignoring nonexistent directory "/usr/lib/gcc/x86_64-linux-gnu/9/include-fixed" +ignoring nonexistent directory "/usr/lib/gcc/x86_64-linux-gnu/9/../../../../x86_64-linux-gnu/include" +#include "..." search starts here: +#include <...> search starts here: + /usr/include/c++/9 + /usr/include/x86_64-linux-gnu/c++/9 + /usr/include/c++/9/backward + /usr/lib/gcc/x86_64-linux-gnu/9/include + /usr/local/include + /usr/include/x86_64-linux-gnu + /usr/include +End of search list. +GNU C++14 (Ubuntu 9.3.0-17ubuntu1~20.04) version 9.3.0 (x86_64-linux-gnu) + compiled by GNU C version 9.3.0, GMP version 6.2.0, MPFR version 4.0.2, MPC version 1.1.0, isl version isl-0.22.1-GMP + +GGC heuristics: --param ggc-min-expand=100 --param ggc-min-heapsize=131072 +Compiler executable checksum: 466f818abe2f30ba03783f22bd12d815 +COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64' + as -v --64 -o CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o /tmp/cced4Kit.s +GNU assembler version 2.34 (x86_64-linux-gnu) using BFD version (GNU Binutils for Ubuntu) 2.34 +COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/ +LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/ +COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64' +Linking CXX executable cmTC_f5071 +/home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/bin/cmake -E cmake_link_script CMakeFiles/cmTC_f5071.dir/link.txt --verbose=1 +/usr/bin/c++ -v -rdynamic CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o -o cmTC_f5071 +Using built-in specs. +COLLECT_GCC=/usr/bin/c++ +COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper +OFFLOAD_TARGET_NAMES=nvptx-none:hsa +OFFLOAD_TARGET_DEFAULT=1 +Target: x86_64-linux-gnu +Configured with: ../src/configure -v --with-pkgversion='Ubuntu 9.3.0-17ubuntu1~20.04' --with-bugurl=file:///usr/share/doc/gcc-9/README.Bugs --enable-languages=c,ada,c++,go,brig,d,fortran,objc,obj-c++,gm2 --prefix=/usr --with-gcc-major-version-only --program-suffix=-9 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib=auto --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none=/build/gcc-9-HskZEa/gcc-9-9.3.0/debian/tmp-nvptx/usr,hsa --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu +Thread model: posix +gcc version 9.3.0 (Ubuntu 9.3.0-17ubuntu1~20.04) +COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/ +LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/ +COLLECT_GCC_OPTIONS='-v' '-rdynamic' '-o' 'cmTC_f5071' '-shared-libgcc' '-mtune=generic' '-march=x86-64' + /usr/lib/gcc/x86_64-linux-gnu/9/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper -plugin-opt=-fresolution=/tmp/ccq4uKhX.res -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_f5071 /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/9 -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/9/../../.. CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o +COLLECT_GCC_OPTIONS='-v' '-rdynamic' '-o' 'cmTC_f5071' '-shared-libgcc' '-mtune=generic' '-march=x86-64' +make[1]: Leaving directory '/home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/CMakeTmp' + + + +Parsed CXX implicit include dir info from above output: rv=done + found start of include info + found start of implicit include info + add: [/usr/include/c++/9] + add: [/usr/include/x86_64-linux-gnu/c++/9] + add: [/usr/include/c++/9/backward] + add: [/usr/lib/gcc/x86_64-linux-gnu/9/include] + add: [/usr/local/include] + add: [/usr/include/x86_64-linux-gnu] + add: [/usr/include] + end of search list found + collapse include dir [/usr/include/c++/9] ==> [/usr/include/c++/9] + collapse include dir [/usr/include/x86_64-linux-gnu/c++/9] ==> [/usr/include/x86_64-linux-gnu/c++/9] + collapse include dir [/usr/include/c++/9/backward] ==> [/usr/include/c++/9/backward] + collapse include dir [/usr/lib/gcc/x86_64-linux-gnu/9/include] ==> [/usr/lib/gcc/x86_64-linux-gnu/9/include] + collapse include dir [/usr/local/include] ==> [/usr/local/include] + collapse include dir [/usr/include/x86_64-linux-gnu] ==> [/usr/include/x86_64-linux-gnu] + collapse include dir [/usr/include] ==> [/usr/include] + implicit include dirs: [/usr/include/c++/9;/usr/include/x86_64-linux-gnu/c++/9;/usr/include/c++/9/backward;/usr/lib/gcc/x86_64-linux-gnu/9/include;/usr/local/include;/usr/include/x86_64-linux-gnu;/usr/include] + + +Parsed CXX implicit link information from above output: + link line regex: [^( *|.*[/\])(ld|CMAKE_LINK_STARTFILE-NOTFOUND|([^/\]+-)?ld|collect2)[^/\]*( |$)] + ignore line: [Change Dir: /home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/CMakeTmp] + ignore line: [] + ignore line: [Run Build Command(s):/usr/bin/make -f Makefile cmTC_f5071/fast && /usr/bin/make -f CMakeFiles/cmTC_f5071.dir/build.make CMakeFiles/cmTC_f5071.dir/build] + ignore line: [make[1]: Entering directory '/home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/CMakeTmp'] + ignore line: [Building CXX object CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o] + ignore line: [/usr/bin/c++ -v -o CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o -c /home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/share/cmake-3.21/Modules/CMakeCXXCompilerABI.cpp] + ignore line: [Using built-in specs.] + ignore line: [COLLECT_GCC=/usr/bin/c++] + ignore line: [OFFLOAD_TARGET_NAMES=nvptx-none:hsa] + ignore line: [OFFLOAD_TARGET_DEFAULT=1] + ignore line: [Target: x86_64-linux-gnu] + ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 9.3.0-17ubuntu1~20.04' --with-bugurl=file:///usr/share/doc/gcc-9/README.Bugs --enable-languages=c ada c++ go brig d fortran objc obj-c++ gm2 --prefix=/usr --with-gcc-major-version-only --program-suffix=-9 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib=auto --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32 m64 mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none=/build/gcc-9-HskZEa/gcc-9-9.3.0/debian/tmp-nvptx/usr hsa --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu] + ignore line: [Thread model: posix] + ignore line: [gcc version 9.3.0 (Ubuntu 9.3.0-17ubuntu1~20.04) ] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] + ignore line: [ /usr/lib/gcc/x86_64-linux-gnu/9/cc1plus -quiet -v -imultiarch x86_64-linux-gnu -D_GNU_SOURCE /home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/share/cmake-3.21/Modules/CMakeCXXCompilerABI.cpp -quiet -dumpbase CMakeCXXCompilerABI.cpp -mtune=generic -march=x86-64 -auxbase-strip CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o -version -fasynchronous-unwind-tables -fstack-protector-strong -Wformat -Wformat-security -fstack-clash-protection -fcf-protection -o /tmp/cced4Kit.s] + ignore line: [GNU C++14 (Ubuntu 9.3.0-17ubuntu1~20.04) version 9.3.0 (x86_64-linux-gnu)] + ignore line: [ compiled by GNU C version 9.3.0 GMP version 6.2.0 MPFR version 4.0.2 MPC version 1.1.0 isl version isl-0.22.1-GMP] + ignore line: [] + ignore line: [GGC heuristics: --param ggc-min-expand=100 --param ggc-min-heapsize=131072] + ignore line: [ignoring duplicate directory "/usr/include/x86_64-linux-gnu/c++/9"] + ignore line: [ignoring nonexistent directory "/usr/local/include/x86_64-linux-gnu"] + ignore line: [ignoring nonexistent directory "/usr/lib/gcc/x86_64-linux-gnu/9/include-fixed"] + ignore line: [ignoring nonexistent directory "/usr/lib/gcc/x86_64-linux-gnu/9/../../../../x86_64-linux-gnu/include"] + ignore line: [#include "..." search starts here:] + ignore line: [#include <...> search starts here:] + ignore line: [ /usr/include/c++/9] + ignore line: [ /usr/include/x86_64-linux-gnu/c++/9] + ignore line: [ /usr/include/c++/9/backward] + ignore line: [ /usr/lib/gcc/x86_64-linux-gnu/9/include] + ignore line: [ /usr/local/include] + ignore line: [ /usr/include/x86_64-linux-gnu] + ignore line: [ /usr/include] + ignore line: [End of search list.] + ignore line: [GNU C++14 (Ubuntu 9.3.0-17ubuntu1~20.04) version 9.3.0 (x86_64-linux-gnu)] + ignore line: [ compiled by GNU C version 9.3.0 GMP version 6.2.0 MPFR version 4.0.2 MPC version 1.1.0 isl version isl-0.22.1-GMP] + ignore line: [] + ignore line: [GGC heuristics: --param ggc-min-expand=100 --param ggc-min-heapsize=131072] + ignore line: [Compiler executable checksum: 466f818abe2f30ba03783f22bd12d815] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] + ignore line: [ as -v --64 -o CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o /tmp/cced4Kit.s] + ignore line: [GNU assembler version 2.34 (x86_64-linux-gnu) using BFD version (GNU Binutils for Ubuntu) 2.34] + ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/] + ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] + ignore line: [Linking CXX executable cmTC_f5071] + ignore line: [/home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/bin/cmake -E cmake_link_script CMakeFiles/cmTC_f5071.dir/link.txt --verbose=1] + ignore line: [/usr/bin/c++ -v -rdynamic CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o -o cmTC_f5071 ] + ignore line: [Using built-in specs.] + ignore line: [COLLECT_GCC=/usr/bin/c++] + ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper] + ignore line: [OFFLOAD_TARGET_NAMES=nvptx-none:hsa] + ignore line: [OFFLOAD_TARGET_DEFAULT=1] + ignore line: [Target: x86_64-linux-gnu] + ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 9.3.0-17ubuntu1~20.04' --with-bugurl=file:///usr/share/doc/gcc-9/README.Bugs --enable-languages=c ada c++ go brig d fortran objc obj-c++ gm2 --prefix=/usr --with-gcc-major-version-only --program-suffix=-9 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib=auto --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32 m64 mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none=/build/gcc-9-HskZEa/gcc-9-9.3.0/debian/tmp-nvptx/usr hsa --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu] + ignore line: [Thread model: posix] + ignore line: [gcc version 9.3.0 (Ubuntu 9.3.0-17ubuntu1~20.04) ] + ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/] + ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-rdynamic' '-o' 'cmTC_f5071' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] + link line: [ /usr/lib/gcc/x86_64-linux-gnu/9/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper -plugin-opt=-fresolution=/tmp/ccq4uKhX.res -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_f5071 /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/9 -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/9/../../.. CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o] + arg [/usr/lib/gcc/x86_64-linux-gnu/9/collect2] ==> ignore + arg [-plugin] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so] ==> ignore + arg [-plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper] ==> ignore + arg [-plugin-opt=-fresolution=/tmp/ccq4uKhX.res] ==> ignore + arg [-plugin-opt=-pass-through=-lgcc_s] ==> ignore + arg [-plugin-opt=-pass-through=-lgcc] ==> ignore + arg [-plugin-opt=-pass-through=-lc] ==> ignore + arg [-plugin-opt=-pass-through=-lgcc_s] ==> ignore + arg [-plugin-opt=-pass-through=-lgcc] ==> ignore + arg [--build-id] ==> ignore + arg [--eh-frame-hdr] ==> ignore + arg [-m] ==> ignore + arg [elf_x86_64] ==> ignore + arg [--hash-style=gnu] ==> ignore + arg [--as-needed] ==> ignore + arg [-export-dynamic] ==> ignore + arg [-dynamic-linker] ==> ignore + arg [/lib64/ld-linux-x86-64.so.2] ==> ignore + arg [-pie] ==> ignore + arg [-znow] ==> ignore + arg [-zrelro] ==> ignore + arg [-o] ==> ignore + arg [cmTC_f5071] ==> ignore + arg [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o] ==> obj [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o] + arg [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o] ==> obj [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o] + arg [/usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o] ==> obj [/usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/9] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/9] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib] + arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu] + arg [-L/lib/../lib] ==> dir [/lib/../lib] + arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu] + arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib] + arg [-L/usr/lib/gcc/x86_64-linux-gnu/9/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/9/../../..] + arg [CMakeFiles/cmTC_f5071.dir/CMakeCXXCompilerABI.cpp.o] ==> ignore + arg [-lstdc++] ==> lib [stdc++] + arg [-lm] ==> lib [m] + arg [-lgcc_s] ==> lib [gcc_s] + arg [-lgcc] ==> lib [gcc] + arg [-lc] ==> lib [c] + arg [-lgcc_s] ==> lib [gcc_s] + arg [-lgcc] ==> lib [gcc] + arg [/usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o] ==> obj [/usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o] + arg [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o] ==> obj [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o] + collapse obj [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o] ==> [/usr/lib/x86_64-linux-gnu/Scrt1.o] + collapse obj [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o] ==> [/usr/lib/x86_64-linux-gnu/crti.o] + collapse obj [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o] ==> [/usr/lib/x86_64-linux-gnu/crtn.o] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/9] ==> [/usr/lib/gcc/x86_64-linux-gnu/9] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib] ==> [/usr/lib] + collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu] + collapse library dir [/lib/../lib] ==> [/lib] + collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu] + collapse library dir [/usr/lib/../lib] ==> [/usr/lib] + collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/9/../../..] ==> [/usr/lib] + implicit libs: [stdc++;m;gcc_s;gcc;c;gcc_s;gcc] + implicit objs: [/usr/lib/x86_64-linux-gnu/Scrt1.o;/usr/lib/x86_64-linux-gnu/crti.o;/usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o;/usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o;/usr/lib/x86_64-linux-gnu/crtn.o] + implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/9;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib] + implicit fwks: [] + + diff --git a/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/clion-environment.txt b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/clion-environment.txt new file mode 100644 index 0000000..270d93c --- /dev/null +++ b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/clion-environment.txt @@ -0,0 +1,3 @@ +ToolSet: 1.0 (local)Options: + +Options: \ No newline at end of file diff --git a/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/clion-log.txt b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/clion-log.txt new file mode 100644 index 0000000..7d52086 --- /dev/null +++ b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/clion-log.txt @@ -0,0 +1,40 @@ +/home/taylor/.local/share/JetBrains/Toolbox/apps/CLion/ch-0/213.6777.58/bin/cmake/linux/bin/cmake -DCMAKE_BUILD_TYPE=Debug -G "CodeBlocks - Unix Makefiles" /home/taylor/multidrone_slam/src/slam_gmapping/gmapping +CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required): + Compatibility with CMake < 2.8.12 will be removed from a future version of + CMake. + + Update the VERSION argument value or use a ... suffix to tell + CMake that the project does not need compatibility with older versions. + + +-- The C compiler identification is GNU 9.3.0 +-- The CXX compiler identification is GNU 9.3.0 +-- Detecting C compiler ABI info +-- Detecting C compiler ABI info - done +-- Check for working C compiler: /usr/bin/cc - skipped +-- Detecting C compile features +-- Detecting C compile features - done +-- Detecting CXX compiler ABI info +-- Detecting CXX compiler ABI info - done +-- Check for working CXX compiler: /usr/bin/c++ - skipped +-- Detecting CXX compile features +-- Detecting CXX compile features - done +CMake Error at CMakeLists.txt:4 (find_package): + By not providing "Findcatkin.cmake" in CMAKE_MODULE_PATH this project has + asked CMake to find a package configuration file provided by "catkin", but + CMake did not find one. + + Could not find a package configuration file provided by "catkin" with any + of the following names: + + catkinConfig.cmake + catkin-config.cmake + + Add the installation prefix of "catkin" to CMAKE_PREFIX_PATH or set + "catkin_DIR" to a directory containing one of the above files. If "catkin" + provides a separate development package or SDK, be sure it has been + installed. + + +-- Configuring incomplete, errors occurred! +See also "/home/taylor/multidrone_slam/src/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/CMakeOutput.log". diff --git a/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/cmake.check_cache b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/cmake.check_cache new file mode 100644 index 0000000..3dccd73 --- /dev/null +++ b/slam_gmapping/gmapping/cmake-build-debug/CMakeFiles/cmake.check_cache @@ -0,0 +1 @@ +# This file is generated by cmake for dependency checking of the CMakeCache.txt file diff --git a/slam_gmapping/gmapping/launch/slam_gmapping_pr2.launch b/slam_gmapping/gmapping/launch/slam_gmapping_pr2.launch new file mode 100644 index 0000000..e33e190 --- /dev/null +++ b/slam_gmapping/gmapping/launch/slam_gmapping_pr2.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/slam_gmapping/gmapping/nodelet_plugins.xml b/slam_gmapping/gmapping/nodelet_plugins.xml new file mode 100644 index 0000000..5d7f34d --- /dev/null +++ b/slam_gmapping/gmapping/nodelet_plugins.xml @@ -0,0 +1,7 @@ + + + + Nodelet ROS wrapper for OpenSlam's Gmapping. + + + diff --git a/slam_gmapping/gmapping/package.xml b/slam_gmapping/gmapping/package.xml new file mode 100644 index 0000000..f7ae0cd --- /dev/null +++ b/slam_gmapping/gmapping/package.xml @@ -0,0 +1,34 @@ + + gmapping + 1.4.2 + This package contains a ROS wrapper for OpenSlam's Gmapping. + The gmapping package provides laser-based SLAM (Simultaneous Localization and Mapping), + as a ROS node called slam_gmapping. Using slam_gmapping, you can create a 2-D occupancy + grid map (like a building floorplan) from laser and pose data collected by a mobile robot. + + Brian Gerkey + ROS Orphaned Package Maintainers + BSD + Apache 2.0 + + http://ros.org/wiki/gmapping + + catkin + + nav_msgs + openslam_gmapping + roscpp + rostest + tf + nodelet + + nav_msgs + openslam_gmapping + roscpp + tf + nodelet + + + + + diff --git a/slam_gmapping/gmapping/src/main.cpp b/slam_gmapping/gmapping/src/main.cpp new file mode 100644 index 0000000..0d4ec52 --- /dev/null +++ b/slam_gmapping/gmapping/src/main.cpp @@ -0,0 +1,96 @@ +/* + * slam_gmapping + * Copyright (c) 2008, Willow Garage, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +/* Author: Brian Gerkey */ + +#include + +#include "slam_gmapping.h" +#include "std_msgs/String.h" +bool shouldReset = false; + + +void sysCmdCallback(const std_msgs::String& sys_cmd) + { + ROS_INFO("GOT A SYS COMMAND!!!"); +// ROS_INFO("I heard: [%s]", sys_cmd->data.c_str()); +// printf("%s", sys_cmd.c_str()); + if (sys_cmd.data == "reset") + { + ROS_WARN("RESETTING!!!!!!"); + shouldReset = true; + } + else{ + ROS_WARN("NOT RESETTING"); + } + } + + + void main_loop() { + + ROS_INFO("HERE"); + + ros::NodeHandle n; + ros::Subscriber sub = n.subscribe("/syscommand", 10, sysCmdCallback); + + while (ros::ok()) { +// ros::Duration(3).sleep(); + ROS_INFO("Back at top of mapping loop!!!"); +// std::cout << ("Back at top of mapping loop"); + { +// ros::Duration(3).sleep(); + SlamGMapping gn; + gn.startLiveSlam(); + + while (!shouldReset) { + ros::spinOnce(); + ros::Duration(0.2).sleep(); + } + ROS_INFO("Resetting map..."); + shouldReset = false; + ROS_INFO("After should reset"); + } +// ROS_INFO("Out of scope"); +// ros::Duration(3).sleep(); + } + ROS_INFO("ROS is no longer okay :("); + } + + int + main(int argc, char** argv) + { + ros::init(argc, argv, "slam_gmapping"); + + // ros::NodeHandle n; + // ros::Subscriber sub = n.subscribe("/syscommand", 10, sysCmdCallback); + main_loop(); +// ros::spin(); + + return(0); + } diff --git a/slam_gmapping/gmapping/src/nodelet.cpp b/slam_gmapping/gmapping/src/nodelet.cpp new file mode 100644 index 0000000..8bcebb2 --- /dev/null +++ b/slam_gmapping/gmapping/src/nodelet.cpp @@ -0,0 +1,55 @@ +/* + * slam_gmapping + * Copyright (c) 2008, Willow Garage, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include +#include + +#include "slam_gmapping.h" + +class SlamGMappingNodelet : public nodelet::Nodelet +{ + public: + SlamGMappingNodelet() {} + + ~SlamGMappingNodelet() {} + + virtual void onInit() + { + NODELET_INFO_STREAM("Initialising Slam GMapping nodelet..."); + sg_.reset(new SlamGMapping(getNodeHandle(), getPrivateNodeHandle())); + NODELET_INFO_STREAM("Starting live SLAM..."); + sg_->startLiveSlam(); + } + + private: + boost::shared_ptr sg_; +}; + +PLUGINLIB_EXPORT_CLASS(SlamGMappingNodelet, nodelet::Nodelet) diff --git a/slam_gmapping/gmapping/src/replay.cpp b/slam_gmapping/gmapping/src/replay.cpp new file mode 100644 index 0000000..f2c71b8 --- /dev/null +++ b/slam_gmapping/gmapping/src/replay.cpp @@ -0,0 +1,88 @@ +/* + * Copyright 2015 Aldebaran + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include "slam_gmapping.h" + +#include +#include + +#include +#include + +int +main(int argc, char** argv) +{ + /** Define and parse the program options + */ + namespace po = boost::program_options; + po::options_description desc("Options"); + desc.add_options() + ("help", "Print help messages") + ("scan_topic", po::value()->default_value("/scan") ,"topic that contains the laserScan in the rosbag") + ("bag_filename", po::value()->required(), "ros bag filename") + ("seed", po::value()->default_value(0), "seed") + ("max_duration_buffer", po::value()->default_value(99999), "max tf buffer duration") + ("on_done", po::value(), "command to execute when done") ; + + po::variables_map vm; + try + { + po::store(po::parse_command_line(argc, argv, desc), + vm); // can throw + + /** --help option + */ + if ( vm.count("help") ) + { + std::cout << "Basic Command Line Parameter App" << std::endl + << desc << std::endl; + return 0; + } + + po::notify(vm); // throws on error, so do after help in case + // there are any problems + } + catch(po::error& e) + { + std::cerr << "ERROR: " << e.what() << std::endl << std::endl; + std::cerr << desc << std::endl; + return -1; + } + + std::string bag_fname = vm["bag_filename"].as(); + std::string scan_topic = vm["scan_topic"].as(); + unsigned long int seed = vm["seed"].as(); + unsigned long int max_duration_buffer = vm["max_duration_buffer"].as(); + + ros::init(argc, argv, "slam_gmapping"); + SlamGMapping gn(seed, max_duration_buffer) ; + gn.startReplay(bag_fname, scan_topic); + ROS_INFO("replay stopped."); + + if ( vm.count("on_done") ) + { + // Run the "on_done" command and then exit + system(vm["on_done"].as().c_str()); + } + else + { + ros::spin(); // wait so user can save the map + } + return(0); + + +} + diff --git a/slam_gmapping/gmapping/src/slam_gmapping.cpp b/slam_gmapping/gmapping/src/slam_gmapping.cpp new file mode 100644 index 0000000..146f779 --- /dev/null +++ b/slam_gmapping/gmapping/src/slam_gmapping.cpp @@ -0,0 +1,886 @@ +/* + * slam_gmapping + * Copyright (c) 2008, Willow Garage, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +/* Author: Brian Gerkey */ +/* Modified by: Charles DuHadway */ + + +/** + +@mainpage slam_gmapping + +@htmlinclude manifest.html + +@b slam_gmapping is a wrapper around the GMapping SLAM library. It reads laser +scans and odometry and computes a map. This map can be +written to a file using e.g. + + "rosrun map_server map_saver static_map:=dynamic_map" + +
+ +@section topic ROS topics + +Subscribes to (name/type): +- @b "scan"/sensor_msgs/LaserScan : data from a laser range scanner +- @b "/tf": odometry from the robot + + +Publishes to (name/type): +- @b "/tf"/tf/tfMessage: position relative to the map + + +@section services + - @b "~dynamic_map" : returns the map + + +@section parameters ROS parameters + +Reads the following parameters from the parameter server + +Parameters used by our GMapping wrapper: + +- @b "~throttle_scans": @b [int] throw away every nth laser scan +- @b "~base_frame": @b [string] the tf frame_id to use for the robot base pose +- @b "~map_frame": @b [string] the tf frame_id where the robot pose on the map is published +- @b "~odom_frame": @b [string] the tf frame_id from which odometry is read +- @b "~map_update_interval": @b [double] time in seconds between two recalculations of the map + + +Parameters used by GMapping itself: + +Laser Parameters: +- @b "~/maxRange" @b [double] maximum range of the laser scans. Rays beyond this range get discarded completely. (default: maximum laser range minus 1 cm, as received in the the first LaserScan message) +- @b "~/maxUrange" @b [double] maximum range of the laser scanner that is used for map building (default: same as maxRange) +- @b "~/sigma" @b [double] standard deviation for the scan matching process (cell) +- @b "~/kernelSize" @b [int] search window for the scan matching process +- @b "~/lstep" @b [double] initial search step for scan matching (linear) +- @b "~/astep" @b [double] initial search step for scan matching (angular) +- @b "~/iterations" @b [int] number of refinement steps in the scan matching. The final "precision" for the match is lstep*2^(-iterations) or astep*2^(-iterations), respectively. +- @b "~/lsigma" @b [double] standard deviation for the scan matching process (single laser beam) +- @b "~/ogain" @b [double] gain for smoothing the likelihood +- @b "~/lskip" @b [int] take only every (n+1)th laser ray for computing a match (0 = take all rays) +- @b "~/minimumScore" @b [double] minimum score for considering the outcome of the scanmatching good. Can avoid 'jumping' pose estimates in large open spaces when using laser scanners with limited range (e.g. 5m). (0 = default. Scores go up to 600+, try 50 for example when experiencing 'jumping' estimate issues) + +Motion Model Parameters (all standard deviations of a gaussian noise model) +- @b "~/srr" @b [double] linear noise component (x and y) +- @b "~/stt" @b [double] angular noise component (theta) +- @b "~/srt" @b [double] linear -> angular noise component +- @b "~/str" @b [double] angular -> linear noise component + +Others: +- @b "~/linearUpdate" @b [double] the robot only processes new measurements if the robot has moved at least this many meters +- @b "~/angularUpdate" @b [double] the robot only processes new measurements if the robot has turned at least this many rads + +- @b "~/resampleThreshold" @b [double] threshold at which the particles get resampled. Higher means more frequent resampling. +- @b "~/particles" @b [int] (fixed) number of particles. Each particle represents a possible trajectory that the robot has traveled + +Likelihood sampling (used in scan matching) +- @b "~/llsamplerange" @b [double] linear range +- @b "~/lasamplerange" @b [double] linear step size +- @b "~/llsamplestep" @b [double] linear range +- @b "~/lasamplestep" @b [double] angular step size + +Initial map dimensions and resolution: +- @b "~/xmin" @b [double] minimum x position in the map [m] +- @b "~/ymin" @b [double] minimum y position in the map [m] +- @b "~/xmax" @b [double] maximum x position in the map [m] +- @b "~/ymax" @b [double] maximum y position in the map [m] +- @b "~/delta" @b [double] size of one pixel [m] + +*/ + + + +#include "slam_gmapping.h" + +#include + +#include + +#include "ros/ros.h" +#include "ros/console.h" +#include "nav_msgs/MapMetaData.h" + +#include "gmapping/sensor/sensor_range/rangesensor.h" +#include "gmapping/sensor/sensor_odometry/odometrysensor.h" + +#include +#include +#include +#define foreach BOOST_FOREACH + +// compute linear index for given map coords +#define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) + +SlamGMapping::SlamGMapping(): + map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))), + laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL) +{ + seed_ = time(NULL); + init(); +} + +SlamGMapping::SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh): + map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))), + laser_count_(0),node_(nh), private_nh_(pnh), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL) +{ + seed_ = time(NULL); + init(); +} + +SlamGMapping::SlamGMapping(long unsigned int seed, long unsigned int max_duration_buffer): + map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))), + laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL), + seed_(seed), tf_(ros::Duration(max_duration_buffer)) +{ + init(); +} + + +void SlamGMapping::init() +{ + // log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME)->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]); + + // The library is pretty chatty + //gsp_ = new GMapping::GridSlamProcessor(std::cerr); + gsp_ = new GMapping::GridSlamProcessor(); + ROS_ASSERT(gsp_); + + tfB_ = new tf::TransformBroadcaster(); + ROS_ASSERT(tfB_); + + gsp_laser_ = NULL; + gsp_odom_ = NULL; + + got_first_scan_ = false; + got_map_ = false; + + + + // Parameters used by our GMapping wrapper + if(!private_nh_.getParam("throttle_scans", throttle_scans_)) + throttle_scans_ = 1; + if(!private_nh_.getParam("base_frame", base_frame_)) + base_frame_ = "base_link"; + if(!private_nh_.getParam("map_frame", map_frame_)) + map_frame_ = "map"; + if(!private_nh_.getParam("odom_frame", odom_frame_)) + odom_frame_ = "odom"; + + private_nh_.param("transform_publish_period", transform_publish_period_, 0.05); + + double tmp; + if(!private_nh_.getParam("map_update_interval", tmp)) + tmp = 5.0; + map_update_interval_.fromSec(tmp); + + // Parameters used by GMapping itself + maxUrange_ = 0.0; maxRange_ = 0.0; // preliminary default, will be set in initMapper() + if(!private_nh_.getParam("minimumScore", minimum_score_)) + minimum_score_ = 0; + if(!private_nh_.getParam("sigma", sigma_)) + sigma_ = 0.05; + if(!private_nh_.getParam("kernelSize", kernelSize_)) + kernelSize_ = 1; + if(!private_nh_.getParam("lstep", lstep_)) + lstep_ = 0.05; + if(!private_nh_.getParam("astep", astep_)) + astep_ = 0.05; + if(!private_nh_.getParam("iterations", iterations_)) + iterations_ = 5; + if(!private_nh_.getParam("lsigma", lsigma_)) + lsigma_ = 0.075; + if(!private_nh_.getParam("ogain", ogain_)) + ogain_ = 3.0; + if(!private_nh_.getParam("lskip", lskip_)) + lskip_ = 0; + if(!private_nh_.getParam("srr", srr_)) + srr_ = 0.1; + if(!private_nh_.getParam("srt", srt_)) + srt_ = 0.2; + if(!private_nh_.getParam("str", str_)) + str_ = 0.1; + if(!private_nh_.getParam("stt", stt_)) + stt_ = 0.2; + if(!private_nh_.getParam("linearUpdate", linearUpdate_)) + linearUpdate_ = 1.0; + if(!private_nh_.getParam("angularUpdate", angularUpdate_)) + angularUpdate_ = 0.5; + if(!private_nh_.getParam("temporalUpdate", temporalUpdate_)) + temporalUpdate_ = -1.0; + if(!private_nh_.getParam("resampleThreshold", resampleThreshold_)) + resampleThreshold_ = 0.5; + if(!private_nh_.getParam("particles", particles_)) + particles_ = 30; + if(!private_nh_.getParam("xmin", xmin_)) + xmin_ = -100.0; + if(!private_nh_.getParam("ymin", ymin_)) + ymin_ = -100.0; + if(!private_nh_.getParam("xmax", xmax_)) + xmax_ = 100.0; + if(!private_nh_.getParam("ymax", ymax_)) + ymax_ = 100.0; + if(!private_nh_.getParam("delta", delta_)) + delta_ = 0.05; + if(!private_nh_.getParam("occ_thresh", occ_thresh_)) + occ_thresh_ = 0.25; + if(!private_nh_.getParam("llsamplerange", llsamplerange_)) + llsamplerange_ = 0.01; + if(!private_nh_.getParam("llsamplestep", llsamplestep_)) + llsamplestep_ = 0.01; + if(!private_nh_.getParam("lasamplerange", lasamplerange_)) + lasamplerange_ = 0.005; + if(!private_nh_.getParam("lasamplestep", lasamplestep_)) + lasamplestep_ = 0.005; + + if(!private_nh_.getParam("tf_delay", tf_delay_)) + tf_delay_ = transform_publish_period_; + +} + + +void SlamGMapping::startLiveSlam() +{ + entropy_publisher_ = private_nh_.advertise("entropy", 1, true); + sst_ = node_.advertise("map", 1, true); + sstm_ = node_.advertise("map_metadata", 1, true); + ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this); + scan_filter_sub_ = new message_filters::Subscriber(node_, "scan", 5); + scan_filter_ = new tf::MessageFilter(*scan_filter_sub_, tf_, odom_frame_, 5); + scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1)); + +// transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_)); +} + +void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_topic) +{ + double transform_publish_period; + ros::NodeHandle private_nh_("~"); + entropy_publisher_ = private_nh_.advertise("entropy", 1, true); + sst_ = node_.advertise("map", 1, true); + sstm_ = node_.advertise("map_metadata", 1, true); + ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this); + + rosbag::Bag bag; + bag.open(bag_fname, rosbag::bagmode::Read); + + std::vector topics; + topics.push_back(std::string("/tf")); + topics.push_back(scan_topic); + rosbag::View viewall(bag, rosbag::TopicQuery(topics)); + + // Store up to 5 messages and there error message (if they cannot be processed right away) + std::queue > s_queue; + foreach(rosbag::MessageInstance const m, viewall) + { + tf::tfMessage::ConstPtr cur_tf = m.instantiate(); + if (cur_tf != NULL) { + for (size_t i = 0; i < cur_tf->transforms.size(); ++i) + { + geometry_msgs::TransformStamped transformStamped; + tf::StampedTransform stampedTf; + transformStamped = cur_tf->transforms[i]; + tf::transformStampedMsgToTF(transformStamped, stampedTf); + tf_.setTransform(stampedTf); + } + } + + sensor_msgs::LaserScan::ConstPtr s = m.instantiate(); + if (s != NULL) { + if (!(ros::Time(s->header.stamp)).is_zero()) + { + s_queue.push(std::make_pair(s, "")); + } + // Just like in live processing, only process the latest 5 scans + if (s_queue.size() > 5) { + ROS_WARN_STREAM("Dropping old scan: " << s_queue.front().second); + s_queue.pop(); + } + // ignoring un-timestamped tf data + } + + // Only process a scan if it has tf data + while (!s_queue.empty()) + { + try + { + tf::StampedTransform t; + tf_.lookupTransform(s_queue.front().first->header.frame_id, odom_frame_, s_queue.front().first->header.stamp, t); + this->laserCallback(s_queue.front().first); + s_queue.pop(); + } + // If tf does not have the data yet + catch(tf2::TransformException& e) + { + // Store the error to display it if we cannot process the data after some time + s_queue.front().second = std::string(e.what()); + break; + } + } + } + + bag.close(); +} + +void SlamGMapping::publishLoop(double transform_publish_period){ + if(transform_publish_period == 0) + return; + + ros::Rate r(1.0 / transform_publish_period); + while(ros::ok()){ + publishTransform(); + r.sleep(); + } +} + +SlamGMapping::~SlamGMapping() +{ + if(transform_thread_){ + transform_thread_->join(); + delete transform_thread_; + } + + delete gsp_; + if(gsp_laser_) + delete gsp_laser_; + if(gsp_odom_) + delete gsp_odom_; + if (scan_filter_) + delete scan_filter_; + if (scan_filter_sub_) + delete scan_filter_sub_; +} + +bool +SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t) +{ + // Get the pose of the centered laser at the right time + centered_laser_pose_.stamp_ = t; + // Get the laser's pose that is centered + tf::Stamped odom_pose; + try + { + tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose); + } + catch(tf::TransformException e) + { + ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what()); + return false; + } + double yaw = tf::getYaw(odom_pose.getRotation()); + + gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(), + odom_pose.getOrigin().y(), + yaw); + return true; +} + +bool +SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan) +{ + laser_frame_ = scan.header.frame_id; + // Get the laser's pose, relative to base. + tf::Stamped ident; + tf::Stamped laser_pose; + ident.setIdentity(); + ident.frame_id_ = laser_frame_; + ident.stamp_ = scan.header.stamp; + try + { + tf_.transformPose(base_frame_, ident, laser_pose); + } + catch(tf::TransformException e) + { + ROS_WARN("Failed to compute laser pose, aborting initialization (%s)", + e.what()); + return false; + } + + // create a point 1m above the laser position and transform it into the laser-frame + tf::Vector3 v; + v.setValue(0, 0, 1 + laser_pose.getOrigin().z()); + tf::Stamped up(v, scan.header.stamp, + base_frame_); + try + { + tf_.transformPoint(laser_frame_, up, up); + ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z()); + } + catch(tf::TransformException& e) + { + ROS_WARN("Unable to determine orientation of laser: %s", + e.what()); + return false; + } + + // gmapping doesnt take roll or pitch into account. So check for correct sensor alignment. + if (fabs(fabs(up.z()) - 1) > 0.001) + { + ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f", + up.z()); + return false; + } + + gsp_laser_beam_count_ = scan.ranges.size(); + + double angle_center = (scan.angle_min + scan.angle_max)/2; + + if (up.z() > 0) + { + do_reverse_range_ = scan.angle_min > scan.angle_max; + centered_laser_pose_ = tf::Stamped(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center), + tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_); + ROS_INFO("Laser is mounted upwards."); + } + else + { + do_reverse_range_ = scan.angle_min < scan.angle_max; + centered_laser_pose_ = tf::Stamped(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center), + tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_); + ROS_INFO("Laser is mounted upside down."); + } + + // Compute the angles of the laser from -x to x, basically symmetric and in increasing order + laser_angles_.resize(scan.ranges.size()); + // Make sure angles are started so that they are centered + double theta = - std::fabs(scan.angle_min - scan.angle_max)/2; + for(unsigned int i=0; igetName(), gsp_laser_)); + gsp_->setSensorMap(smap); + + gsp_odom_ = new GMapping::OdometrySensor(odom_frame_); + ROS_ASSERT(gsp_odom_); + + + /// @todo Expose setting an initial pose + GMapping::OrientedPoint initialPose; + if(!getOdomPose(initialPose, scan.header.stamp)) + { + ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero."); + initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0); + } + + gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_, + kernelSize_, lstep_, astep_, iterations_, + lsigma_, ogain_, lskip_); + + gsp_->setMotionModelParameters(srr_, srt_, str_, stt_); + gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_); + gsp_->setUpdatePeriod(temporalUpdate_); + gsp_->setgenerateMap(false); + gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_, + delta_, initialPose); + gsp_->setllsamplerange(llsamplerange_); + gsp_->setllsamplestep(llsamplestep_); + /// @todo Check these calls; in the gmapping gui, they use + /// llsamplestep and llsamplerange intead of lasamplestep and + /// lasamplerange. It was probably a typo, but who knows. + gsp_->setlasamplerange(lasamplerange_); + gsp_->setlasamplestep(lasamplestep_); + gsp_->setminimumScore(minimum_score_); + + // Call the sampling function once to set the seed. + GMapping::sampleGaussian(1,seed_); + + ROS_INFO("Initialization complete"); + + return true; +} + +bool +SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose) +{ + if(!getOdomPose(gmap_pose, scan.header.stamp)) + return false; + + if(scan.ranges.size() != gsp_laser_beam_count_) + return false; + + // GMapping wants an array of doubles... + double* ranges_double = new double[scan.ranges.size()]; + // If the angle increment is negative, we have to invert the order of the readings. + if (do_reverse_range_) + { + ROS_DEBUG("Inverting scan"); + int num_ranges = scan.ranges.size(); + for(int i=0; i < num_ranges; i++) + { + // Must filter out short readings, because the mapper won't + if(scan.ranges[num_ranges - i - 1] < scan.range_min) + ranges_double[i] = (double)scan.range_max; + else + ranges_double[i] = (double)scan.ranges[num_ranges - i - 1]; + } + } else + { + for(unsigned int i=0; i < scan.ranges.size(); i++) + { + // Must filter out short readings, because the mapper won't + if(scan.ranges[i] < scan.range_min) + ranges_double[i] = (double)scan.range_max; + else + ranges_double[i] = (double)scan.ranges[i]; + } + } + + GMapping::RangeReading reading(scan.ranges.size(), + ranges_double, + gsp_laser_, + scan.header.stamp.toSec()); + + // ...but it deep copies them in RangeReading constructor, so we don't + // need to keep our array around. + delete[] ranges_double; + + reading.setPose(gmap_pose); + + /* + ROS_DEBUG("scanpose (%.3f): %.3f %.3f %.3f\n", + scan.header.stamp.toSec(), + gmap_pose.x, + gmap_pose.y, + gmap_pose.theta); + */ + ROS_DEBUG("processing scan"); + + return gsp_->processScan(reading); +} + +void +SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) +{ + laser_count_++; + if ((laser_count_ % throttle_scans_) != 0) + return; + + static ros::Time last_map_update(0,0); + + // We can't initialize the mapper until we've got the first scan + if(!got_first_scan_) + { + if(!initMapper(*scan)) + return; + got_first_scan_ = true; + } + + GMapping::OrientedPoint odom_pose; + + if(addScan(*scan, odom_pose)) + { + ROS_DEBUG("scan processed"); + + GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose; + ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta); + ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta); + ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta); + + tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse(); + tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0)); + + map_to_odom_mutex_.lock(); + map_to_odom_ = (odom_to_laser * laser_to_map).inverse(); + map_to_odom_mutex_.unlock(); + + if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_) + { + updateMap(*scan); + last_map_update = scan->header.stamp; + ROS_DEBUG("Updated the map"); + } + } else + ROS_DEBUG("cannot process scan"); +} + +double +SlamGMapping::computePoseEntropy() +{ + double weight_total=0.0; + for(std::vector::const_iterator it = gsp_->getParticles().begin(); + it != gsp_->getParticles().end(); + ++it) + { + weight_total += it->weight; + } + double entropy = 0.0; + for(std::vector::const_iterator it = gsp_->getParticles().begin(); + it != gsp_->getParticles().end(); + ++it) + { + if(it->weight/weight_total > 0.0) + entropy += it->weight/weight_total * log(it->weight/weight_total); + } + return -entropy; +} + +void +SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan) +{ + ROS_DEBUG("Update map"); + boost::mutex::scoped_lock map_lock (map_mutex_); + GMapping::ScanMatcher matcher; + + matcher.setLaserParameters(scan.ranges.size(), &(laser_angles_[0]), + gsp_laser_->getPose()); + + matcher.setlaserMaxRange(maxRange_); + matcher.setusableRange(maxUrange_); + matcher.setgenerateMap(true); + + GMapping::GridSlamProcessor::Particle best = + gsp_->getParticles()[gsp_->getBestParticleIndex()]; + std_msgs::Float64 entropy; + entropy.data = computePoseEntropy(); + if(entropy.data > 0.0) + entropy_publisher_.publish(entropy); + + if(!got_map_) { + map_.map.info.resolution = delta_; + map_.map.info.origin.position.x = 0.0; + map_.map.info.origin.position.y = 0.0; + map_.map.info.origin.position.z = 0.0; + map_.map.info.origin.orientation.x = 0.0; + map_.map.info.origin.orientation.y = 0.0; + map_.map.info.origin.orientation.z = 0.0; + map_.map.info.origin.orientation.w = 1.0; + } + + GMapping::Point center; + center.x=(xmin_ + xmax_) / 2.0; + center.y=(ymin_ + ymax_) / 2.0; + + GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_, + delta_); + + ROS_DEBUG("Trajectory tree:"); + for(GMapping::GridSlamProcessor::TNode* n = best.node; + n; + n = n->parent) + { + ROS_DEBUG(" %.3f %.3f %.3f", + n->pose.x, + n->pose.y, + n->pose.theta); + if(!n->reading) + { + ROS_DEBUG("Reading is NULL"); + continue; + } + matcher.invalidateActiveArea(); + matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0])); + matcher.registerScan(smap, n->pose, &((*n->reading)[0])); + } + + // the map may have expanded, so resize ros message as well + if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY()) { + + // NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor + // so we must obtain the bounding box in a different way + GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0)); + GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY())); + xmin_ = wmin.x; ymin_ = wmin.y; + xmax_ = wmax.x; ymax_ = wmax.y; + + ROS_DEBUG("map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(), + xmin_, ymin_, xmax_, ymax_); + + map_.map.info.width = smap.getMapSizeX(); + map_.map.info.height = smap.getMapSizeY(); + map_.map.info.origin.position.x = xmin_; + map_.map.info.origin.position.y = ymin_; + map_.map.data.resize(map_.map.info.width * map_.map.info.height); + + ROS_DEBUG("map origin: (%f, %f)", map_.map.info.origin.position.x, map_.map.info.origin.position.y); + } + + for(int x=0; x < smap.getMapSizeX(); x++) + { + for(int y=0; y < smap.getMapSizeY(); y++) + { + /// @todo Sort out the unknown vs. free vs. obstacle thresholding + GMapping::IntPoint p(x, y); + double occ=smap.cell(p); + assert(occ <= 1.0); + if(occ < 0) + map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1; + else if(occ > occ_thresh_) + { + //map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0); + map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100; + } + else + map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0; + } + } + removeRobotsObstacles(); + got_map_ = true; + + //make sure to set the header information on the map + map_.map.header.stamp = ros::Time::now(); + map_.map.header.frame_id = tf_.resolve( map_frame_ ); + + sst_.publish(map_.map); + sstm_.publish(map_.map.info); +} + +void SlamGMapping::removeRobotsObstacles() +{ + for(int robot_i=0; robot_i0) + { + if(isLocationFarEnough(point, robots_past_locations[robot_number].back())) + { + robots_past_locations[robot_number].push_back(point); + } + } + else + { + robots_past_locations[robot_number].push_back(point); + } + +} + +bool SlamGMapping::isLocationFarEnough(geometry_msgs::Point point_1, geometry_msgs::Point point_2) +{ + float x = point_1.x - point_2.x; + float y = point_1.y - point_2.y; + + float dist = std::hypot(x,y); + return dist>robot_location_movement_threshold; +} + +void SlamGMapping::setRobotPixelFree() +{ + for(int robot_i = 0; robot_imap_.map.info.width || oby_p+j>map_.map.info.height) + continue; + map_.map.data[MAP_IDX(map_.map.info.width, obx_p+i, oby_p+j)] = 1; + } + } +} + +bool +SlamGMapping::mapCallback(nav_msgs::GetMap::Request &req, + nav_msgs::GetMap::Response &res) +{ + boost::mutex::scoped_lock map_lock (map_mutex_); + if(got_map_ && map_.map.info.width && map_.map.info.height) + { + res = map_; + return true; + } + else + return false; +} + +void SlamGMapping::publishTransform() +{ + map_to_odom_mutex_.lock(); + ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_); + tfB_->sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_)); + map_to_odom_mutex_.unlock(); +} diff --git a/slam_gmapping/gmapping/src/slam_gmapping.h b/slam_gmapping/gmapping/src/slam_gmapping.h new file mode 100644 index 0000000..3c3d332 --- /dev/null +++ b/slam_gmapping/gmapping/src/slam_gmapping.h @@ -0,0 +1,163 @@ +/* + * slam_gmapping + * Copyright (c) 2008, Willow Garage, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +/* Author: Brian Gerkey */ + +#include "ros/ros.h" +#include "sensor_msgs/LaserScan.h" +#include "std_msgs/Float64.h" +#include "nav_msgs/GetMap.h" +#include "tf/transform_listener.h" +#include "tf/transform_broadcaster.h" +#include "message_filters/subscriber.h" +#include "tf/message_filter.h" + +#include "gmapping/gridfastslam/gridslamprocessor.h" +#include "gmapping/sensor/sensor_base/sensor.h" +#include + +#include + +#define NUMBER_OF_ROBOTS 4 + +class SlamGMapping +{ + public: + SlamGMapping(); + SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh); + SlamGMapping(unsigned long int seed, unsigned long int max_duration_buffer); + ~SlamGMapping(); + + void init(); + void startLiveSlam(); + void startReplay(const std::string & bag_fname, std::string scan_topic); + void publishTransform(); + + void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan); + bool mapCallback(nav_msgs::GetMap::Request &req, + nav_msgs::GetMap::Response &res); + void publishLoop(double transform_publish_period); + + private: + ros::NodeHandle node_; + ros::Publisher entropy_publisher_; + ros::Publisher sst_; + ros::Publisher sstm_; + ros::ServiceServer ss_; + tf::TransformListener tf_; + message_filters::Subscriber* scan_filter_sub_; + tf::MessageFilter* scan_filter_; + tf::TransformBroadcaster* tfB_; + + GMapping::GridSlamProcessor* gsp_; + GMapping::RangeSensor* gsp_laser_; + // The angles in the laser, going from -x to x (adjustment is made to get the laser between + // symmetrical bounds as that's what gmapping expects) + std::vector laser_angles_; + // The pose, in the original laser frame, of the corresponding centered laser with z facing up + tf::Stamped centered_laser_pose_; + // Depending on the order of the elements in the scan and the orientation of the scan frame, + // We might need to change the order of the scan + bool do_reverse_range_; + unsigned int gsp_laser_beam_count_; + GMapping::OdometrySensor* gsp_odom_; + + bool got_first_scan_; + + bool got_map_; + nav_msgs::GetMap::Response map_; + + ros::Duration map_update_interval_; + tf::Transform map_to_odom_; + boost::mutex map_to_odom_mutex_; + boost::mutex map_mutex_; + + int laser_count_; + int throttle_scans_; + + boost::thread* transform_thread_; + + std::string base_frame_; + std::string laser_frame_; + std::string map_frame_; + std::string odom_frame_; + + void updateMap(const sensor_msgs::LaserScan& scan); + bool getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t); + bool initMapper(const sensor_msgs::LaserScan& scan); + bool addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose); + double computePoseEntropy(); + void removeRobotsObstacles(); + void setRobotPixelFree(); + void addRobotLocationToHistory(const tf::StampedTransform& t, int robot_number); + bool isLocationFarEnough(geometry_msgs::Point point_1, geometry_msgs::Point point_2); + + std::vector robots_past_locations[NUMBER_OF_ROBOTS]; + float robot_location_movement_threshold = 0.3; + // int robot_ignore_box_dimention_ = 6; + // Parameters used by GMapping + double maxRange_; + double maxUrange_; + double maxrange_; + double minimum_score_; + double sigma_; + int kernelSize_; + double lstep_; + double astep_; + int iterations_; + double lsigma_; + double ogain_; + int lskip_; + double srr_; + double srt_; + double str_; + double stt_; + double linearUpdate_; + double angularUpdate_; + double temporalUpdate_; + double resampleThreshold_; + int particles_; + double xmin_; + double ymin_; + double xmax_; + double ymax_; + double delta_; + double occ_thresh_; + double llsamplerange_; + double llsamplestep_; + double lasamplerange_; + double lasamplestep_; + + ros::NodeHandle private_nh_; + + unsigned long int seed_; + + double transform_publish_period_; + double tf_delay_; +}; diff --git a/slam_gmapping/gmapping/test/basic_localization_laser_different_beamcount.test b/slam_gmapping/gmapping/test/basic_localization_laser_different_beamcount.test new file mode 100644 index 0000000..b9174e1 --- /dev/null +++ b/slam_gmapping/gmapping/test/basic_localization_laser_different_beamcount.test @@ -0,0 +1,11 @@ + + + + + + + + + diff --git a/slam_gmapping/gmapping/test/basic_localization_stage.launch b/slam_gmapping/gmapping/test/basic_localization_stage.launch new file mode 100644 index 0000000..db77863 --- /dev/null +++ b/slam_gmapping/gmapping/test/basic_localization_stage.launch @@ -0,0 +1,11 @@ + + + + + + + + + diff --git a/slam_gmapping/gmapping/test/basic_localization_stage_replay.launch b/slam_gmapping/gmapping/test/basic_localization_stage_replay.launch new file mode 100644 index 0000000..8396380 --- /dev/null +++ b/slam_gmapping/gmapping/test/basic_localization_stage_replay.launch @@ -0,0 +1,8 @@ + + + + + + diff --git a/slam_gmapping/gmapping/test/basic_localization_stage_replay2.launch b/slam_gmapping/gmapping/test/basic_localization_stage_replay2.launch new file mode 100644 index 0000000..661fbf7 --- /dev/null +++ b/slam_gmapping/gmapping/test/basic_localization_stage_replay2.launch @@ -0,0 +1,5 @@ + + + + diff --git a/slam_gmapping/gmapping/test/basic_localization_symmetry.launch b/slam_gmapping/gmapping/test/basic_localization_symmetry.launch new file mode 100644 index 0000000..4b30017 --- /dev/null +++ b/slam_gmapping/gmapping/test/basic_localization_symmetry.launch @@ -0,0 +1,5 @@ + + + + diff --git a/slam_gmapping/gmapping/test/basic_localization_upside_down.launch b/slam_gmapping/gmapping/test/basic_localization_upside_down.launch new file mode 100644 index 0000000..052468f --- /dev/null +++ b/slam_gmapping/gmapping/test/basic_localization_upside_down.launch @@ -0,0 +1,5 @@ + + + + diff --git a/slam_gmapping/gmapping/test/rtest.cpp b/slam_gmapping/gmapping/test/rtest.cpp new file mode 100644 index 0000000..86ee000 --- /dev/null +++ b/slam_gmapping/gmapping/test/rtest.cpp @@ -0,0 +1,180 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* Author: Brian Gerkey */ + +#include +#include +#include +#include +#include +#include + + +ros::NodeHandle* g_n=NULL; +double g_res, g_width, g_height, g_min_free_ratio, g_max_free_ratio; + +class MapClientTest : public testing::Test +{ + public: + MapClientTest() + { + got_map_ = false; + got_map_metadata_ = false; + } + + ~MapClientTest() + { + } + + bool got_map_; + boost::shared_ptr map_; + void mapCallback(const boost::shared_ptr& map) + { + map_ = map; + got_map_ = true; + } + + bool got_map_metadata_; + boost::shared_ptr map_metadata_; + void mapMetaDataCallback(const boost::shared_ptr& map_metadata) + { + map_metadata_ = map_metadata; + got_map_metadata_ = true; + } + + void checkMapMetaData(const nav_msgs::MapMetaData& map_metadata) + { + EXPECT_FLOAT_EQ(map_metadata.resolution, g_res); + EXPECT_FLOAT_EQ(map_metadata.width, g_width); + EXPECT_FLOAT_EQ(map_metadata.height, g_height); + } + + void checkMapData(const nav_msgs::OccupancyGrid& map) + { + unsigned int i; + unsigned int free_cnt = 0; + for(i=0; i < map.info.width * map.info.height; i++) + { + if(map.data[i] == 0) + free_cnt++; + } + double free_ratio = free_cnt / (double)(i); + ROS_INFO("Min / ratio / Max free ratio: %f / %f / %f", g_min_free_ratio, free_ratio, g_max_free_ratio); + EXPECT_GE(free_ratio, g_min_free_ratio); + EXPECT_LE(free_ratio, g_max_free_ratio); + } +}; + +/* Try to retrieve the map via service, and compare to ground truth */ +TEST_F(MapClientTest, call_service) +{ + nav_msgs::GetMap::Request req; + nav_msgs::GetMap::Response resp; + ASSERT_TRUE(ros::service::waitForService("dynamic_map", 5000)); + ASSERT_TRUE(ros::service::call("dynamic_map", req, resp)); + checkMapMetaData(resp.map.info); + checkMapData(resp.map); +} + +/* Try to retrieve the map via topic, and compare to ground truth */ +TEST_F(MapClientTest, subscribe_topic) +{ + ros::Subscriber sub = g_n->subscribe("map", 1, boost::bind(&MapClientTest::mapCallback, this, _1)); + + // Try a few times, because the server may not be up yet. + int i=20; + while(!got_map_ && i > 0) + { + ros::spinOnce(); + ros::WallDuration d(0.25); + d.sleep(); + i--; + } + ASSERT_TRUE(got_map_); + checkMapMetaData(map_->info); + checkMapData(*(map_.get())); +} + +/* Try to retrieve the metadata via topic, and compare to ground truth */ +TEST_F(MapClientTest, subscribe_topic_metadata) +{ + ros::Subscriber sub = g_n->subscribe("map_metadata", 1, boost::bind(&MapClientTest::mapMetaDataCallback, this, _1)); + + // Try a few times, because the server may not be up yet. + int i=20; + while(!got_map_metadata_ && i > 0) + { + ros::spinOnce(); + ros::WallDuration d(0.25); + d.sleep(); + i--; + } + ASSERT_TRUE(got_map_metadata_); + checkMapMetaData(*(map_metadata_.get())); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + + ros::init(argc, argv, "map_client_test"); + g_n = new ros::NodeHandle(); + + // Required args are, in order: + // + ROS_ASSERT(argc == 7); + ros::Duration delay = ros::Duration(atof(argv[1])); + g_res = atof(argv[2]); + g_width = atof(argv[3]); + g_height = atof(argv[4]); + g_min_free_ratio = atof(argv[5]); + g_max_free_ratio = atof(argv[6]); + + while(ros::Time::now().toSec() == 0.0) + { + ROS_INFO("Waiting for initial time publication"); + ros::Duration(0.25).sleep(); + ros::spinOnce(); + } + ros::Time start_time = ros::Time::now(); + while((ros::Time::now() - start_time) < delay) + { + ROS_INFO("Waiting for delay expiration (%.3f - %.3f) < %.3f", + ros::Time::now().toSec(), + start_time.toSec(), + delay.toSec()); + ros::Duration(0.25).sleep(); + ros::spinOnce(); + } + + int result = RUN_ALL_TESTS(); + delete g_n; + return result; +} diff --git a/slam_gmapping/gmapping/test/test_map.py b/slam_gmapping/gmapping/test/test_map.py new file mode 100755 index 0000000..7bc1dd4 --- /dev/null +++ b/slam_gmapping/gmapping/test/test_map.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# + + +import PIL.Image +import unittest +import subprocess +import sys + +import roslib +import os +roslib.load_manifest('gmapping') +import rostest + +class TestGmapping(unittest.TestCase): + + # Test that 2 map files are approximately the same + def cmp_maps(self, f0, f1): + im0 = PIL.Image.open(f0+'.pgm') + im1 = PIL.Image.open(f1+'.pgm') + + size = 100,100 + im0.thumbnail(size,PIL.Image.ANTIALIAS) + im1.thumbnail(size,PIL.Image.ANTIALIAS) + + # get raw data for comparison + im0d = im0.getdata() + im1d = im1.getdata() + + # assert len(i0)==len(i1) + self.assertTrue(len(im0d) == len(im1d)) + + #compare pixel by pixel for thumbnails + error_count = 0 + error_total = 0 + pixel_tol = 0 + total_error_tol = 0.1 + for i in range(len(im0d)): + (p0) = im0d[i] + (p1) = im1d[i] + if abs(p0-p1) > pixel_tol: + error_count = error_count + 1 + error_total = error_total + abs(p0-p1) + error_avg = float(error_total)/float(len(im0d)) + print '%d / %d = %.6f (%.6f)'%(error_total,len(im0d),error_avg,total_error_tol) + self.assertTrue(error_avg <= total_error_tol) + + def test_basic_localization_stage(self): + if sys.argv > 1: + target_time = float(sys.argv[1]) + + import time + import rospy + rospy.init_node('test', anonymous=True) + while(rospy.rostime.get_time() == 0.0): + print 'Waiting for initial time publication' + time.sleep(0.1) + start_time = rospy.rostime.get_time() + + while (rospy.rostime.get_time() - start_time) < target_time: + print 'Waiting for end time %.6f (current: %.6f)'%(target_time,(rospy.rostime.get_time() - start_time)) + time.sleep(0.1) + + f0 = os.path.join(roslib.packages.get_pkg_dir('gmapping'),'test','basic_localization_stage_groundtruth') + f1 = os.path.join(roslib.packages.get_pkg_dir('gmapping'),'test','basic_localization_stage_generated') + + cmd = ['rosrun', 'map_server', 'map_saver', 'map:=dynamic_map', '-f', f1] + self.assertTrue(subprocess.call(cmd) == 0) + + self.cmp_maps(f0,f1) + +if __name__ == '__main__': + rostest.run('gmapping', 'gmapping_slam', TestGmapping, sys.argv) diff --git a/slam_gmapping/slam_gmapping/CHANGELOG.rst b/slam_gmapping/slam_gmapping/CHANGELOG.rst new file mode 100644 index 0000000..4c4345f --- /dev/null +++ b/slam_gmapping/slam_gmapping/CHANGELOG.rst @@ -0,0 +1,50 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package slam_gmapping +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.4.2 (2020-10-02) +------------------ + +1.4.1 (2020-03-16) +------------------ + +1.4.0 (2019-07-12) +------------------ +* update license to BSD and maintainer to ros-orphaned-packages@googlegroups.com + since original gmapping source and ROS openslam_gmapping package has been updated to the BSD-3 license, I think we have no reason to use CC for slam_gmapping package +* Contributors: Kei Okada + +1.3.10 (2018-01-23) +------------------- + +1.3.9 (2017-10-22) +------------------ + +1.3.8 (2015-07-31) +------------------ + +1.3.7 (2015-07-04) +------------------ + +1.3.6 (2015-06-26) +------------------ + +1.3.5 (2014-08-28) +------------------ + +1.3.4 (2014-08-07) +------------------ + +1.3.3 (2014-06-23) +------------------ + +1.3.2 (2014-01-14) +------------------ + +1.3.1 (2014-01-13) +------------------ + +1.3.0 (2013-06-28) +------------------ +* Renamed to gmapping, adding metapackage for slam_gmapping +* Contributors: Mike Ferguson diff --git a/slam_gmapping/slam_gmapping/CMakeLists.txt b/slam_gmapping/slam_gmapping/CMakeLists.txt new file mode 100644 index 0000000..3a82b45 --- /dev/null +++ b/slam_gmapping/slam_gmapping/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(slam_gmapping) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/slam_gmapping/slam_gmapping/package.xml b/slam_gmapping/slam_gmapping/package.xml new file mode 100644 index 0000000..b88ca14 --- /dev/null +++ b/slam_gmapping/slam_gmapping/package.xml @@ -0,0 +1,20 @@ + + slam_gmapping + 1.4.2 + slam_gmapping contains a wrapper around gmapping which provides SLAM capabilities. + Brian Gerkey + ROS Orphaned Package Maintainers + BSD + Apache 2.0 + + http://ros.org/wiki/slam_gmapping + + catkin + + openslam_gmapping + gmapping + + + + + diff --git a/turtlebot3_simulations/.github/workflows/ros-ci.yml b/turtlebot3_simulations/.github/workflows/ros-ci.yml new file mode 100644 index 0000000..333333b --- /dev/null +++ b/turtlebot3_simulations/.github/workflows/ros-ci.yml @@ -0,0 +1,52 @@ +name: noetic-devel + +# Controls when the action will run. Triggers the workflow on push or pull request +on: + push: + branches: [ master, develop, noetic-devel ] + pull_request: + branches: [ master, develop, noetic-devel ] + +# A workflow run is made up of one or more jobs that can run sequentially or in parallel +jobs: + master-ci: + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + ros_distribution: + # - kinetic + # - melodic + - noetic + include: + # Kinetic Kame (May 2016 - May 2021) + # - docker_image: ubuntu:xenial + # ros_distribution: kinetic + # ros_version: 1 + # Melodic Morenia (May 2018 - May 2023) + # - docker_image: ubuntu:bionic + # ros_distribution: melodic + # ros_version: 1 + # Noetic Ninjemys (May 2020 - May 2025) + - docker_image: ubuntu:focal + ros_distribution: noetic + ros_version: 1 + container: + image: ${{ matrix.docker_image }} + steps: + - name: Setup directories + run: mkdir -p ros_ws/src + - name: checkout + uses: actions/checkout@v2 + with: + path: ros_ws/src + - name: Setup ROS environment + uses: ros-tooling/setup-ros@0.2.1 + with: + required-ros-distributions: ${{ matrix.ros_distribution }} + - name: Build and Test + uses: ros-tooling/action-ros-ci@v0.2 + with: + package-name: turtlebot3_simulations + target-ros1-distro: ${{ matrix.ros_distribution }} + vcs-repo-file-url: "" diff --git a/turtlebot3_simulations/turtlebot3_gazebo/launch/empty_world.launch b/turtlebot3_simulations/turtlebot3_gazebo/launch/empty_world.launch index 698a496..f7c5c0f 100644 --- a/turtlebot3_simulations/turtlebot3_gazebo/launch/empty_world.launch +++ b/turtlebot3_simulations/turtlebot3_gazebo/launch/empty_world.launch @@ -2,7 +2,7 @@ - + diff --git a/turtlebot3_simulations/turtlebot3_gazebo/launch/multi_turtlebot3_all.launch b/turtlebot3_simulations/turtlebot3_gazebo/launch/multi_turtlebot3_all.launch index 763f253..39046fb 100644 --- a/turtlebot3_simulations/turtlebot3_gazebo/launch/multi_turtlebot3_all.launch +++ b/turtlebot3_simulations/turtlebot3_gazebo/launch/multi_turtlebot3_all.launch @@ -10,8 +10,8 @@ - - + + @@ -21,8 +21,8 @@ - - + + @@ -30,21 +30,27 @@ - + + + + - + + + + @@ -54,10 +60,14 @@ - + + + + + diff --git a/turtlebot3_simulations/turtlebot3_gazebo/launch/multi_turtlebot3_single.launch b/turtlebot3_simulations/turtlebot3_gazebo/launch/multi_turtlebot3_single.launch index 2e8c9c3..e6937db 100644 --- a/turtlebot3_simulations/turtlebot3_gazebo/launch/multi_turtlebot3_single.launch +++ b/turtlebot3_simulations/turtlebot3_gazebo/launch/multi_turtlebot3_single.launch @@ -24,7 +24,7 @@ - + @@ -35,6 +35,9 @@ + + + @@ -42,7 +45,7 @@ - +