├── .gitignore ├── LICENSE ├── README.md ├── data └── maps │ └── lee_j_floor │ ├── lee_j_map.pgm │ └── lee_j_map.yaml ├── data_capture ├── CMakeLists.txt ├── README.md ├── data │ └── .gitignore ├── package.xml └── src │ ├── data_capture.py │ ├── data_capture_node.py │ └── stamp_cmd_vel_node.py ├── deep_learning_model ├── .gitignore ├── LICENSE ├── Makefile ├── README.md ├── data │ ├── external │ │ └── .gitkeep │ ├── interim │ │ └── .gitkeep │ ├── mixers │ │ ├── .gitkeep │ │ ├── data.mix │ │ └── evaluation.mix │ ├── processed │ │ └── .gitkeep │ └── raw │ │ └── .gitkeep ├── docs │ ├── Makefile │ ├── commands.rst │ ├── conf.py │ ├── getting-started.rst │ ├── index.rst │ └── make.bat ├── models │ ├── .gitkeep │ └── default │ │ └── .gitkeep ├── notebooks │ └── .gitkeep ├── references │ └── .gitkeep ├── reports │ ├── .gitkeep │ └── figures │ │ └── .gitkeep ├── requirements.txt ├── src │ ├── __init__.py │ ├── data │ │ ├── .gitkeep │ │ ├── __init__.py │ │ ├── analyze_trajectories.ipynb │ │ ├── custom_data_runner.py │ │ ├── data_handler.py │ │ ├── fast_data_handler.py │ │ ├── make_dataset.py │ │ └── stack_hdf5.py │ ├── features │ │ ├── .gitkeep │ │ └── build_features.py │ ├── model │ │ ├── .gitkeep │ │ ├── __init__.py │ │ ├── _init_paths.py │ │ ├── conv_model.py │ │ ├── model.py │ │ ├── model_to_graph.py │ │ ├── predict_model.py │ │ ├── train_model.py │ │ └── training_wrapper.py │ └── visualization │ │ ├── .gitkeep │ │ └── visualize.py └── tox.ini ├── deep_motion_planner ├── CMakeLists.txt ├── README.md ├── launch │ ├── deep_motion_planner.launch │ └── deep_motion_planner_robot.launch ├── models │ └── model.pb ├── package.xml ├── python │ └── deep_motion_planner │ │ ├── __init__.py │ │ ├── deep_motion_planner.py │ │ ├── tensorflow_wrapper.py │ │ └── util │ │ └── __init__.py ├── setup.py └── src │ └── deep_motion_planner_node.py ├── mission_control ├── CMakeLists.txt ├── README.md ├── launch │ ├── demo_mission.launch │ ├── generate_mission.launch │ ├── lab_mission.launch │ ├── office_mission.launch │ ├── rooms_mission.launch │ ├── shapes_mission.launch │ └── test_mission.launch ├── missions │ ├── demo.txt │ ├── generate.txt │ ├── lab.txt │ ├── office.txt │ ├── rooms.txt │ ├── shapes.txt │ └── test.txt ├── package.xml └── src │ ├── mission_control.py │ ├── mission_control_node.py │ ├── mission_file_parser.py │ ├── write_mission.py │ └── write_mission_node.py ├── planner_comparison ├── CMakeLists.txt ├── launch │ ├── core_stage.launch │ ├── planner_comparison.launch │ └── world_comparison.launch ├── package.xml ├── python │ └── planner_comparison │ │ ├── PlannerComparison.py │ │ ├── RosbagInterface.py │ │ ├── TimeMsgContainer.py │ │ ├── __init__.py │ │ ├── plan_scoring.py │ │ └── util │ │ └── __init__.py ├── setup.py └── src │ ├── compare_deep_ros.py │ ├── compare_maps_for_model.py │ ├── compare_models.py │ ├── planner_comparison_node.py │ ├── plot_missions.py │ ├── plot_missions_real_data.py │ ├── plot_single_mission_real_data.py │ └── score_trajectories.py ├── safety_module ├── CMakeLists.txt ├── include │ └── safety_module │ │ ├── SafetyModule.hpp │ │ └── SafetyModuleWrapper.hpp ├── package.xml └── src │ ├── SafetyModule.cpp │ ├── SafetyModuleWrapper.cpp │ └── node.cpp ├── stage_worlds ├── CMakeLists.txt ├── README.md ├── cfg │ ├── base_local_planner_params.yaml │ ├── blobs.yaml │ ├── borderless.yaml │ ├── boxes.yaml │ ├── costmap_common_params.yaml │ ├── empty.yaml │ ├── global_costmap_params.yaml │ ├── gui_config.rviz │ ├── lee_j_map.yaml │ ├── local_costmap_params.yaml │ ├── office.yaml │ ├── rooms.yaml │ ├── shapes.yaml │ ├── simple.yaml │ ├── simple_real.yaml │ └── turtlebot_planner_params │ │ ├── dwa_local_planner_params.yaml │ │ ├── global_costmap_params.yaml │ │ ├── global_planner_params.yaml │ │ ├── move_base_params.yaml │ │ └── navfn_global_planner_params.yaml ├── launch │ ├── blobs.launch │ ├── borderless.launch │ ├── boxes.launch │ ├── core.launch │ ├── demo.launch │ ├── empty.launch │ ├── lab.launch │ ├── office.launch │ ├── rooms.launch │ ├── rviz.launch │ ├── shapes.launch │ ├── simple.launch │ └── simple_real.launch ├── package.xml └── worlds │ ├── bitmaps │ ├── blobs.png │ ├── borderless.png │ ├── boxes.png │ ├── empty.png │ ├── lee_j_map.pgm │ ├── office.png │ ├── rooms.png │ ├── shapes.png │ ├── simple.png │ ├── simple_real.png │ ├── simple_real.xcf │ └── worlds.xcf │ ├── blobs.world │ ├── borderless.world │ ├── boxes.world │ ├── empty.world │ ├── europa │ ├── camera.inc │ ├── europa.inc │ ├── hokuyo.inc │ └── sick.inc │ ├── lab.world │ ├── map.inc │ ├── office.world │ ├── rooms.world │ ├── shapes.world │ ├── simple.world │ ├── simple_real.world │ ├── turtlebot │ ├── camera.inc │ ├── hokuyo.inc │ └── turtlebot.inc │ └── window.inc ├── tools ├── learning_sync.sh ├── pull.sh ├── rsync_exclude └── sync.sh ├── turtlebot_controller ├── CMakeLists.txt ├── launch │ └── turtlebot_controller.launch ├── package.xml └── src │ ├── turtlebot_controller.py │ └── turtlebot_controller_node.py └── visualization ├── CMakeLists.txt ├── package.xml └── src ├── laser_visualization_2d.py └── laser_visualization_2d_node.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *~ 3 | *project -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, ETHZ ASL 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # deep_motion_planning 2 | Using Deep Neural Networks for robot navigation purposes: 3 | 4 | The repository contains various ROS packages that are used to generate the training data. 5 | 6 | ## Usage 7 | 8 | ### With Move Base 9 | In order to start the simulation, the navigation and load the static map, run: 10 | ``` 11 | roslaunch stage_worlds shapes.launch deep_motion_planning:=False 12 | ``` 13 | 14 | You can then start a mission by executing: 15 | ``` 16 | rosrun mission_control mission_control_node.py _mission_file:=src/deep_motion_planning/mission_control/missions/rooms.txt _deep_motion_planner:=False 17 | ``` 18 | The parameter <_mission_file> defines the path to the mission definition that you want to execute. 19 | The example above assumes that you are in the top folder of your catkin workspace. 20 | 21 | Finally, if you want to see a visualization of the system, run: 22 | ``` 23 | roslaunch stage_worlds rviz.launch 24 | ``` 25 | 26 | ### With Deep Motion Planner 27 | To start the simulation in combination with the deep motion planner, run: 28 | ``` 29 | roslaunch stage_worlds shapes.launch deep_motion_planning:=True 30 | ``` 31 | 32 | You can then start a mission by executing: 33 | ``` 34 | rosrun mission_control mission_control_node.py _mission_file:=src/deep_motion_planning/mission_control/missions/rooms.txt _deep_motion_planner:=True 35 | ``` 36 | The parameter <_mission_file> defines the path to the mission definition that you want to execute. 37 | The example above assumes that you are in the top folder of your catkin workspace. 38 | 39 | ## Packages 40 | ### stage_worlds 41 | The stage_worlds package contains the configuration files for the simulation, various world 42 | definitions and their ROS launch files. 43 | 44 | ### mission_control 45 | This package contains a mission control node which executes a user defined mission. Therefor, 46 | a txt file is parsed and a sequence of commands is generated. This sequence is then processed 47 | step-by-step. For more details on the definition of a mission, please refer to the README file 48 | in the package directory. 49 | 50 | ### data_capture 51 | The data_capture node subscribes to a set of topics and writes the time-synchronized messages 52 | into a .csv file. Currently, the node records data from a laser scanner, the relative target 53 | pose for the navigation and the control commands that are send to the robot. 54 | 55 | ### deep_motion_planner 56 | The package wraps a Tensorflow model for motion planning with a deep neural network. The node loads 57 | a pretrained model and computes the control commands for the robot from raw sensor data. 58 | 59 | ### deep_learning_model 60 | This is not a ROS package, but a independent project to train a Tensorflow model on data that is 61 | captured with the *data_capture* package. The resulting trained model can then be loaded and executed 62 | in the *deep_motion_planner* package 63 | -------------------------------------------------------------------------------- /data/maps/lee_j_floor/lee_j_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/data/maps/lee_j_floor/lee_j_map.pgm -------------------------------------------------------------------------------- /data/maps/lee_j_floor/lee_j_map.yaml: -------------------------------------------------------------------------------- 1 | image: /tmp/lee_j_map.pgm 2 | resolution: 0.020000 3 | origin: [-50.480000, -30.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /data_capture/README.md: -------------------------------------------------------------------------------- 1 | # data_capture 2 | The package recordes a set of ROS topics and writes the synchronized messages into a .csv file. 3 | 4 | ## Usage 5 | ``` 6 | rosrun data_capture data_capture_node.py 7 | ``` 8 | After receiving a message on the _start_ topic, the synchronized messages are captured into a 9 | buffer. Currently, the node subscribes to a laser scan, a target pose and the cmd_vel topics. 10 | It is important, that those message all contain a header as we need the timestamp of the messages 11 | for synchronization. 12 | 13 | The _stop_ topic ends the recording and writes the buffered data into a target_#.csv file where # 14 | is a running sequence number. The _abort_ topic will clear the cache without writing the captured 15 | data into a file. 16 | 17 | ## Parameters 18 | ### storage_path (default: /data/) 19 | The location where the written .csv files are put. 20 | -------------------------------------------------------------------------------- /data_capture/data/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | !.gitignore 3 | -------------------------------------------------------------------------------- /data_capture/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | data_capture 4 | 0.0.0 5 | The package writes synchronized ROS msgs into a csv file 6 | 7 | 8 | 9 | 10 | michael 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | rospy 44 | rospy 45 | message_filters 46 | message_filters 47 | std_msgs 48 | std_msgs 49 | sensor_msgs 50 | sensor_msgs 51 | geometry_msgs 52 | geometry_msgs 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /data_capture/src/data_capture.py: -------------------------------------------------------------------------------- 1 | 2 | import rospy 3 | import message_filters 4 | from sensor_msgs.msg import LaserScan 5 | from std_msgs.msg import Empty 6 | from geometry_msgs.msg import TwistStamped, PoseStamped 7 | 8 | import tf 9 | 10 | import csv 11 | import os 12 | from datetime import datetime 13 | 14 | class DataCapture(): 15 | """Class that captures various ROS messages and saves them into a .csv file""" 16 | def __init__(self, storage_path): 17 | 18 | # Prepare the generation of the storage folder 19 | date_str = datetime.strftime(datetime.now(), '%Y-%m-%d_%H-%M-%S') 20 | self.storage_path = os.path.join(storage_path, date_str) 21 | 22 | rospy.loginfo(self.storage_path) 23 | 24 | self.enable_capture = False 25 | self.data_buffer = list() 26 | self.target_count = 1 #sequential number for the file name 27 | self.first_file = True 28 | 29 | # ROS topics 30 | rospy.Subscriber('/start', Empty, self.start_callback) 31 | rospy.Subscriber('/stop', Empty, self.stop_callback) 32 | rospy.Subscriber('/abort', Empty, self.abort_callback) 33 | 34 | # Synchronized messages 35 | scan_sub = message_filters.Subscriber('scan', LaserScan) 36 | cmd_sub = message_filters.Subscriber('cmd_vel', TwistStamped) 37 | target_sub = message_filters.Subscriber('relative_target', PoseStamped) 38 | 39 | self.synchonizer = message_filters.TimeSynchronizer([scan_sub, cmd_sub, target_sub], 10) 40 | self.synchonizer.registerCallback(self.sync_callback) 41 | 42 | def start_callback(self, data): 43 | """ 44 | Enable the data capture 45 | """ 46 | if not self.enable_capture: 47 | rospy.loginfo('Start data capture') 48 | self.enable_capture = True 49 | 50 | def stop_callback(self, data): 51 | """ 52 | Disable data capture and write the cached messages into a file 53 | """ 54 | if self.enable_capture: 55 | rospy.loginfo('Stop data capture') 56 | self.enable_capture = False 57 | 58 | self.__write_data_to_file__() 59 | 60 | def abort_callback(self, data): 61 | """ 62 | Disable data capture and clear cache without writing it into a file 63 | """ 64 | if self.enable_capture: 65 | rospy.loginfo('Abort and clear buffered data') 66 | self.data_buffer = list() 67 | self.enable_capture = False 68 | 69 | def sync_callback(self, scan, cmd, target): 70 | """ 71 | Callback for the synchonizer that caches the synchonized messages 72 | """ 73 | if self.enable_capture: 74 | # concatenate the data and add it to the buffer 75 | orientation = [target.pose.orientation.x, target.pose.orientation.y, 76 | target.pose.orientation.z, target.pose.orientation.w] 77 | yaw = tf.transformations.euler_from_quaternion(orientation)[2] 78 | new_row = [cmd.header.stamp.to_nsec(), cmd.twist.linear.x, cmd.twist.angular.z] + \ 79 | list(scan.ranges) + [target.pose.position.x, target.pose.position.y, yaw] 80 | self.data_buffer.append(new_row) 81 | 82 | def __write_data_to_file__(self): 83 | """ 84 | Write the cached data into a .csv file 85 | """ 86 | rospy.loginfo('Write data to file: {} items'.format(len(self.data_buffer))) 87 | 88 | # Prevent creation of empty files 89 | if len(self.data_buffer) == 0: 90 | rospy.loginfo('Received no messages: No csv file is written') 91 | return 92 | 93 | # Create the storage folder when writing the first file 94 | if self.first_file: 95 | os.mkdir(self.storage_path) 96 | self.first_file = False 97 | 98 | # Create the first line of the csv file with column names 99 | # We define the length of the laser by the length of captured data, minus the fields 100 | # that are not related to the laser (stamp, commands and target position) 101 | column_line = ['stamp','linear_x','angular_z'] + \ 102 | ['laser_' + str(i) for i in range(len(self.data_buffer[0]) - 6)] + ['target_x', 103 | 'target_y', 'target_yaw'] 104 | 105 | # write the data into a csv file and reset the buffer 106 | with open(os.path.join(self.storage_path,('target_' + str(self.target_count) + '.csv')), \ 107 | 'wb') as output_file: 108 | 109 | writer = csv.writer(output_file, delimiter=',') 110 | writer.writerow(column_line) 111 | for l in self.data_buffer: 112 | writer.writerow(l) 113 | 114 | self.data_buffer = list() 115 | self.target_count += 1 116 | 117 | -------------------------------------------------------------------------------- /data_capture/src/data_capture_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | from roslib.packages import get_pkg_dir 5 | 6 | from data_capture import DataCapture 7 | 8 | # Get the root folder of this package and append data 9 | default_storage_path = get_pkg_dir('data_capture') + '/data/' 10 | 11 | def main(): 12 | 13 | rospy.init_node('data_capture_node') 14 | 15 | storage_path = rospy.get_param('~storage_path',default_storage_path) 16 | DataCapture(storage_path) 17 | 18 | rospy.spin() 19 | 20 | if __name__ == "__main__": 21 | main() 22 | -------------------------------------------------------------------------------- /data_capture/src/stamp_cmd_vel_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | from geometry_msgs.msg import Twist, TwistStamped 5 | 6 | class StampTwistMsgs(): 7 | """ 8 | Class to transform a Twist message into a TwistStamped message 9 | """ 10 | def __init__(self): 11 | self.cmd_vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.callback) 12 | self.cmd_vel_stamped_pub = rospy.Publisher('/cmd_vel_stamped', TwistStamped, queue_size=1) 13 | 14 | 15 | def callback(self, data): 16 | msg = TwistStamped() 17 | msg.twist = data 18 | msg.header.stamp = rospy.Time.now() 19 | 20 | self.cmd_vel_stamped_pub.publish(msg) 21 | 22 | def main(): 23 | 24 | rospy.init_node('stamp_cmd_vel_node') 25 | 26 | StampTwistMsgs() 27 | 28 | rospy.spin() 29 | 30 | if __name__ == "__main__": 31 | main() 32 | -------------------------------------------------------------------------------- /deep_learning_model/.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | 5 | # C extensions 6 | *.so 7 | 8 | # Distribution / packaging 9 | .Python 10 | env/ 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | *.egg-info/ 23 | .installed.cfg 24 | *.egg 25 | 26 | # PyInstaller 27 | # Usually these files are written by a python script from a template 28 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 29 | *.manifest 30 | *.spec 31 | 32 | # Installer logs 33 | pip-log.txt 34 | pip-delete-this-directory.txt 35 | 36 | # Unit test / coverage reports 37 | htmlcov/ 38 | .tox/ 39 | .coverage 40 | .coverage.* 41 | .cache 42 | nosetests.xml 43 | coverage.xml 44 | *,cover 45 | 46 | # Translations 47 | *.mo 48 | *.pot 49 | 50 | # Django stuff: 51 | *.log 52 | 53 | # Sphinx documentation 54 | docs/_build/ 55 | 56 | # PyBuilder 57 | target/ 58 | 59 | # DotEnv configuration 60 | .env 61 | 62 | # Database 63 | *.db 64 | *.rdb 65 | 66 | # Pycharm 67 | .idea 68 | 69 | # IPython NB Checkpoints 70 | .ipynb_checkpoints/ 71 | 72 | # exclude data from source control by default 73 | data/* 74 | models/* 75 | reports/* 76 | -------------------------------------------------------------------------------- /deep_learning_model/LICENSE: -------------------------------------------------------------------------------- 1 | 2 | The MIT License (MIT) 3 | Copyright (c) 2016, Michael Schaeuble 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | 7 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | 9 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 10 | 11 | -------------------------------------------------------------------------------- /deep_learning_model/Makefile: -------------------------------------------------------------------------------- 1 | .PHONY: clean data lint requirements sync_data_to_s3 sync_data_from_s3 2 | 3 | ################################################################################# 4 | # GLOBALS # 5 | ################################################################################# 6 | 7 | BUCKET = [OPTIONAL] your-bucket-for-syncing-data (do not include 's3://') 8 | 9 | ################################################################################# 10 | # COMMANDS # 11 | ################################################################################# 12 | 13 | DATASET = combined 14 | DATASET_EVAL = evaluation 15 | 16 | requirements: 17 | pip install -q -r requirements.txt 18 | 19 | data: 20 | python src/data/make_dataset.py --random data/mixers/data.mix $(DATASET).h5 21 | 22 | train: 23 | python src/model/train_model.py data/processed/$(DATASET).h5 \ 24 | data/processed/$(DATASET_EVAL).h5 \ 25 | -l 0.0001 -s 2000000 -b 128 --mail 26 | 27 | eval: 28 | python src/model/predict_model.py -c reports/evaluation.csv data/processed/$(DATASET_EVAL).h5 \ 29 | ../deep_motion_planner/models/model.pb 30 | 31 | clean: 32 | find . -name "*.pyc" -exec rm {} \; 33 | 34 | lint: 35 | flake8 --exclude=lib/,bin/ . 36 | 37 | sync_data_to_s3: 38 | aws s3 sync data/ s3://$(BUCKET)/data/ 39 | 40 | sync_data_from_s3: 41 | aws s3 sync s3://$(BUCKET)/data/ data/ 42 | 43 | tensorboard: 44 | tensorboard --logdir=models/default 45 | 46 | ################################################################################# 47 | # PROJECT RULES # 48 | ################################################################################# 49 | -------------------------------------------------------------------------------- /deep_learning_model/README.md: -------------------------------------------------------------------------------- 1 | # deep_learning_model 2 | 3 | Learn how to navigate in urban environments with deep learning 4 | 5 | ## Usage 6 | ### Data Generation 7 | Run: 8 | ``` 9 | make data 10 | ``` 11 | to take the single .csv files and generate a single HDF5 container with the entire data. Please 12 | refer to the Makefile to change the files and folders that are used for the data preparation. 13 | 14 | ### Train a Model 15 | ``` 16 | make train 17 | ``` 18 | This will start the training of your model. Please refer to the Makefile to change the files and folders 19 | that are used during training. Furthermore, we set some metaparameters (initial learning rate, 20 | maximum number of steps and the batch size) in this file. 21 | 22 | ### Tensorboard 23 | 24 | ``` 25 | make tensorboard 26 | ``` 27 | This will start Googles Tensorboard with the ./models/default folder as log directory. 28 | 29 | ## Project Organization 30 | 31 | ├── LICENSE 32 | ├── Makefile <- Makefile with commands like `make data` or `make train` 33 | ├── README.md <- The top-level README for developers using this project. 34 | ├── data 35 | │   ├── external <- Data from third party sources. 36 | │   ├── interim <- Intermediate data that has been transformed. 37 | │   ├── processed <- The final, canonical data sets for modeling. 38 | │   └── raw <- The original, immutable data dump. 39 | │ 40 | ├── docs <- A default Sphinx project; see sphinx-doc.org for details 41 | │ 42 | ├── models <- Trained and serialized models, model predictions, or model summaries 43 | │ 44 | ├── notebooks <- Jupyter notebooks. Naming convention is a number (for ordering), 45 | │ the creator's initials, and a short `-` delimited description, e.g. 46 | │ `1.0-jqp-initial-data-exploration`. 47 | │ 48 | ├── references <- Data dictionaries, manuals, and all other explanatory materials. 49 | │ 50 | ├── reports <- Generated analysis as HTML, PDF, LaTeX, etc. 51 | │   └── figures <- Generated graphics and figures to be used in reporting 52 | │ 53 | ├── requirements.txt <- The requirements file for reproducing the analysis environment, e.g. 54 | │ generated with `pip freeze > requirements.txt` 55 | │ 56 | ├── src <- Source code for use in this project. 57 | │   ├── __init__.py <- Makes src a Python module 58 | │ │ 59 | │   ├── data <- Scripts to download or generate data 60 | │   │   └── make_dataset.py 61 | │ │ 62 | │   ├── features <- Scripts to turn raw data into features for modeling 63 | │   │   └── build_features.py 64 | │ │ 65 | │   ├── models <- Scripts to train models and then use trained models to make 66 | │ │ │ predictions 67 | │   │   ├── predict_model.py 68 | │   │   └── train_model.py 69 | │ │ 70 | │   └── visualization <- Scripts to create exploratory and results oriented visualizations 71 | │   └── visualize.py 72 | │ 73 | └── tox.ini <- tox file with settings for running tox; see tox.testrun.org 74 | -------------------------------------------------------------------------------- /deep_learning_model/data/external/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/data/external/.gitkeep -------------------------------------------------------------------------------- /deep_learning_model/data/interim/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/data/interim/.gitkeep -------------------------------------------------------------------------------- /deep_learning_model/data/mixers/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/data/mixers/.gitkeep -------------------------------------------------------------------------------- /deep_learning_model/data/mixers/data.mix: -------------------------------------------------------------------------------- 1 | # Example 2 | # data/raw/simple_test 5 3 | -------------------------------------------------------------------------------- /deep_learning_model/data/mixers/evaluation.mix: -------------------------------------------------------------------------------- 1 | data/raw/evaluation/2016-09-26_blobs -1 10000 2 | data/raw/evaluation/2016-09-26_borderless -1 10000 3 | data/raw/evaluation/2016-09-26_boxes -1 10000 4 | data/raw/evaluation/2016-09-26_lab -1 10000 5 | data/raw/evaluation/2016-09-26_office -1 10000 6 | data/raw/evaluation/2016-09-26_shapes -1 10000 7 | 8 | -------------------------------------------------------------------------------- /deep_learning_model/data/processed/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/data/processed/.gitkeep -------------------------------------------------------------------------------- /deep_learning_model/data/raw/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/data/raw/.gitkeep -------------------------------------------------------------------------------- /deep_learning_model/docs/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | PAPER = 8 | BUILDDIR = _build 9 | 10 | # Internal variables. 11 | PAPEROPT_a4 = -D latex_paper_size=a4 12 | PAPEROPT_letter = -D latex_paper_size=letter 13 | ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . 14 | # the i18n builder cannot share the environment and doctrees with the others 15 | I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . 16 | 17 | .PHONY: help clean html dirhtml singlehtml pickle json htmlhelp qthelp devhelp epub latex latexpdf text man changes linkcheck doctest gettext 18 | 19 | help: 20 | @echo "Please use \`make ' where is one of" 21 | @echo " html to make standalone HTML files" 22 | @echo " dirhtml to make HTML files named index.html in directories" 23 | @echo " singlehtml to make a single large HTML file" 24 | @echo " pickle to make pickle files" 25 | @echo " json to make JSON files" 26 | @echo " htmlhelp to make HTML files and a HTML help project" 27 | @echo " qthelp to make HTML files and a qthelp project" 28 | @echo " devhelp to make HTML files and a Devhelp project" 29 | @echo " epub to make an epub" 30 | @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" 31 | @echo " latexpdf to make LaTeX files and run them through pdflatex" 32 | @echo " text to make text files" 33 | @echo " man to make manual pages" 34 | @echo " texinfo to make Texinfo files" 35 | @echo " info to make Texinfo files and run them through makeinfo" 36 | @echo " gettext to make PO message catalogs" 37 | @echo " changes to make an overview of all changed/added/deprecated items" 38 | @echo " linkcheck to check all external links for integrity" 39 | @echo " doctest to run all doctests embedded in the documentation (if enabled)" 40 | 41 | clean: 42 | -rm -rf $(BUILDDIR)/* 43 | 44 | html: 45 | $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html 46 | @echo 47 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." 48 | 49 | dirhtml: 50 | $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml 51 | @echo 52 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." 53 | 54 | singlehtml: 55 | $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml 56 | @echo 57 | @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." 58 | 59 | pickle: 60 | $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle 61 | @echo 62 | @echo "Build finished; now you can process the pickle files." 63 | 64 | json: 65 | $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json 66 | @echo 67 | @echo "Build finished; now you can process the JSON files." 68 | 69 | htmlhelp: 70 | $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp 71 | @echo 72 | @echo "Build finished; now you can run HTML Help Workshop with the" \ 73 | ".hhp project file in $(BUILDDIR)/htmlhelp." 74 | 75 | qthelp: 76 | $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp 77 | @echo 78 | @echo "Build finished; now you can run "qcollectiongenerator" with the" \ 79 | ".qhcp project file in $(BUILDDIR)/qthelp, like this:" 80 | @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/deep_learning_model.qhcp" 81 | @echo "To view the help file:" 82 | @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/deep_learning_model.qhc" 83 | 84 | devhelp: 85 | $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp 86 | @echo 87 | @echo "Build finished." 88 | @echo "To view the help file:" 89 | @echo "# mkdir -p $$HOME/.local/share/devhelp/deep_learning_model" 90 | @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/deep_learning_model" 91 | @echo "# devhelp" 92 | 93 | epub: 94 | $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub 95 | @echo 96 | @echo "Build finished. The epub file is in $(BUILDDIR)/epub." 97 | 98 | latex: 99 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 100 | @echo 101 | @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." 102 | @echo "Run \`make' in that directory to run these through (pdf)latex" \ 103 | "(use \`make latexpdf' here to do that automatically)." 104 | 105 | latexpdf: 106 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 107 | @echo "Running LaTeX files through pdflatex..." 108 | $(MAKE) -C $(BUILDDIR)/latex all-pdf 109 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 110 | 111 | text: 112 | $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text 113 | @echo 114 | @echo "Build finished. The text files are in $(BUILDDIR)/text." 115 | 116 | man: 117 | $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man 118 | @echo 119 | @echo "Build finished. The manual pages are in $(BUILDDIR)/man." 120 | 121 | texinfo: 122 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 123 | @echo 124 | @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." 125 | @echo "Run \`make' in that directory to run these through makeinfo" \ 126 | "(use \`make info' here to do that automatically)." 127 | 128 | info: 129 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 130 | @echo "Running Texinfo files through makeinfo..." 131 | make -C $(BUILDDIR)/texinfo info 132 | @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." 133 | 134 | gettext: 135 | $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale 136 | @echo 137 | @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." 138 | 139 | changes: 140 | $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes 141 | @echo 142 | @echo "The overview file is in $(BUILDDIR)/changes." 143 | 144 | linkcheck: 145 | $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck 146 | @echo 147 | @echo "Link check complete; look for any errors in the above output " \ 148 | "or in $(BUILDDIR)/linkcheck/output.txt." 149 | 150 | doctest: 151 | $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest 152 | @echo "Testing of doctests in the sources finished, look at the " \ 153 | "results in $(BUILDDIR)/doctest/output.txt." 154 | -------------------------------------------------------------------------------- /deep_learning_model/docs/commands.rst: -------------------------------------------------------------------------------- 1 | Commands 2 | ======== 3 | 4 | The Makefile contains the central entry points for common tasks related to this project. 5 | 6 | Syncing data to S3 7 | ^^^^^^^^^^^^^^^^^^ 8 | 9 | * `make sync_data_to_s3` will use `s3cmd` to recursively sync files in `data/` up to `s3://[OPTIONAL] your-bucket-for-syncing-data (do not include 's3://')/data/`. 10 | * `make sync_data_from_s3` will use `s3cmd` to recursively sync files from `s3://[OPTIONAL] your-bucket-for-syncing-data (do not include 's3://')/data/` to `data/`. -------------------------------------------------------------------------------- /deep_learning_model/docs/getting-started.rst: -------------------------------------------------------------------------------- 1 | Getting started 2 | =============== 3 | 4 | This is where you describe how to get set up on a clean install, including the 5 | commands necessary to get the raw data (using the `sync_data_from_s3` command, 6 | for example, and then how to make the cleaned, final data sets. 7 | -------------------------------------------------------------------------------- /deep_learning_model/docs/index.rst: -------------------------------------------------------------------------------- 1 | .. deep_learning_model documentation master file, created by 2 | sphinx-quickstart. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | deep_learning_model documentation! 7 | ============================================== 8 | 9 | Contents: 10 | 11 | .. toctree:: 12 | :maxdepth: 2 13 | 14 | getting-started 15 | commands 16 | 17 | 18 | 19 | Indices and tables 20 | ================== 21 | 22 | * :ref:`genindex` 23 | * :ref:`modindex` 24 | * :ref:`search` 25 | -------------------------------------------------------------------------------- /deep_learning_model/docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | REM Command file for Sphinx documentation 4 | 5 | if "%SPHINXBUILD%" == "" ( 6 | set SPHINXBUILD=sphinx-build 7 | ) 8 | set BUILDDIR=_build 9 | set ALLSPHINXOPTS=-d %BUILDDIR%/doctrees %SPHINXOPTS% . 10 | set I18NSPHINXOPTS=%SPHINXOPTS% . 11 | if NOT "%PAPER%" == "" ( 12 | set ALLSPHINXOPTS=-D latex_paper_size=%PAPER% %ALLSPHINXOPTS% 13 | set I18NSPHINXOPTS=-D latex_paper_size=%PAPER% %I18NSPHINXOPTS% 14 | ) 15 | 16 | if "%1" == "" goto help 17 | 18 | if "%1" == "help" ( 19 | :help 20 | echo.Please use `make ^` where ^ is one of 21 | echo. html to make standalone HTML files 22 | echo. dirhtml to make HTML files named index.html in directories 23 | echo. singlehtml to make a single large HTML file 24 | echo. pickle to make pickle files 25 | echo. json to make JSON files 26 | echo. htmlhelp to make HTML files and a HTML help project 27 | echo. qthelp to make HTML files and a qthelp project 28 | echo. devhelp to make HTML files and a Devhelp project 29 | echo. epub to make an epub 30 | echo. latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter 31 | echo. text to make text files 32 | echo. man to make manual pages 33 | echo. texinfo to make Texinfo files 34 | echo. gettext to make PO message catalogs 35 | echo. changes to make an overview over all changed/added/deprecated items 36 | echo. linkcheck to check all external links for integrity 37 | echo. doctest to run all doctests embedded in the documentation if enabled 38 | goto end 39 | ) 40 | 41 | if "%1" == "clean" ( 42 | for /d %%i in (%BUILDDIR%\*) do rmdir /q /s %%i 43 | del /q /s %BUILDDIR%\* 44 | goto end 45 | ) 46 | 47 | if "%1" == "html" ( 48 | %SPHINXBUILD% -b html %ALLSPHINXOPTS% %BUILDDIR%/html 49 | if errorlevel 1 exit /b 1 50 | echo. 51 | echo.Build finished. The HTML pages are in %BUILDDIR%/html. 52 | goto end 53 | ) 54 | 55 | if "%1" == "dirhtml" ( 56 | %SPHINXBUILD% -b dirhtml %ALLSPHINXOPTS% %BUILDDIR%/dirhtml 57 | if errorlevel 1 exit /b 1 58 | echo. 59 | echo.Build finished. The HTML pages are in %BUILDDIR%/dirhtml. 60 | goto end 61 | ) 62 | 63 | if "%1" == "singlehtml" ( 64 | %SPHINXBUILD% -b singlehtml %ALLSPHINXOPTS% %BUILDDIR%/singlehtml 65 | if errorlevel 1 exit /b 1 66 | echo. 67 | echo.Build finished. The HTML pages are in %BUILDDIR%/singlehtml. 68 | goto end 69 | ) 70 | 71 | if "%1" == "pickle" ( 72 | %SPHINXBUILD% -b pickle %ALLSPHINXOPTS% %BUILDDIR%/pickle 73 | if errorlevel 1 exit /b 1 74 | echo. 75 | echo.Build finished; now you can process the pickle files. 76 | goto end 77 | ) 78 | 79 | if "%1" == "json" ( 80 | %SPHINXBUILD% -b json %ALLSPHINXOPTS% %BUILDDIR%/json 81 | if errorlevel 1 exit /b 1 82 | echo. 83 | echo.Build finished; now you can process the JSON files. 84 | goto end 85 | ) 86 | 87 | if "%1" == "htmlhelp" ( 88 | %SPHINXBUILD% -b htmlhelp %ALLSPHINXOPTS% %BUILDDIR%/htmlhelp 89 | if errorlevel 1 exit /b 1 90 | echo. 91 | echo.Build finished; now you can run HTML Help Workshop with the ^ 92 | .hhp project file in %BUILDDIR%/htmlhelp. 93 | goto end 94 | ) 95 | 96 | if "%1" == "qthelp" ( 97 | %SPHINXBUILD% -b qthelp %ALLSPHINXOPTS% %BUILDDIR%/qthelp 98 | if errorlevel 1 exit /b 1 99 | echo. 100 | echo.Build finished; now you can run "qcollectiongenerator" with the ^ 101 | .qhcp project file in %BUILDDIR%/qthelp, like this: 102 | echo.^> qcollectiongenerator %BUILDDIR%\qthelp\deep_learning_model.qhcp 103 | echo.To view the help file: 104 | echo.^> assistant -collectionFile %BUILDDIR%\qthelp\deep_learning_model.ghc 105 | goto end 106 | ) 107 | 108 | if "%1" == "devhelp" ( 109 | %SPHINXBUILD% -b devhelp %ALLSPHINXOPTS% %BUILDDIR%/devhelp 110 | if errorlevel 1 exit /b 1 111 | echo. 112 | echo.Build finished. 113 | goto end 114 | ) 115 | 116 | if "%1" == "epub" ( 117 | %SPHINXBUILD% -b epub %ALLSPHINXOPTS% %BUILDDIR%/epub 118 | if errorlevel 1 exit /b 1 119 | echo. 120 | echo.Build finished. The epub file is in %BUILDDIR%/epub. 121 | goto end 122 | ) 123 | 124 | if "%1" == "latex" ( 125 | %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex 126 | if errorlevel 1 exit /b 1 127 | echo. 128 | echo.Build finished; the LaTeX files are in %BUILDDIR%/latex. 129 | goto end 130 | ) 131 | 132 | if "%1" == "text" ( 133 | %SPHINXBUILD% -b text %ALLSPHINXOPTS% %BUILDDIR%/text 134 | if errorlevel 1 exit /b 1 135 | echo. 136 | echo.Build finished. The text files are in %BUILDDIR%/text. 137 | goto end 138 | ) 139 | 140 | if "%1" == "man" ( 141 | %SPHINXBUILD% -b man %ALLSPHINXOPTS% %BUILDDIR%/man 142 | if errorlevel 1 exit /b 1 143 | echo. 144 | echo.Build finished. The manual pages are in %BUILDDIR%/man. 145 | goto end 146 | ) 147 | 148 | if "%1" == "texinfo" ( 149 | %SPHINXBUILD% -b texinfo %ALLSPHINXOPTS% %BUILDDIR%/texinfo 150 | if errorlevel 1 exit /b 1 151 | echo. 152 | echo.Build finished. The Texinfo files are in %BUILDDIR%/texinfo. 153 | goto end 154 | ) 155 | 156 | if "%1" == "gettext" ( 157 | %SPHINXBUILD% -b gettext %I18NSPHINXOPTS% %BUILDDIR%/locale 158 | if errorlevel 1 exit /b 1 159 | echo. 160 | echo.Build finished. The message catalogs are in %BUILDDIR%/locale. 161 | goto end 162 | ) 163 | 164 | if "%1" == "changes" ( 165 | %SPHINXBUILD% -b changes %ALLSPHINXOPTS% %BUILDDIR%/changes 166 | if errorlevel 1 exit /b 1 167 | echo. 168 | echo.The overview file is in %BUILDDIR%/changes. 169 | goto end 170 | ) 171 | 172 | if "%1" == "linkcheck" ( 173 | %SPHINXBUILD% -b linkcheck %ALLSPHINXOPTS% %BUILDDIR%/linkcheck 174 | if errorlevel 1 exit /b 1 175 | echo. 176 | echo.Link check complete; look for any errors in the above output ^ 177 | or in %BUILDDIR%/linkcheck/output.txt. 178 | goto end 179 | ) 180 | 181 | if "%1" == "doctest" ( 182 | %SPHINXBUILD% -b doctest %ALLSPHINXOPTS% %BUILDDIR%/doctest 183 | if errorlevel 1 exit /b 1 184 | echo. 185 | echo.Testing of doctests in the sources finished, look at the ^ 186 | results in %BUILDDIR%/doctest/output.txt. 187 | goto end 188 | ) 189 | 190 | :end 191 | -------------------------------------------------------------------------------- /deep_learning_model/models/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/models/.gitkeep -------------------------------------------------------------------------------- /deep_learning_model/models/default/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/models/default/.gitkeep -------------------------------------------------------------------------------- /deep_learning_model/notebooks/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/notebooks/.gitkeep -------------------------------------------------------------------------------- /deep_learning_model/references/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/references/.gitkeep -------------------------------------------------------------------------------- /deep_learning_model/reports/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/reports/.gitkeep -------------------------------------------------------------------------------- /deep_learning_model/reports/figures/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/reports/figures/.gitkeep -------------------------------------------------------------------------------- /deep_learning_model/requirements.txt: -------------------------------------------------------------------------------- 1 | Sphinx 2 | coverage 3 | #awscli 4 | flake8 5 | #python-dotenv>=0.5.1 6 | -------------------------------------------------------------------------------- /deep_learning_model/src/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/src/__init__.py -------------------------------------------------------------------------------- /deep_learning_model/src/data/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/src/data/.gitkeep -------------------------------------------------------------------------------- /deep_learning_model/src/data/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/src/data/__init__.py -------------------------------------------------------------------------------- /deep_learning_model/src/data/custom_data_runner.py: -------------------------------------------------------------------------------- 1 | """ 2 | File: custom_data_runner.py 3 | Description: 4 | Implementation of data runner from 5 | https://indico.io/blog/tensorflow-data-input-part2-extensions/ 6 | """ 7 | import threading 8 | 9 | import tensorflow as tf 10 | 11 | from data.fast_data_handler import FastDataHandler 12 | 13 | INPUT_SIZE = 1083 14 | 15 | class CustomDataRunner(): 16 | """Class to manage threads which fill a queue of data""" 17 | def __init__(self, filepath, batch_size, chunksize): 18 | with tf.device("/cpu:0"): 19 | self.batch_size = batch_size 20 | self.data_handler = FastDataHandler(filepath, batch_size, chunksize) 21 | self.data_x = tf.placeholder(dtype=tf.float32, shape=[None, INPUT_SIZE]) 22 | self.data_y = tf.placeholder(dtype=tf.float32, shape=[None, 2]) 23 | 24 | # The actual queue of data. The queue contains a vector for 25 | # the mnist features, and a scalar label. 26 | self.queue = tf.RandomShuffleQueue(shapes=[[INPUT_SIZE], [2]], 27 | dtypes=[tf.float32, tf.float32], 28 | capacity=2000, 29 | min_after_dequeue=1000) 30 | 31 | # The symbolic operation to add data to the queue 32 | # we could do some preprocessing here or do it in numpy. In this example 33 | # we do the scaling in numpy 34 | self.enqueue_op = self.queue.enqueue_many([self.data_x, self.data_y]) 35 | self.threads_stop = False 36 | self.threads = list() 37 | self.sess = None 38 | 39 | def close(self): 40 | self.threads_stop = True 41 | self.data_handler.close() 42 | if self.sess: 43 | self.sess.run(self.queue.close(cancel_pending_enqueues=True)) 44 | for thr in self.threads: 45 | thr.join() 46 | 47 | def get_inputs(self): 48 | """ 49 | Return's tensors containing a batch of images and labels 50 | """ 51 | data_batch, cmd_batch = self.queue.dequeue_many(self.batch_size) 52 | return data_batch, cmd_batch 53 | 54 | def thread_main(self, sess, coord): 55 | """ 56 | Function run on alternate thread. Basically, keep adding data to the queue. 57 | """ 58 | while not (coord.should_stop() or self.threads_stop): 59 | data_x, data_y =self.data_handler.next_batch() 60 | try: 61 | sess.run(self.enqueue_op, feed_dict={self.data_x:data_x, self.data_y:data_y}) 62 | except tf.errors.CancelledError as e: 63 | # This happens if we stop processing and the enque operation is pending 64 | pass 65 | 66 | def start_threads(self, sess, coord, n_threads=1): 67 | """ Start background threads to feed queue """ 68 | self.sess = sess 69 | threads = [] 70 | for n in range(n_threads): 71 | t = threading.Thread(target=self.thread_main, args=(sess,coord)) 72 | t.daemon = True 73 | t.start() 74 | self.threads.append(t) 75 | -------------------------------------------------------------------------------- /deep_learning_model/src/data/data_handler.py: -------------------------------------------------------------------------------- 1 | import os 2 | import pandas as pd 3 | import numpy as np 4 | 5 | class DataHandler(): 6 | """Class to load data from HDF5 storages in a random and chunckwise manner""" 7 | def __init__(self, filepath, chunksize=1000, shuffle = True): 8 | self.filepath = filepath 9 | self.chunksize = chunksize 10 | self.shuffle = shuffle 11 | self.perception_radius = 10.0 12 | self.mean_filter_size = 5 13 | 14 | if not os.path.exists(filepath): 15 | raise IOError('File does not exists: {}'.format(filepath)) 16 | 17 | # Get the number of rows without loading any data into memory 18 | with pd.HDFStore(filepath) as store: 19 | self.nrows = store.get_storer('data').nrows 20 | 21 | # Make sure, we can return a chunk with the correct size 22 | if chunksize > self.nrows: 23 | raise ValueError('Chunksize is to large for the dataset: {} chunks > {} rows'.format(chunksize, self.nrows)) 24 | 25 | # Initialize the batches 26 | self.batches = self.__next_permutation__() 27 | 28 | def __next_permutation__(self): 29 | """ 30 | Generator for the permutations. 31 | 32 | @return Indices of the elements in the batch 33 | @rtype Generator 34 | """ 35 | current_index = 0 36 | if self.shuffle: 37 | permutation = np.random.permutation(self.nrows) 38 | else: 39 | permutation = np.arange(self.nrows) 40 | while True: 41 | yield permutation[current_index:current_index+self.chunksize] 42 | current_index += self.chunksize 43 | 44 | # We iterated over the entire dataset, so resample and reset 45 | if (current_index + self.chunksize) > self.nrows: 46 | permutation = np.random.permutation(self.nrows) 47 | current_index = 0 48 | 49 | def next_batch(self): 50 | """ 51 | Load the next random batch from the loaded data file 52 | 53 | @return (input data, ground truth commands) 54 | @rtype Tuple 55 | """ 56 | # Load the data for the next batch 57 | ind = next(self.batches) 58 | df = pd.read_hdf(self.filepath, 'data', where='index=ind') 59 | 60 | df['filtered_linear'] = pd.rolling_mean(df['linear_x'], 61 | window=self.mean_filter_size, center=True).fillna(df['linear_x']) 62 | df['filtered_angular'] = pd.rolling_mean(df['angular_z'], 63 | window=self.mean_filter_size, center=True).fillna(df['angular_z']) 64 | 65 | laser_columns = list() 66 | goal_columns = list() 67 | cmd_columns = list() 68 | for j,column in enumerate(df.columns): 69 | if column.split('_')[0] in ['laser']: 70 | laser_columns.append(j) 71 | if column.split('_')[0] in ['target'] and not column.split('_')[1] == 'id': 72 | goal_columns.append(j) 73 | if column in ['filtered_linear','filtered_angular']: 74 | cmd_columns.append(j) 75 | 76 | #Only use the center n_scans elements as input 77 | n_scans = 1080 78 | drop_n_elements = (len(laser_columns) - n_scans) // 2 79 | 80 | if drop_n_elements < 0: 81 | raise ValueError('Number of scans is to small: {} < {}' 82 | .format(len(laser_columns), n_scans)) 83 | elif drop_n_elements > 0: 84 | laser_columns = laser_columns[drop_n_elements:-drop_n_elements] 85 | 86 | if len(laser_columns) == n_scans+1: 87 | laser_columns = laser_columns[0:-1] 88 | 89 | laser = np.minimum(df.iloc[:,laser_columns].values, self.perception_radius) 90 | goal = df.iloc[:,goal_columns].values 91 | angle = np.arctan2(goal[:,1],goal[:,0]).reshape([self.chunksize, 1]) 92 | norm = np.minimum(np.linalg.norm(goal[:,0:2], ord=2, axis=1).reshape([self.chunksize, 1]), self.perception_radius) 93 | data = np.concatenate((laser, angle, norm, goal[:,2].reshape([self.chunksize, 1])), axis=1) 94 | 95 | return (data.copy(), df.iloc[:, cmd_columns].copy(deep=True).values) 96 | 97 | 98 | -------------------------------------------------------------------------------- /deep_learning_model/src/data/stack_hdf5.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | from __future__ import print_function 4 | 5 | import os 6 | import logging 7 | import argparse 8 | 9 | import pandas as pd 10 | 11 | def parse_args(): 12 | """Parse input arguments.""" 13 | parser = argparse.ArgumentParser(description='Stack the given HDF5 container into a new file') 14 | parser.add_argument('input_files', help='Files to stack', nargs=argparse.REMAINDER) 15 | 16 | def check_extension(extensions, filename): 17 | ext = os.path.splitext(filename)[1] 18 | if ext not in extensions: 19 | parser.error("Unsupported file extension: Use {}".format(extensions)) 20 | return filename 21 | 22 | parser.add_argument('-o', '--output_file', help='Filename of the combined data', type=lambda 23 | s:check_extension([".h5"],s), default='stacked.h5') 24 | parser.add_argument('-s', '--chunksize', help='Size of chunks read from the input file', default=10000, type=int) 25 | args = parser.parse_args() 26 | 27 | return args, parser 28 | 29 | def main(project_dir): 30 | logger = logging.getLogger(__name__) 31 | logger.info('Combine HDF5 container into a single one') 32 | 33 | (args, parser) = parse_args() 34 | 35 | # Make some input file checks 36 | input_files = args.input_files 37 | if len(input_files) < 2: 38 | logger.error('You have to specify at least two files') 39 | parser.print_help() 40 | exit(-1) 41 | 42 | for f in input_files: 43 | if not os.path.exists(f): 44 | logger.error('At least one input file does not exist: {}'.format(f)) 45 | exit(-1) 46 | elif os.path.basename(f).split('.')[1] != 'h5': 47 | logger.error('File must be HDF5 (.h5): {}'.format(f)) 48 | exit(-1) 49 | 50 | # Do not overwrite existing data containers 51 | if os.path.exists(args.output_file): 52 | logger.error('Output file already exists: {}'.format(args.output_file)) 53 | exit(-1) 54 | 55 | with pd.HDFStore(args.output_file) as store: 56 | num_elements = 0 57 | 58 | # Iterate over all input files and add them to the HDF5 container 59 | for i,f in enumerate(input_files): 60 | 61 | with pd.HDFStore(f, mode='r') as input_file: 62 | nrows = input_file.get_storer('data').nrows 63 | 64 | nadded = 0 65 | for chunk_index in range(nrows // args.chunksize + 1): 66 | chunk = input_file.select('data', 67 | start=chunk_index*args.chunksize, stop=(chunk_index+1)*args.chunksize) 68 | 69 | progress = (chunk_index*args.chunksize*100.0) / nrows 70 | print('{:.2f}% ({}/{}) {}'.format(progress, chunk_index*args.chunksize, nrows, f), end='\r') 71 | 72 | # Make sure the final dataframe has a continous index 73 | chunk.index = pd.Series(chunk.index) + num_elements 74 | store.append('data', chunk) 75 | 76 | num_elements += chunk.shape[0] 77 | nadded += chunk.shape[0] 78 | 79 | print('{:.2f}% ({}/{}) {}'.format(100.0, nadded, nrows, f)) 80 | 81 | logger.info('We combined {} lines'.format(num_elements)) 82 | logger.info('File saved: {}'.format(args.output_file)) 83 | 84 | 85 | if __name__ == '__main__': 86 | log_fmt = '%(asctime)s - %(name)s - %(levelname)s - %(message)s' 87 | logging.basicConfig(level=logging.INFO, format=log_fmt) 88 | 89 | # not used in this stub but often useful for finding various files 90 | project_dir = os.path.join(os.path.dirname(__file__), os.pardir, os.pardir) 91 | 92 | main(project_dir) 93 | 94 | -------------------------------------------------------------------------------- /deep_learning_model/src/features/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/src/features/.gitkeep -------------------------------------------------------------------------------- /deep_learning_model/src/features/build_features.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/src/features/build_features.py -------------------------------------------------------------------------------- /deep_learning_model/src/model/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/src/model/.gitkeep -------------------------------------------------------------------------------- /deep_learning_model/src/model/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/src/model/__init__.py -------------------------------------------------------------------------------- /deep_learning_model/src/model/_init_paths.py: -------------------------------------------------------------------------------- 1 | """ 2 | Add the src directory to the Python path 3 | """ 4 | import os 5 | import sys 6 | 7 | def add_path(path): 8 | if path not in sys.path: 9 | sys.path.insert(0, path) 10 | 11 | project_dir = os.path.join(os.path.dirname(__file__), os.pardir, os.pardir) 12 | add_path(os.path.join(project_dir, 'src')) 13 | add_path(os.path.join(project_dir, '..', 'deep_motion_planner', 'python')) 14 | 15 | -------------------------------------------------------------------------------- /deep_learning_model/src/model/conv_model.py: -------------------------------------------------------------------------------- 1 | """ 2 | This file contains methods that describe a Tensorflow model. 3 | It can be used as template for a completely new model and is imported 4 | in the training script 5 | """ 6 | import logging 7 | 8 | import tensorflow as tf 9 | from tensorflow.contrib.layers import batch_norm, fully_connected, conv2d, xavier_initializer_conv2d, xavier_initializer, l1_regularizer 10 | import tensorflow.contrib as contrib 11 | 12 | # Give the model a descriptive name 13 | NAME = 'convolutional' 14 | 15 | # The size of the input layer 16 | INPUT_SIZE = 1083 17 | # The size of the output layer 18 | CMD_SIZE = 2 19 | 20 | def learning_rate(initial): 21 | """ 22 | Define learning rate for your model 23 | """ 24 | # We use an exponential decy for the model 25 | global_step = tf.Variable(0, name='global_step', trainable=False) 26 | learning_rate = tf.train.exponential_decay(initial, global_step, 27 | 250000, 0.85, staircase=True) 28 | return global_step, learning_rate 29 | 30 | def __get_variable__(index, input_size, output_size): 31 | return (tf.get_variable('weights_hidden{}'.format(index), shape=[input_size, output_size], 32 | initializer=tf.contrib.layers.xavier_initializer()), 33 | tf.get_variable('biases_hidden{}'.format(index), [output_size])) 34 | 35 | def inference(data, keep_prob, sample_size, training=True, reuse=False, output_name='prediction'): 36 | """ 37 | Define the deep neural network used for inference 38 | """ 39 | 40 | # slice 709 elements to get the correct size for the next convolutions 41 | laser = tf.slice(data, [0,0], [sample_size,1080]) 42 | goal = tf.slice(data, [0,1080], [sample_size,3]) 43 | 44 | laser = tf.reshape(laser, [sample_size, 1, 1080, 1]) 45 | hidden_1 = conv2d(laser, 64, [1,7], stride=3, normalizer_fn=batch_norm, 46 | weights_initializer=xavier_initializer_conv2d(), 47 | weights_regularizer=l1_regularizer(0.001), reuse=reuse, trainable=training, scope='layer_scope_1') 48 | hidden_1 = contrib.layers.max_pool2d(hidden_1, [1,3],[1,3], 'SAME') 49 | hidden_2 = conv2d(hidden_1, 64, [1,3], normalizer_fn=batch_norm, 50 | weights_initializer=xavier_initializer_conv2d(), 51 | weights_regularizer=l1_regularizer(0.001), reuse=reuse, trainable=training, scope='layer_scope_2') 52 | hidden_3 = conv2d(hidden_2, 64, [1,3], activation_fn=None, normalizer_fn=batch_norm, 53 | weights_initializer=xavier_initializer_conv2d(), 54 | weights_regularizer=l1_regularizer(0.001), reuse=reuse, trainable=training, scope='layer_scope_3') 55 | hidden_3 = tf.nn.relu(hidden_3 + hidden_1) 56 | hidden_4 = conv2d(hidden_3, 64, [1,3], normalizer_fn=batch_norm, 57 | weights_initializer=xavier_initializer_conv2d(), 58 | weights_regularizer=l1_regularizer(0.001), reuse=reuse, trainable=training, scope='layer_scope_4') 59 | hidden_5 = conv2d(hidden_4, 64, [1,3], activation_fn=None, normalizer_fn=batch_norm, 60 | weights_initializer=xavier_initializer_conv2d(), 61 | weights_regularizer=l1_regularizer(0.001), reuse=reuse, trainable=training, scope='layer_scope_5') 62 | hidden_5 = tf.nn.relu(hidden_5 + hidden_3) 63 | 64 | pooling = contrib.layers.avg_pool2d(hidden_5, [1,3],[1,3], 'SAME') 65 | pooling = contrib.layers.flatten(pooling) 66 | combined = tf.concat(1,[pooling, goal]) 67 | fc_5 = fully_connected(combined, 1024, weights_initializer=xavier_initializer(), 68 | weights_regularizer=l1_regularizer(0.001), reuse=reuse, trainable=training, scope='fc_scope_5') 69 | fc_6 = fully_connected(fc_5, 1024, weights_initializer=xavier_initializer(), 70 | weights_regularizer=l1_regularizer(0.001), reuse=reuse, trainable=training, scope='fc_scope_6') 71 | fc_7 = fully_connected(fc_6, 512, weights_initializer=xavier_initializer(), 72 | weights_regularizer=l1_regularizer(0.001), reuse=reuse, trainable=training, scope='fc_scope_7') 73 | prediction = fully_connected(fc_7, CMD_SIZE, activation_fn=None, reuse=reuse, trainable=training, scope='layer_scope_pred') 74 | 75 | prediction = tf.identity(prediction, name=output_name) 76 | 77 | return prediction 78 | 79 | def loss(prediction, cmd): 80 | """ 81 | Define the loss used during the training steps 82 | """ 83 | loss_split = tf.reduce_mean(tf.abs(prediction - cmd), 0) 84 | loss = tf.reduce_mean(tf.abs(prediction - cmd)) 85 | 86 | return loss, loss_split 87 | 88 | def training(loss, loss_split, learning_rate, global_step): 89 | """ 90 | Perform a single training step optimization 91 | """ 92 | optimizer = tf.train.AdamOptimizer(learning_rate) 93 | train_op = optimizer.minimize(loss, global_step=global_step) 94 | 95 | return train_op 96 | 97 | def evaluation(prediction, cmd): 98 | """ 99 | Define how to evaluate the model 100 | """ 101 | loss_split = tf.reduce_mean(tf.abs(prediction - cmd), 0) 102 | loss = tf.reduce_mean(tf.abs(prediction - cmd)) 103 | 104 | return loss, loss_split 105 | -------------------------------------------------------------------------------- /deep_learning_model/src/model/model.py: -------------------------------------------------------------------------------- 1 | """ 2 | This file contains methods that describe a Tensorflow model. 3 | It can be used as template for a completely new model and is imported 4 | in the training script 5 | """ 6 | import logging 7 | 8 | import tensorflow as tf 9 | 10 | # Give the model a descriptive name 11 | NAME = 'two_fully_connected' 12 | 13 | # The size of the input layer 14 | INPUT_SIZE = 1083 15 | # The size of the output layer 16 | CMD_SIZE = 2 17 | 18 | def learning_rate(initial): 19 | """ 20 | Define learning rate for your model 21 | """ 22 | # We use an exponential decy for the model 23 | global_step = tf.Variable(0, name='global_step', trainable=False) 24 | learning_rate = tf.train.exponential_decay(initial, global_step, 25 | 250000, 0.85, staircase=True) 26 | return global_step, learning_rate 27 | 28 | def __get_variable__(index, input_size, output_size): 29 | return (tf.get_variable('weights_hidden{}'.format(index), shape=[input_size, output_size], 30 | initializer=tf.contrib.layers.xavier_initializer()), 31 | tf.get_variable('biases_hidden{}'.format(index), [output_size])) 32 | 33 | def inference(data, keep_prob, sample_size, training=True, reuse=False, output_name='prediction'): 34 | """ 35 | Define the deep neural network used for inference 36 | """ 37 | with tf.variable_scope("prediction_scope") as scope: 38 | if reuse: 39 | scope.reuse_variables() 40 | 41 | weights_hidden_6, biases_hidden_6 = __get_variable__(6, INPUT_SIZE, 2048) 42 | weights_hidden_7, biases_hidden_7 = __get_variable__(7, 2048, 2048) 43 | weights_hidden_8, biases_hidden_8 = __get_variable__(8, 2048, 1024) 44 | weights_out = tf.get_variable('weights_out', [1024, CMD_SIZE], 45 | initializer=tf.truncated_normal_initializer(stddev=0.1)) 46 | biases_out = tf.get_variable('biases_out', [CMD_SIZE], 47 | initializer=tf.constant_initializer(0.0)) 48 | 49 | hidden_6 = tf.nn.relu(tf.add(tf.matmul(data, weights_hidden_6), biases_hidden_6)) 50 | hidden_7 = tf.nn.relu(tf.add(tf.matmul(hidden_6, weights_hidden_7), biases_hidden_7)) 51 | hidden_7 = tf.nn.dropout(hidden_7, keep_prob) 52 | hidden_8 = tf.nn.relu(tf.add(tf.matmul(hidden_7, weights_hidden_8), biases_hidden_8)) 53 | hidden_8 = tf.nn.dropout(hidden_8, keep_prob) 54 | prediction = tf.add(tf.matmul(hidden_8, weights_out), biases_out, name=output_name) 55 | 56 | return prediction 57 | 58 | def loss(prediction, cmd): 59 | """ 60 | Define the loss used during the training steps 61 | """ 62 | loss_split = tf.reduce_mean(tf.abs(prediction - cmd), 0) 63 | loss = tf.reduce_mean(tf.abs(prediction - cmd)) 64 | 65 | return loss, loss_split 66 | 67 | def training(loss, loss_split, learning_rate, global_step): 68 | """ 69 | Perform a single training step optimization 70 | """ 71 | optimizer = tf.train.AdamOptimizer(learning_rate) 72 | train_op = optimizer.minimize(loss, global_step=global_step) 73 | 74 | return train_op 75 | 76 | def evaluation(prediction, cmd): 77 | """ 78 | Define how to evaluate the model 79 | """ 80 | loss_split = tf.reduce_mean(tf.abs(prediction - cmd), 0) 81 | loss = tf.reduce_mean(tf.abs(prediction - cmd)) 82 | 83 | return loss, loss_split 84 | -------------------------------------------------------------------------------- /deep_learning_model/src/model/model_to_graph.py: -------------------------------------------------------------------------------- 1 | 2 | import tensorflow as tf 3 | from tensorflow.python.platform import gfile 4 | 5 | import os 6 | import argparse 7 | 8 | def parse_args(): 9 | """Parse input arguments.""" 10 | parser = argparse.ArgumentParser(description='Save model to generate graph') 11 | parser.add_argument('graph_file', help='Path to the graph definition (.pb)') 12 | parser.add_argument('storage_path', help='Path to write the output') 13 | 14 | args = parser.parse_args() 15 | 16 | return args 17 | def main(): 18 | 19 | args = parse_args() 20 | 21 | with gfile.FastGFile(args.graph_file,'rb') as f: 22 | graph_def = tf.GraphDef() 23 | graph_def.ParseFromString(f.read()) 24 | tf.import_graph_def(graph_def, name='') 25 | 26 | print('Loaded protobuf file: {}'.format(args.graph_file)) 27 | 28 | with tf.Session() as sess: 29 | 30 | filename = os.path.join(args.storage_path, 'final') 31 | writer = tf.train.SummaryWriter(filename, sess.graph) 32 | 33 | if __name__ == "__main__": 34 | main() 35 | -------------------------------------------------------------------------------- /deep_learning_model/src/model/predict_model.py: -------------------------------------------------------------------------------- 1 | import _init_paths 2 | 3 | import argparse 4 | import logging 5 | import os 6 | import time 7 | import math 8 | 9 | import numpy as np 10 | import pandas as pd 11 | 12 | from data.data_handler import DataHandler 13 | from deep_motion_planner.tensorflow_wrapper import TensorflowWrapper 14 | 15 | class Config(): 16 | """Configuration object to store various parameters used in the code""" 17 | perception_radius = 10.0 # Radius to truncate the sensor data 18 | number_of_scans = 1080 # Number of used laser scans 19 | model = None # Path to the evaluated model 20 | data = None # Path to the data used in the evaluation 21 | eval_n_elements = 60000 # Number of samples used to evaluate the model 22 | use_snapshots = False # Specify if the model should be loaded from snapshots 23 | write_result = False # Specify if we write the results into a .csv file 24 | results = None # Path to the .csv result file 25 | mean_filter_size = 5 26 | 27 | def parse_args(): 28 | """Parse input arguments.""" 29 | parser = argparse.ArgumentParser(description='Evaluate the given model on the given data') 30 | 31 | def check_extension(extensions, filename): 32 | ext = os.path.splitext(filename)[1] 33 | if ext not in extensions: 34 | parser.error("Unsupported file extension: Use {}".format(extensions)) 35 | return filename 36 | 37 | parser.add_argument('datafile_eval', help='Filename of the evaluation data', type=lambda 38 | s:check_extension(['.h5'],s)) 39 | parser.add_argument('model', help='Path to the model') 40 | parser.add_argument('-s', '--snapshots', help='Load model from a snapshot directory', 41 | action='store_true') 42 | parser.add_argument('-c', '--capture', help='Filepath to write (append) results', default=None) 43 | args = parser.parse_args() 44 | 45 | return args 46 | 47 | 48 | def run_evaluation(cfg): 49 | """Test a tensorflow model""" 50 | logger = logging.getLogger(run_evaluation.__name__) 51 | 52 | data, ground_truth = DataHandler(cfg.data, cfg.eval_n_elements, shuffle=False).next_batch() 53 | 54 | prediction = np.zeros(ground_truth.shape) 55 | samples = prediction.shape[0] 56 | tictoc = np.zeros(samples) 57 | print_every = 2500 58 | 59 | logger.info('Load Tensorflow model') 60 | with TensorflowWrapper(os.path.dirname(cfg.model), os.path.basename(cfg.model), 61 | cfg.use_snapshots) as model: 62 | 63 | logger.info('Start model evaluation') 64 | for i in range(samples): 65 | 66 | start_time = time.time() 67 | prediction[i,:] = model.inference(data[i,:]) 68 | tictoc[i%print_every] = time.time() - start_time 69 | 70 | next_i = i+1 71 | if next_i > 0 and next_i % print_every == 0 or next_i == samples: 72 | logger.info('Processed: {:6.2f}% ({:5d}/{}) ({:.{width}f} ms/sample)'.format(next_i/samples*100.0, next_i, samples, 73 | np.mean(tictoc)*1e3, width=math.ceil(math.log10(samples)))) 74 | 75 | # Compute the loss 76 | error = np.mean(np.abs(ground_truth - prediction), axis=0) 77 | comined_error = np.mean(error) 78 | 79 | logger.info('Evaluation error: {:.5f} ({:.5f}/{:.5f})'.format(comined_error, error[0], error[1])) 80 | 81 | # Write the results into a .csv file 82 | if cfg.write_result: 83 | add_header_line = not os.path.isfile(cfg.results) 84 | 85 | with open(cfg.results, 'a') as capture: 86 | if add_header_line: 87 | capture.write('date_time,model,error_combined,error_linear,error_angular,execution_time\n') 88 | 89 | capture.write('{},{},{},{},{},{}\n'.format( 90 | time.strftime('%Y-%m-%d %H:%M:%S'), 91 | cfg.model, 92 | comined_error, 93 | error[0], 94 | error[1], 95 | np.mean(tictoc) 96 | )) 97 | 98 | def main(): 99 | logger = logging.getLogger(__name__) 100 | logger.info('Test our model on the given dataset:') 101 | 102 | args = parse_args() 103 | logger.info(args.datafile_eval) 104 | 105 | cfg = Config() 106 | cfg.model = os.path.abspath(args.model) 107 | cfg.data = args.datafile_eval 108 | cfg.use_snapshots = args.snapshots 109 | if args.capture: 110 | cfg.write_result = True 111 | cfg.results = args.capture 112 | 113 | logger.info('Start evaluation') 114 | run_evaluation(cfg) 115 | 116 | if __name__ == "__main__": 117 | log_fmt = '%(asctime)s - %(name)s - %(levelname)s - %(message)s' 118 | logging.basicConfig(level=logging.INFO, format=log_fmt) 119 | 120 | main() 121 | -------------------------------------------------------------------------------- /deep_learning_model/src/model/train_model.py: -------------------------------------------------------------------------------- 1 | import _init_paths 2 | 3 | import argparse 4 | import logging 5 | import os 6 | import time 7 | 8 | import numpy as np 9 | import tensorflow as tf 10 | 11 | from model.training_wrapper import TrainingWrapper 12 | 13 | def parse_args(): 14 | """Parse input arguments.""" 15 | parser = argparse.ArgumentParser(description='Train our model on the given dataset') 16 | 17 | def check_extension(extensions, filename): 18 | ext = os.path.splitext(filename)[1] 19 | if ext not in extensions: 20 | parser.error("Unsupported file extension: Use {}".format(extensions)) 21 | return filename 22 | 23 | parser.add_argument('datafile_train', help='Filename of the training data', type=lambda 24 | s:check_extension(['.h5'],s)) 25 | parser.add_argument('datafile_eval', help='Filename of the evaluation data', type=lambda 26 | s:check_extension(['.h5'],s)) 27 | parser.add_argument('-s', '--max_steps', help='Number of batches to run', type=int, default=1000000) 28 | parser.add_argument('-e', '--eval_steps', help='Evaluate model every N steps', type=int, default=25000) 29 | parser.add_argument('-b', '--batch_size', help='Size of training batches', type=int, default=16) 30 | parser.add_argument('-t', '--train_dir', help='Directory to save the model snapshots', 31 | default='./models/default') 32 | parser.add_argument('-l', '--learning_rate', help='Initial learning rate', type=float, default=0.01) 33 | parser.add_argument('--weight_initialize', help='Initialize network weights with this checkpoint\ 34 | file', type=str) 35 | parser.add_argument('-m', '--mail', help='Send an email when training finishes', 36 | action='store_true') 37 | args = parser.parse_args() 38 | 39 | return args 40 | 41 | def run_training(args): 42 | """Train a tensorflow model""" 43 | with TrainingWrapper(args) as wrapper: 44 | wrapper.run() 45 | 46 | def main(): 47 | logger = logging.getLogger(__name__) 48 | logger.info('Train our model on the given dataset:') 49 | 50 | args = parse_args() 51 | logger.info(args.datafile_train) 52 | 53 | logger.info('Start training') 54 | run_training(args) 55 | 56 | if __name__ == "__main__": 57 | log_fmt = '%(asctime)s - %(name)s - %(levelname)s - %(message)s' 58 | logging.basicConfig(level=logging.INFO, format=log_fmt) 59 | 60 | main() 61 | -------------------------------------------------------------------------------- /deep_learning_model/src/visualization/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/src/visualization/.gitkeep -------------------------------------------------------------------------------- /deep_learning_model/src/visualization/visualize.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_learning_model/src/visualization/visualize.py -------------------------------------------------------------------------------- /deep_learning_model/tox.ini: -------------------------------------------------------------------------------- 1 | [flake8] 2 | max-line-length = 79 3 | max-complexity = 10 4 | -------------------------------------------------------------------------------- /deep_motion_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ########### 2 | ## Setup ## 3 | ########### 4 | 5 | cmake_minimum_required(VERSION 2.8.3) 6 | project(deep_motion_planner) 7 | 8 | find_package(catkin_simple REQUIRED) 9 | catkin_simple(ALL_DEPS_REQUIRED) 10 | 11 | catkin_python_setup() -------------------------------------------------------------------------------- /deep_motion_planner/README.md: -------------------------------------------------------------------------------- 1 | # deep_motion_planner 2 | 3 | This package wraps a trained Tensorflow model as ROS node. It takes the a goal position, laser 4 | scan data and the current robot pose to perform a motion planning using a deep neural network. We 5 | tried to imitate the action API of the move pase package to ensure its compatibility. 6 | 7 | ## Usage 8 | ``` 9 | roslaunch deep_motion_planner deep_motion_planner.launch 10 | ``` 11 | This will start the deep motion planner node and initialize it with a simple trained model. 12 | -------------------------------------------------------------------------------- /deep_motion_planner/launch/deep_motion_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /deep_motion_planner/launch/deep_motion_planner_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /deep_motion_planner/models/model.pb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_motion_planner/models/model.pb -------------------------------------------------------------------------------- /deep_motion_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | deep_motion_planner 3 | 0.0.0 4 | Package to execute a deep neural network for navigation 5 | michael 6 | 7 | TODO 8 | 9 | catkin 10 | catkin_simple 11 | rospy 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /deep_motion_planner/python/deep_motion_planner/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/deep_motion_planner/python/deep_motion_planner/__init__.py -------------------------------------------------------------------------------- /deep_motion_planner/python/deep_motion_planner/tensorflow_wrapper.py: -------------------------------------------------------------------------------- 1 | 2 | from tensorflow.python.platform import gfile 3 | import tensorflow as tf 4 | import os 5 | import sys 6 | import numpy as np 7 | 8 | class TensorflowWrapper(): 9 | """ 10 | The class is used to load a pretrained tensorflow model and 11 | use it on live sensor data 12 | """ 13 | def __init__(self, storage_path, protobuf_file='graph.pb', use_checkpoints=False): 14 | """Initialize a new TensorflowWrapper object 15 | 16 | @param storage_path: The path to the protobuf_file and the snapshots 17 | @type : string 18 | 19 | @param protobuf_file: The protobuf file to load 20 | @type : string 21 | 22 | @param use_checkpoints: If the given protobuf_file does not contain the trained weights, you 23 | can use checkpoint files to initialize the weights 24 | @type : bool 25 | 26 | """ 27 | # Load the graph definition from a binary protobuf file 28 | with gfile.FastGFile(os.path.join(storage_path, protobuf_file),'rb') as f: 29 | graph_def = tf.GraphDef() 30 | graph_def.ParseFromString(f.read()) 31 | tf.import_graph_def(graph_def, name='') 32 | 33 | print('Loaded protobuf file: {}'.format(os.path.join(storage_path, protobuf_file))) 34 | 35 | self.sess = tf.Session() 36 | 37 | # If the protobuf file does not contain the trained weights, you can load them from 38 | # a checkpoint file in the storage_path folder 39 | if use_checkpoints: 40 | # Get all checkpoints 41 | ckpt = tf.train.get_checkpoint_state(storage_path) 42 | if ckpt and ckpt.model_checkpoint_path: 43 | # Open the model with the highest step count (the last snapshot) 44 | print('Found checkpoints: \n{}'.format(ckpt)) 45 | print('Load: {}'.format(ckpt.model_checkpoint_path)) 46 | self.sess.run(['save/restore_all'], {'save/Const:0': os.path.join(storage_path, 47 | ckpt.model_checkpoint_path)}) 48 | else: 49 | print('No checkpoint files in folder: {}'.format(storage_path)) 50 | 51 | def __enter__(self): 52 | return self 53 | 54 | def __exit__(self, exc_type, exc_value, traceback): 55 | # Make sure to close the session and clena up all the used resources 56 | self.sess.close() 57 | 58 | def inference(self, data): 59 | """ 60 | Take the given data and perform the model inference on it 61 | """ 62 | feed_dict = {'data_input:0': [data]} 63 | 64 | prediction = self.sess.run(['model_inference:0'], feed_dict=feed_dict)[0] 65 | 66 | return (prediction[0,0], prediction[0,1]) 67 | 68 | -------------------------------------------------------------------------------- /deep_motion_planner/python/deep_motion_planner/util/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | import tf 5 | import rospy 6 | import numpy as np 7 | 8 | def compute_relative_target_raw(current_pose, target_pose): 9 | """ 10 | Computes the relative target pose which has to be fed to the network as an input. 11 | Both target pose and current_pose have to be in the same coordinate frame (gloabl map). 12 | """ 13 | # Compute the relative goal position 14 | goal_position_difference = [target_pose.pose.position.x - current_pose.pose.position.x, 15 | target_pose.pose.position.y - current_pose.pose.position.y] 16 | 17 | # Get the current orientation and the goal orientation 18 | current_orientation = current_pose.pose.orientation 19 | p = [current_orientation.x, current_orientation.y, current_orientation.z, current_orientation.w] 20 | goal_orientation = target_pose.pose.orientation 21 | q = [goal_orientation.x, goal_orientation.y, goal_orientation.z, goal_orientation.w] 22 | 23 | # Rotate the relative goal position into the base frame (robot frame) 24 | goal_position_base_frame = tf.transformations.quaternion_multiply(tf.transformations.quaternion_inverse(p), 25 | tf.transformations.quaternion_multiply([goal_position_difference[0], 26 | goal_position_difference[1], 27 | 0, 28 | 0], 29 | p)) 30 | 31 | # Compute the difference to the goal orientation 32 | orientation_to_target = tf.transformations.quaternion_multiply(q, tf.transformations.quaternion_inverse(p)) 33 | yaw = tf.transformations.euler_from_quaternion(orientation_to_target)[2] 34 | 35 | return (goal_position_base_frame[0], -goal_position_base_frame[1], yaw) 36 | 37 | 38 | def adjust_laser_scans_to_model(raw_scan_ranges, scan_stride, n_scans_output, perception_radius=10.0): 39 | scans = list(raw_scan_ranges[::scan_stride]) 40 | cut_n_elements = (len(scans) - n_scans_output) // 2 41 | cropped_scans = scans 42 | if cut_n_elements > 0: 43 | rospy.logdebug("Cutting input vector by {0} elements on each side.".format(cut_n_elements)) 44 | cropped_scans = scans[cut_n_elements:-cut_n_elements] 45 | if len(cropped_scans)==n_scans_output+1: 46 | rospy.logdebug("Input vector has one scan too much. Cutting off last one.") 47 | cropped_scans = cropped_scans[0:-1] 48 | 49 | return np.minimum(cropped_scans, perception_radius).tolist() 50 | -------------------------------------------------------------------------------- /deep_motion_planner/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | # fetch values from package.xml 5 | setup_args = generate_distutils_setup( 6 | packages=['deep_motion_planner'], 7 | package_dir={'':'python'}, 8 | scripts=[]) 9 | 10 | setup(**setup_args) -------------------------------------------------------------------------------- /deep_motion_planner/src/deep_motion_planner_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | 5 | from deep_motion_planner.deep_motion_planner import DeepMotionPlanner 6 | 7 | def main(): 8 | 9 | rospy.init_node('deep_motion_planner') 10 | 11 | with DeepMotionPlanner() as planner: 12 | rospy.spin() 13 | 14 | if __name__ == "__main__": 15 | main() 16 | -------------------------------------------------------------------------------- /mission_control/README.md: -------------------------------------------------------------------------------- 1 | # mission_control 2 | 3 | The mission control package loads a mission from a .txt file, parses the given file and 4 | executes the mission. The node processes the mission step-by-step and interacts with other ROS 5 | nodes to actually execute the command on the robot itself. For example, waypoint commands are 6 | forwarded to the navigation stack. 7 | 8 | ## Usage 9 | ``` 10 | rosrun mission_control mission_control_node.py _mission_file:=src/deep_motion_planning/mission_control/missions/rooms.txt 11 | ``` 12 | 13 | ## Parameter 14 | ### ~mission_file (default: None) 15 | Defines the path to the mission file that will be parsed and executed. See [Mission Definition](#commands) for more details on its content. 16 | 17 | ### ~command_timeout (default: 360) 18 | Specify the maximum time in seconds to finish the execution of a single command. If the timeout is 19 | reached, we reset the simulation, abort the data capture and retry/skip the command. 20 | 21 | ## Mission Definition 22 | Missions are defined in a plain text file that is parsed line by line. Each line represents one 23 | command. Lines that start with # are seen as comment and not parsed as command. 24 | 25 | The following commands are currently supported in the parser and mission execution: 26 | 27 | ### Waypoint 28 | ``` 29 | wp: 2.0 4.0 90.0 30 | ``` 31 | Waypoint command start with followed by three floats (x y yaw) specifying the position in 32 | meters and the orientation in degrees. The reference frame depends on the navigation stack, but 33 | should be the '/map' frame. If the execution of the waypoint is not successful, we skip it and 34 | continue with the next command in the mission file. 35 | 36 | ### Random Waypoints 37 | ``` 38 | rd: 100 -5.0 5.0 -5.0 5.0 39 | ``` 40 | The random waypoints command samples N waypoints from a uniform distribution. The five floats after 41 | define the number of waypoints N and the range in x and y direction, the waypoints are sampled 42 | from (N min_x max_x min_y max_y). The orientation is uniformly sampled from 0 to 360 degree. If the 43 | execution of the sampled waypoint is not successful, we resample it and try again. 44 | -------------------------------------------------------------------------------- /mission_control/launch/demo_mission.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /mission_control/launch/generate_mission.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /mission_control/launch/lab_mission.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /mission_control/launch/office_mission.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /mission_control/launch/rooms_mission.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /mission_control/launch/shapes_mission.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /mission_control/launch/test_mission.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /mission_control/missions/demo.txt: -------------------------------------------------------------------------------- 1 | wp: 2.0 4.0 90.0 2 | wp: 2.0 4.0 270.0 3 | wp: 3.5 -1.0 180.0 4 | wp: -4.0 -3.0 0.0 5 | wp: 0.0 2.0 180.0 6 | wp: -4.0 2.0 90.0 7 | wp: 1.5 -4.24 0.0 8 | -------------------------------------------------------------------------------- /mission_control/missions/generate.txt: -------------------------------------------------------------------------------- 1 | rd: 3000 -5.0 5.0 -5.0 5.0 2 | -------------------------------------------------------------------------------- /mission_control/missions/lab.txt: -------------------------------------------------------------------------------- 1 | wp: 7.0 2.0 90.0 2 | wp: 4.0 2.0 270.0 3 | wp: 6.0 5.0 0.0 4 | wp: 18.0 6.5 305.0 5 | wp: 19.0 4.0 270.0 6 | wp: 18.0 -1.0 90.0 7 | wp: 6.0 -3.0 270.0 8 | wp: 4.5 -5.25 90.0 9 | wp: 0.0 -2.0 180.0 10 | wp: 7.0 4.0 270.0 11 | wp: -10.0 -2.0 0.0 12 | wp: 1.0 -1.5 0.0 13 | wp: 4.0 -4.0 270.0 14 | wp: 7.0 -6.0 180.0 15 | wp: 7.0 -3.75 0.0 16 | wp: 17.0 -1.0 0.0 17 | wp: 22.0 2.0 180.0 18 | wp: 17.0 6.5 180.0 19 | wp: 5.0 5.0 270.0 20 | wp: 5.0 -2.0 0.0 21 | -------------------------------------------------------------------------------- /mission_control/missions/office.txt: -------------------------------------------------------------------------------- 1 | wp: 4.0 -1.5 90.0 2 | wp: 1.0 4.5 180.0 3 | wp: -2.5 1.5 90.0 4 | wp: -4.0 4.0 270.0 5 | wp: -4.25 -1.0 270.0 6 | wp: 4.5 -2.5 270.0 7 | wp: -1.0 -4.0 180.0 8 | wp: -4.0 4.5 0.0 9 | wp: -2.0 0.0 45.0 10 | wp: 4.0 2.0 0.0 11 | -------------------------------------------------------------------------------- /mission_control/missions/rooms.txt: -------------------------------------------------------------------------------- 1 | rd: 100 -5.0 5.0 -5.0 5.0 2 | wp: 2.0 4.0 90.0 3 | wp: 2.0 4.0 270.0 4 | wp: 2.0 3.0 360.0 5 | wp: -3.0 3.0 180.0 6 | wp: -3.0 -2.0 0.0 7 | wp: 1.0 -1.0 0.0 8 | -------------------------------------------------------------------------------- /mission_control/missions/shapes.txt: -------------------------------------------------------------------------------- 1 | wp: 2.0 4.0 90.0 2 | wp: 2.0 4.0 270.0 3 | wp: 3.0 -1.0 360.0 4 | wp: -4.0 3.0 180.0 5 | wp: -4.0 -2.0 0.0 6 | wp: 2.0 3.0 0.0 7 | wp: 1.0 -4.0 0.0 8 | -------------------------------------------------------------------------------- /mission_control/missions/test.txt: -------------------------------------------------------------------------------- 1 | wp: 2.0 4.0 90.0 2 | wp: 2.0 4.0 270.0 3 | wp: 2.0 3.0 360.0 4 | wp: -3.0 3.0 180.0 5 | wp: -3.0 -2.0 0.0 6 | rd: 100 -4.5 4.5 -4.5 4.5 7 | -------------------------------------------------------------------------------- /mission_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mission_control 4 | 0.0.0 5 | The mission_control package 6 | 7 | 8 | 9 | 10 | michael 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | rospy 44 | rospy 45 | actionlib 46 | actionlib 47 | move_base_msgs 48 | move_base_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /mission_control/src/mission_control_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | from mission_control import MissionControl 5 | 6 | def main(): 7 | 8 | rospy.init_node('mission_control') 9 | 10 | mission = MissionControl() 11 | 12 | rospy.spin() 13 | 14 | if __name__ == "__main__": 15 | main() 16 | -------------------------------------------------------------------------------- /mission_control/src/mission_file_parser.py: -------------------------------------------------------------------------------- 1 | 2 | class MissionFileParser(): 3 | 4 | """ 5 | Define a parser for our custom mission syntax 6 | """ 7 | 8 | def __init__(self, mission_file): 9 | # Parse the given file 10 | self.mission = self.__parse_file__(mission_file) 11 | 12 | def get_mission(self): 13 | """ 14 | Return the parsed mission as python list 15 | """ 16 | return self.mission 17 | 18 | def __parse_file__(self, mission_file): 19 | """ 20 | Parse the given mission file 21 | """ 22 | mission = list() 23 | 24 | # Open the file and read it line by line 25 | with open(mission_file, 'r') as file: 26 | 27 | for l in file: 28 | l = l.strip() # Remove whitespaces 29 | 30 | # First character is a # 31 | # Do not parse empty lines or comments 32 | if len(l) == 0 or l[0] == '#': 33 | continue 34 | 35 | parsed = self.__parse_line__(l) 36 | if not parsed == None and not parsed[1] == None: 37 | mission.append(parsed) 38 | else: 39 | print('Error while parsing line: {}'.format(l)) 40 | 41 | return mission 42 | 43 | def __parse_line__(self, line): 44 | """ 45 | Parse a single line as one command 46 | """ 47 | line_split = line.split(':') 48 | 49 | if len(line_split) == 2: 50 | # Check which command is defined in the line and call the matching 51 | # command parser with the remainding part of the current line 52 | if line_split[0] == 'wp': 53 | return ('wp', self.__parse_waypoint__(line_split[1].strip())) 54 | elif line_split[0] == 'rd': 55 | return ('rd', self.__parse_random__(line_split[1].strip())) 56 | elif line_split[0] == 'cmd': 57 | return ('cmd', self.__parse_command__(line_split[1].strip())) 58 | else: 59 | print('Command not supported: {}'.format(line_split[0])) 60 | return None 61 | else: 62 | print('Line has no command') 63 | return None 64 | 65 | def __parse_waypoint__(self, input): 66 | """ 67 | Parse the input string as single waypoint 68 | 69 | Returns: 70 | None if parsing fails and (x,y,yaw) otherwise 71 | """ 72 | waypoint = [float(e) for e in input.split(' ')] 73 | if len(waypoint) == 3: 74 | return waypoint 75 | else: 76 | print('Waypoint has not 3 elements: {} given'.format(len(waypoint))) 77 | return None 78 | 79 | def __parse_random__(self, input): 80 | """ 81 | Parse the input string as random waypoint command 82 | 83 | Returns: 84 | None if parsing fails and (N,min_x,max_x,min_y,max_y) otherwise 85 | """ 86 | randomizer = [float(e) for e in input.split(' ')] 87 | if len(randomizer) == 5: 88 | return randomizer 89 | else: 90 | print('Random command has not 5 elements: {} given'.format(len(randomizer))) 91 | return None 92 | 93 | def __parse_command__(self, input): 94 | """ 95 | Parse an arbitrary command 96 | 97 | Returns: 98 | The input string 99 | """ 100 | return input.split('\n')[0] 101 | -------------------------------------------------------------------------------- /mission_control/src/write_mission.py: -------------------------------------------------------------------------------- 1 | import os 2 | import rospy 3 | import tf 4 | import rospkg 5 | import math 6 | from geometry_msgs.msg import PoseStamped 7 | from sensor_msgs.msg import Joy 8 | from datetime import * 9 | 10 | 11 | class WriteMission(): 12 | 13 | def __init__(self): 14 | """ 15 | Drive with joystick and select targets for autonomous mission. 16 | Requires robot to be localized. 17 | """ 18 | print("Press green (A) button to add a target to the list or red (B) button to delete the last recorded target from the list.") 19 | storage_path = rospy.get_param('~storage_path', default=rospkg.RosPack().get_path('mission_control') + "/missions/") 20 | date_str = datetime.strftime(datetime.now(), '%Y-%m-%d_%H-%M-%S') 21 | self.storage_file = os.path.join(storage_path, date_str) + '.txt' 22 | self.transform_listener = tf.TransformListener() 23 | self.joystick_sub = rospy.Subscriber('/joy', Joy, self.__save_position_callback__) 24 | self.target_poses = [] 25 | 26 | def __save_targets_to_file__(self): 27 | with open(self.storage_file, 'w') as file: 28 | for pose in self.target_poses: 29 | euler_angles = tf.transformations.euler_from_quaternion([pose.pose.orientation.x, 30 | pose.pose.orientation.y, 31 | pose.pose.orientation.z, 32 | pose.pose.orientation.w]) 33 | heading_degree = euler_angles[2]*180.0 / math.pi 34 | file.write('wp: {0} {1} {2}\n'.format(pose.pose.position.x, pose.pose.position.y, heading_degree)) 35 | 36 | def __remove_last_target_from_file(self): 37 | os.system("sed -i '$ d' " + self.storage_file) 38 | 39 | 40 | def __save_position_callback__(self, data): 41 | if data.buttons[0] == 1: 42 | (base_position,base_orientation) = self.transform_listener.lookupTransform('/map', '/base_link', rospy.Time()) 43 | current_pose = PoseStamped() 44 | current_pose.pose.position.x = base_position[0] 45 | current_pose.pose.position.y = base_position[1] 46 | current_pose.pose.position.z = base_position[2] 47 | current_pose.pose.orientation.x = base_orientation[0] 48 | current_pose.pose.orientation.y = base_orientation[1] 49 | current_pose.pose.orientation.z = base_orientation[2] 50 | current_pose.pose.orientation.w = base_orientation[3] 51 | self.target_poses.append(current_pose) 52 | rospy.loginfo('Saving pose with (x,y,phi)=({0}, {1}, {2})'.format(base_position[0], base_position[1], 53 | tf.transformations.euler_from_quaternion(base_orientation)[2])) 54 | rospy.loginfo('Target stack now has size {0}'.format(len(self.target_poses))) 55 | self.__save_targets_to_file__() 56 | 57 | elif data.buttons[1] == 1: 58 | rospy.loginfo('Removing latest target') 59 | if len(self.target_poses) >= 1: 60 | self.target_poses.pop() 61 | self.__remove_last_target_from_file() 62 | rospy.loginfo('Target stack now has size {0}'.format(len(self.target_poses))) 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /mission_control/src/write_mission_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | from write_mission import WriteMission 5 | 6 | def main(): 7 | 8 | rospy.init_node('write_mission') 9 | 10 | write_mission = WriteMission() 11 | 12 | rospy.spin() 13 | 14 | if __name__ == "__main__": 15 | main() -------------------------------------------------------------------------------- /planner_comparison/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(planner_comparison) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | catkin_python_setup() -------------------------------------------------------------------------------- /planner_comparison/launch/core_stage.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /planner_comparison/launch/planner_comparison.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /planner_comparison/launch/world_comparison.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /planner_comparison/package.xml: -------------------------------------------------------------------------------- 1 | 2 | planner_comparison 3 | 0.0.0 4 | The planner_comparison package 5 | pfmark 6 | TODO 7 | 8 | catkin 9 | catkin_simple 10 | rospy 11 | 12 | -------------------------------------------------------------------------------- /planner_comparison/python/planner_comparison/PlannerComparison.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from geometry_msgs.msg import Twist, PoseStamped, Point, Quaternion 3 | from sensor_msgs.msg import LaserScan 4 | from nav_msgs.msg import Path 5 | 6 | import logging 7 | import tf 8 | import math 9 | import numpy as np 10 | 11 | 12 | def parse_args(): 13 | """Parse input arguments.""" 14 | parser = argparse.ArgumentParser(description='Compare different motion planners') 15 | 16 | parser.add_argument('-m', '--mail', help='Send an email when training finishes', 17 | action='store_true') 18 | args = parser.parse_args() 19 | 20 | return args 21 | 22 | 23 | class PlannerComparison(): 24 | """ 25 | Compare the performance of different local motion planners (with local knowledge) 26 | compared to the global planner. 27 | """ 28 | def __init__(self): 29 | 30 | self.current_pose = PoseStamped() 31 | self.ros_plan_cmd = None 32 | self.deep_plan_cmd = None 33 | 34 | # ROS params 35 | self.executed_plan = rospy.get_param('~executed_plan') 36 | rospy.loginfo("Executing plan that comes from %s".format(self.executed_plan)) 37 | 38 | # ROS topics 39 | self.cmd_ros_plan_sub = rospy.Subscriber('ros_planner/cmd_vel', Twist, self.__callback_ros_plan_cmd__) 40 | self.cmd_deep_plan_sub = rospy.Subscriber('deep_planner/cmd_vel', Twist, self.__callback_deep_plan__) 41 | self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) 42 | self.deep_plan_pub = rospy.Publisher('/deep_planner/path', Path, queue_size=1) 43 | 44 | # Transformations 45 | self.tf_listener = tf.TransformListener() 46 | 47 | 48 | def __callback_ros_plan_cmd__(self, data): 49 | self.ros_plan_cmd = data 50 | if self.executed_plan == 'ros': 51 | self.__publish_vel_cmd__(data) 52 | self.__callback_current_pose__() 53 | 54 | def __callback_deep_plan__(self, data): 55 | self.deep_plan_cmd = data 56 | self.__publish_path_from_vel_cmd__(data, rospy.Duration(1.7)) 57 | if self.executed_plan == 'deep': 58 | self.__publish_vel_cmd__(data) 59 | self.__callback_current_pose__() 60 | 61 | def __callback_current_pose__(self): 62 | """ 63 | Set the current pose of the robot (robot coordinate frame vs. odom frame) 64 | """ 65 | try: 66 | (trans,rot) = self.tf_listener.lookupTransform('odom', 'base_footprint', rospy.Time(0)) 67 | self.current_pose.header.stamp = self.tf_listener.getLatestCommonTime('odom', 'base_footprint') 68 | self.current_pose.pose.position = Point(*trans) 69 | self.current_pose.pose.orientation = Quaternion(*rot) 70 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 71 | rospy.logerr('Transformation does not exist.') 72 | 73 | def __publish_vel_cmd__(self, cmd_vel): 74 | """ 75 | Publish velocity command under topic "/cmd_vel" (which the simulation listens to) 76 | """ 77 | cmd = Twist() 78 | cmd.linear.x = cmd_vel.linear.x 79 | cmd.angular.z = cmd_vel.angular.z 80 | self.cmd_pub.publish(cmd) 81 | 82 | 83 | def __publish_path_from_vel_cmd__(self, cmd_vel, sim_time, dt=rospy.Duration(0.05)): 84 | """ 85 | Compute plan from velocity command (assuming constant translational and rotational velocity) 86 | for the succeeding time interval specified with sim_time 87 | """ 88 | path = Path() 89 | start_time = self.current_pose.header.stamp 90 | final_time = start_time + sim_time 91 | path.header.stamp = start_time 92 | path.header.frame_id = 'odom' 93 | path.poses.append(self.current_pose) 94 | 95 | t = start_time 96 | trans_vel = cmd_vel.linear.x 97 | rot_vel = cmd_vel.angular.z 98 | while t < start_time + sim_time: 99 | t += dt 100 | new_pose = PoseStamped() 101 | new_pose.header.stamp = t 102 | quat = path.poses[-1].pose.orientation 103 | euler = tf.transformations.euler_from_quaternion([quat.x, quat.y, quat.z, quat.w]) 104 | current_yaw = euler[2] 105 | new_yaw = current_yaw + dt.to_sec()*rot_vel 106 | yaw_mean = (current_yaw + new_yaw) / 2.0 107 | old_pos = path.poses[-1].pose.position 108 | new_pose.pose.position = Point(old_pos.x + trans_vel*math.cos(yaw_mean)*dt.to_sec(), 109 | old_pos.y + trans_vel*math.sin(yaw_mean)*dt.to_sec(), 110 | old_pos.z + 0.0) 111 | new_pose.pose.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0.0, 0.0, new_yaw)) 112 | path.poses.append(new_pose) 113 | 114 | self.deep_plan_pub.publish(path) 115 | -------------------------------------------------------------------------------- /planner_comparison/python/planner_comparison/RosbagInterface.py: -------------------------------------------------------------------------------- 1 | import rosbag 2 | import pickle 3 | import os 4 | 5 | # Messages 6 | from geometry_msgs.msg import Twist, PoseStamped, Point, Quaternion 7 | from sensor_msgs.msg import LaserScan 8 | from nav_msgs.msg import Path, Odometry 9 | from move_base_msgs.msg import MoveBaseActionFeedback 10 | from std_msgs.msg import Empty 11 | from actionlib_msgs.msg import GoalStatus 12 | 13 | from time_msg_container import * 14 | 15 | class RosbagInterface(): 16 | 17 | def __init__(self, log_path): 18 | self.log_path = log_path 19 | self.bag = rosbag.Bag(self.log_path) 20 | self.msg_container = {} 21 | self.msg_container = self.load_messages() 22 | 23 | def load_messages(self, topics=None): 24 | if topics is None: 25 | # Get ros planner commands 26 | vel_cmd_msgs = TimeMsgContainer() 27 | for topic, msg, t in self.bag.read_messages(topics=['/cmd_vel']): 28 | vel_cmd_msgs.times.append(t) 29 | vel_cmd_msgs.msgs.append(msg) 30 | self.msg_container['vel_cmd'] = vel_cmd_msgs 31 | if len(vel_cmd_msgs) == 0: 32 | vel_cmd_msgs = TimeMsgContainer() 33 | for topic, msg, t in self.bag.read_messages(topics=['/cmd_vel_mux/input/navi']): 34 | vel_cmd_msgs.times.append(t) 35 | vel_cmd_msgs.msgs.append(msg) 36 | self.msg_container['vel_cmd'] = vel_cmd_msgs 37 | 38 | # Get scans 39 | scan_msgs = TimeMsgContainer() 40 | for topic, msg, t in self.bag.read_messages(topics=['/base_scan']): 41 | scan_msgs.times.append(t) 42 | scan_msgs.msgs.append(msg) 43 | self.msg_container['scan'] = scan_msgs 44 | 45 | # Get published goal positions 46 | goal_msgs = TimeMsgContainer() 47 | for topic, msg, t in self.bag.read_messages(topics=['/move_base/current_goal']): 48 | goal_msgs.times.append(t) 49 | goal_msgs.msgs.append(msg) 50 | if len(goal_msgs) > 0: 51 | self.msg_container['goal'] = goal_msgs 52 | else: 53 | goal_msgs = TimeMsgContainer() 54 | for topic, msg, t in self.bag.read_messages(topics=['/deep_move_base/goal']): 55 | goal_msgs.times.append(t) 56 | goal_msgs.msgs.append(msg) 57 | self.msg_container['goal'] = goal_msgs 58 | 59 | # Get position/localization messages 60 | loc_msgs = TimeMsgContainer() 61 | for topic, msg, t in self.bag.read_messages(topics=['/amcl_pose']): 62 | loc_msgs.times.append(t) 63 | loc_msgs.msgs.append(msg) 64 | self.msg_container['loc'] = loc_msgs 65 | if len(loc_msgs) == 0: 66 | loc_msgs = TimeMsgContainer() 67 | for topic, msg, t in self.bag.read_messages(topics=['/move_base/feedback']): 68 | loc_msgs.times.append(t) 69 | loc_msgs.msgs.append(msg) 70 | self.msg_container['loc'] = loc_msgs 71 | 72 | # Odometry data 73 | odom_msgs = TimeMsgContainer() 74 | for topic, msg, t in self.bag.read_messages(topics=['/base_pose_ground_truth']): 75 | odom_msgs.times.append(t) 76 | odom_msgs.msgs.append(msg) 77 | self.msg_container['odom'] = odom_msgs 78 | if len(odom_msgs) == 0: 79 | odom_msgs = TimeMsgContainer() 80 | for topic, msg, t in self.bag.read_messages(topics=['/odom']): 81 | odom_msgs.times.append(t) 82 | odom_msgs.msgs.append(msg) 83 | self.msg_container['odom'] = odom_msgs 84 | 85 | # Mission start 86 | start_msgs = TimeMsgContainer() 87 | for topic, msg, t in self.bag.read_messages(topics=['/start']): 88 | start_msgs.times.append(t) 89 | start_msgs.msgs.append(msg) 90 | self.msg_container['start'] = start_msgs 91 | 92 | # Mission stop 93 | stop_msgs = TimeMsgContainer() 94 | for topic, msg, t in self.bag.read_messages(topics=['/stop']): 95 | stop_msgs.times.append(t) 96 | stop_msgs.msgs.append(msg) 97 | self.msg_container['stop'] = stop_msgs 98 | 99 | # Joystick 100 | joy_msgs = TimeMsgContainer() 101 | for topic, msg, t in self.bag.read_messages(topics=['/joy']): 102 | joy_msgs.times.append(t) 103 | joy_msgs.msgs.append(msg) 104 | self.msg_container['joy'] = joy_msgs 105 | 106 | # Goal status messages 107 | goal_status_msgs = TimeMsgContainer() 108 | for topic, msg, t in self.bag.read_messages(topics=['/move_base/status']): 109 | goal_status_msgs.times.append(t) 110 | goal_status_msgs.msgs.append(msg) 111 | self.msg_container['goal_status'] = goal_status_msgs 112 | 113 | # Map msg 114 | map_msgs = TimeMsgContainer() 115 | for topic, msg, t in self.bag.read_messages(topics=['/map']): 116 | map_msgs.times.append(t) 117 | map_msgs.msgs.append(msg) 118 | self.msg_container['map'] = map_msgs 119 | else: 120 | for topic in topics: 121 | time_msg_container = TimeMsgContainer() 122 | for topic, msg, t in self.bag.read_messages(topics=[topic]): 123 | time_msg_container.times.append(t) 124 | time_msg_container.msgs.append(msg) 125 | self.msg_container[topic] = time_msg_container 126 | 127 | return self.msg_container 128 | 129 | def get_topics(self): 130 | return self.msg_container.keys() 131 | 132 | def save_container(self, filename, path=''): 133 | pickle.dump(self.msg_container, open(os.path.join(path, filename), 'wb')) -------------------------------------------------------------------------------- /planner_comparison/python/planner_comparison/TimeMsgContainer.py: -------------------------------------------------------------------------------- 1 | import bisect 2 | 3 | class TimeMsgContainer(): 4 | 5 | def __init__(self): 6 | self.times = [] 7 | self.msgs = [] 8 | 9 | def __len__(self): 10 | if (len(self.times) == len(self.msgs)): 11 | return len(self.times) 12 | else: 13 | raise Exception("There's something wrong with the vector lengths.") 14 | 15 | def get_data_for_interval(self, start_time, end_time): 16 | start_time = max(start_time, self.times[0]) 17 | end_time = min(end_time, self.times[-1]) 18 | start_idx = bisect.bisect(self.times, start_time) 19 | end_idx = bisect.bisect(self.times, end_time) 20 | time_msg_out = TimeMsgContainer() 21 | time_msg_out.times = self.times[start_idx:end_idx] 22 | time_msg_out.msgs = self.msgs[start_idx:end_idx] 23 | return time_msg_out 24 | 25 | def get_next_msg(self, query_time): 26 | return self.msgs[bisect.bisect(self.times, query_time)] 27 | 28 | def get_previous_msg(self, query_time): 29 | return self.msgs[bisect.bisect(self.times, query_time) - 1] -------------------------------------------------------------------------------- /planner_comparison/python/planner_comparison/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/planner_comparison/python/planner_comparison/__init__.py -------------------------------------------------------------------------------- /planner_comparison/python/planner_comparison/util/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import pylab as pl 4 | from planner_comparison.plan_scoring import * 5 | from bisect import * 6 | 7 | def compute_detailed_cost_sum(mission_vec): 8 | detailed_cost = mission_vec[0].compute_cost()[1] 9 | for m in mission_vec[1:]: 10 | for k in detailed_cost.keys(): 11 | detailed_cost[k] += m.compute_cost()[1][k] 12 | return detailed_cost 13 | 14 | def joystick_trigger_times(mission): 15 | trigger_times = [] 16 | for idx, msg in enumerate(mission.joy_msgs.msgs): 17 | if idx > 0: 18 | if msg.buttons[4] == 1 and mission.joy_msgs.msgs[idx-1].buttons[4] == 0: 19 | trigger_times.append(mission.joy_msgs.times[idx]) 20 | else: 21 | if msg.buttons[4] == 1: # active in the beginning 22 | trigger_times.append(mission.joy_msgs.times[idx]) 23 | return trigger_times 24 | 25 | def find_next_joystick_release_time(trigger_time, mission): 26 | start_idx = bisect(mission.joy_msgs.times, trigger_time) 27 | for idx in range(start_idx, len(mission.joy_msgs)-1): 28 | if mission.joy_msgs.msgs[idx+1].buttons[4] == 1 and mission.joy_msgs.msgs[idx+1].buttons[4] == 0: 29 | return mission.joy_msgs.times[idx] 30 | return mission.joy_msgs.times[-1] 31 | 32 | def joystick_active_time_intervals(mission): 33 | time_intervals = [] 34 | trigger_times = joystick_trigger_times(mission) 35 | for t_time in trigger_times: 36 | time_intervals.append((t_time, find_next_joystick_release_time(t_time, mission))) 37 | return time_intervals 38 | 39 | def get_joystick_trajectories(mission): 40 | joystick_intervals = joystick_active_time_intervals(mission) 41 | trajectories = [] 42 | for interval in joystick_intervals: 43 | trajectories.append(mission.get_trajectory_for_time_interval(interval)) 44 | return trajectories 45 | 46 | def plot_joystick_interference(ax, mission, color='r', alpha=1.0, linewidth=1.0 ): 47 | trajectories = get_joystick_trajectories(mission) 48 | th = [] 49 | for traj in trajectories: 50 | # t = ax.plot(traj[0,:], traj[1,:], color=color, alpha=alpha, linewidth=1, marker='o', markersize=2, mew=0.1, mec='none', mfc=color) 51 | t = ax.plot(traj[0,:], traj[1,:], color=color, alpha=alpha, linewidth=1) 52 | th.append(t) 53 | return th 54 | 55 | 56 | def plot_mission(ax, mission, id=None, color='b', plot_numbers=False, plot_joystick=False, fontsize=9, alpha=1.0, linewidth=1.0, shift_direction=None, shift_dist=0.2): 57 | traj = mission.get_trajectory() 58 | 59 | try: 60 | goal = mission.goal.pose.position 61 | except AttributeError: 62 | goal = mission.goal.goal.target_pose.pose.position 63 | th = ax.plot(traj[0,:], traj[1,:], color=color, alpha=alpha) 64 | ax.plot(goal.x, goal.y, marker='o', mfc='r', mec='r') 65 | if id==0 and plot_numbers: 66 | ax.plot(traj[0,0], traj[1,0], marker='o', mfc='r', mec='r') 67 | if plot_numbers: 68 | shift_dist = shift_dist 69 | shift_dict = {'l': (-shift_dist, 0), 'r': (shift_dist, 0), 'u':(0, shift_dist), 'd':(0, -shift_dist), 0:(0,0), 70 | 'ld': (-shift_dist, -shift_dist), 'lu':(-shift_dist, shift_dist), 'rd':(shift_dist, -shift_dist), 'ru':(shift_dist, shift_dist)} 71 | shift_position = (0, 0) 72 | if shift_direction: 73 | shift_position = shift_dict[shift_direction] 74 | if id is not None: 75 | ax.text(traj[0,-1] + shift_position[0], traj[1,-1] + shift_position[1], str(id), color='r', fontsize=fontsize) 76 | return th 77 | 78 | def plot_error_bars(ax, cost, color='b', shift=0.0, bar_width=0.2): 79 | bars = [] 80 | for idx,key in enumerate(cost.keys()): 81 | bar = ax.bar(idx+1+shift, cost[key], width=bar_width, color=color, align='center') 82 | bars.append(bar) 83 | return bars 84 | 85 | def plot_relative_error_bars(ax, rel_cost, colors=['g', 'k'], bar_width=0.2): 86 | bars = [] 87 | for idx,key in enumerate(rel_cost.keys()): 88 | color = colors[0] if rel_cost[key]>0 else colors[1] 89 | bar = ax.bar(idx+1, 100*rel_cost[key], width=bar_width, color=color, align='center') 90 | bars.append(bar) 91 | ax.plot([-100, 100], [0, 0], color='k') 92 | ax.set_xlim(0, len(rel_cost)+1) 93 | x_tick_pos = np.arange(len(rel_cost)) + 1 94 | pl.xticks(x_tick_pos, rel_cost.keys(), rotation=35, fontsize=7) 95 | return bars 96 | 97 | def get_complete_missions(missions): 98 | complete = [] 99 | for m in missions: 100 | if len(joystick_active_time_intervals(m)) == 0: 101 | complete.append(True) 102 | else: 103 | complete.append(False) 104 | 105 | return complete 106 | 107 | 108 | def compute_joystick_distance(mission): 109 | joystick_distance = 0.0 110 | joystick_trajs = get_joystick_trajectories(mission) 111 | 112 | for traj in joystick_trajs: 113 | if traj.shape[1] > 1: 114 | pos_old = traj[:,0] 115 | for ii in range(1, len(traj)): 116 | pos_new = traj[:,ii] 117 | joystick_distance += np.linalg.norm(pos_new-pos_old) 118 | else: 119 | joystick_distance += 0.03 120 | 121 | return joystick_distance 122 | 123 | def compute_autonomous_percent_mission(mission): 124 | if absolute_distance > 0: 125 | return 1.0 - compute_joystick_distance(mission) / mission.distance() 126 | else: 127 | return 1.0 128 | 129 | -------------------------------------------------------------------------------- /planner_comparison/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | # fetch values from package.xml 5 | setup_args = generate_distutils_setup( 6 | packages=['planner_comparison'], 7 | package_dir={'':'python'}, 8 | scripts=[]) 9 | 10 | setup(**setup_args) -------------------------------------------------------------------------------- /planner_comparison/src/compare_maps_for_model.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | import rospy 3 | import rospkg 4 | import os 5 | import pylab as pl 6 | import numpy as np 7 | 8 | from planner_comparison.time_msg_container import * 9 | 10 | # Messages 11 | from geometry_msgs.msg import Twist, PoseStamped, Point, Quaternion 12 | from sensor_msgs.msg import LaserScan 13 | from nav_msgs.msg import Path 14 | from move_base_msgs.msg import MoveBaseActionFeedback 15 | 16 | class ErrorValues(): 17 | 18 | def __init__(self, name): 19 | self.rot_mean_training = 0.0 20 | self.trans_mean_training = 0.0 21 | self.rot_mean_eval = 0.0 22 | self.trans_mean_eval = 0.0 23 | self.rot_std_training = 0.0 24 | self.trans_std_training = 0.0 25 | self.rot_std_eval = 0.0 26 | self.trans_std_eval = 0.0 27 | self.name = name 28 | 29 | pl.close('all') 30 | figure_path = '/home/pfmark/jade_catkin_ws/src/deep_motion_planning/planner_comparison/data/figures/' 31 | fig_width_pt = 245.71811 # Get this from LaTeX using \showthe\columnwidth 32 | inches_per_pt = 1.0/72.27 # Convert pt to inch 33 | # golden_mean = (np.sqrt(5)-1.0)/2.0 # Aesthetic ratio 34 | fig_width = fig_width_pt*inches_per_pt # width in inches 35 | fig_height = fig_width*0.7 # height in inches 36 | fig_size = [fig_width,fig_height] 37 | fontsize = 9 38 | params = {'backend': 'ps', 39 | 'axes.labelsize': fontsize, 40 | 'text.fontsize': fontsize, 41 | 'title.fontsize': fontsize, 42 | 'legend.fontsize': fontsize, 43 | 'xtick.labelsize': fontsize, 44 | 'ytick.labelsize': fontsize, 45 | 'text.usetex': True, 46 | 'font.family': 'serif', 47 | 'font.serif': 'Computer Modern Roman', 48 | 'figure.figsize': fig_size} 49 | pl.rcParams.update(params) 50 | 51 | data_path = rospkg.RosPack().get_path('planner_comparison') + "/data/" 52 | filenames = ['simple.pkl', 'shapes.pkl', 'office.pkl'] 53 | names = ['training', 'eval_{simple}', 'eval_{complex}'] 54 | 55 | data_storage = {} 56 | for idx, name in enumerate(filenames): 57 | data_storage[names[idx]] = pickle.load(open(data_path + name, 'rb')) 58 | 59 | # Boxplots 60 | box_width = 0.2 61 | box_dist = 0.1 62 | pl.figure('Boxplots error') 63 | ax = pl.subplot(111) 64 | boxplot_data_trans = [] 65 | boxplot_data_rot = [] 66 | boxplot_positions_trans = [] 67 | boxplot_positions_rot = [] 68 | 69 | bps_trans = [] 70 | bps_rot = [] 71 | for idx, data in enumerate(data_storage.values()): 72 | bps_trans.append(ax.boxplot(data['vel_trans_diff'], positions=[idx + 1 - box_width/2 - box_dist/2])) 73 | bps_rot.append(ax.boxplot(data['vel_rot_diff'], positions=[idx + 1 + box_width/2 + box_dist/2])) 74 | 75 | color_fill = 'r' 76 | color_median = 'm' 77 | for bp in bps_trans: 78 | pl.setp(bp['boxes'], color=color_fill, lw=1.0, alpha=1.0) 79 | pl.setp(bp['whiskers'], color=color_fill, ls='-') 80 | pl.setp(bp['fliers'], marker='o', markerfacecolor=color_fill, markersize=1.0, alpha=1.0, markeredgecolor=color_fill, markeredgewidth=0.1) 81 | pl.setp(bp['caps'], color=color_fill) 82 | pl.setp(bp['medians'], color=color_median, lw=1.5) 83 | 84 | color_fill = 'b' 85 | color_median = 'c' 86 | for bp in bps_rot: 87 | pl.setp(bp['boxes'], color=color_fill, lw=1.0, alpha=1.0) 88 | pl.setp(bp['whiskers'], color=color_fill, ls='-') 89 | pl.setp(bp['fliers'], marker='o', markerfacecolor=color_fill, markersize=1.0, alpha=1.0, markeredgecolor=color_fill, markeredgewidth=0.1) 90 | pl.setp(bp['caps'], color=color_fill) 91 | pl.setp(bp['medians'], color=color_median, lw=1.5) 92 | 93 | ax.set_xlim(0, len(names)+1) 94 | ax.set_ylabel('[m/s] / [rad/s]') 95 | x_tick_pos = np.arange(len(names)) + 1 96 | pl.xticks(x_tick_pos, names, rotation=0, fontsize=fontsize) 97 | h = [] 98 | h.append(ax.plot([],color='r', lw=7, label='trans. error')) 99 | h.append(ax.plot([],color='b', lw=7, label='rot. error')) 100 | ax.legend(loc='upper left', fancybox = True, framealpha = 0.5) 101 | pl.subplots_adjust(left=0.17, right=0.95, top=0.92, bottom=0.3) 102 | 103 | print('Saving figure.') 104 | pl.savefig(os.path.join(figure_path, 'error_boxplots.pdf')) 105 | 106 | pl.show(block=False) 107 | 108 | -------------------------------------------------------------------------------- /planner_comparison/src/compare_models.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | import rospy 3 | import rospkg 4 | 5 | 6 | import pylab as pl 7 | import numpy as np 8 | 9 | from time_msg_container import * 10 | 11 | # Messages 12 | from geometry_msgs.msg import Twist, PoseStamped, Point, Quaternion 13 | from sensor_msgs.msg import LaserScan 14 | from nav_msgs.msg import Path 15 | from move_base_msgs.msg import MoveBaseActionFeedback 16 | 17 | class ErrorValues(): 18 | 19 | def __init__(self, name): 20 | self.rot_mean_training = 0.0 21 | self.trans_mean_training = 0.0 22 | self.rot_mean_eval = 0.0 23 | self.trans_mean_eval = 0.0 24 | self.rot_std_training = 0.0 25 | self.trans_std_training = 0.0 26 | self.rot_std_eval = 0.0 27 | self.trans_std_eval = 0.0 28 | self.name = name 29 | 30 | # pl.close('all') 31 | pl.rc('text', usetex=True) 32 | pl.rc('font', family='serif') 33 | fontsize = 14 34 | 35 | data_path = rospkg.RosPack().get_path('planner_comparison') + "/data/" 36 | filenames_training = ['FC_BS128_S500000_noDrop_LRconst_training.pkl','FC_BS128_S500000_LRconst_training.pkl', 'FC_BS128_S500000_REG_LRconst_training.pkl', 'FC_4Layers_BS128_S2000000_REG_LRconst_training.pkl'] 37 | filenames_eval = ['FC_BS128_S500000_noDrop_LRconst_eval.pkl','FC_BS128_S500000_LRconst_eval.pkl', 'FC_BS128_S500000_REG_LRconst_eval.pkl', 'FC_4Layers_BS128_S2000000_REG_LRconst_eval.pkl'] 38 | names = ['fully connected','dropout', 'dropout+reg.', '4 Layers'] 39 | 40 | data_storage_training = [] 41 | data_storage_eval = [] 42 | 43 | for name in filenames_training: 44 | data_storage_training.append(pickle.load(open(data_path + name, 'rb'))) 45 | 46 | for name in filenames_eval: 47 | data_storage_eval.append(pickle.load(open(data_path + name, 'rb'))) 48 | 49 | err_container = [] 50 | 51 | for idx, data in enumerate(data_storage_training): 52 | err = ErrorValues(names[idx]) 53 | # Training data 54 | err.rot_mean_training = data_storage_training[idx]['rot_error'][0] 55 | err.rot_std_training = data_storage_training[idx]['rot_error'][1] 56 | err.trans_mean_training = data_storage_training[idx]['trans_error'][0] 57 | err.trans_std_training = data_storage_training[idx]['trans_error'][1] 58 | 59 | # Evaluation data 60 | err.rot_mean_eval = data_storage_eval[idx]['rot_error'][0] 61 | err.rot_std_eval = data_storage_eval[idx]['rot_error'][1] 62 | err.trans_mean_eval = data_storage_eval[idx]['trans_error'][0] 63 | err.trans_std_eval = data_storage_eval[idx]['trans_error'][1] 64 | 65 | err_container.append(err) 66 | 67 | pl.figure('Error Plot') 68 | ax = pl.subplot(111) 69 | width = 0.2 70 | for idx,err in enumerate(err_container): 71 | train_bar_trans = ax.bar(idx+1-width/2, err.trans_mean_training, width=width, color='b', align='center') 72 | train_bar_rot = ax.bar(idx+1-width/2, err.rot_mean_training, width=width, color='b', alpha= 0.6, align='center', bottom=err.trans_mean_training) 73 | eval_bar_trans = ax.bar(idx+1+width/2, err.trans_mean_eval, width=width, color='r', align='center') 74 | eval_bar_rot = ax.bar(idx+1+width/2, err.rot_mean_eval, width=width, color='r', alpha=0.6, align='center', bottom=err.trans_mean_eval) 75 | ax.set_ylabel('Error [-]', fontsize=fontsize) 76 | ax.grid('on') 77 | x_tick_pos = np.arange(len(err_container)) + 1 78 | pl.xticks(x_tick_pos, names, rotation=45, fontsize=fontsize) 79 | pl.legend((train_bar_trans, train_bar_rot, eval_bar_trans, eval_bar_rot), 80 | ('training map trans.', 'training map rot.', 'eval map trans.', 'eval map rot.'), loc='best', fancybox = True, framealpha = 0.5, fontsize=fontsize) 81 | pl.tight_layout() 82 | 83 | pl.show(block=False) 84 | 85 | -------------------------------------------------------------------------------- /planner_comparison/src/planner_comparison_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | from planner_comparison import PlannerComparison 5 | 6 | def main(): 7 | 8 | rospy.init_node('planner_comparison') 9 | 10 | mission = PlannerComparison() 11 | 12 | rospy.spin() 13 | 14 | if __name__ == "__main__": 15 | main() 16 | -------------------------------------------------------------------------------- /planner_comparison/src/plot_missions.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import numpy 3 | import pylab as pl 4 | import rospkg 5 | import argparse 6 | from matplotlib import gridspec 7 | import matplotlib.ticker as ticker 8 | 9 | from planner_comparison.plan_scoring import * 10 | from planner_comparison.RosbagInterface import * 11 | import planner_comparison.util as pc_util 12 | 13 | def parse_args(): 14 | """ 15 | Parse input arguments. 16 | """ 17 | parser = argparse.ArgumentParser(description='Compare different motion planners') 18 | parser.add_argument('--paths1', nargs='+', help='List of bag files of first map that should be analyzed (deep + ros)', type=str, required=True) 19 | parser.add_argument('--paths2', nargs='+', help='List of bag files of first map that should be analyzed (deep + ros)', type=str, required=False, default=[]) 20 | args = parser.parse_args() 21 | return args 22 | 23 | def compute_relative_cost_deep(cost_deep, cost_ros): 24 | if cost_ros > 0: 25 | return (cost_deep - cost_ros) / cost_ros 26 | else: 27 | return cost_deep 28 | 29 | def plot_trajectory_comparison(ax, deep_missions, ros_missions, grid, colors=['g', 'k'], show_labels=False, shift_directions=None): 30 | pl.imshow(grid, extent=[-5, 5, -5, 5], origin='lower', cmap='Greys') 31 | for idx, m in enumerate(deep_missions): 32 | if shift_directions: 33 | dh = pc_util.plot_mission(ax, m, idx+1, color=colors[0], plot_numbers=True, shift_direction=shift_directions[idx], shift_dist=0.7) 34 | else: 35 | dh = pc_util.plot_mission(ax, m, idx+1, color=colors[0], plot_numbers=True) 36 | for idx, m in enumerate(ros_missions): 37 | rh = pc_util.plot_mission(ax, m, idx+1, color=colors[1], plot_numbers=False) 38 | if show_labels: 39 | pl.legend((dh[0], rh[0]), ('CNN', 'ROS'), fancybox=True, framealpha=0.5) 40 | 41 | def plot_relative_error(ax, deep_missions, ros_missions, colors=['g', 'k'], show_labels=False): 42 | cost_deep = pc_util.compute_detailed_cost_sum(deep_missions) 43 | cost_ros = pc_util.compute_detailed_cost_sum(ros_missions) 44 | cost_rel = cost_deep 45 | for key in cost_deep.keys(): 46 | cost_rel[key] = compute_relative_cost_deep(cost_deep[key], cost_ros[key]) 47 | pc_util.plot_relative_error_bars(ax, cost_rel, colors=colors, bar_width=0.3) 48 | h = [] 49 | if show_labels: 50 | h.append(ax.plot([],color=colors[0], lw=7, label='CNN')) 51 | h.append(ax.plot([],color=colors[1], lw=7, label='ROS')) 52 | return h 53 | 54 | args = parse_args() 55 | pl.close('all') 56 | fig_width_pt = 245.71811 # Get this from LaTeX using \showthe\columnwidth 57 | inches_per_pt = 1.0/72.27 # Convert pt to inch 58 | # golden_mean = (np.sqrt(5)-1.0)/2.0 # Aesthetic ratio 59 | fig_width = fig_width_pt*inches_per_pt # width in inches 60 | fig_height = fig_width*0.8 # height in inches 61 | fig_size = [fig_width,fig_height] 62 | fontsize = 9 63 | params = {'backend': 'ps', 64 | 'axes.labelsize': fontsize, 65 | 'text.fontsize': fontsize, 66 | 'title.fontsize': fontsize, 67 | 'legend.fontsize': fontsize, 68 | 'xtick.labelsize': fontsize, 69 | 'ytick.labelsize': fontsize, 70 | 'text.usetex': True, 71 | 'font.family': 'serif', 72 | 'font.serif': 'Computer Modern Roman', 73 | 'figure.figsize': fig_size} 74 | pl.rcParams.update(params) 75 | figure_path = '/home/pfmark/jade_catkin_ws/src/deep_motion_planning/planner_comparison/data/figures/' 76 | 77 | data_path = rospkg.RosPack().get_path('planner_comparison') + "/data/" 78 | names = ['deep', 'ros'] 79 | 80 | planner_missions1 = [] 81 | planner_missions2 = [] 82 | 83 | for path in args.paths1: 84 | rosbag_if = RosbagInterface(path) 85 | missions = extract_missions(rosbag_if.msg_container) 86 | planner_missions1.append(missions) 87 | map = rosbag_if.msg_container['map'].msgs[0] 88 | grid1 = np.reshape(map.data, [map.info.height, map.info.width]) 89 | 90 | two_maps = len(args.paths2) > 0 91 | if two_maps: 92 | for path in args.paths2: 93 | rosbag_if = RosbagInterface(path) 94 | missions = extract_missions(rosbag_if.msg_container) 95 | planner_missions2.append(missions) 96 | map = rosbag_if.msg_container['map'].msgs[0] 97 | grid2 = np.reshape(map.data, [map.info.height, map.info.width]) 98 | 99 | colors = ['g', 'k'] 100 | map = rosbag_if.msg_container['map'].msgs[0] 101 | grid = np.reshape(map.data, [map.info.height, map.info.width]) 102 | pl.figure('Missions') 103 | 104 | if two_maps: 105 | ax = pl.subplot2grid((5,2), (0,0), rowspan=3) 106 | else: 107 | ax = pl.subplot2grid((5,1), (0,0), rowspan=3) 108 | shift_directions=['lu', 'rd', 'r', 'r', 'u', 'rd', 'u', 'r'] 109 | plot_trajectory_comparison(ax, planner_missions1[0], planner_missions1[1], grid1, colors=['g', 'k'], show_labels=False, shift_directions=shift_directions) 110 | ax.get_xaxis().set_ticks([]) 111 | ax.get_yaxis().set_ticks([]) 112 | 113 | if two_maps: 114 | ax = pl.ax = pl.subplot2grid((5,2), (3,0), rowspan=2) 115 | else: 116 | ax = pl.ax = pl.subplot2grid((5,1), (3,0), rowspan=2) 117 | plot_relative_error(ax, planner_missions1[0], planner_missions1[1], colors=['g', 'k'], show_labels=True) 118 | ax.legend(loc='best', fancybox = True, framealpha = 0.5) 119 | ax.yaxis.set_major_locator(ticker.MultipleLocator(30)) 120 | ax.set_ylabel('\%', fontsize=6) 121 | for tick in ax.yaxis.get_major_ticks(): 122 | tick.label.set_fontsize(6) 123 | ax.set_ylim(-40, 160) 124 | ax.yaxis.grid() 125 | 126 | if two_maps: 127 | ax = pl.ax = pl.subplot2grid((5,2), (0,1), rowspan=3) 128 | shift_directions=['r', 'u', 'r', 'u', 'ru', 'u', 'r', 'u'] 129 | plot_trajectory_comparison(ax, planner_missions2[0], planner_missions2[1], grid2, colors=['g', 'k'], show_labels=False, shift_directions=shift_directions) 130 | ax.get_xaxis().set_ticks([]) 131 | ax.get_yaxis().set_ticks([]) 132 | 133 | ax = pl.ax = pl.subplot2grid((5,2), (3,1), rowspan=2) 134 | plot_relative_error(ax, planner_missions2[0], planner_missions2[1], colors=['g', 'k'], show_labels=False) 135 | ax.legend(loc='best', fancybox = True, framealpha = 0.5) 136 | ax.yaxis.set_major_locator(ticker.MultipleLocator(30)) 137 | ax.set_ylim(-40, 160) 138 | ax.set_ylabel('\%', fontsize=6) 139 | for tick in ax.yaxis.get_major_ticks(): 140 | tick.label.set_fontsize(6) 141 | ax.yaxis.grid() 142 | 143 | 144 | 145 | pl.subplots_adjust(left=0.10, right=0.97, top=0.97, bottom=0.17, wspace=None, hspace=None) 146 | pl.tight_layout() 147 | pl.show(block=False) 148 | 149 | print('Saving figure.') 150 | pl.savefig(os.path.join(figure_path, 'trajectory_comparison.pdf')) 151 | -------------------------------------------------------------------------------- /planner_comparison/src/plot_missions_real_data.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import numpy 3 | import pylab as pl 4 | import rospkg 5 | import argparse 6 | import os 7 | from matplotlib import gridspec 8 | 9 | from planner_comparison.plan_scoring import * 10 | from planner_comparison.RosbagInterface import * 11 | import planner_comparison.util as pc_util 12 | 13 | def parse_args(): 14 | """ 15 | Parse input arguments. 16 | """ 17 | parser = argparse.ArgumentParser(description='Compare different motion planners') 18 | parser.add_argument('--paths', nargs='+', help='List of bag files that should be analyzed', type=str, required=False) 19 | args = parser.parse_args() 20 | return args 21 | 22 | 23 | save_figures = True 24 | figure_path = '/home/pfmark/jade_catkin_ws/src/deep_motion_planning/planner_comparison/data/figures/' 25 | 26 | fig_width_pt = 245.71811 # Get this from LaTeX using \showthe\columnwidth 27 | inches_per_pt = 1.0/72.27 # Convert pt to inch 28 | # golden_mean = (np.sqrt(5)-1.0)/2.0 # Aesthetic ratio 29 | fig_width = fig_width_pt*inches_per_pt # width in inches 30 | fig_height = fig_width*0.8 # height in inches 31 | fig_size = [fig_width,fig_height] 32 | fontsize = 9 33 | params = {'backend': 'ps', 34 | 'axes.labelsize': fontsize, 35 | 'text.fontsize': fontsize, 36 | 'title.fontsize': fontsize, 37 | 'legend.fontsize': fontsize, 38 | 'xtick.labelsize': fontsize, 39 | 'ytick.labelsize': fontsize, 40 | 'text.usetex': True, 41 | 'font.family': 'serif', 42 | 'font.serif': 'Computer Modern Roman', 43 | 'figure.figsize': fig_size} 44 | pl.rcParams.update(params) 45 | 46 | args = parse_args() 47 | pl.close('all') 48 | pl.rc('text', usetex=True) 49 | pl.rc('font', family='serif') 50 | 51 | data_path = rospkg.RosPack().get_path('planner_comparison') + "/data/" 52 | names = ['CNN\_bigFC', 'ROS'] 53 | 54 | paths_comparison = ['/data/rosbags/deep_motion_planning/turtle/deep_SO_4M_updated_targets.bag', 55 | '/data/rosbags/deep_motion_planning/turtle/ros_planner.bag'] 56 | 57 | # paths_background = [] 58 | paths_background = ['/data/rosbags/deep_motion_planning/turtle/deep_SO_4M_2nd_run.bag', 59 | '/data/rosbags/deep_motion_planning/turtle/deep_SO_4M_3rd_run.bag', 60 | '/data/rosbags/deep_motion_planning/turtle/deep_SO_4M_4th_run.bag', 61 | '/data/rosbags/deep_motion_planning/turtle/deep_SO_4M_5th_run.bag', 62 | '/data/rosbags/deep_motion_planning/turtle/deep_SO_4M_6th_run.bag'] 63 | 64 | planner_missions = [] 65 | planner_missions_background = [] 66 | 67 | for path in paths_comparison: 68 | rosbag_if = RosbagInterface(path) 69 | missions = extract_missions(rosbag_if.msg_container) 70 | planner_missions.append(missions) 71 | 72 | for idx, path in enumerate(paths_background): 73 | print('Extracting background path {0}'.format(idx+1)) 74 | rosbag_if = RosbagInterface(path) 75 | missions = extract_missions(rosbag_if.msg_container) 76 | planner_missions_background.append(missions) 77 | 78 | 79 | shift_numbers = ['rd', 'r', 'u', 'l', 'u', 'u', 'u', 'r', 'u', 'rd', 'u', 'u', 'r'] 80 | colors = ['g', 'k'] 81 | manual_offset_x = 0.25 82 | manual_offset_y = 0.25 83 | map = rosbag_if.msg_container['map'].msgs[0] 84 | grid = np.reshape(map.data, [map.info.height, map.info.width]) 85 | pl.figure('Missions') 86 | ax = pl.subplot(111) 87 | map_size = [map.info.width*map.info.resolution/2, map.info.height*map.info.resolution/2] 88 | map_offset = [map.info.origin.position.x * map.info.resolution + manual_offset_x, map.info.origin.position.y * map.info.resolution + manual_offset_y] 89 | pl.imshow(grid, extent=[-map_size[0] + map_offset[0], map_size[0] + map_offset[0], -map_size[1] + map_offset[1], map_size[1] + map_offset[1]], origin='lower', cmap='Greys') 90 | handles = [] 91 | joystick_handle = None 92 | for missions in planner_missions_background: 93 | for m in missions: 94 | pc_util.plot_mission(ax, m, None, color='g', plot_numbers=False, alpha=0.3) 95 | pc_util.plot_joystick_interference(ax, m, color='m', alpha=0.3, linewidth=1.0) 96 | for ii, missions in enumerate(planner_missions): 97 | for jj, m in enumerate(missions): 98 | plot_numbers = True if ii is 0 else False 99 | th = pc_util.plot_mission(ax, m, jj+1, color=colors[ii], plot_numbers=plot_numbers, shift_direction=shift_numbers[jj], shift_dist=0.7) 100 | if ii == 0: 101 | jh = pc_util.plot_joystick_interference(ax, m, color='m', alpha=1.0, linewidth=1.0) 102 | if len(jh) > 0: 103 | joystick_handle = jh 104 | if jj == 0: 105 | handles.append(th) 106 | ax.set_xlabel('x [m]', fontsize=fontsize) 107 | ax.set_ylabel('y [m]', fontsize=fontsize) 108 | pl.axis('equal') 109 | ax.set_xlim([-15, 6]) 110 | ax.set_ylim([-8, 2]) 111 | ax.tick_params(labelsize=fontsize) 112 | pl.legend((handles[0][0], handles[1][0], joystick_handle[0][0]), (names[0], names[1], 'Joystick'), loc='best', fancybox=True, framealpha=0.5, numpoints=1) 113 | pl.locator_params(nbins=8) 114 | pl.tight_layout() 115 | pl.subplots_adjust(left=0.17, right=0.95, top=0.95, bottom=0.15) 116 | 117 | 118 | abs_dist = np.zeros([len(planner_missions[0])]) 119 | joy_dist = np.zeros([len(planner_missions[0])]) 120 | fused_missions = planner_missions_background 121 | fused_missions.append(planner_missions[0]) 122 | for miss in fused_missions: 123 | for ii, m in enumerate(miss): 124 | abs_dist[ii] += m.distance() 125 | joy_dist[ii] += pc_util.compute_joystick_distance(m) 126 | 127 | autonomous_ratio = 100 * (1 - joy_dist / abs_dist) 128 | 129 | if save_figures: 130 | print('Saving figure.') 131 | pl.savefig(os.path.join(figure_path, 'comparison_real_robot.pdf')) 132 | 133 | pl.show(block=False) -------------------------------------------------------------------------------- /planner_comparison/src/plot_single_mission_real_data.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import numpy 3 | import pylab as pl 4 | import rospkg 5 | import argparse 6 | import os 7 | from matplotlib import gridspec 8 | 9 | from planner_comparison.plan_scoring import * 10 | from planner_comparison.RosbagInterface import * 11 | import planner_comparison.util as pc_util 12 | 13 | def parse_args(): 14 | """ 15 | Parse input arguments. 16 | """ 17 | parser = argparse.ArgumentParser(description='Compare different motion planners') 18 | parser.add_argument('--paths', nargs='+', help='List of bag files that should be analyzed', type=str, required=False) 19 | args = parser.parse_args() 20 | return args 21 | 22 | 23 | save_figures = True 24 | figure_path = '/home/pfmark/jade_catkin_ws/src/deep_motion_planning/planner_comparison/data/figures/' 25 | 26 | fig_width_pt = 245.71811 # Get this from LaTeX using \showthe\columnwidth 27 | inches_per_pt = 1.0/72.27 # Convert pt to inch 28 | # golden_mean = (np.sqrt(5)-1.0)/2.0 # Aesthetic ratio 29 | fig_width = fig_width_pt*inches_per_pt # width in inches 30 | fig_height = fig_width*3 # height in inches 31 | fig_size = [fig_width,fig_height] 32 | fontsize = 9 33 | params = {'backend': 'ps', 34 | 'axes.labelsize': fontsize, 35 | 'text.fontsize': fontsize, 36 | 'title.fontsize': fontsize, 37 | 'legend.fontsize': fontsize, 38 | 'xtick.labelsize': fontsize, 39 | 'ytick.labelsize': fontsize, 40 | 'text.usetex': True, 41 | 'font.family': 'serif', 42 | 'font.serif': 'Computer Modern Roman', 43 | 'figure.figsize': fig_size} 44 | pl.rcParams.update(params) 45 | 46 | args = parse_args() 47 | pl.close('all') 48 | bagfile = '/data/rosbags/deep_motion_planning/turtle/deep_SO_4M_updated_targets.bag' 49 | 50 | rosbag_if = RosbagInterface(bagfile) 51 | missions = extract_missions(rosbag_if.msg_container) 52 | 53 | color = 'g' 54 | idx_mission = 1 55 | manual_offset_x = 0.25 56 | manual_offset_y = 0.25 57 | map = rosbag_if.msg_container['map'].msgs[0] 58 | grid = np.reshape(map.data, [map.info.height, map.info.width]) 59 | pl.figure('Missions') 60 | ax = pl.subplot(111) 61 | map_size = [map.info.width*map.info.resolution/2, map.info.height*map.info.resolution/2] 62 | map_offset = [map.info.origin.position.x * map.info.resolution + manual_offset_x, map.info.origin.position.y * map.info.resolution + manual_offset_y] 63 | pl.imshow(grid, extent=[-map_size[0] + map_offset[0], map_size[0] + map_offset[0], -map_size[1] + map_offset[1], map_size[1] + map_offset[1]], origin='lower', cmap='Greys') 64 | pc_util.plot_mission(ax, missions[idx_mission], None, color='g', plot_numbers=False, alpha=1, linewidth=2) 65 | traj = missions[idx_mission].get_trajectory() 66 | ax.plot(traj[0,0], traj[1,0], marker='o', mfc='r', mec='r') 67 | pl.axis('equal') 68 | ax.set_xlim([-0.5, 2.5]) 69 | ax.set_ylim([-6, 0]) 70 | ax.get_xaxis().set_ticks([]) 71 | ax.get_yaxis().set_ticks([]) 72 | pl.tight_layout() 73 | pl.subplots_adjust(left=0.17, right=0.95, top=0.95, bottom=0.15) 74 | 75 | if save_figures: 76 | print('Saving figure.') 77 | pl.savefig(os.path.join(figure_path, 'single_traj.pdf')) 78 | 79 | pl.show(block=False) -------------------------------------------------------------------------------- /planner_comparison/src/score_trajectories.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import numpy 3 | import pylab as pl 4 | import rospkg 5 | import argparse 6 | 7 | from planner_comparison.plan_scoring import * 8 | from planner_comparison.RosbagInterface import * 9 | import planner_comparison.util as pc_util 10 | 11 | def parse_args(): 12 | """ 13 | Parse input arguments. 14 | """ 15 | parser = argparse.ArgumentParser(description='Compare different motion planners') 16 | parser.add_argument('--paths', nargs='+', help='List of bag files that should be analyzed', type=str, required=True) 17 | args = parser.parse_args() 18 | return args 19 | 20 | args = parse_args() 21 | pl.close('all') 22 | pl.rc('text', usetex=False) 23 | pl.rc('font', family='serif') 24 | fontsize = 14 25 | 26 | data_path = rospkg.RosPack().get_path('planner_comparison') + "/data/" 27 | filenames = [] 28 | 29 | planner_missions = {} 30 | 31 | for path in args.paths: 32 | planner_name = path.split('/')[-1][:-4] 33 | rosbag_if = RosbagInterface(path) 34 | missions = extract_missions(rosbag_if.msg_container) 35 | planner_missions[planner_name] = missions 36 | 37 | 38 | # Plotting 39 | pl.figure('Error Plot') 40 | ax = pl.subplot(111) 41 | width = 0.2 42 | colors = ['r', 'b', 'g', 'k', 'c'] 43 | for idx, planner_name in enumerate(planner_missions.keys()): 44 | cost = np.sum([m.compute_cost()[0] for m in planner_missions[planner_name]]) 45 | cost_bar = ax.bar(idx+1, cost, width=width, color=colors[idx], align='center') 46 | ax.set_ylabel('Cost [-]', fontsize=fontsize) 47 | ax.grid('on') 48 | x_tick_pos = np.arange(len(planner_missions)) + 1 49 | pl.xticks(x_tick_pos, planner_missions.keys(), rotation=45, fontsize=fontsize) 50 | pl.tight_layout() 51 | 52 | pl.show(block=False) -------------------------------------------------------------------------------- /safety_module/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(safety_module) 3 | 4 | ## Find catkin macros and libraries 5 | find_package(catkin REQUIRED COMPONENTS 6 | geometry_msgs 7 | roscpp 8 | sensor_msgs 9 | nav_msgs 10 | message_filters 11 | tf 12 | ) 13 | 14 | SET(CMAKE_CXX_FLAGS "-std=c++0x") 15 | 16 | ## Find system dependencies 17 | find_package(Eigen3 REQUIRED) 18 | 19 | catkin_package( 20 | INCLUDE_DIRS include 21 | # LIBRARIES safety_module 22 | # CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs 23 | # DEPENDS system_lib 24 | ) 25 | 26 | ########### 27 | ## Build ## 28 | ########### 29 | 30 | ## Specify additional locations of header files 31 | include_directories( 32 | ${catkin_INCLUDE_DIRS} 33 | ${EIGEN3_INCLUDE_DIR} 34 | include/ 35 | ) 36 | 37 | add_executable(safety_module_node 38 | src/node.cpp 39 | src/SafetyModuleWrapper.cpp 40 | src/SafetyModule.cpp 41 | ) 42 | 43 | target_link_libraries(safety_module_node 44 | ${catkin_LIBRARIES} 45 | ) 46 | -------------------------------------------------------------------------------- /safety_module/include/safety_module/SafetyModule.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SAFETYMODULE_H_ 2 | #define SAFETYMODULE_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | inline int sign(const double x) { return x >= 0 ? 1 : -1; } 9 | 10 | class SafetyModule 11 | { 12 | 13 | public: 14 | typedef Eigen::Matrix LaserMeasurementMatrix; 15 | 16 | SafetyModule() {} 17 | SafetyModule(double robotRadius, double minimumDistanceToRobot, double timeOffset, 18 | double maximumDeceleration); 19 | ~SafetyModule() {} 20 | 21 | bool motionIsSafe(const Eigen::Vector2d& currentPosition, 22 | const Eigen::Quaterniond& currentOrientation, const double transVel, 23 | const double rotVel, const std::vector& pointsLaser); 24 | 25 | void reset() 26 | { 27 | _motionIsSafe = true; 28 | _statusMsg.clear(); 29 | } 30 | 31 | double computeStoppingDistance(const double transVel); 32 | 33 | Eigen::Vector2d transformPointPolar2KarthesianCoordinates(const double range, const double alpha); 34 | 35 | int getSignOfDistance(const Eigen::Hyperplane& hyperplane, 36 | const Eigen::Vector2d& point); 37 | 38 | Eigen::Vector2d computeStoppingPoint(const double transVel, const double rotVel, 39 | const Eigen::Vector2d& positionStart, 40 | const Eigen::Vector2d& positionCenter); 41 | 42 | Eigen::Vector2d computeRotationCenterOfTrajectory(const double radius, const double transVel, 43 | const double rotVel, 44 | const Eigen::Vector2d& positionStart, 45 | const Eigen::Vector2d& currentHeading); 46 | 47 | double computeTrajectoryRadius(const double transVel, const double rotVel); 48 | 49 | bool objectTooClose(const Eigen::Vector2d& laserMeas, const Eigen::Vector2d& positionRobot, 50 | double& laserPointDistToCenterSquared); 51 | 52 | double getRotVelThreshold() const { return _rotVelThreshold; } 53 | void setRotVelThreshold(double rotVelThreshold = 1e-3) { _rotVelThreshold = rotVelThreshold; } 54 | 55 | double getMaxDeceleration() const { return _maxDeceleration; } 56 | void setMaxDeceleration(double maxDeceleration = 1) { _maxDeceleration = maxDeceleration; } 57 | 58 | double getRobotRadius() const { return _robotRadius; } 59 | void setRobotRadius(double robotRadius = 0.4) { _robotRadius = robotRadius; } 60 | 61 | double getTimeOffset() const { return _timeOffset; } 62 | void setTimeOffset(double timeOffset = 0.3) { _timeOffset = timeOffset; } 63 | 64 | double getMinDistToRobot() const { return _minimumDistanceToRobot; } 65 | void setMinDistToRobot(double minDistToRobot) { _minimumDistanceToRobot = minDistToRobot; } 66 | 67 | double getMinDistToSensor() const { return _minimumDistanceToSensor; } 68 | void setMinDistToSensor(double minDistToSensor) { _minimumDistanceToSensor = minDistToSensor; } 69 | 70 | double getMinLateralDist() const { return _minimumLateralDistance; } 71 | void setMinLateralDist(double minLateralDist) { _minimumLateralDistance = minLateralDist; } 72 | 73 | double getMinLongitudinalDist() const { return _minimumLongitudinalDistance; } 74 | void setMinLongitudinalDist(double minLongitudinalDist) 75 | { 76 | _minimumLongitudinalDistance = minLongitudinalDist; 77 | } 78 | 79 | const std::string& getStatusMessage() const { return _statusMsg; } 80 | 81 | private: 82 | // parameters 83 | double _robotRadius = 0.18; 84 | double _minimumDistanceToRobot = 0.01; // m 85 | double _timeOffset = 0.0; // s (time until sent velocity command gets executed by the robot) 86 | double _maxDeceleration = 20.0; // m/s^2 87 | double _rotVelThreshold = 1e-3; // rad/s 88 | double _distSensorToEdge = 0.02; // m 89 | double _minimumDistanceToSensor = 0.0; 90 | double _minimumLateralDistance = 0.01; 91 | double _minimumLongitudinalDistance = 0.01; 92 | 93 | // safety state 94 | bool _motionIsSafe = true; 95 | 96 | std::string _statusMsg; 97 | }; 98 | 99 | #endif /* SAFETYMODULE_H_ */ 100 | -------------------------------------------------------------------------------- /safety_module/include/safety_module/SafetyModuleWrapper.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SAFETY_MODULE_WRAPPER_HPP_ 2 | #define _SAFETY_MODULE_WRAPPER_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include "safety_module/SafetyModule.hpp" 14 | 15 | namespace safety_module 16 | { 17 | class SafetyModuleWrapper 18 | { 19 | public: 20 | SafetyModuleWrapper(); 21 | ~SafetyModuleWrapper() = default; 22 | 23 | private: 24 | SafetyModule _safetyModule; 25 | 26 | ros::NodeHandle _nh; 27 | tf::TransformListener _tfListener; 28 | 29 | ros::Publisher _safetyInterruptPub; //!< Publish message in an unsafe state 30 | ros::Publisher _cmdPub; //!< Forward the velocity commands if it is safe 31 | 32 | message_filters::Subscriber _laserSub; 33 | message_filters::Subscriber _cmdSub; 34 | message_filters::TimeSynchronizer _sync; 35 | 36 | void syncCallback(const sensor_msgs::LaserScan::ConstPtr& laser, 37 | const geometry_msgs::TwistStamped::ConstPtr& cmd); 38 | }; 39 | } /* safety_module */ 40 | 41 | #endif /* _SAFETY_MODULE_WRAPPER_HPP_ */ 42 | -------------------------------------------------------------------------------- /safety_module/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | safety_module 4 | 0.0.0 5 | The safety_module package 6 | 7 | michael 8 | 9 | TODO 10 | 11 | catkin 12 | geometry_msgs 13 | roscpp 14 | sensor_msgs 15 | nav_msgs 16 | message_filters 17 | geometry_msgs 18 | roscpp 19 | sensor_msgs 20 | nav_msgs 21 | message_filters 22 | 23 | 24 | -------------------------------------------------------------------------------- /safety_module/src/SafetyModuleWrapper.cpp: -------------------------------------------------------------------------------- 1 | #include "safety_module/SafetyModuleWrapper.hpp" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace safety_module 9 | { 10 | 11 | SafetyModuleWrapper::SafetyModuleWrapper() 12 | : _safetyModule(0.18, 0.01, 0.0, 20.0), _nh("~"), _laserSub(_nh, "/base_scan", 1), 13 | _cmdSub(_nh, "/deep_planner/cmd_vel_stamped", 1), _sync(_laserSub, _cmdSub, 10) 14 | { 15 | _safetyInterruptPub = _nh.advertise("/safety_interrupt", 1); 16 | _cmdPub = _nh.advertise("/cmd_vel", 1); 17 | _sync.registerCallback(&SafetyModuleWrapper::syncCallback, this); 18 | } 19 | 20 | void SafetyModuleWrapper::syncCallback(const sensor_msgs::LaserScan::ConstPtr& laser, 21 | const geometry_msgs::TwistStamped::ConstPtr& cmd) 22 | { 23 | // Get the transformation between robot base and laser base 24 | tf::StampedTransform transform; 25 | _tfListener.lookupTransform("/base_link", "/base_laser_link", ros::Time(0), transform); 26 | Eigen::Vector2d currentPosition(transform.getOrigin().getX(), transform.getOrigin().getY()); 27 | Eigen::Quaterniond currentOrientation( 28 | transform.getRotation().getW(), transform.getRotation().getX(), 29 | transform.getRotation().getY(), transform.getRotation().getZ()); 30 | 31 | // Convert the laser measurements to 2D points in the laser frame 32 | std::vector pointsList; 33 | for (size_t i = 0; i < laser->ranges.size(); ++i) { 34 | double alpha = laser->angle_min + static_cast(i) * laser->angle_increment; 35 | Eigen::Vector2d laserMeas(cos(alpha) * laser->ranges.at(i), sin(alpha) * laser->ranges.at(i)); 36 | pointsList.push_back(laserMeas); 37 | } 38 | 39 | if (_safetyModule.motionIsSafe(currentPosition, currentOrientation, cmd->twist.linear.x, 40 | cmd->twist.angular.z, pointsList)) { 41 | geometry_msgs::Twist output; 42 | output = cmd->twist; 43 | _cmdPub.publish(output); 44 | } else { 45 | std_msgs::Empty msg; 46 | _safetyInterruptPub.publish(msg); 47 | 48 | // Publish a message with zeros 49 | geometry_msgs::Twist output; 50 | _cmdPub.publish(output); 51 | } 52 | // TODO Handle reset more carefully 53 | _safetyModule.reset(); 54 | } 55 | 56 | } /* safety_module */ 57 | -------------------------------------------------------------------------------- /safety_module/src/node.cpp: -------------------------------------------------------------------------------- 1 | #include "safety_module/SafetyModuleWrapper.hpp" 2 | #include 3 | 4 | int main(int argc, char* argv[]) 5 | { 6 | ros::init(argc, argv, "safety_module_node"); 7 | 8 | safety_module::SafetyModuleWrapper wrapper; 9 | 10 | ros::spin(); 11 | return 0; 12 | } 13 | -------------------------------------------------------------------------------- /stage_worlds/README.md: -------------------------------------------------------------------------------- 1 | # stage_worlds 2 | This package is a wrapper around the stage_ros package which contains the simulator stage and a 3 | ROS node that makes the simulation accessible as ROS topic. 4 | This package contains various configuration files for stage that define the Europa robot, various 5 | worlds and ROS specific configuration and launch files. 6 | 7 | ## Usage 8 | ``` 9 | roslaunch stage_worlds shapes.launch 10 | ``` 11 | This start the simulation, the ROS navigation and loads the static map server. 12 | 13 | ## Worlds 14 | A world is defined as grayscale image (see the ./worlds/bitmaps folder for examples) 15 | 16 | These are the worlds that are defined: 17 | ### empty.world 18 | ![empty](./worlds/bitmaps/empty.png) 19 | ### simple.world 20 | ![simple](./worlds/bitmaps/simple.png) 21 | ### shapes.world 22 | ![shapes](./worlds/bitmaps/shapes.png) 23 | ### rooms.world 24 | ![rooms](./worlds/bitmaps/rooms.png) 25 | ### boxes.world 26 | ![boxes](./worlds/bitmaps/boxes.png) 27 | -------------------------------------------------------------------------------- /stage_worlds/cfg/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | 2 | planner_frequency: 1.0 3 | controller_frequency: 20.0 4 | base_local_planner: "dwa_local_planner/DWAPlannerROS" 5 | 6 | DWAPlannerROS: 7 | 8 | max_vel_x: 0.6 9 | min_vel_x: 0.0 10 | max_vel_y: 0.0 11 | min_vel_y: 0.0 12 | 13 | max_rot_vel: 1.57 14 | min_rot_vel: 0.05 15 | 16 | acc_lim_th: 4.0 17 | acc_lim_x: 4.0 18 | acc_lim_y: 0.0 19 | 20 | xy_goal_tolerance: 0.1 21 | 22 | sim_time: 1.0 23 | vx_samples: 8 24 | vy_samples: 1 25 | vtheta_samples: 20 26 | 27 | occdist_scale: 1.0 28 | path_distance_bias: 50.0 29 | -------------------------------------------------------------------------------- /stage_worlds/cfg/blobs.yaml: -------------------------------------------------------------------------------- 1 | image: ../worlds/bitmaps/blobs.png 2 | resolution: 0.01 3 | origin: [-4.975, -4.975, 0.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 7 | -------------------------------------------------------------------------------- /stage_worlds/cfg/borderless.yaml: -------------------------------------------------------------------------------- 1 | image: ../worlds/bitmaps/borderless.png 2 | resolution: 0.01 3 | origin: [-4.975, -4.975, 0.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 7 | -------------------------------------------------------------------------------- /stage_worlds/cfg/boxes.yaml: -------------------------------------------------------------------------------- 1 | image: ../worlds/bitmaps/boxes.png 2 | resolution: 0.01 3 | origin: [-4.975, -4.975, 0.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 7 | -------------------------------------------------------------------------------- /stage_worlds/cfg/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | #footprint: [[0.178,0],[0.165,0.068],[0.126,0.126],[0.068,0.165],[0,0.178],[-0.068,0.165],[-0.126,0.126],[-0.165,0.068],[-0.178,0],[-0.165,-0.068],[-0.126,-0.126],[-0.068,-0.165],[0,-0.178],[0.068,-0.165],[0.126,-0.126],[0.165,-0.068]] 2 | robot_radius: 0.178 3 | footprint_padding: 0.01 4 | 5 | inflation_radius: 0.75 6 | 7 | resolution: 0.025 8 | 9 | cost_scaling_factor: 10.0 10 | lethal_cost_threshold: 100 11 | observation_sources: base_scan 12 | base_scan: {data_type: LaserScan, expected_update_rate: 0.4, sensor_frame: /base_laser_link, topic: base_scan, 13 | observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} 14 | -------------------------------------------------------------------------------- /stage_worlds/cfg/empty.yaml: -------------------------------------------------------------------------------- 1 | image: ../worlds/bitmaps/empty.png 2 | resolution: 0.01 3 | origin: [-4.975, -4.975, 0.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 7 | -------------------------------------------------------------------------------- /stage_worlds/cfg/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 1.0 5 | publish_frequency: 1.0 6 | static_map: true 7 | -------------------------------------------------------------------------------- /stage_worlds/cfg/lee_j_map.yaml: -------------------------------------------------------------------------------- 1 | image: ../worlds/bitmaps/lee_j_map.pgm 2 | resolution: 0.020000 3 | origin: [-21.65, -8.1000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /stage_worlds/cfg/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 5.0 5 | publish_frequency: 5.0 6 | static_map: false 7 | rolling_window: true 8 | width: 4.0 9 | height: 4.0 10 | resolution: 0.025 11 | -------------------------------------------------------------------------------- /stage_worlds/cfg/office.yaml: -------------------------------------------------------------------------------- 1 | image: ../worlds/bitmaps/office.png 2 | resolution: 0.01 3 | origin: [-4.975, -4.975, 0.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 7 | -------------------------------------------------------------------------------- /stage_worlds/cfg/rooms.yaml: -------------------------------------------------------------------------------- 1 | image: ../worlds/bitmaps/rooms.png 2 | resolution: 0.01 3 | origin: [-4.975, -4.975, 0.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 7 | -------------------------------------------------------------------------------- /stage_worlds/cfg/shapes.yaml: -------------------------------------------------------------------------------- 1 | image: ../worlds/bitmaps/shapes.png 2 | resolution: 0.01 3 | origin: [-4.975, -4.975, 0.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 7 | -------------------------------------------------------------------------------- /stage_worlds/cfg/simple.yaml: -------------------------------------------------------------------------------- 1 | image: ../worlds/bitmaps/simple.png 2 | resolution: 0.01 3 | origin: [-4.975, -4.975, 0.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 7 | -------------------------------------------------------------------------------- /stage_worlds/cfg/simple_real.yaml: -------------------------------------------------------------------------------- 1 | image: ../worlds/bitmaps/simple_real.png 2 | resolution: 0.01 3 | origin: [-4.975, -4.975, 0.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 7 | -------------------------------------------------------------------------------- /stage_worlds/cfg/turtlebot_planner_params/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | 3 | max_vel_x: 0.4 # 0.55 4 | min_vel_x: 0.0 5 | 6 | max_vel_y: 0.0 # diff drive robot 7 | min_vel_y: 0.0 # diff drive robot 8 | 9 | max_trans_vel: 0.5 # choose slightly less than the base's capability 10 | min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity 11 | trans_stopped_vel: 0.1 12 | 13 | # Warning! 14 | # do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities 15 | # are non-negligible and small in place rotational velocities will be created. 16 | 17 | max_rot_vel: 5.0 # choose slightly less than the base's capability 18 | min_rot_vel: 0.4 # this is the min angular velocity when there is negligible translational velocity 19 | rot_stopped_vel: 0.4 20 | 21 | acc_lim_x: 1.0 # maximum is theoretically 2.0, but we 22 | acc_lim_theta: 2.0 23 | acc_lim_y: 0.0 # diff drive robot 24 | 25 | # Goal Tolerance Parameters 26 | yaw_goal_tolerance: 0.3 # 0.05 27 | xy_goal_tolerance: 0.20 # 0.10 28 | # latch_xy_goal_tolerance: false 29 | 30 | # Forward Simulation Parameters 31 | sim_time: 1.0 # 1.7 32 | vx_samples: 10 # 6 33 | vy_samples: 1 # diff drive robot, there is only one sample 34 | vtheta_samples: 20 # 20 35 | 36 | # Trajectory Scoring Parameters 37 | path_distance_bias: 70.0 # 32.0 - weighting for how much it should stick to the global path plan 38 | goal_distance_bias: 24.0 # 24.0 - wighting for how much it should attempt to reach its goal 39 | occdist_scale: 1.0 # 0.01 - weighting for how much the controller should avoid obstacles (0.5 is ok) 40 | forward_point_distance: 0.325 # 0.325 - how far along to place an additional scoring point 41 | stop_time_buffer: 0.2 # 0.2 - amount of time a robot must stop in before colliding for a valid traj. 42 | scaling_speed: 0.25 # 0.25 - absolute velocity at which to start scaling the robot's footprint 43 | max_scaling_factor: 0.2 # 0.2 - how much to scale the robot's footprint when at speed. 44 | 45 | # Oscillation Prevention Parameters 46 | oscillation_reset_dist: 0.05 # 0.05 - how far to travel before resetting oscillation flags 47 | 48 | # Debugging 49 | publish_traj_pc : true 50 | publish_cost_grid_pc: true 51 | global_frame_id: odom 52 | -------------------------------------------------------------------------------- /stage_worlds/cfg/turtlebot_planner_params/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: base_footprint 4 | update_frequency: 1.0 5 | publish_frequency: 0.5 6 | static_map: true 7 | transform_tolerance: 0.5 8 | plugins: 9 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 10 | - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 11 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 12 | 13 | -------------------------------------------------------------------------------- /stage_worlds/cfg/turtlebot_planner_params/global_planner_params.yaml: -------------------------------------------------------------------------------- 1 | 2 | GlobalPlanner: # Also see: http://wiki.ros.org/global_planner 3 | old_navfn_behavior: false # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false 4 | use_quadratic: true # Use the quadratic approximation of the potential. Otherwise, use a simpler calculation, default true 5 | use_dijkstra: true # Use dijkstra's algorithm. Otherwise, A*, default true 6 | use_grid_path: false # Create a path that follows the grid boundaries. Otherwise, use a gradient descent method, default false 7 | 8 | allow_unknown: false # Allow planner to plan through unknown space, default true 9 | #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work 10 | planner_window_x: 0.0 # default 0.0 11 | planner_window_y: 0.0 # default 0.0 12 | default_tolerance: 0.0 # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0 13 | 14 | publish_scale: 100 # Scale by which the published potential gets multiplied, default 100 15 | planner_costmap_publish_frequency: 0.0 # default 0.0 16 | 17 | lethal_cost: 253 # default 253 18 | neutral_cost: 50 # default 50 19 | cost_factor: 3.0 # Factor to multiply each cost from costmap by, default 3.0 20 | publish_potential: true # Publish Potential Costmap (this is not like the navfn pointcloud2 potential), default true 21 | -------------------------------------------------------------------------------- /stage_worlds/cfg/turtlebot_planner_params/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | # Move base node parameters. For full documentation of the parameters in this file, please see 2 | # 3 | # http://www.ros.org/wiki/move_base 4 | # 5 | shutdown_costmaps: false 6 | 7 | controller_frequency: 5.0 8 | controller_patience: 3.0 9 | 10 | 11 | planner_frequency: 1.0 # 1.0 12 | planner_patience: 5.0 13 | 14 | oscillation_timeout: 10.0 15 | oscillation_distance: 0.2 16 | 17 | # local planner - default is trajectory rollout 18 | base_local_planner: "dwa_local_planner/DWAPlannerROS" 19 | 20 | base_global_planner: "navfn/NavfnROS" #alternatives: global_planner/GlobalPlanner, carrot_planner/CarrotPlanner 21 | -------------------------------------------------------------------------------- /stage_worlds/cfg/turtlebot_planner_params/navfn_global_planner_params.yaml: -------------------------------------------------------------------------------- 1 | 2 | NavfnROS: 3 | visualize_potential: false #Publish potential for rviz as pointcloud2, not really helpful, default false 4 | allow_unknown: false #Specifies whether or not to allow navfn to create plans that traverse unknown space, default true 5 | #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work 6 | planner_window_x: 0.0 #Specifies the x size of an optional window to restrict the planner to, default 0.0 7 | planner_window_y: 0.0 #Specifies the y size of an optional window to restrict the planner to, default 0.0 8 | 9 | default_tolerance: 0.0 #If the goal is in an obstacle, the planer will plan to the nearest point in the radius of default_tolerance, default 0.0 10 | #The area is always searched, so could be slow for big values 11 | -------------------------------------------------------------------------------- /stage_worlds/launch/blobs.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /stage_worlds/launch/borderless.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /stage_worlds/launch/boxes.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /stage_worlds/launch/core.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /stage_worlds/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /stage_worlds/launch/empty.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /stage_worlds/launch/lab.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /stage_worlds/launch/office.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /stage_worlds/launch/rooms.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /stage_worlds/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /stage_worlds/launch/shapes.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /stage_worlds/launch/simple.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /stage_worlds/launch/simple_real.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /stage_worlds/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | stage_worlds 4 | 0.0.0 5 | The ros_stage_worlds package 6 | 7 | 8 | 9 | 10 | michael 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /stage_worlds/worlds/bitmaps/blobs.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/stage_worlds/worlds/bitmaps/blobs.png -------------------------------------------------------------------------------- /stage_worlds/worlds/bitmaps/borderless.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/stage_worlds/worlds/bitmaps/borderless.png -------------------------------------------------------------------------------- /stage_worlds/worlds/bitmaps/boxes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/stage_worlds/worlds/bitmaps/boxes.png -------------------------------------------------------------------------------- /stage_worlds/worlds/bitmaps/empty.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/stage_worlds/worlds/bitmaps/empty.png -------------------------------------------------------------------------------- /stage_worlds/worlds/bitmaps/lee_j_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/stage_worlds/worlds/bitmaps/lee_j_map.pgm -------------------------------------------------------------------------------- /stage_worlds/worlds/bitmaps/office.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/stage_worlds/worlds/bitmaps/office.png -------------------------------------------------------------------------------- /stage_worlds/worlds/bitmaps/rooms.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/stage_worlds/worlds/bitmaps/rooms.png -------------------------------------------------------------------------------- /stage_worlds/worlds/bitmaps/shapes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/stage_worlds/worlds/bitmaps/shapes.png -------------------------------------------------------------------------------- /stage_worlds/worlds/bitmaps/simple.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/stage_worlds/worlds/bitmaps/simple.png -------------------------------------------------------------------------------- /stage_worlds/worlds/bitmaps/simple_real.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/stage_worlds/worlds/bitmaps/simple_real.png -------------------------------------------------------------------------------- /stage_worlds/worlds/bitmaps/simple_real.xcf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/stage_worlds/worlds/bitmaps/simple_real.xcf -------------------------------------------------------------------------------- /stage_worlds/worlds/bitmaps/worlds.xcf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/deep_motion_planning/6512f651bafbb56710ddbae501a5b4c22d56ac66/stage_worlds/worlds/bitmaps/worlds.xcf -------------------------------------------------------------------------------- /stage_worlds/worlds/blobs.world: -------------------------------------------------------------------------------- 1 | # simple.world - for testing 2 | # $Id$ 3 | 4 | include "map.inc" 5 | include "turtlebot/turtlebot.inc" 6 | include "window.inc" 7 | 8 | interval_sim 40 9 | quit_time 0 10 | resolution 0.02 11 | 12 | show_clock 1 13 | show_clock_interval 100 14 | 15 | floorplan 16 | ( 17 | name "floor" 18 | 19 | map_resolution 0.05 20 | size [10.000 10.000 0.800] 21 | pose [0 0 0 0] 22 | 23 | bitmap "bitmaps/blobs.png" 24 | ) 25 | 26 | turtlebot 27 | ( 28 | name "turtlebot" 29 | pose [0.0 2.0 0 0.0] 30 | ) 31 | -------------------------------------------------------------------------------- /stage_worlds/worlds/borderless.world: -------------------------------------------------------------------------------- 1 | # simple.world - for testing 2 | # $Id$ 3 | 4 | include "map.inc" 5 | include "turtlebot/turtlebot.inc" 6 | include "window.inc" 7 | 8 | interval_sim 40 9 | quit_time 0 10 | resolution 0.02 11 | 12 | show_clock 1 13 | show_clock_interval 100 14 | 15 | floorplan 16 | ( 17 | name "floor" 18 | 19 | map_resolution 0.05 20 | size [10.000 10.000 0.800] 21 | pose [0 0 0 0] 22 | 23 | bitmap "bitmaps/borderless.png" 24 | ) 25 | 26 | turtlebot 27 | ( 28 | name "turtlebot" 29 | pose [0.0 2.0 0 0.0] 30 | ) 31 | -------------------------------------------------------------------------------- /stage_worlds/worlds/boxes.world: -------------------------------------------------------------------------------- 1 | # simple.world - for testing 2 | # $Id$ 3 | 4 | include "map.inc" 5 | include "turtlebot/turtlebot.inc" 6 | include "window.inc" 7 | 8 | interval_sim 40 9 | quit_time 0 10 | resolution 0.02 11 | 12 | show_clock 1 13 | show_clock_interval 100 14 | 15 | floorplan 16 | ( 17 | name "floor" 18 | 19 | map_resolution 0.05 20 | size [10.000 10.000 0.800] 21 | pose [0 0 0 0] 22 | 23 | bitmap "bitmaps/boxes.png" 24 | ) 25 | 26 | turtlebot 27 | ( 28 | name "turtlebot" 29 | pose [0.0 0.0 0 0.0] 30 | ) 31 | -------------------------------------------------------------------------------- /stage_worlds/worlds/empty.world: -------------------------------------------------------------------------------- 1 | # simple.world - for testing 2 | # $Id$ 3 | 4 | include "map.inc" 5 | include "turtlebot/turtlebot.inc" 6 | include "window.inc" 7 | 8 | interval_sim 40 9 | quit_time 0 10 | resolution 0.02 11 | 12 | show_clock 1 13 | show_clock_interval 100 14 | 15 | floorplan 16 | ( 17 | name "floor" 18 | 19 | map_resolution 0.05 20 | size [10.000 10.000 0.800] 21 | pose [0 0 0 0] 22 | 23 | bitmap "bitmaps/empty.png" 24 | ) 25 | 26 | turtlebot 27 | ( 28 | name "turtlebot" 29 | pose [0.0 0.0 0 0] 30 | ) 31 | -------------------------------------------------------------------------------- /stage_worlds/worlds/europa/camera.inc: -------------------------------------------------------------------------------- 1 | 2 | define europa_camera camera 3 | ( 4 | resolution [640 480] 5 | fov [70 50] 6 | 7 | size [0.1 0.07 0.05] 8 | 9 | block( points 4 10 | point[0] [0 0] 11 | point[1] [0 1] 12 | point[2] [1 1] 13 | point[3] [1 0] 14 | z [0 0.21] 15 | ) 16 | ) 17 | -------------------------------------------------------------------------------- /stage_worlds/worlds/europa/europa.inc: -------------------------------------------------------------------------------- 1 | include "europa/sick.inc" 2 | include "europa/hokuyo.inc" 3 | include "europa/camera.inc" 4 | 5 | define europa_base position 6 | ( 7 | color "red" # Default color. 8 | gui_nose 1 # Draw a nose on the robot so we can see which way it points 9 | obstacle_return 1 # Can hit things. 10 | ranger_return 0.5 # reflects sonar beams 11 | blob_return 1 # Seen by blobfinders 12 | fiducial_return 1 # Seen as "1" fiducial finders 13 | 14 | localization "gps" 15 | localization_origin [0 0 0 0] # Start odometry at (0, 0, 0). 16 | 17 | # alternative odometric localization with simple error model 18 | # localization "odom" # Change to "gps" to have impossibly perfect, global odometry 19 | # odom_error [ 0.05 0.05 0.1 ] # Odometry error or slip in X, Y and Theta 20 | # (Uniform random distribution) 21 | 22 | # four DOF kinematics limits 23 | # [ xmin xmax ymin ymax zmin zmax amin amax ] 24 | velocity_bounds [-1.0 1.0 0 0 0 0 -90.0 90.0 ] 25 | acceleration_bounds [-1.0 1.0 0 0 0 0 -90 90.0 ] 26 | 27 | # actual size 28 | size [0.80 0.70 0.22] 29 | 30 | # the center of rotation is offset from its center of area 31 | origin [0 0 0 0] 32 | 33 | # draw a nose on the robot so we can see which way it points 34 | gui_nose 1 35 | 36 | # estimated mass in KG 37 | mass 100.0 38 | # differential steering model 39 | drive "diff" 40 | ) 41 | 42 | define europa_base_sensors europa_base 43 | ( 44 | hokuyolaser( pose [0 0 0 0]) 45 | europa_camera( pose [0 0 0.05 0]) 46 | ) 47 | 48 | define europa europa_base_sensors 49 | ( 50 | # simplified Body shape: 51 | block( 52 | points 8 53 | point[0] [0.4 0.15] 54 | point[1] [0.2 0.35] 55 | point[2] [-0.2 0.35] 56 | point[3] [-0.4 0.15] 57 | point[4] [-0.4 -0.15] 58 | point[5] [-0.2 -0.35] 59 | point[6] [0.2 -0.35] 60 | point[7] [0.4 -0.15] 61 | z [0 0.22] 62 | ) 63 | ) 64 | 65 | -------------------------------------------------------------------------------- /stage_worlds/worlds/europa/hokuyo.inc: -------------------------------------------------------------------------------- 1 | 2 | define hokuyo_common ranger 3 | ( 4 | sensor( 5 | # laser-specific properties 6 | range [ 0.0 18.0 ] 7 | fov 360.0 8 | samples 720 9 | ) 10 | # generic model properties 11 | color "blue" 12 | size [ 0.07 0.07 0.05 ] # dimensions from LMS200 data sheet 13 | ) 14 | 15 | define hokuyo_base hokuyo_common 16 | ( 17 | block( points 4 18 | point[0] [0 0] 19 | point[1] [0 1] 20 | point[2] [1 1] 21 | point[3] [1 0] 22 | z [0 0.21] 23 | ) 24 | ) 25 | 26 | define hokuyolaser hokuyo_base() 27 | -------------------------------------------------------------------------------- /stage_worlds/worlds/europa/sick.inc: -------------------------------------------------------------------------------- 1 | 2 | define sicksensor sensor 3 | ( 4 | # factory settings for LMS200 5 | range [ 0.0 8.0 ] 6 | pose [ 0 0 0.1 0 ] 7 | fov 180 8 | samples 180 9 | #samples 90 # still useful but much faster to compute 10 | color_rgba [ 0 0 1 0.15 ] 11 | ) 12 | 13 | define sickcommon ranger 14 | ( 15 | color "blue" 16 | size [ 0.156 0.155 0.19 ] # dimensions from LMS200 data sheet 17 | ) 18 | 19 | define sickbase sickcommon 20 | ( 21 | block( points 4 22 | point[0] [0 0] 23 | point[1] [0 1] 24 | point[2] [1 1] 25 | point[3] [1 0] 26 | z [0 0.21] 27 | ) 28 | ) 29 | 30 | # extends sicklaser to add nice-looking but relatively expensive geometry 31 | define fancysickbase sickcommon 32 | ( 33 | # bottom 34 | block( 35 | points 4 36 | point[0] [ -0.02 -0.077 ] 37 | point[1] [ 0.078 -0.077 ] 38 | point[2] [ 0.078 0.077 ] 39 | point[3] [ -0.02 0.077 ] 40 | z [0 0.02 ] 41 | ) 42 | 43 | # back 44 | block( 45 | points 4 46 | point[0] [ -0.078 -0.077 ] 47 | point[1] [ -0.02 -0.077 ] 48 | point[2] [ -0.02 0.077 ] 49 | point[3] [ -0.078 0.077 ] 50 | z [0 0.21 ] 51 | ) 52 | 53 | # top 54 | block( points 4 55 | point[0] [ -0.02 -0.077 ] 56 | point[1] [ 0.078 -0.077 ] 57 | point[2] [ 0.078 0.077 ] 58 | point[3] [ -0.02 0.077 ] 59 | z [0.12 0.21 ] 60 | ) 61 | 62 | # laser bit 63 | block( points 4 64 | point[0] [ -0.02 -0.05 ] 65 | point[1] [ 0.06 -0.05 ] 66 | point[2] [ 0.06 0.05 ] 67 | point[3] [ -0.02 0.05 ] 68 | z [0.02 0.12 ] 69 | color "gray10" 70 | ) 71 | ) 72 | 73 | define sicklaser sickbase ( sicksensor() ) 74 | 75 | define fancysicklaser fancysickbase ( sicksensor() ) 76 | 77 | -------------------------------------------------------------------------------- /stage_worlds/worlds/lab.world: -------------------------------------------------------------------------------- 1 | # simple.world - for testing 2 | # $Id$ 3 | 4 | include "map.inc" 5 | include "turtlebot/turtlebot.inc" 6 | include "window.inc" 7 | 8 | interval_sim 40 9 | quit_time 0 10 | resolution 0.02 11 | 12 | show_clock 1 13 | show_clock_interval 100 14 | 15 | floorplan 16 | ( 17 | name "floor" 18 | 19 | map_resolution 0.02 20 | size [47.300 16.100 0.800] 21 | # pose [-50.48 -30.0 0 0] 22 | pose [2 0 0 0] 23 | 24 | bitmap "bitmaps/lee_j_map.pgm" 25 | ) 26 | 27 | turtlebot 28 | ( 29 | name "turtlebot" 30 | pose [5.0 -2.0 0 0.0] 31 | ) 32 | -------------------------------------------------------------------------------- /stage_worlds/worlds/map.inc: -------------------------------------------------------------------------------- 1 | # map.inc - useful setup for a floorplan bitmap 2 | # Authors: Richard Vaughan 3 | # $Id$ 4 | 5 | define floorplan model 6 | ( 7 | # sombre, sensible, artistic 8 | color "gray30" 9 | 10 | # most maps will need a bounding box 11 | boundary 0 12 | 13 | gui_grid 0 14 | gui_nose 0 15 | gui_move 0 16 | gui_outline 0 17 | gripper_return 0 18 | fiducial_return 0 19 | ranger_return 0.5 20 | 21 | obstacle_return 1 22 | ) 23 | 24 | define zone model 25 | ( 26 | color "orange" 27 | size [ 4 4 0.02 ] 28 | 29 | gui_nose 0 30 | gui_grid 0 31 | gui_move 1 32 | gui_outline 0 33 | 34 | # insensible to collision and range sensors 35 | obstacle_return 0 36 | ranger_return -1 # transparent to range sensors 37 | ) 38 | -------------------------------------------------------------------------------- /stage_worlds/worlds/office.world: -------------------------------------------------------------------------------- 1 | # simple.world - for testing 2 | # $Id$ 3 | 4 | include "map.inc" 5 | include "turtlebot/turtlebot.inc" 6 | include "window.inc" 7 | 8 | interval_sim 40 9 | quit_time 0 10 | resolution 0.02 11 | 12 | show_clock 1 13 | show_clock_interval 100 14 | 15 | floorplan 16 | ( 17 | name "floor" 18 | 19 | map_resolution 0.05 20 | size [10.000 10.000 0.800] 21 | pose [0 0 0 0] 22 | 23 | bitmap "bitmaps/office.png" 24 | ) 25 | 26 | turtlebot 27 | ( 28 | name "turtlebot" 29 | pose [-2.0 0.0 0 0.0] 30 | ) 31 | -------------------------------------------------------------------------------- /stage_worlds/worlds/rooms.world: -------------------------------------------------------------------------------- 1 | # simple.world - for testing 2 | # $Id$ 3 | 4 | include "map.inc" 5 | include "turtlebot/turtlebot.inc" 6 | include "window.inc" 7 | 8 | interval_sim 40 9 | quit_time 0 10 | resolution 0.02 11 | 12 | show_clock 1 13 | show_clock_interval 100 14 | 15 | floorplan 16 | ( 17 | name "floor" 18 | 19 | map_resolution 0.05 20 | size [10.000 10.000 0.800] 21 | pose [0 0 0 0] 22 | 23 | bitmap "bitmaps/rooms.png" 24 | ) 25 | 26 | turtlebot 27 | ( 28 | name "turtlebot" 29 | pose [-3.0 -3.0 0 90.0] 30 | ) 31 | -------------------------------------------------------------------------------- /stage_worlds/worlds/shapes.world: -------------------------------------------------------------------------------- 1 | # simple.world - for testing 2 | # $Id$ 3 | 4 | include "map.inc" 5 | include "turtlebot/turtlebot.inc" 6 | include "window.inc" 7 | 8 | interval_sim 40 9 | quit_time 0 10 | resolution 0.02 11 | 12 | show_clock 1 13 | show_clock_interval 100 14 | 15 | floorplan 16 | ( 17 | name "floor" 18 | 19 | map_resolution 0.05 20 | size [10.000 10.000 0.800] 21 | pose [0 0 0 0] 22 | 23 | bitmap "bitmaps/shapes.png" 24 | ) 25 | 26 | turtlebot 27 | ( 28 | name "turtlebot" 29 | pose [-3.0 0.0 0 0.0] 30 | ) 31 | -------------------------------------------------------------------------------- /stage_worlds/worlds/simple.world: -------------------------------------------------------------------------------- 1 | # simple.world - for testing 2 | # $Id$ 3 | 4 | include "map.inc" 5 | include "turtlebot/turtlebot.inc" 6 | include "window.inc" 7 | 8 | interval_sim 40 9 | quit_time 0 10 | resolution 0.02 11 | 12 | show_clock 1 13 | show_clock_interval 100 14 | 15 | floorplan 16 | ( 17 | name "floor" 18 | 19 | map_resolution 0.05 20 | size [10.000 10.000 0.800] 21 | pose [0 0 0 0] 22 | 23 | bitmap "bitmaps/simple.png" 24 | ) 25 | 26 | turtlebot 27 | ( 28 | name "turtlebot" 29 | pose [-3.0 0.0 0 90.0] 30 | ) 31 | -------------------------------------------------------------------------------- /stage_worlds/worlds/simple_real.world: -------------------------------------------------------------------------------- 1 | # simple.world - for testing 2 | # $Id$ 3 | 4 | include "map.inc" 5 | include "turtlebot/turtlebot.inc" 6 | include "window.inc" 7 | 8 | interval_sim 40 9 | quit_time 0 10 | resolution 0.02 11 | 12 | show_clock 1 13 | show_clock_interval 100 14 | 15 | floorplan 16 | ( 17 | name "floor" 18 | 19 | map_resolution 0.05 20 | size [10.000 10.000 0.800] 21 | pose [0 0 0 0] 22 | 23 | bitmap "bitmaps/simple_real.png" 24 | ) 25 | 26 | turtlebot 27 | ( 28 | name "turtlebot" 29 | pose [-4.0 -2.0 0 90.0] 30 | ) 31 | -------------------------------------------------------------------------------- /stage_worlds/worlds/turtlebot/camera.inc: -------------------------------------------------------------------------------- 1 | 2 | define turtlebot_camera camera 3 | ( 4 | resolution [640 480] 5 | fov [70 50] 6 | 7 | size [0.1 0.07 0.05] 8 | 9 | block( points 4 10 | point[0] [0 0] 11 | point[1] [0 1] 12 | point[2] [1 1] 13 | point[3] [1 0] 14 | z [0 0.21] 15 | ) 16 | ) 17 | -------------------------------------------------------------------------------- /stage_worlds/worlds/turtlebot/hokuyo.inc: -------------------------------------------------------------------------------- 1 | 2 | define hokuyo_common ranger 3 | ( 4 | sensor( 5 | # laser-specific properties 6 | range [ 0.0 60.0 ] 7 | fov 270.0 8 | samples 1080 9 | ) 10 | # generic model properties 11 | color "blue" 12 | size [ 0.07 0.07 0.05 ] # dimensions from LMS200 data sheet 13 | ) 14 | 15 | define hokuyo_base hokuyo_common 16 | ( 17 | block( points 4 18 | point[0] [0 0] 19 | point[1] [0 1] 20 | point[2] [1 1] 21 | point[3] [1 0] 22 | z [0 0.21] 23 | ) 24 | ) 25 | 26 | define hokuyolaser hokuyo_base() 27 | -------------------------------------------------------------------------------- /stage_worlds/worlds/turtlebot/turtlebot.inc: -------------------------------------------------------------------------------- 1 | include "turtlebot/hokuyo.inc" 2 | include "turtlebot/camera.inc" 3 | 4 | define turtlebot_base position 5 | ( 6 | color "red" # Default color. 7 | gui_nose 1 # Draw a nose on the robot so we can see which way it points 8 | obstacle_return 1 # Can hit things. 9 | ranger_return 0.5 # reflects sonar beams 10 | blob_return 1 # Seen by blobfinders 11 | fiducial_return 1 # Seen as "1" fiducial finders 12 | 13 | localization "gps" 14 | localization_origin [0 0 0 0] # Start odometry at (0, 0, 0). 15 | 16 | # alternative odometric localization with simple error model 17 | # localization "odom" # Change to "gps" to have impossibly perfect, global odometry 18 | # odom_error [0.03 0.03 999999 999999 999999 0.02] # Odometry error or slip in X, Y and Theta 19 | # (Uniform random distribution) 20 | 21 | # four DOF kinematics limits 22 | # [ xmin xmax ymin ymax zmin zmax amin amax ] 23 | velocity_bounds [-0.7 0.7 0 0 0 0 -180.0 180.0 ] 24 | acceleration_bounds [-2.0 2.0 0 0 0 0 -120.0 120.0 ] 25 | 26 | # actual size 27 | size [0.356 0.356 0.109] 28 | 29 | # the center of rotation is offset from its center of area 30 | origin [0 0 0 0] 31 | 32 | # draw a nose on the robot so we can see which way it points 33 | gui_nose 1 34 | 35 | # estimated mass in KG 36 | mass 2.4 37 | # differential steering model 38 | drive "diff" 39 | ) 40 | 41 | define turtlebot_base_sensors turtlebot_base 42 | ( 43 | hokuyolaser( pose [0.13 0 0 0]) 44 | turtlebot_camera( pose [0.13 0 0.05 0]) 45 | ) 46 | 47 | define turtlebot turtlebot_base_sensors 48 | ( 49 | size [0.356 0.356 0.109] 50 | size [0.356 0.356 0.109] 51 | # this block approximates the circular shape of a Roomba 52 | block( 53 | points 16 54 | point[0] [ 0.225 0.000 ] 55 | point[1] [ 0.208 0.086 ] 56 | point[2] [ 0.159 0.159 ] 57 | point[3] [ 0.086 0.208 ] 58 | point[4] [ 0.000 0.225 ] 59 | point[5] [ -0.086 0.208 ] 60 | point[6] [ -0.159 0.159 ] 61 | point[7] [ -0.208 0.086 ] 62 | point[8] [ -0.225 0.000 ] 63 | point[9] [ -0.208 -0.086 ] 64 | point[10] [ -0.159 -0.159 ] 65 | point[11] [ -0.086 -0.208 ] 66 | point[12] [ -0.000 -0.225 ] 67 | point[13] [ 0.086 -0.208 ] 68 | point[14] [ 0.159 -0.159 ] 69 | point[15] [ 0.208 -0.086 ] 70 | z [0 0.22] 71 | ) 72 | ) 73 | 74 | -------------------------------------------------------------------------------- /stage_worlds/worlds/window.inc: -------------------------------------------------------------------------------- 1 | window 2 | ( 3 | size [ 400 300 ] 4 | 5 | # camera options 6 | center [ 0 0 ] 7 | rotate [ 0 0 ] 8 | scale 30.0 9 | 10 | # perspective camera options 11 | pcam_loc [ 0 -4 2 ] 12 | pcam_angle [ 70 0 ] 13 | 14 | # GUI options 15 | show_data 0 16 | show_flags 1 17 | show_blocks 1 18 | show_clock 1 19 | show_footprints 0 20 | show_grid 1 21 | show_trailarrows 0 22 | show_trailrise 0 23 | show_trailfast 0 24 | show_occupancy 0 25 | pcam_on 0 26 | screenshots 0 27 | ) 28 | -------------------------------------------------------------------------------- /tools/learning_sync.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | THIS_PATH=$(pwd)/$(dirname $0) 4 | REMOTE_USER= 5 | REMOTE_HOST=129.132.39.88 6 | 7 | rsync -avz --exclude-from=$THIS_PATH/rsync_exclude -r $THIS_PATH/../deep_learning_model $REMOTE_USER@$REMOTE_HOST: 8 | -------------------------------------------------------------------------------- /tools/pull.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ "$#" -ne 1 ]; then 4 | echo "Usage: ./pull.sh " 5 | echo "Example: ./pull.sh 2016-08-05_22-02_convolutional" 6 | exit 1 7 | fi 8 | echo ${1} 9 | 10 | THIS_PATH=$(pwd)/$(dirname $0) 11 | REMOTE_USER= 12 | REMOTE_HOST=129.132.39.88 13 | 14 | scp -r $REMOTE_USER@$REMOTE_HOST:deep_learning_model/models/default/${1}/model.pb $THIS_PATH/../deep_motion_planner/models/ 15 | 16 | -------------------------------------------------------------------------------- /tools/rsync_exclude: -------------------------------------------------------------------------------- 1 | .git 2 | *.pyc 3 | *.h5 4 | *.csv 5 | *.tar.gz 6 | .tags 7 | *.tfrecords 8 | models/ 9 | -------------------------------------------------------------------------------- /tools/sync.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | REMOTE_USER= 4 | REMOTE_HOST= 5 | 6 | rsync -avz --exclude-from=./tools/rsync_exclude --port 22 ../deep_motion_planning \ 7 | $REMOTE_USER@$REMOTE_HOST: 8 | 9 | -------------------------------------------------------------------------------- /turtlebot_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ########### 2 | ## Setup ## 3 | ########### 4 | 5 | cmake_minimum_required(VERSION 2.8.3) 6 | project(turtlebot_controller) 7 | 8 | find_package(catkin_simple REQUIRED) 9 | catkin_simple(ALL_DEPS_REQUIRED) 10 | -------------------------------------------------------------------------------- /turtlebot_controller/launch/turtlebot_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /turtlebot_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | turtlebot_controller 3 | 0.0.0 4 | Intermediate controller for turtlebot 5 | mark 6 | 7 | TODO 8 | 9 | catkin 10 | catkin_simple 11 | rospy 12 | 13 | -------------------------------------------------------------------------------- /turtlebot_controller/src/turtlebot_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | 4 | import numpy as np 5 | import math 6 | 7 | # Messages 8 | from sensor_msgs.msg import LaserScan 9 | from geometry_msgs.msg import PoseStamped, Twist 10 | from nav_msgs.msg import Odometry 11 | 12 | class TurtlebotController(): 13 | """ 14 | Controller with integration for turtlebot in order to achieve accurate velocity tracking 15 | """ 16 | 17 | def __init__(self): 18 | self.measured_velocity = None 19 | self.integral_trans_vel = 0.0 20 | self.integral_rot_vel = 0.0 21 | self.last_control_time = rospy.Time.now() 22 | 23 | self.cmd_vel_sub = rospy.Subscriber('/deep_planner/cmd_vel', Twist, self.__cmd_vel_callback__) 24 | self.meas_vel_sub = rospy.Subscriber('/odom', Odometry, self.__meas_vel_callback__) 25 | self.controller_vel_pub = rospy.Publisher('/navigation_velocity_smoother/raw_cmd_vel', Twist, queue_size=1) 26 | 27 | # Controller weights 28 | self.ffwd_weight = 1.0 29 | self.trans_weight = 0.05 30 | self.rot_weight = 0.1 31 | 32 | 33 | def __cmd_vel_callback__(self, cmd_vel): 34 | new_time = rospy.Time.now() 35 | if self.measured_velocity.linear.x < 0.1 and self.measured_velocity.angular.z < 0.1 and self.measured_velocity is not None: 36 | self.integral_trans_vel += (cmd_vel.linear.x - self.measured_velocity.linear.x) / (new_time - self.last_control_time).to_sec() 37 | self.integral_rot_vel += (cmd_vel.angular.z - self.measured_velocity.angular.z) / (new_time - self.last_control_time).to_sec() 38 | cmd_vel_controller = Twist() 39 | cmd_vel_controller.linear.x = self.ffwd_weight*cmd_vel.linear.x + self.trans_weight*self.integral_trans_vel 40 | cmd_vel_controller.angular.z = self.ffwd_weight*cmd_vel.angular.z + self.rot_weight*self.integral_rot_vel 41 | rospy.logdebug("cmd_vel = ({0}, {1}) \t meas_vel = ({2}, {3})".format(cmd_vel.linear.x, cmd_vel.angular.z, self.measured_velocity.linear.x, self.measured_velocity.angular.z)) 42 | rospy.logdebug("integral part = ({0}, {1})".format(self.integral_trans_vel, self.integral_rot_vel)) 43 | rospy.logdebug("resulting command = ({0}, {1})".format(cmd_vel_controller.linear.x, cmd_vel_controller.angular.z)) 44 | self.controller_vel_pub.publish(cmd_vel_controller) 45 | else: 46 | # Reset integrators 47 | rospy.logdebug("Resetting integrators.") 48 | self.integral_trans_vel = 0.0 49 | self.integral_rot_vel = 0.0 50 | self.controller_vel_pub.publish(cmd_vel) 51 | 52 | def __meas_vel_callback__(self, odom): 53 | self.measured_velocity = odom.twist.twist 54 | 55 | def __enter__(self): 56 | return self 57 | 58 | def __exit__(self, exc_type, exc_value, traceback): 59 | return self -------------------------------------------------------------------------------- /turtlebot_controller/src/turtlebot_controller_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | from turtlebot_controller import TurtlebotController 5 | 6 | def main(): 7 | 8 | rospy.init_node('turtlebot_controller') 9 | 10 | with TurtlebotController() as controller: 11 | rospy.spin() 12 | 13 | if __name__ == "__main__": 14 | main() 15 | -------------------------------------------------------------------------------- /visualization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ########### 2 | ## Setup ## 3 | ########### 4 | 5 | cmake_minimum_required(VERSION 2.8.3) 6 | project(visualization) 7 | 8 | find_package(catkin_simple REQUIRED) 9 | catkin_simple(ALL_DEPS_REQUIRED) 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /visualization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | visualization 3 | 0.0.0 4 | Visualization tools 5 | pfmark 6 | TODO 7 | catkin 8 | catkin_simple 9 | rospy 10 | 11 | -------------------------------------------------------------------------------- /visualization/src/laser_visualization_2d.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | 4 | import cv2 5 | import numpy as np 6 | import math 7 | import tf 8 | 9 | # Messages 10 | from sensor_msgs.msg import LaserScan 11 | from geometry_msgs.msg import PoseStamped 12 | 13 | class LaserVisualization2d(): 14 | """ 15 | Visualize laser data of 2D laser range finders 16 | """ 17 | def __init__(self): 18 | self.laser_sub = rospy.Subscriber("/base_scan", LaserScan, self.laser_scan_callback) 19 | self.relative_target_sub = rospy.Subscriber("/relative_target", PoseStamped, self.relative_target_callback) 20 | self.relative_target = None 21 | self.relative_target_update_time = None 22 | 23 | def laser_scan_callback(self, data): 24 | frame = np.ones((500, 500,3), np.uint8)*255 25 | 26 | # Laser scans 27 | angle = data.angle_min 28 | for r in data.ranges: 29 | x = math.trunc((r * 30) * math.cos(angle + (-90*math.pi/180))) 30 | y = math.trunc((r * 30) * math.sin(angle + (-90*math.pi/180))) 31 | cv2.line(frame, (250, 250), (-x+250,y+250), (0,0,255), 2) 32 | angle= angle + data.angle_increment 33 | 34 | 35 | # Target 36 | if self.do_plot_target(): 37 | target_pos = (250+int(self.relative_target[1]*30), 250-int(self.relative_target[0]*30)) 38 | length = 15.0 39 | color_target = (255, 100, 0) 40 | x = math.trunc(length * math.cos(self.relative_target[2] - math.pi/2)) 41 | y = math.trunc(length * math.sin(self.relative_target[2] - math.pi/2)) 42 | cv2.circle(img=frame, center=target_pos, radius=4, thickness=2, color=color_target) 43 | cv2.line(frame, target_pos, (-x+target_pos[0],y+target_pos[1]), color_target, 2) 44 | 45 | # Robot 46 | cv2.circle(img=frame, center=(250, 250), radius=4, thickness=7, color=(0, 0, 0)) 47 | cv2.line(frame, (250, 250), (250,244), (255,255,255), 2) 48 | cv2.imshow('laser',frame) 49 | cv2.waitKey(1) 50 | 51 | def relative_target_callback(self, data): 52 | x = data.pose.position.x 53 | y = data.pose.position.y 54 | phi = tf.transformations.euler_from_quaternion([data.pose.orientation.x, 55 | data.pose.orientation.y, 56 | data.pose.orientation.z, 57 | data.pose.orientation.w])[2] 58 | self.relative_target = [x, y, phi] 59 | self.relative_target_update_time = rospy.Time.now() 60 | 61 | def do_plot_target(self): 62 | if self.relative_target_update_time == None or self.relative_target == None: return False 63 | if (rospy.Time.now() - self.relative_target_update_time).to_sec() > 2: return False 64 | return True 65 | 66 | def __enter__(self): 67 | return self 68 | 69 | def __exit__(self, exc_type, exc_value, traceback): 70 | return self 71 | -------------------------------------------------------------------------------- /visualization/src/laser_visualization_2d_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | from laser_visualization_2d import LaserVisualization2d 5 | 6 | def main(): 7 | 8 | rospy.init_node('laser_visualization') 9 | 10 | with LaserVisualization2d() as vis: 11 | rospy.spin() 12 | 13 | if __name__ == "__main__": 14 | main() 15 | --------------------------------------------------------------------------------