├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── package.xml ├── scripts ├── waypoint_generator.py └── waypoints.yaml └── usage.png /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | MANIFEST 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | .pytest_cache/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | db.sqlite3 58 | 59 | # Flask stuff: 60 | instance/ 61 | .webassets-cache 62 | 63 | # Scrapy stuff: 64 | .scrapy 65 | 66 | # Sphinx documentation 67 | docs/_build/ 68 | 69 | # PyBuilder 70 | target/ 71 | 72 | # Jupyter Notebook 73 | .ipynb_checkpoints 74 | 75 | # pyenv 76 | .python-version 77 | 78 | # celery beat schedule file 79 | celerybeat-schedule 80 | 81 | # SageMath parsed files 82 | *.sage.py 83 | 84 | # Environments 85 | .env 86 | .venv 87 | env/ 88 | venv/ 89 | ENV/ 90 | env.bak/ 91 | venv.bak/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # Rope project settings 98 | .ropeproject 99 | 100 | # mkdocs documentation 101 | /site 102 | 103 | # mypy 104 | .mypy_cache/ 105 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(waypoint_generator) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | geometry_msgs 7 | ) 8 | 9 | catkin_package( 10 | CATKIN_DEPENDS rospy geometry_msgs 11 | ) 12 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, Jihoon Lee, Zhi Yan 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 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. 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 | 3. 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 | # waypoint_generator 2 | 3 | [![Build Status](https://travis-ci.org/epan-utbm/waypoint_generator.svg?branch=master)](https://travis-ci.org/epan-utbm/waypoint_generator) [![Codacy Badge](https://app.codacy.com/project/badge/Grade/75325bd3876a4a27810e8e308fea27c3)](https://www.codacy.com/gh/epan-utbm/waypoint_generator?utm_source=github.com&utm_medium=referral&utm_content=epan-utbm/waypoint_generator&utm_campaign=Badge_Grade) [![License](https://img.shields.io/badge/License-BSD%203--Clause-gree.svg)](https://opensource.org/licenses/BSD-3-Clause) 4 | 5 | Modified from [https://github.com/jihoonl/waypoint/tree/master/waypoint_generator](https://github.com/jihoonl/waypoint/tree/master/waypoint_generator) with "orientation" added. 6 | 7 | ## Usage 8 | 9 | ```console 10 | $ rosrun waypoint_generator waypoint_generator.py 11 | ``` 12 | 13 | or 14 | 15 | ```console 16 | $ cd waypoint_generator/scripts 17 | $ chmod +x waypoint_generator.py 18 | $ ./waypoint_generator.py 19 | ``` 20 | 21 | ![usage.png](usage.png) 22 | 23 | *Tested with Ubuntu 16.04 LTS + ROS Kinetic* 24 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | waypoint_generator 4 | 0.0.1 5 | Generates waypoint yaml file 6 | 7 | Jihoon Lee 8 | Zhi Yan 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | geometry_msgs 15 | rospy 16 | 17 | geometry_msgs 18 | rospy 19 | 20 | -------------------------------------------------------------------------------- /scripts/waypoint_generator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import yaml 4 | import rospy 5 | import geometry_msgs.msg as geometry_msgs 6 | 7 | class WaypointGenerator(object): 8 | 9 | def __init__(self, filename): 10 | """init""" 11 | self._sub_pose = rospy.Subscriber('/goal', geometry_msgs.PoseStamped, self._process_pose, queue_size=1) 12 | self._waypoints = [] 13 | self._filename = filename 14 | 15 | def _process_pose(self, msg): 16 | p = msg.pose 17 | 18 | data = {} 19 | data['frame_id'] = msg.header.frame_id 20 | data['pose'] = {} 21 | data['pose']['position'] = {'x': p.position.x, 'y': p.position.y, 'z': p.position.z} 22 | data['pose']['orientation'] = {'x': p.orientation.x, 'y': p.orientation.y, 'z': p.orientation.z, 'w':p.orientation.w} 23 | data['name'] = '%s_%s' % (p.position.x, p.position.y) 24 | 25 | self._waypoints.append(data) 26 | rospy.loginfo("Clicked : (%s, %s, %s)" % (p.position.x, p.position.y, p.position.z)) 27 | 28 | def _write_file(self): 29 | ways = {} 30 | ways['waypoints'] = self._waypoints 31 | with open(self._filename, 'w') as f: 32 | f.write(yaml.dump(ways, default_flow_style=False)) 33 | 34 | def spin(self): 35 | rospy.spin() 36 | self._write_file() 37 | 38 | 39 | if __name__ == '__main__': 40 | 41 | rospy.init_node('waypoint_generator') 42 | filename = rospy.get_param('~filename', 'waypoints.yaml') 43 | 44 | g = WaypointGenerator(filename) 45 | rospy.loginfo('Initialized') 46 | g.spin() 47 | rospy.loginfo('ByeBye') 48 | -------------------------------------------------------------------------------- /scripts/waypoints.yaml: -------------------------------------------------------------------------------- 1 | waypoints: 2 | - frame_id: map 3 | name: -0.364745616913_-0.388486862183 4 | pose: 5 | orientation: 6 | w: 0.6959756819897694 7 | x: 0.0 8 | y: 0.0 9 | z: -0.7180653522339561 10 | position: 11 | x: -0.3647456169128418 12 | y: -0.3884868621826172 13 | z: 0.0 14 | - frame_id: map 15 | name: -1.22324705124_1.35261678696 16 | pose: 17 | orientation: 18 | w: 0.3913613242630964 19 | x: 0.0 20 | y: 0.0 21 | z: 0.9202370965523154 22 | position: 23 | x: -1.2232470512390137 24 | y: 1.352616786956787 25 | z: 0.0 26 | - frame_id: map 27 | name: 1.1960940361_0.959330797195 28 | pose: 29 | orientation: 30 | w: 0.9847152205320973 31 | x: 0.0 32 | y: 0.0 33 | z: 0.17417214028777084 34 | position: 35 | x: 1.196094036102295 36 | y: 0.9593307971954346 37 | z: 0.0 38 | -------------------------------------------------------------------------------- /usage.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epan-utbm/waypoint_generator/ac234219dab3409ccb0f3f4b3ad69a6f0f12f77b/usage.png --------------------------------------------------------------------------------