├── route_network ├── src │ └── route_network │ │ ├── __init__.py │ │ └── planner.py ├── mainpage.dox ├── .gitignore ├── rosdoc.yaml ├── route_network.dia ├── tests │ ├── planner.test │ └── test_planner.py ├── route_network.planner.rst ├── launch │ ├── viz_plan.launch │ └── viz_routes.launch ├── setup.py ├── index.rst ├── package.xml ├── CMakeLists.txt ├── CHANGELOG.rst ├── cfg │ └── RouteNetwork.cfg ├── scripts │ ├── viz_plan │ ├── viz_routes │ ├── plan_route │ ├── route_network │ └── rviz_goal └── conf.py ├── osm_cartography ├── src │ └── osm_cartography │ │ ├── __init__.py │ │ ├── geo_map.py │ │ └── xml_map.py ├── tests │ ├── empty.osm │ ├── tiny.osm │ ├── test_viz_osm.launch │ ├── test_xml_map.py │ ├── test_geo_map.py │ └── arl-building.osm ├── mainpage.dox ├── rosdoc.yaml ├── .gitignore ├── osm_cartography.geo_map.rst ├── osm_cartography.xml_map.rst ├── setup.py ├── launch │ ├── viz_osm.launch │ └── geo_planner.launch ├── index.rst ├── package.xml ├── CMakeLists.txt ├── CHANGELOG.rst ├── cfg │ └── VizOSM.cfg ├── scripts │ ├── osm_client │ ├── osm_server │ └── viz_osm ├── rviz │ └── geo_planner.rviz └── conf.py ├── test_osm ├── .gitignore ├── CMakeLists.txt ├── CHANGELOG.rst ├── tests │ └── osm_test.launch ├── package.xml └── rviz │ ├── prc.vcg │ └── small-screen-prc.vcg └── README.rst /route_network/src/route_network/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /osm_cartography/src/osm_cartography/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /osm_cartography/tests/empty.osm: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /osm_cartography/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | */ 6 | -------------------------------------------------------------------------------- /route_network/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | */ 6 | -------------------------------------------------------------------------------- /test_osm/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.cfgc 3 | *.pyc 4 | cfg/cpp 5 | docs 6 | src/osm_cartography 7 | -------------------------------------------------------------------------------- /osm_cartography/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | name: Python API 3 | output_dir: python 4 | -------------------------------------------------------------------------------- /route_network/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.cfgc 3 | *.pyc 4 | cfg/cpp 5 | docs 6 | src/route_network 7 | -------------------------------------------------------------------------------- /route_network/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | name: Python API 3 | output_dir: python 4 | -------------------------------------------------------------------------------- /osm_cartography/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.cfgc 3 | *.pyc 4 | cfg/cpp 5 | docs 6 | src/osm_cartography 7 | -------------------------------------------------------------------------------- /route_network/route_network.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-geographic-info/open_street_map/HEAD/route_network/route_network.dia -------------------------------------------------------------------------------- /route_network/tests/planner.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /route_network/route_network.planner.rst: -------------------------------------------------------------------------------- 1 | route_network.planner 2 | --------------------- 3 | 4 | .. automodule:: route_network.planner 5 | :members: 6 | -------------------------------------------------------------------------------- /osm_cartography/osm_cartography.geo_map.rst: -------------------------------------------------------------------------------- 1 | osm_cartography.geo_map 2 | ----------------------- 3 | 4 | .. automodule:: osm_cartography.geo_map 5 | :members: 6 | -------------------------------------------------------------------------------- /osm_cartography/osm_cartography.xml_map.rst: -------------------------------------------------------------------------------- 1 | osm_cartography.xml_map 2 | ----------------------- 3 | 4 | .. automodule:: osm_cartography.xml_map 5 | :members: 6 | -------------------------------------------------------------------------------- /route_network/launch/viz_plan.launch: -------------------------------------------------------------------------------- 1 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /osm_cartography/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages=["osm_cartography"], 6 | package_dir={"": "src"}, 7 | install_requires=[ 8 | "rospy", 9 | "geodesy", 10 | "geographic_msgs", 11 | "geometry_msgs", 12 | "std_msgs", 13 | "visualization_msgs", 14 | ], 15 | ) 16 | 17 | setup(**d) 18 | -------------------------------------------------------------------------------- /route_network/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages=["route_network"], 6 | package_dir={"": "src"}, 7 | install_requires=[ 8 | "rospy", 9 | "geodesy", 10 | "geographic_msgs", 11 | "geometry_msgs", 12 | "nav_msgs", 13 | "rospy", 14 | "visualization_msgs", 15 | ], 16 | ) 17 | 18 | setup(**d) 19 | -------------------------------------------------------------------------------- /route_network/index.rst: -------------------------------------------------------------------------------- 1 | .. route_network documentation master file, created by 2 | sphinx-quickstart on Wed Apr 25 16:01:20 2012. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | route_network: Route network graphing 7 | ===================================== 8 | 9 | Contents: 10 | 11 | .. toctree:: 12 | :maxdepth: 2 13 | 14 | route_network.planner 15 | 16 | Indices and tables 17 | ================== 18 | 19 | * :ref:`genindex` 20 | * :ref:`modindex` 21 | 22 | -------------------------------------------------------------------------------- /osm_cartography/launch/viz_osm.launch: -------------------------------------------------------------------------------- 1 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /route_network/launch/viz_routes.launch: -------------------------------------------------------------------------------- 1 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /osm_cartography/index.rst: -------------------------------------------------------------------------------- 1 | .. osm_cartography documentation master file, created by 2 | sphinx-quickstart on Wed Apr 25 16:01:20 2012. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | osm_cartography: Geographic mapping using Open Street Map data 7 | ============================================================== 8 | 9 | Contents: 10 | 11 | .. toctree:: 12 | :maxdepth: 2 13 | 14 | osm_cartography.geo_map 15 | osm_cartography.xml_map 16 | 17 | Indices and tables 18 | ================== 19 | 20 | * :ref:`genindex` 21 | * :ref:`modindex` 22 | 23 | -------------------------------------------------------------------------------- /test_osm/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(test_osm) 3 | 4 | # list catkin dependencies 5 | set(_ROS_PKG_DEPS 6 | osm_cartography 7 | geodesy 8 | geographic_msgs 9 | route_network) 10 | 11 | # Find catkin macros and libraries, also other catkin packages. 12 | find_package(catkin REQUIRED COMPONENTS ${_ROS_PKG_DEPS} rostest) 13 | 14 | catkin_package(CATKIN_DEPENDS ${_ROS_PKG_DEPS}) 15 | 16 | # install some useful files 17 | install(DIRECTORY tests/ 18 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/tests) 19 | install(DIRECTORY rviz/ 20 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz) 21 | -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | Overview 2 | ======== 3 | 4 | `Open_street_map`_ is a repository of ROS_ packages for working with 5 | `Open Street Map`_ information. 6 | 7 | **Warning**:: 8 | 9 | The master branch normally contains code being tested for the next 10 | ROS release. It will not always work with all previous releases. 11 | Sometimes, it may not work at all. 12 | 13 | The current master works with Hydro and Indigo. To build for Fuerte 14 | or Groovy, check out the rosbuild branch instead of master. 15 | 16 | .. _`Open Street Map`: http://openstreetmap.org 17 | .. _`Open_street_map`: http://www.ros.org/wiki/open_street_map 18 | .. _ROS: http://www.ros.org 19 | -------------------------------------------------------------------------------- /test_osm/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | Change history 2 | ============== 3 | 4 | 0.3.0 (2021-06-03) 5 | ------------------ 6 | 7 | 0.2.5 (2020-01-14) 8 | ------------------ 9 | 10 | 0.2.4 (2017-12-06) 11 | ------------------ 12 | * Convert to package xml format 2, add rostest dependency 13 | * Contributors: Bence Magyar 14 | 15 | 0.2.3 (2015-10-12) 16 | ------------------ 17 | 18 | 0.2.2 (2015-10-08) 19 | ------------------ 20 | 21 | 0.2.1 (2013-09-24) 22 | ------------------ 23 | 24 | 0.2.0 (2013-09-18) 25 | ------------------ 26 | 27 | * Convert to catkin (`#2`_). 28 | * Release to Hydro. 29 | 30 | 0.1.0 (2012-07-16) 31 | ------------------ 32 | 33 | * initial experimental release to Electric, Fuerte and Groovy, uses 34 | robuild. 35 | 36 | .. _`#2`: https://github.com/ros-geographic-info/open_street_map/issues/2 37 | -------------------------------------------------------------------------------- /test_osm/tests/osm_test.launch: -------------------------------------------------------------------------------- 1 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /osm_cartography/tests/tiny.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /test_osm/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | test_osm 4 | 0.3.0 5 | 6 | These are regression tests for the osm_cartography and 7 | route_network packages. They are packaged separately to avoid 8 | unnecessary implementation dependencies. 9 | 10 | Jack O'Quin 11 | Jack O'Quin 12 | BSD 13 | 14 | http://ros.org/wiki/test_osm 15 | https://github.com/ros-geographic-info/open_street_map.git 16 | https://github.com/ros-geographic-info/open_street_map/issues 17 | 18 | catkin 19 | 20 | osm_cartography 21 | route_network 22 | geodesy 23 | geographic_msgs 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /route_network/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | route_network 4 | 0.3.0 5 | 6 | Route network graphing and path planning. 7 | 8 | Jack O'Quin 9 | Jack O'Quin 10 | BSD 11 | 12 | http://ros.org/wiki/route_network 13 | https://github.com/ros-geographic-info/open_street_map.git 14 | https://github.com/ros-geographic-info/open_street_map/issues 15 | 16 | catkin 17 | 18 | dynamic_reconfigure 19 | geodesy 20 | geographic_msgs 21 | geometry_msgs 22 | nav_msgs 23 | rospy 24 | visualization_msgs 25 | 26 | rostest 27 | roslaunch 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /osm_cartography/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | osm_cartography 4 | 0.3.0 5 | 6 | Geographic mapping using Open Street Map data. 7 | 8 | Jack O'Quin 9 | Jack O'Quin 10 | BSD 11 | 12 | http://ros.org/wiki/osm_cartography 13 | https://github.com/ros-geographic-info/open_street_map.git 14 | https://github.com/ros-geographic-info/open_street_map/issues 15 | 16 | catkin 17 | 18 | dynamic_reconfigure 19 | geodesy 20 | geographic_msgs 21 | geometry_msgs 22 | rospy 23 | tf 24 | std_msgs 25 | visualization_msgs 26 | 27 | route_network 28 | 29 | roslaunch 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /osm_cartography/tests/test_viz_osm.launch: -------------------------------------------------------------------------------- 1 | 14 | 15 | 16 | 17 | 18 | 19 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /route_network/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(route_network) 3 | 4 | # list catkin dependencies 5 | set(_ROS_PKG_DEPS 6 | dynamic_reconfigure 7 | geodesy 8 | geographic_msgs 9 | geometry_msgs 10 | nav_msgs 11 | rospy 12 | visualization_msgs) 13 | 14 | 15 | # Find catkin macros and libraries, also other catkin packages. 16 | find_package(catkin REQUIRED COMPONENTS ${_ROS_PKG_DEPS}) 17 | include_directories(include ${catkin_INCLUDE_DIRS}) 18 | catkin_python_setup() 19 | 20 | # auto-generate dynamic reconfiguration GUI: before catkin_package() 21 | # and after catkin_python_setup(). 22 | generate_dynamic_reconfigure_options(cfg/RouteNetwork.cfg) 23 | 24 | catkin_package(CATKIN_DEPENDS ${_ROS_PKG_DEPS}) 25 | 26 | install(DIRECTORY scripts/ 27 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 28 | USE_SOURCE_PERMISSIONS) 29 | 30 | install(DIRECTORY launch/ 31 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 32 | 33 | # unit tests 34 | if (CATKIN_ENABLE_TESTING) 35 | 36 | find_package(catkin REQUIRED rostest roslaunch) 37 | 38 | add_rostest(tests/planner.test) 39 | 40 | roslaunch_add_file_check(launch) 41 | 42 | endif (CATKIN_ENABLE_TESTING) 43 | -------------------------------------------------------------------------------- /osm_cartography/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(osm_cartography) 3 | 4 | # list catkin dependencies 5 | set(_ROS_PKG_DEPS 6 | dynamic_reconfigure 7 | geodesy 8 | geographic_msgs 9 | geometry_msgs 10 | rospy 11 | tf 12 | std_msgs 13 | visualization_msgs) 14 | 15 | 16 | # Find catkin macros and libraries, also other catkin packages. 17 | find_package(catkin REQUIRED COMPONENTS ${_ROS_PKG_DEPS}) 18 | include_directories(include ${catkin_INCLUDE_DIRS}) 19 | catkin_python_setup() 20 | 21 | # auto-generate dynamic reconfiguration GUI: before catkin_package() 22 | # and after catkin_python_setup(). 23 | generate_dynamic_reconfigure_options(cfg/VizOSM.cfg) 24 | 25 | catkin_package(CATKIN_DEPENDS ${_ROS_PKG_DEPS}) 26 | 27 | install(DIRECTORY scripts/ 28 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 29 | USE_SOURCE_PERMISSIONS) 30 | 31 | install(DIRECTORY launch/ 32 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 33 | 34 | # unit tests 35 | if (CATKIN_ENABLE_TESTING) 36 | 37 | catkin_add_nosetests(tests/test_geo_map.py) 38 | catkin_add_nosetests(tests/test_xml_map.py) 39 | 40 | # check all the launch files 41 | find_package(roslaunch REQUIRED) 42 | roslaunch_add_file_check(launch) 43 | 44 | endif (CATKIN_ENABLE_TESTING) 45 | -------------------------------------------------------------------------------- /route_network/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | Change history 2 | ============== 3 | 4 | 0.3.0 (2021-06-03) 5 | ------------------ 6 | * Migrate to Noetic 7 | * Contributors: Bence Magyar 8 | 9 | 0.2.5 (2020-01-14) 10 | ------------------ 11 | 12 | 0.2.4 (2017-12-06) 13 | ------------------ 14 | * Convert to package xml format 2 and add launch file dependencies 15 | * Create new launch file to start all the needed nodes to plan on a map. 16 | * Update to use the original version of the GeoPath msg and the new version of the GetGeoPath srv. 17 | * Complete function documentation/comments for the new features. 18 | * Add an RVIZ interface to plan and directly select start and goal positions by clicking on the map. 19 | * Add the possibility to plan from and to geographic points that are not nodes of the graph, but rather lie nearby. 20 | * Contributors: Bence Magyar, Diego Ramos 21 | 22 | 0.2.3 (2015-10-12) 23 | ------------------ 24 | 25 | * Fix route planner test failure (`#3`_). 26 | 27 | 0.2.2 (2015-10-08) 28 | ------------------ 29 | 30 | * Add publisher ``queue_size`` for Indigo (`#3`_). 31 | * Remove references to deprecated ``geodesy.gen_uuid`` module. 32 | * Release to Indigo. 33 | * Contributors: Augusto Luis Ballardini, Jack O'Quin 34 | 35 | 0.2.1 (2013-09-24) 36 | ------------------ 37 | 38 | * Install missing launch scripts (`#2`_). 39 | 40 | 0.2.0 (2013-09-18) 41 | ------------------ 42 | 43 | * Convert to catkin (`#2`_). 44 | * Release to Hydro. 45 | 46 | 0.1.0 (2012-07-16) 47 | ------------------ 48 | 49 | * initial experimental release to Electric, Fuerte and Groovy, uses 50 | robuild. 51 | 52 | .. _`#2`: https://github.com/ros-geographic-info/open_street_map/issues/2 53 | .. _`#3`: https://github.com/ros-geographic-info/open_street_map/issues/3 54 | -------------------------------------------------------------------------------- /osm_cartography/tests/test_xml_map.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import unittest 4 | 5 | from geodesy import bounding_box 6 | 7 | from osm_cartography import xml_map 8 | 9 | 10 | class TestXmlMap(unittest.TestCase): 11 | """Unit tests for OSM XML map parser.""" 12 | 13 | def test_tiny_osm_file(self): 14 | # :todo: deeper results verification 15 | m = xml_map.get_osm( 16 | "package://osm_cartography/tests/tiny.osm", bounding_box.makeGlobal() 17 | ) 18 | self.assertEqual(len(m.points), 3) 19 | self.assertEqual(len(m.features), 2) 20 | 21 | def test_prc_osm_file(self): 22 | # :todo: deeper results verification 23 | m = xml_map.get_osm( 24 | "package://osm_cartography/tests/prc.osm", bounding_box.makeGlobal() 25 | ) 26 | self.assertEqual(len(m.points), 986) 27 | self.assertEqual(len(m.features), 84) 28 | 29 | def test_invalid_url(self): 30 | self.assertRaises( 31 | ValueError, 32 | xml_map.get_osm, 33 | "ftp://osm_cartography/tests/prc.osm", 34 | bounding_box.makeGlobal(), 35 | ) 36 | 37 | def test_empty_osm_file(self): 38 | self.assertRaises( 39 | ValueError, 40 | xml_map.get_osm, 41 | "package://osm_cartography/tests/empty.osm", 42 | bounding_box.makeGlobal(), 43 | ) 44 | 45 | def test_missing_osm_file(self): 46 | self.assertRaises( 47 | ValueError, 48 | xml_map.get_osm, 49 | "package://osm_cartography/tests/missing.osm", 50 | bounding_box.makeGlobal(), 51 | ) 52 | 53 | 54 | if __name__ == "__main__": 55 | import rosunit 56 | 57 | rosunit.unitrun("osm_cartography", "test_xml_map_py", TestXmlMap) 58 | -------------------------------------------------------------------------------- /osm_cartography/launch/geo_planner.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 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /osm_cartography/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | Change history 2 | ============== 3 | 4 | 0.3.0 (2021-06-03) 5 | ------------------ 6 | * Migrate to Noetic 7 | * Contributors: Bence Magyar 8 | 9 | 0.2.5 (2020-01-14) 10 | ------------------ 11 | * Remove dependency on rviz from osm_cartography (`#18 `_) 12 | rviz isn't a strict requirement for running the map server and people who want to use the rviz functionality probably already have rviz installed. 13 | * Publish static marker with time 0. (`#14 `_) 14 | * Contributors: Ronald Ensing, Will Gardner 15 | 16 | 0.2.4 (2017-12-06) 17 | ------------------ 18 | * Convert to package xml format 2 and add launch file dependencies 19 | * Possibility to compute paths between GeoPoints. 20 | * Remove unused map file. 21 | * Add Ixtapa testing map. 22 | * Add default configuration for RVIZ. 23 | * Create new launch file to start all the needed nodes to plan on a map. 24 | * Contributors: Bence Magyar, Diego Ramos 25 | 26 | 0.2.3 (2015-10-12) 27 | ------------------ 28 | 29 | * move static transform from viz_osm.launch to 30 | tests/test_viz_osm.launch (`#5`_) 31 | 32 | 0.2.2 (2015-10-08) 33 | ------------------ 34 | 35 | * Add publisher ``queue_size`` for Indigo (`#3`_). 36 | * Remove references to deprecated ``geodesy.gen_uuid`` module. 37 | * Release to Indigo. 38 | * Contributors: Augusto Luis Ballardini, Jack O'Quin 39 | 40 | 0.2.1 (2013-09-24) 41 | ------------------ 42 | 43 | * Install missing launch scripts (`#2`_). 44 | 45 | 0.2.0 (2013-09-18) 46 | ------------------ 47 | 48 | * Convert to catkin (`#2`_). 49 | * Release to Hydro. 50 | 51 | 0.1.0 (2012-07-16) 52 | ------------------ 53 | 54 | * initial experimental release to Electric, Fuerte and Groovy, uses 55 | robuild. 56 | 57 | .. _`#2`: https://github.com/ros-geographic-info/open_street_map/issues/2 58 | .. _`#3`: https://github.com/ros-geographic-info/open_street_map/issues/3 59 | .. _`#5`: https://github.com/ros-geographic-info/open_street_map/issues/5 60 | -------------------------------------------------------------------------------- /test_osm/rviz/prc.vcg: -------------------------------------------------------------------------------- 1 | Background\ ColorB=0 2 | Background\ ColorG=0 3 | Background\ ColorR=0 4 | Camera\ Config=0.950398 4.71358 2418 0 0 0 5 | Camera\ Type=rviz::OrbitViewController 6 | Fixed\ Frame=/local_map 7 | Grid.Alpha=0.5 8 | Grid.Cell\ Size=100 9 | Grid.ColorB=0.5 10 | Grid.ColorG=0.5 11 | Grid.ColorR=0.5 12 | Grid.Enabled=1 13 | Grid.Line\ Style=0 14 | Grid.Line\ Width=0.03 15 | Grid.Normal\ Cell\ Count=0 16 | Grid.OffsetX=0 17 | Grid.OffsetY=0 18 | Grid.OffsetZ=0 19 | Grid.Plane=0 20 | Grid.Plane\ Cell\ Count=10 21 | Grid.Reference\ Frame=/local_map 22 | Marker.Enabled=1 23 | Marker.Marker\ Topic=visualization_marker 24 | Marker.Queue\ Size=100 25 | Marker.bounds_osm=1 26 | Marker.buildings_osm=1 27 | Marker.other_osm=1 28 | Marker.railroad_osm=1 29 | Marker.roads_osm=0 30 | Marker.route_segments=1 31 | Marker.route_waypoints=1 32 | Marker.waypoints_osm=1 33 | Property\ Grid\ Splitter=778,78 34 | Property\ Grid\ State=expanded=.Global Options,Grid.Enabled,Marker.Enabled,Marker.Namespaces;splitterratio=0.5 35 | QMainWindow=000000ff00000000fd00000003000000000000011d000003aafc0200000001fb000000100044006900730070006c006100790073010000001d000003aa000000ee00ffffff0000000100000130000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000001d000000db0000006700fffffffb0000000a00560069006500770073000000001d000003a2000000bb00fffffffb0000001200530065006c0065006300740069006f006e00000002430000012b0000006700ffffff00000003000005320000003efc0100000001fb0000000800540069006d0065010000000000000532000002bf00ffffff0000040f000003aa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 36 | TF.All\ Enabled=1 37 | TF.Enabled=0 38 | TF.Frame\ Timeout=15 39 | TF.Marker\ Scale=1 40 | TF.Show\ Arrows=0 41 | TF.Show\ Axes=1 42 | TF.Show\ Names=1 43 | TF.Update\ Interval=0 44 | Target\ Frame=/local_map 45 | Tool\ 2D\ Nav\ GoalTopic=/move_base_simple/goal 46 | Tool\ 2D\ Pose\ EstimateTopic=initialpose 47 | [Display0] 48 | ClassName=rviz::GridDisplay 49 | Name=Grid 50 | [Display1] 51 | ClassName=rviz::MarkerDisplay 52 | Name=Marker 53 | [Display2] 54 | ClassName=rviz::TFDisplay 55 | Name=TF 56 | [Window] 57 | Height=1072 58 | Width=1346 59 | X=438 60 | Y=83 61 | -------------------------------------------------------------------------------- /osm_cartography/cfg/VizOSM.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (C) 2012, Jack O'Quin 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the author nor of other contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | 34 | # OSM data visualization node dynamic configuration 35 | 36 | from dynamic_reconfigure.parameter_generator_catkin import * 37 | 38 | # only includes parameters reconfigurable while running 39 | 40 | gen = ParameterGenerator() 41 | 42 | # Name, Type, Reconfiguration level, Description, Default, Min, Max 43 | 44 | gen.add("map_url", str_t, 0x00000001, 45 | "URL for locating XML map data (existing map, if empty).", "") 46 | 47 | PACKAGE='osm_cartography' 48 | exit(gen.generate(PACKAGE, "viz_osm.py", "VizOSM")) 49 | -------------------------------------------------------------------------------- /route_network/cfg/RouteNetwork.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (C) 2012, Jack O'Quin 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the author nor of other contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | 34 | # Route network visualization node dynamic configuration 35 | 36 | from dynamic_reconfigure.parameter_generator_catkin import * 37 | 38 | # only includes parameters reconfigurable while running 39 | 40 | gen = ParameterGenerator() 41 | 42 | # Name, Type, Reconfiguration level, Description, Default, Min, Max 43 | 44 | gen.add("map_url", str_t, 0x00000001, 45 | "URL for locating XML map data (existing map, if empty).", "") 46 | 47 | PACKAGE='route_network' 48 | exit(gen.generate(PACKAGE, "route_network.py", "RouteNetwork")) 49 | -------------------------------------------------------------------------------- /osm_cartography/tests/test_geo_map.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import unittest 4 | 5 | try: 6 | import unique_id 7 | except ImportError: 8 | pass 9 | 10 | from geographic_msgs.msg import GeographicMap 11 | 12 | from geodesy import bounding_box 13 | from osm_cartography.geo_map import * 14 | from osm_cartography import xml_map 15 | 16 | # def fromLatLong(lat, lon, alt=float('nan')): 17 | # """Generate WayPoint from latitude, longitude and (optional) altitude. 18 | # 19 | # :returns: minimal WayPoint object just for test cases. 20 | # """ 21 | # geo_pt = GeoPoint(latitude = lat, longitude = lon, altitude = alt) 22 | # return WayPoint(position = geo_pt) 23 | 24 | 25 | class TestGeoMap(unittest.TestCase): 26 | """Unit tests for GeoMap class.""" 27 | 28 | def test_empty_map_features(self): 29 | gm = GeoMap(GeographicMap()) 30 | self.assertEqual(gm.n_features, 0) 31 | 32 | # test GeoMapFeatures iterator with empty list 33 | gf = GeoMapFeatures(gm) 34 | i = 0 35 | for f in gf: 36 | self.fail(msg="there are no features in this map") 37 | i += 1 38 | self.assertEqual(i, 0) 39 | self.assertEqual(len(gf), 0) 40 | uu = "da7c242f-2efe-5175-9961-49cc621b80b9" 41 | with self.assertRaises(KeyError): 42 | x = gf[uu] 43 | 44 | def test_tiny_map_features(self): 45 | gm = GeoMap( 46 | xml_map.get_osm( 47 | "package://osm_cartography/tests/tiny.osm", bounding_box.makeGlobal() 48 | ) 49 | ) 50 | self.assertEqual(gm.n_features, 2) 51 | 52 | # expected feature IDs 53 | uuids = [ 54 | "8e8b355f-f1e8-5d82-827d-91e688e807e4", 55 | "199dd143-8309-5401-9728-6ca5e1c6e235", 56 | ] 57 | 58 | # test GeoMapFeatures iterator 59 | gf = GeoMapFeatures(gm) 60 | i = 0 61 | for f in gf: 62 | if type(f.id.uuid) == "": # old-style message? 63 | self.assertEqual(f.id.uuid, uuids[i]) 64 | else: 65 | self.assertEqual(unique_id.toHexString(f.id), uuids[i]) 66 | i += 1 67 | self.assertEqual(i, 2) 68 | self.assertEqual(len(gf), 2) 69 | 70 | 71 | if __name__ == "__main__": 72 | import rosunit 73 | 74 | rosunit.unitrun("osm_cartography", "test_geo_map_py", TestGeoMap) 75 | -------------------------------------------------------------------------------- /test_osm/rviz/small-screen-prc.vcg: -------------------------------------------------------------------------------- 1 | Background\ ColorB=0 2 | Background\ ColorG=0 3 | Background\ ColorR=0 4 | Camera\ Config=0.729044 1.92625 622791 3.3604e+06 1505.18 5 | Camera\ Type=rviz::FPSViewController 6 | Fixed\ Frame=/map 7 | Grid.Alpha=0.5 8 | Grid.Cell\ Size=100 9 | Grid.ColorB=0.5 10 | Grid.ColorG=0.5 11 | Grid.ColorR=0.5 12 | Grid.Enabled=1 13 | Grid.Line\ Style=0 14 | Grid.Line\ Width=0.03 15 | Grid.Normal\ Cell\ Count=0 16 | Grid.OffsetX=622100 17 | Grid.OffsetY=3.362e+06 18 | Grid.OffsetZ=0 19 | Grid.Plane=0 20 | Grid.Plane\ Cell\ Count=10 21 | Grid.Reference\ Frame= 22 | Marker.Enabled=1 23 | Marker.Marker\ Topic=visualization_marker 24 | Marker.Queue\ Size=100 25 | Marker.bounds_osm=1 26 | Marker.buildings_osm=1 27 | Marker.other_osm=1 28 | Marker.plan_segments=1 29 | Marker.railroad_osm=1 30 | Marker.roads_osm=0 31 | Marker.route_segments=1 32 | Marker.route_waypoints=1 33 | Marker.waypoints_osm=1 34 | Property\ Grid\ Splitter=600,78 35 | Property\ Grid\ State=expanded=.Global Options,Marker.Enabled,Marker.Namespaces,Grid.Enabled,Grid.Offset;splitterratio=0.5 36 | QMainWindow=000000ff00000000fd00000003000000000000011d000003bffc0200000001fb000000100044006900730070006c006100790073020000024600000000000001250000030000000001000001300000031afc0200000003fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000001d000001250000006700fffffffb0000000a00560069006500770073020000041e0000001d0000013800000282fb0000001200530065006c0065006300740069006f006e000000001d0000031a0000006700ffffff00000003000005560000003efc0100000001fb0000000800540069006d0065010000000000000556000002bf00ffffff000005560000028700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 37 | Target\ Frame=/map 38 | Tool\ 2D\ Nav\ GoalTopic=goal 39 | Tool\ 2D\ Pose\ EstimateTopic=initialpose 40 | [Display0] 41 | ClassName=rviz::MarkerDisplay 42 | Name=Marker 43 | [Display1] 44 | ClassName=rviz::GridDisplay 45 | Name=Grid 46 | [Views] 47 | 0Name=PRC 48 | 1Name=topdown 49 | 2Name=fps 50 | 3Name=orbit 51 | [Views/0] 52 | Config=0.785398 0.785398 10 622191 3362024 0 53 | Target=/map 54 | Type=rviz::OrbitViewController 55 | [Views/1] 56 | Config=0.739298 622115 3.36206e+06 0 57 | Target=/map 58 | Type=rviz::FixedOrientationOrthoViewController 59 | [Views/2] 60 | Config=0.919119 3.14159 622693 3.36202e+06 639.06 61 | Target=/map 62 | Type=rviz::FPSViewController 63 | [Views/3] 64 | Config=0.919119 -2.53157e-06 3.4192e+06 -1.45112e+06 3.36203e+06 -2.71786e+06 65 | Target=/map 66 | Type=rviz::OrbitViewController 67 | [Window] 68 | Height=780 69 | Width=1382 70 | X=80 71 | Y=38 72 | -------------------------------------------------------------------------------- /osm_cartography/scripts/osm_client: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (C) 2012, Jack O'Quin 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the author nor of other contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # Revision $Id$ 35 | 36 | """ 37 | Request geographic information maps from Open Street Map server. 38 | 39 | This is just a toy node, useful for testing. 40 | """ 41 | 42 | 43 | PKG_NAME = "osm_cartography" 44 | import roslib 45 | 46 | roslib.load_manifest(PKG_NAME) 47 | import rospy 48 | 49 | import sys 50 | 51 | from geodesy import bounding_box 52 | 53 | from geographic_msgs.srv import GetGeographicMap 54 | 55 | 56 | def client_node(url): 57 | 58 | rospy.init_node("osm_client") 59 | rospy.wait_for_service("get_geographic_map") 60 | 61 | try: 62 | get_map = rospy.ServiceProxy("get_geographic_map", GetGeographicMap) 63 | resp = get_map(url, bounding_box.makeGlobal()) 64 | if resp.success: 65 | print(resp.map) 66 | else: 67 | print("get_geographic_map failed, status: ", str(resp.status)) 68 | 69 | except rospy.ServiceException, e: 70 | print("Service call failed: " + str(e)) 71 | 72 | 73 | if __name__ == "__main__": 74 | 75 | url = "" 76 | if len(sys.argv) == 2: 77 | url = sys.argv[1] 78 | else: 79 | print("usage: osm_client ") 80 | sys.exit(-9) 81 | 82 | try: 83 | client_node(url) 84 | except rospy.ROSInterruptException: 85 | pass 86 | -------------------------------------------------------------------------------- /osm_cartography/scripts/osm_server: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (C) 2012, Jack O'Quin 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the author nor of other contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # Revision $Id$ 35 | 36 | """ 37 | Provide geographic information maps from Open Street Map on request. 38 | 39 | This is just a toy node, useful for testing. 40 | """ 41 | 42 | 43 | import rospy 44 | 45 | from geographic_msgs.srv import GetGeographicMap 46 | from geographic_msgs.srv import GetGeographicMapResponse 47 | 48 | from osm_cartography import xml_map 49 | 50 | 51 | class ServerNode: 52 | def __init__(self): 53 | rospy.init_node("osm_server") 54 | self.srv = rospy.Service( 55 | "get_geographic_map", GetGeographicMap, self.map_server 56 | ) 57 | self.resp = None 58 | 59 | def map_server(self, req): 60 | """GetGeographicMap service callback. 61 | 62 | :param req: Request. 63 | :returns: Response. 64 | """ 65 | url = str(req.url) 66 | self.url = url 67 | rospy.loginfo("[get_geographic_map] " + url) 68 | 69 | # if empty URL, return existing map 70 | if url == "" and self.resp is not None: 71 | return self.resp 72 | 73 | self.resp = GetGeographicMapResponse() 74 | try: 75 | self.resp.map = xml_map.get_osm(url, req.bounds) 76 | except (OSError, ValueError) as e: 77 | rospy.logerr(str(e)) 78 | self.resp.success = False 79 | self.resp.status = str(e) 80 | else: 81 | self.resp.success = True 82 | self.resp.status = url 83 | self.resp.map.header.stamp = rospy.Time() 84 | self.resp.map.header.frame_id = "/map" 85 | return self.resp 86 | 87 | def run(self): 88 | rospy.spin() 89 | 90 | 91 | if __name__ == "__main__": 92 | server = ServerNode() 93 | try: 94 | server.run() 95 | except rospy.ROSInterruptException: 96 | pass 97 | -------------------------------------------------------------------------------- /osm_cartography/rviz/geo_planner.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 895 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.0299999993 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Class: rviz/MarkerArray 52 | Enabled: true 53 | Marker Topic: visualization_marker_array 54 | Name: MarkerArray 55 | Namespaces: 56 | bounds_osm: true 57 | buildings_osm: true 58 | goal: true 59 | goal distance: true 60 | goal segment: true 61 | path: true 62 | roads_osm: true 63 | start: true 64 | start segment: true 65 | waypoints_osm: true 66 | Queue Size: 100 67 | Value: true 68 | Enabled: true 69 | Global Options: 70 | Background Color: 48; 48; 48 71 | Fixed Frame: local_map 72 | Frame Rate: 30 73 | Name: root 74 | Tools: 75 | - Class: rviz/Interact 76 | Hide Inactive Objects: true 77 | - Class: rviz/MoveCamera 78 | - Class: rviz/Select 79 | - Class: rviz/FocusCamera 80 | - Class: rviz/Measure 81 | - Class: rviz/SetInitialPose 82 | Topic: /initialpose 83 | - Class: rviz/SetGoal 84 | Topic: /goal 85 | - Class: rviz/PublishPoint 86 | Single click: true 87 | Topic: /clicked_point 88 | Value: true 89 | Views: 90 | Current: 91 | Class: rviz/Orbit 92 | Distance: 2606.38159 93 | Enable Stereo Rendering: 94 | Stereo Eye Separation: 0.0599999987 95 | Stereo Focal Distance: 1 96 | Swap Stereo Eyes: false 97 | Value: false 98 | Focal Point: 99 | X: 0 100 | Y: 0 101 | Z: 0 102 | Focal Shape Fixed Size: true 103 | Focal Shape Size: 0.0500000007 104 | Invert Z Axis: false 105 | Name: Current View 106 | Near Clip Distance: 0.00999999978 107 | Pitch: 1.50979638 108 | Target Frame: 109 | Value: Orbit (rviz) 110 | Yaw: 0.950397849 111 | Saved: ~ 112 | Window Geometry: 113 | Displays: 114 | collapsed: false 115 | Height: 1176 116 | Hide Left Dock: false 117 | Hide Right Dock: false 118 | QMainWindow State: 000000ff00000000fd00000004000000000000016a0000040efc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000040e000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000028000001540000006400fffffffb0000000a005600690065007700730100000182000002b4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000040e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 119 | Selection: 120 | collapsed: false 121 | Time: 122 | collapsed: false 123 | Tool Properties: 124 | collapsed: false 125 | Views: 126 | collapsed: false 127 | Width: 1855 128 | X: 65 129 | Y: 24 130 | -------------------------------------------------------------------------------- /osm_cartography/tests/arl-building.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /osm_cartography/src/osm_cartography/geo_map.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (C) 2012, Jack O'Quin 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 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of the author nor of other contributors may be 17 | # used to endorse or promote products derived from this software 18 | # without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | # 33 | # Revision $Id$ 34 | 35 | """ 36 | .. module:: geo_map 37 | 38 | Class for manipulating GeographicMap data. 39 | 40 | .. warning:: 41 | 42 | This module is *unstable* and still evolving. It will probably end 43 | up residing in some other package. 44 | 45 | .. _`geographic_msgs/BoundingBox`: http://ros.org/doc/api/geographic_msgs/html/msg/BoundingBox.html 46 | .. _`geographic_msgs/GeographicMap`: http://ros.org/doc/api/geographic_msgs/html/msg/GeographicMap.html 47 | .. _`std_msgs/Header`: http://ros.org/doc/api/std_msgs/html/msg/Header.html 48 | 49 | """ 50 | 51 | PKG = "osm_cartography" 52 | import roslib 53 | 54 | roslib.load_manifest(PKG) 55 | 56 | from geographic_msgs.msg import GeographicMap 57 | from geographic_msgs.msg import WayPoint 58 | from geometry_msgs.msg import Point 59 | 60 | 61 | class GeoMap: 62 | """ 63 | :class:`GeoMap` provides an internal 64 | `geographic_msgs/GeographicMap`_ representation. 65 | 66 | :param gmap: `geographic_msgs/GeographicMap`_ message. 67 | """ 68 | 69 | def __init__(self, gmap): 70 | """Constructor. 71 | 72 | Collects relevant information from the geographic map message, 73 | and provides convenient access to the data. 74 | """ 75 | self.gmap = gmap 76 | 77 | # Initialize feature information. 78 | self.feature_ids = {} # feature symbol table 79 | self.n_features = len(self.gmap.features) 80 | for fid in range(self.n_features): 81 | feat = self.gmap.features 82 | self.feature_ids[feat[fid].id.uuid] = fid 83 | 84 | def bounds(self): 85 | """ 86 | :returns: `geographic_msgs/BoundingBox`_ from the `geographic_msgs/GeographicMap`_ message. 87 | """ 88 | return self.gmap.bounds 89 | 90 | def header(self): 91 | """ 92 | :returns: `std_msgs/Header`_ from the `geographic_msgs/GeographicMap`_ message. 93 | """ 94 | return self.gmap.header 95 | 96 | 97 | class GeoMapFeatures: 98 | """ 99 | :class:`GeoMapFeatures` provides a filtering iterator for the 100 | features in a :class:`osm_cartography.geo_map.GeoMap`. 101 | 102 | :param geomap: :class:`GeoMap` object. 103 | 104 | :class:`GeoMapFeatures` provides these standard container operations: 105 | 106 | .. describe:: len(features) 107 | 108 | :returns: The number of points in the set. 109 | 110 | .. describe:: features[uuid] 111 | 112 | :returns: The point with key *uuid*. Raises a :exc:`KeyError` 113 | if *uuid* is not in the set. 114 | 115 | .. describe:: uuid in features 116 | 117 | :returns: ``True`` if *features* has a key *uuid*, else ``False``. 118 | 119 | .. describe:: uuid not in features 120 | 121 | Equivalent to ``not uuid in features``. 122 | 123 | .. describe:: iter(features) 124 | 125 | :returns: An iterator over all the features. This is a 126 | shortcut for :meth:`iterkeys`. 127 | 128 | Example:: 129 | 130 | gm = GeoMap(msg) 131 | gf = GeoMapFeatures(gm) 132 | for feat in gf: 133 | print str(feat) 134 | """ 135 | 136 | def __init__(self, geomap): 137 | """Constructor. """ 138 | self.gmap = geomap 139 | 140 | def __contains__(self, item): 141 | """ Feature set membership. """ 142 | return item in self.gmap.feature_ids 143 | 144 | def __getitem__(self, key): 145 | """ Feature accessor.""" 146 | index = self.gmap.feature_ids[key] 147 | return self.gmap.features[index] 148 | 149 | def __iter__(self): 150 | """ Features iterator. """ 151 | self.iter_index = 0 152 | return self 153 | 154 | def __len__(self): 155 | """Features vector length.""" 156 | return len(self.gmap.gmap.features) 157 | 158 | def __next__(self): 159 | """Next matching feature. 160 | 161 | :returns: :class:`geodesy.wu_point.WuPoint` object for next point 162 | :raises: :exc:`StopIteration` when finished. 163 | """ 164 | i = self.iter_index 165 | if i >= self.gmap.n_features: 166 | raise StopIteration 167 | self.iter_index = i + 1 168 | return self.gmap.gmap.features[i] 169 | -------------------------------------------------------------------------------- /route_network/scripts/viz_plan: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (C) 2012, Jack O'Quin 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the author nor of other contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # Revision $Id$ 35 | 36 | """ 37 | Visualize route plan for geographic map. 38 | """ 39 | 40 | import rospy 41 | 42 | import sys 43 | import random 44 | import geodesy.wu_point 45 | 46 | from geographic_msgs.msg import RouteNetwork 47 | from geographic_msgs.msg import RouteSegment 48 | from geographic_msgs.srv import GetRoutePlan 49 | from geometry_msgs.msg import Point 50 | from geometry_msgs.msg import Quaternion 51 | from geometry_msgs.msg import Vector3 52 | from std_msgs.msg import ColorRGBA 53 | from visualization_msgs.msg import Marker 54 | from visualization_msgs.msg import MarkerArray 55 | 56 | try: 57 | from geographic_msgs.msg import UniqueID 58 | except ImportError: 59 | from uuid_msgs.msg import UniqueID 60 | 61 | 62 | class VizPlanNode: 63 | def __init__(self): 64 | """ROS node to visualize a route plan.""" 65 | rospy.init_node("viz_plan") 66 | self.graph = None 67 | 68 | # advertise visualization marker topic 69 | self.pub = rospy.Publisher( 70 | "visualization_marker_array", MarkerArray, latch=True, queue_size=10 71 | ) 72 | 73 | rospy.wait_for_service("get_geographic_map") 74 | self.get_plan = rospy.ServiceProxy("get_route_plan", GetRoutePlan) 75 | 76 | # subscribe to route network 77 | self.sub = rospy.Subscriber("route_network", RouteNetwork, self.graph_callback) 78 | 79 | self.timer_interval = rospy.Duration(4) 80 | self.marker_life = self.timer_interval + rospy.Duration(1) 81 | rospy.Timer(self.timer_interval, self.timer_callback) 82 | 83 | def graph_callback(self, graph): 84 | """Handle RouteNetwork message. 85 | 86 | :param graph: RouteNetwork message. 87 | 88 | :post: self.graph = RouteNetwork message 89 | :post: self.points = visualization markers message. 90 | """ 91 | self.points = geodesy.wu_point.WuPointSet(graph.points) 92 | self.segment_ids = {} # segments symbol table 93 | for sid in xrange(len(graph.segments)): 94 | self.segment_ids[graph.segments[sid].id.uuid] = sid 95 | self.graph = graph 96 | 97 | def timer_callback(self, event): 98 | """ Called periodically. """ 99 | if self.graph is None: 100 | print "still waiting for graph" 101 | return 102 | 103 | # select two different way points at random 104 | idx0, idx1 = random.sample(xrange(len(self.graph.points)), 2) 105 | start = self.graph.points[idx0].id.uuid 106 | goal = self.graph.points[idx1].id.uuid 107 | rospy.loginfo("plan from " + start + " to " + goal) 108 | 109 | try: 110 | resp = self.get_plan( 111 | self.graph.id, UniqueID(uuid=start), UniqueID(uuid=goal) 112 | ) 113 | except rospy.ServiceException as e: 114 | rospy.logerr("Service call failed: " + str(e)) 115 | else: # get_map returned 116 | if resp.success: 117 | self.mark_plan(resp.plan) 118 | else: 119 | rospy.logerr("get_route_plan failed, status: " + str(resp.status)) 120 | 121 | def mark_plan(self, plan): 122 | """Publish visualization markers for a RoutePath. 123 | 124 | :param plan: RoutePath message 125 | """ 126 | marks = MarkerArray() 127 | hdr = self.graph.header 128 | hdr.stamp = rospy.Time.now() 129 | index = 0 130 | for seg_msg in plan.segments: 131 | marker = Marker( 132 | header=hdr, 133 | ns="plan_segments", 134 | id=index, 135 | type=Marker.LINE_STRIP, 136 | action=Marker.ADD, 137 | scale=Vector3(x=4.0), 138 | color=ColorRGBA(r=1.0, g=1.0, b=1.0, a=0.8), 139 | lifetime=self.marker_life, 140 | ) 141 | index += 1 142 | segment = self.graph.segments[self.segment_ids[seg_msg.uuid]] 143 | marker.points.append(self.points[segment.start.uuid].toPointXY()) 144 | marker.points.append(self.points[segment.end.uuid].toPointXY()) 145 | marks.markers.append(marker) 146 | 147 | self.pub.publish(marks) 148 | 149 | 150 | def main(): 151 | node_class = VizPlanNode() 152 | try: 153 | rospy.spin() # wait for messages 154 | except rospy.ROSInterruptException: 155 | pass 156 | 157 | 158 | if __name__ == "__main__": 159 | # run main function and exit 160 | sys.exit(main()) 161 | -------------------------------------------------------------------------------- /route_network/scripts/viz_routes: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (C) 2012, Jack O'Quin 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the author nor of other contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # Revision $Id$ 35 | 36 | """ 37 | Create route network messages for geographic information maps. 38 | """ 39 | 40 | import rospy 41 | 42 | import sys 43 | import itertools 44 | import geodesy.props 45 | import geodesy.wu_point 46 | 47 | from geographic_msgs.msg import RouteNetwork 48 | from geographic_msgs.msg import RouteSegment 49 | from geometry_msgs.msg import Point 50 | from geometry_msgs.msg import Quaternion 51 | from geometry_msgs.msg import Vector3 52 | from std_msgs.msg import ColorRGBA 53 | from visualization_msgs.msg import Marker 54 | from visualization_msgs.msg import MarkerArray 55 | 56 | 57 | class RouteVizNode: 58 | def __init__(self): 59 | """ROS node to publish the route network graph for a GeographicMap.""" 60 | rospy.init_node("viz_routes") 61 | self.graph = None 62 | self.marks = None 63 | 64 | # advertise visualization marker topic 65 | self.pub = rospy.Publisher( 66 | "visualization_marker_array", MarkerArray, latch=True, queue_size=10 67 | ) 68 | 69 | # refresh the markers every three seconds, making them last four. 70 | self.timer_interval = rospy.Duration(3) 71 | self.marker_life = self.timer_interval + rospy.Duration(1) 72 | rospy.Timer(self.timer_interval, self.timer_callback) 73 | 74 | # subscribe to route network 75 | self.sub = rospy.Subscriber("route_network", RouteNetwork, self.graph_callback) 76 | 77 | def graph_callback(self, graph): 78 | """Create visualization markers from a RouteNetwork graph. 79 | 80 | :param graph: RouteNetwork message 81 | 82 | :post: self.marks = visualization markers message. 83 | :post: self.graph = RouteNetwork message. 84 | """ 85 | self.graph = graph 86 | self.marks = MarkerArray() 87 | self.points = geodesy.wu_point.WuPointSet(graph.points) 88 | 89 | self.mark_way_points(ColorRGBA(r=1.0, g=1.0, b=0.0, a=0.8)) 90 | self.mark_segments( 91 | ColorRGBA(r=1.0, g=1.0, b=0.0, a=0.8), ColorRGBA(r=1.0, g=0.0, b=1.0, a=0.8) 92 | ) 93 | 94 | def mark_segments(self, color1, color2): 95 | """Create lines for segments. 96 | 97 | :param color1: RGBA value for one-way segment 98 | :param color2: RGBA value for two-way segment 99 | """ 100 | index = 0 101 | for segment in self.graph.segments: 102 | color = color2 103 | if geodesy.props.match(segment, {"oneway"}): 104 | color = color1 105 | marker = Marker( 106 | header=self.graph.header, 107 | ns="route_segments", 108 | id=index, 109 | type=Marker.LINE_STRIP, 110 | action=Marker.ADD, 111 | scale=Vector3(x=2.0), 112 | color=color, 113 | lifetime=self.marker_life, 114 | ) 115 | index += 1 116 | marker.points.append(self.points[segment.start.uuid].toPointXY()) 117 | marker.points.append(self.points[segment.end.uuid].toPointXY()) 118 | self.marks.markers.append(marker) 119 | 120 | def mark_way_points(self, color): 121 | """Create slightly transparent disks for way-points. 122 | 123 | :param color: disk RGBA value 124 | """ 125 | index = 0 126 | for wp in self.points: 127 | marker = Marker( 128 | header=self.graph.header, 129 | ns="route_waypoints", 130 | id=index, 131 | type=Marker.CYLINDER, 132 | action=Marker.ADD, 133 | scale=Vector3(x=2.0, y=2.0, z=0.2), 134 | color=color, 135 | lifetime=self.marker_life, 136 | ) 137 | index += 1 138 | # use easting and northing coordinates (ignoring altitude) 139 | marker.pose.position = wp.toPointXY() 140 | marker.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) 141 | self.marks.markers.append(marker) 142 | 143 | def timer_callback(self, event): 144 | """ Called periodically to refresh route network visualization. """ 145 | if self.marks is not None: 146 | now = rospy.Time.now() 147 | for m in self.marks.markers: 148 | m.header.stamp = now 149 | self.pub.publish(self.marks) 150 | 151 | 152 | def main(): 153 | node_class = RouteVizNode() 154 | try: 155 | rospy.spin() # wait for messages 156 | except rospy.ROSInterruptException: 157 | pass 158 | 159 | 160 | if __name__ == "__main__": 161 | # run main function and exit 162 | sys.exit(main()) 163 | -------------------------------------------------------------------------------- /route_network/scripts/plan_route: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (C) 2012, Jack O'Quin 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the author nor of other contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # Revision $Id$ 35 | 36 | """ 37 | Plan the shortest route through a geographic information network. 38 | 39 | :todo: add server for local GetPlan service. 40 | """ 41 | 42 | import rospy 43 | import sys 44 | 45 | from route_network import planner 46 | 47 | from geographic_msgs.msg import GeoPath 48 | from geographic_msgs.msg import GeoPose 49 | from geographic_msgs.msg import GeoPoseStamped 50 | from geographic_msgs.msg import RouteNetwork 51 | from geographic_msgs.msg import RoutePath 52 | 53 | from geographic_msgs.srv import GetRoutePlan 54 | from geographic_msgs.srv import GetRoutePlanResponse 55 | from geographic_msgs.srv import GetGeoPath 56 | from geographic_msgs.srv import GetGeoPathResponse 57 | 58 | import geodesy 59 | 60 | 61 | class RoutePlannerNode: 62 | def __init__(self): 63 | """ROS node to provide a navigation plan graph for a RouteNetwork.""" 64 | rospy.init_node("route_planner") 65 | self.graph = None 66 | self.planner = None 67 | self._calc_idx = 0 68 | 69 | # advertise route planning service 70 | self.srv = rospy.Service("get_route_plan", GetRoutePlan, self.route_planner) 71 | # advertise geographic route planning service 72 | self.srv_geo = rospy.Service("get_geo_path", GetGeoPath, self.geo_path_planner) 73 | self.resp = None 74 | 75 | # subscribe to route network 76 | self.sub = rospy.Subscriber("route_network", RouteNetwork, self.graph_callback) 77 | 78 | def graph_callback(self, graph): 79 | """RouteNetwork graph message callback. 80 | :param graph: RouteNetwork message 81 | 82 | :post: graph information saved in self.graph and self.planner 83 | """ 84 | self.graph = graph 85 | self.planner = planner.Planner(graph) 86 | 87 | def route_planner(self, req): 88 | """ 89 | :param req: GetRoutePlanRequest message 90 | :returns: GetRoutePlanResponse message 91 | """ 92 | rospy.loginfo("GetRoutePlan: " + str(req)) 93 | self.resp = GetRoutePlanResponse(plan=RoutePath()) 94 | if self.graph is None: 95 | self.resp.success = False 96 | self.resp.status = "no RouteNetwork available for GetRoutePlan" 97 | rospy.logerr(self.resp.status) 98 | return self.resp 99 | 100 | try: 101 | # plan a path to the goal 102 | self.resp.plan = self.planner.planner(req) 103 | except (ValueError, planner.NoPathToGoalError) as e: 104 | self.resp.success = False 105 | self.resp.status = str(e) 106 | rospy.logerr("route planner exception: " + str(e)) 107 | else: 108 | self.resp.success = True 109 | self.resp.plan.header.stamp = rospy.Time.now() 110 | self.resp.plan.header.frame_id = self.graph.header.frame_id 111 | return self.resp 112 | 113 | def geo_path_planner(self, req): 114 | """Processes the planning request. 115 | 116 | If the planning service fails, self.resp.success is set to False. 117 | 118 | :param req: The planning request (start and goal positions). 119 | :type req: geographic_msgs/GetGeoPath 120 | 121 | :return: The plan from start to goal position. 122 | :rtype: geographic_msgs/GetGeoPathResponse 123 | """ 124 | rospy.loginfo("GetGeoPath: " + str(req)) 125 | self._calc_idx += 1 126 | self.resp = GetGeoPathResponse() 127 | self.resp.plan.header.seq = self._calc_idx 128 | self.resp.plan.header.stamp = rospy.Time.now() 129 | if self.graph is None: 130 | self.resp.success = False 131 | self.resp.status = "No RouteNetwork avialable for GetGeoPath." 132 | rospy.logerr(self.resp.status) 133 | return self.resp 134 | 135 | try: 136 | planner_result = self.planner.geo_path(req) 137 | 138 | self.resp.plan.poses = [ 139 | GeoPoseStamped(pose=GeoPose(position=point)) 140 | for point in planner_result[0] 141 | ] 142 | self.resp.network = planner_result[1] 143 | self.resp.start_seg = planner_result[2] 144 | self.resp.goal_seg = planner_result[3] 145 | self.resp.distance = planner_result[4] 146 | self.resp.success = not (self.resp.distance == -1) 147 | if not self.resp.success: 148 | self.resp.status = ( 149 | "No route found from :\n" 150 | + str(req.start) 151 | + " to:\n" 152 | + str(req.goal) 153 | ) 154 | rospy.loginfo("GetGeoPath result: " + str(self.resp)) 155 | except (ValueError) as e: 156 | self.resp.success = False 157 | self.resp.status = str(e) 158 | self.resp.network = self.graph.id 159 | self.resp.distance = -1 160 | rospy.logerr("Route planner exception: " + str(e)) 161 | 162 | return self.resp 163 | 164 | 165 | def main(): 166 | node_class = RoutePlannerNode() 167 | try: 168 | rospy.spin() # wait for messages 169 | except rospy.ROSInterruptException: 170 | pass 171 | return 0 172 | 173 | 174 | if __name__ == "__main__": 175 | # run main function and exit 176 | sys.exit(main()) 177 | -------------------------------------------------------------------------------- /route_network/scripts/route_network: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (C) 2012, Jack O'Quin 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the author nor of other contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | 34 | """ 35 | Create route network messages for geographic information maps. 36 | """ 37 | 38 | import rospy 39 | 40 | import sys 41 | import itertools 42 | import unique_id 43 | import geodesy.props 44 | import geodesy.wu_point 45 | from geodesy import bounding_box 46 | 47 | from geographic_msgs.msg import KeyValue 48 | from geographic_msgs.msg import RouteNetwork 49 | from geographic_msgs.msg import RouteSegment 50 | from geographic_msgs.srv import GetGeographicMap 51 | 52 | try: 53 | from geographic_msgs.msg import UniqueID 54 | except ImportError: 55 | from uuid_msgs.msg import UniqueID 56 | 57 | 58 | # dynamic parameter reconfiguration 59 | from dynamic_reconfigure.server import Server as ReconfigureServer 60 | import route_network.cfg.RouteNetworkConfig as Config 61 | 62 | 63 | def is_oneway(feature): 64 | """One-way route predicate. 65 | :returns: True if feature is one way. 66 | """ 67 | return geodesy.props.match(feature, {"oneway"}) 68 | 69 | 70 | def is_route(feature): 71 | """Drivable feature predicate. 72 | :returns: True if feature is drivable. 73 | """ 74 | return geodesy.props.match(feature, {"bridge", "highway", "tunnel"}) 75 | 76 | 77 | # URL, unique to this package 78 | PKG_URL = "http://ros.org/wiki/" + PKG_NAME 79 | 80 | 81 | def makeGraph(msg): 82 | """Make RouteNetwork message. 83 | 84 | :param msg: GeographicMap message. 85 | :returns: RouteNetwork message. 86 | """ 87 | uu = unique_id.toMsg( 88 | unique_id.fromURL(PKG_URL + "/map/" + str(msg.id.uuid) + "/routes") 89 | ) 90 | return RouteNetwork(header=msg.header, id=uu, bounds=msg.bounds) 91 | 92 | 93 | def makeSeg(start, end, oneway=False): 94 | """Make RouteSegment message. 95 | 96 | :param start: Initial UUID. 97 | :param end: Final UUID. 98 | :param oneway: True if segment is one-way. 99 | :returns: RouteSegment message. 100 | """ 101 | uu = unique_id.toMsg(unique_id.fromURL(PKG_URL + "/" + str(start) + "/" + str(end))) 102 | 103 | seg = RouteSegment(id=uu, start=start, end=end) 104 | if oneway: 105 | seg.props.append(KeyValue(key="oneway", value="yes")) 106 | return seg 107 | 108 | 109 | class RouteNetNode: 110 | def __init__(self): 111 | """ROS node to publish the route network graph for a GeographicMap.""" 112 | rospy.init_node("route_network") 113 | self.config = None 114 | 115 | # advertise visualization marker topic 116 | self.pub = rospy.Publisher( 117 | "route_network", RouteNetwork, latch=True, queue_size=10 118 | ) 119 | self.graph = None 120 | rospy.wait_for_service("get_geographic_map") 121 | self.get_map = rospy.ServiceProxy("get_geographic_map", GetGeographicMap) 122 | 123 | # register dynamic reconfigure callback, which runs immediately 124 | self.reconf_server = ReconfigureServer(Config, self.reconfigure) 125 | 126 | def build_graph(self, msg): 127 | """Build RouteNetwork graph for a GeographicMap message. 128 | 129 | :post: self.graph = RouteNetwork message 130 | """ 131 | self.map = msg 132 | self.map_points = geodesy.wu_point.WuPointSet(msg.points) 133 | self.graph = makeGraph(msg) 134 | 135 | # process each feature marked as a route 136 | for feature in itertools.ifilter(is_route, self.map.features): 137 | oneway = is_oneway(feature) 138 | start = None 139 | for mbr in feature.components: 140 | pt = self.map_points.get(mbr.uuid) 141 | if pt is not None: # known way point? 142 | self.graph.points.append(pt.toWayPoint()) 143 | end = UniqueID(uuid=mbr.uuid) 144 | if start is not None: 145 | self.graph.segments.append(makeSeg(start, end, oneway)) 146 | if not oneway: 147 | self.graph.segments.append(makeSeg(end, start)) 148 | start = end 149 | 150 | def reconfigure(self, config, level): 151 | """Dynamic reconfigure callback. 152 | 153 | :param config: New configuration. 154 | :param level: 0x00000001 bit set if URL changed (ignored). 155 | 156 | :returns: New config if valid, old one otherwise. That updates 157 | the dynamic reconfigure GUI window. 158 | """ 159 | if self.config is None: 160 | self.config = config 161 | rospy.loginfo("Map URL: " + str(config["map_url"])) 162 | 163 | try: 164 | resp = self.get_map(config["map_url"], bounding_box.makeGlobal()) 165 | except rospy.ServiceException as e: 166 | rospy.logerr("Service call failed: " + str(e)) 167 | # ignore new config, it failed 168 | else: # get_map returned 169 | if resp.success: 170 | self.build_graph(resp.map) 171 | self.config = config # save new URL 172 | # publish visualization markers (on a latched topic) 173 | self.pub.publish(self.graph) 174 | else: 175 | rospy.logerr("get_geographic_map failed, status: " + str(resp.status)) 176 | 177 | return self.config 178 | 179 | 180 | def main(): 181 | node_class = RouteNetNode() 182 | try: 183 | rospy.spin() 184 | except rospy.ROSInterruptException: 185 | pass 186 | 187 | 188 | if __name__ == "__main__": 189 | # run main function and exit 190 | sys.exit(main()) 191 | -------------------------------------------------------------------------------- /osm_cartography/src/osm_cartography/xml_map.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (C) 2012, Jack O'Quin 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the author nor of other contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # Revision $Id$ 35 | 36 | """ 37 | Generate geographic information maps based on Open Street Map XML data. 38 | 39 | .. _`geographic_msgs/BoundingBox`: http://ros.org/doc/api/geographic_msgs/html/msg/BoundingBox.html 40 | .. _`geographic_msgs/GeographicMap`: http://ros.org/doc/api/geographic_msgs/html/msg/GeographicMap.html 41 | .. _`geographic_msgs/KeyValue`: http://ros.org/doc/api/geographic_msgs/html/msg/KeyValue.html 42 | .. _`geographic_msgs/UniqueID`: http://ros.org/doc/api/geographic_msgs/html/msg/UniqueID.html 43 | 44 | """ 45 | 46 | 47 | from xml.etree import ElementTree 48 | 49 | PKG_NAME = "osm_cartography" 50 | import roslib 51 | 52 | roslib.load_manifest(PKG_NAME) 53 | 54 | import unique_id 55 | from geodesy import bounding_box 56 | 57 | from geographic_msgs.msg import GeographicMap 58 | from geographic_msgs.msg import KeyValue 59 | from geographic_msgs.msg import MapFeature 60 | from geographic_msgs.msg import WayPoint 61 | 62 | try: 63 | from geographic_msgs.msg import UniqueID 64 | except ImportError: 65 | from uuid_msgs.msg import UniqueID 66 | 67 | 68 | def get_required_attribute(el, key): 69 | """Get attribute key of element *el*. 70 | 71 | :raises: :exc:`ValueError` if key not found 72 | """ 73 | val = el.get(key) 74 | if val == None: 75 | raise ValueError("required attribute missing: " + key) 76 | return val 77 | 78 | 79 | def makeOsmUniqueID(namespace, el_id): 80 | """Make UniqueID message for *el_id* number in OSM sub-namespace *namespace*. 81 | 82 | :param namespace: OSM sub-namespace 83 | :type namespace: string 84 | :param el_id: OSM identifier within that namespace 85 | :type el_id: int or string containing an integer 86 | 87 | :returns: corresponding `geographic_msgs/UniqueID`_ message. 88 | :raises: :exc:`ValueError` 89 | """ 90 | if not namespace in {"node", "way", "relation"}: 91 | raise ValueError("invalid OSM namespace: " + namespace) 92 | ns = "http://openstreetmap.org/" + namespace + "/" 93 | return unique_id.toMsg(unique_id.fromURL(ns + str(el_id))) 94 | 95 | 96 | def get_tag(el): 97 | """ :returns: `geographic_msgs/KeyValue`_ message for `` *el* if any, None otherwise. """ 98 | pair = None 99 | key = el.get("k") 100 | if key != None: 101 | pair = KeyValue() 102 | pair.key = key 103 | pair.value = get_required_attribute(el, "v") 104 | return pair 105 | 106 | 107 | def get_osm(url, bounds): 108 | """Get `geographic_msgs/GeographicMap`_ from Open Street Map XML data. 109 | 110 | The latitude and longitude of the bounding box returned may differ 111 | from the requested bounds. 112 | 113 | :param url: Uniform Resource Locator for map. 114 | :param bounds: Desired `geographic_msgs/BoundingBox`_ for map (presently ignored). 115 | :returns: `geographic_msgs/GeographicMap`_ message (header not filled in). 116 | """ 117 | # parse the URL 118 | filename = "" 119 | if url.startswith("file:///"): 120 | filename = url[7:] 121 | elif url.startswith("package://"): 122 | pkg_name, slash, pkg_path = url[10:].partition("/") 123 | pkg_dir = roslib.packages.get_pkg_dir(pkg_name) 124 | filename = pkg_dir + "/" + pkg_path 125 | else: 126 | raise ValueError("unsupported URL: " + url) 127 | 128 | gmap = GeographicMap(id=unique_id.toMsg(unique_id.fromURL(url))) 129 | xm = None 130 | try: 131 | f = open(filename) 132 | xm = ElementTree.parse(f) 133 | except OSError: 134 | raise ValueError("unable to read " + str(url)) 135 | except ElementTree.ParseError: 136 | raise ValueError("XML parse failed for " + str(url)) 137 | osm = xm.getroot() 138 | 139 | # get map bounds 140 | for el in osm.iterfind("bounds"): 141 | minlat = float(get_required_attribute(el, "minlat")) 142 | minlon = float(get_required_attribute(el, "minlon")) 143 | maxlat = float(get_required_attribute(el, "maxlat")) 144 | maxlon = float(get_required_attribute(el, "maxlon")) 145 | gmap.bounds = bounding_box.makeBounds2D(minlat, minlon, maxlat, maxlon) 146 | 147 | # get map way-point nodes 148 | for el in osm.iterfind("node"): 149 | 150 | way = WayPoint() 151 | el_id = el.get("id") 152 | if el_id is None: 153 | raise ValueError("node id missing") 154 | way.id = makeOsmUniqueID("node", el_id) 155 | 156 | way.position.latitude = float(get_required_attribute(el, "lat")) 157 | way.position.longitude = float(get_required_attribute(el, "lon")) 158 | way.position.altitude = float(el.get("ele", float("nan"))) 159 | 160 | for tag_list in el.iterfind("tag"): 161 | kv = get_tag(tag_list) 162 | if kv != None: 163 | way.props.append(kv) 164 | 165 | gmap.points.append(way) 166 | 167 | # get map paths 168 | for el in osm.iterfind("way"): 169 | 170 | feature = MapFeature() 171 | el_id = el.get("id") 172 | if el_id is None: 173 | raise ValueError("way id missing") 174 | feature.id = makeOsmUniqueID("way", el_id) 175 | 176 | for nd in el.iterfind("nd"): 177 | way_id = get_required_attribute(nd, "ref") 178 | feature.components.append(makeOsmUniqueID("node", way_id)) 179 | 180 | for tag_list in el.iterfind("tag"): 181 | kv = get_tag(tag_list) 182 | if kv != None: 183 | feature.props.append(kv) 184 | 185 | gmap.features.append(feature) 186 | 187 | # get relations 188 | for el in osm.iterfind("relation"): 189 | 190 | feature = MapFeature() 191 | el_id = el.get("id") 192 | if el_id is None: 193 | raise ValueError("relation id missing") 194 | feature.id = makeOsmUniqueID("relation", el_id) 195 | 196 | for mbr in el.iterfind("member"): 197 | mbr_type = get_required_attribute(mbr, "type") 198 | if mbr_type in {"node", "way", "relation"}: 199 | mbr_id = get_required_attribute(mbr, "ref") 200 | feature.components.append(makeOsmUniqueID(mbr_type, mbr_id)) 201 | else: 202 | print("unknown relation member type: " + mbr_type) 203 | 204 | for tag_list in el.iterfind("tag"): 205 | kv = get_tag(tag_list) 206 | if kv != None: 207 | feature.props.append(kv) 208 | 209 | gmap.features.append(feature) 210 | 211 | return gmap 212 | 213 | 214 | interesting_tags = { 215 | "access", 216 | "amenity", 217 | "boundary", 218 | "bridge", 219 | "building", 220 | "ele", 221 | "highway", 222 | "landuse", 223 | "lanes", 224 | "layer", 225 | "maxheight", 226 | "maxspeed", 227 | "maxwidth", 228 | "name", 229 | "network", 230 | "oneway", 231 | "railway", 232 | "ref", 233 | "restriction", 234 | "route", 235 | "street", 236 | "tunnel", 237 | "type", 238 | "width", 239 | } 240 | 241 | 242 | ignored_values = { 243 | "bridleway", 244 | "construction", 245 | "cycleway", 246 | "footway", 247 | "path", 248 | "pedestrian", 249 | "proposed", 250 | "steps", 251 | } 252 | -------------------------------------------------------------------------------- /route_network/conf.py: -------------------------------------------------------------------------------- 1 | # 2 | # route_network documentation build configuration file, created by 3 | # sphinx-quickstart on Wed Apr 25 16:01:20 2012. 4 | # 5 | # This file is execfile()d with the current directory set to its containing dir. 6 | # 7 | # Note that not all possible configuration values are present in this 8 | # autogenerated file. 9 | # 10 | # All configuration values have a default; values that are commented out 11 | # serve to show the default. 12 | 13 | import sys, os 14 | 15 | # If extensions (or modules to document with autodoc) are in another directory, 16 | # add these directories to sys.path here. If the directory is relative to the 17 | # documentation root, use os.path.abspath to make it absolute, like shown here. 18 | # sys.path.insert(0, os.path.abspath('.')) 19 | 20 | # -- General configuration ----------------------------------------------------- 21 | 22 | # If your documentation needs a minimal Sphinx version, state it here. 23 | # needs_sphinx = '1.0' 24 | 25 | # Add any Sphinx extension module names here, as strings. They can be extensions 26 | # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. 27 | extensions = [ 28 | "sphinx.ext.autodoc", 29 | "sphinx.ext.intersphinx", 30 | "sphinx.ext.todo", 31 | "sphinx.ext.viewcode", 32 | ] 33 | 34 | # Add any paths that contain templates here, relative to this directory. 35 | templates_path = ["_templates"] 36 | 37 | # The suffix of source filenames. 38 | source_suffix = ".rst" 39 | 40 | # The encoding of source files. 41 | # source_encoding = 'utf-8-sig' 42 | 43 | # The master toctree document. 44 | master_doc = "index" 45 | 46 | # General information about the project. 47 | project = "route_network" 48 | copyright = "2012, Jack O'Quin" 49 | 50 | # The version info for the project you're documenting, acts as replacement for 51 | # |version| and |release|, also used in various other places throughout the 52 | # built documents. 53 | # 54 | # The short X.Y version. 55 | version = "0.2.0" 56 | # The full version, including alpha/beta/rc tags. 57 | release = "0.2.0" 58 | 59 | # The language for content autogenerated by Sphinx. Refer to documentation 60 | # for a list of supported languages. 61 | # language = None 62 | 63 | # There are two options for replacing |today|: either, you set today to some 64 | # non-false value, then it is used: 65 | # today = '' 66 | # Else, today_fmt is used as the format for a strftime call. 67 | # today_fmt = '%B %d, %Y' 68 | 69 | # List of patterns, relative to source directory, that match files and 70 | # directories to ignore when looking for source files. 71 | exclude_patterns = ["_build"] 72 | 73 | # The reST default role (used for this markup: `text`) to use for all documents. 74 | # default_role = None 75 | 76 | # If true, '()' will be appended to :func: etc. cross-reference text. 77 | # add_function_parentheses = True 78 | 79 | # If true, the current module name will be prepended to all description 80 | # unit titles (such as .. function::). 81 | # add_module_names = True 82 | 83 | # If true, sectionauthor and moduleauthor directives will be shown in the 84 | # output. They are ignored by default. 85 | # show_authors = False 86 | 87 | # The name of the Pygments (syntax highlighting) style to use. 88 | pygments_style = "sphinx" 89 | 90 | # A list of ignored prefixes for module index sorting. 91 | # modindex_common_prefix = [] 92 | 93 | 94 | # -- Options for HTML output --------------------------------------------------- 95 | 96 | # The theme to use for HTML and HTML Help pages. See the documentation for 97 | # a list of builtin themes. 98 | html_theme = "default" 99 | 100 | # Theme options are theme-specific and customize the look and feel of a theme 101 | # further. For a list of options available for each theme, see the 102 | # documentation. 103 | # html_theme_options = {} 104 | 105 | # Add any paths that contain custom themes here, relative to this directory. 106 | # html_theme_path = [] 107 | 108 | # The name for this set of Sphinx documents. If None, it defaults to 109 | # " v documentation". 110 | # html_title = None 111 | 112 | # A shorter title for the navigation bar. Default is the same as html_title. 113 | # html_short_title = None 114 | 115 | # The name of an image file (relative to this directory) to place at the top 116 | # of the sidebar. 117 | # html_logo = None 118 | 119 | # The name of an image file (within the static path) to use as favicon of the 120 | # docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 121 | # pixels large. 122 | # html_favicon = None 123 | 124 | # Add any paths that contain custom static files (such as style sheets) here, 125 | # relative to this directory. They are copied after the builtin static files, 126 | # so a file named "default.css" will overwrite the builtin "default.css". 127 | html_static_path = ["_static"] 128 | 129 | # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, 130 | # using the given strftime format. 131 | # html_last_updated_fmt = '%b %d, %Y' 132 | 133 | # If true, SmartyPants will be used to convert quotes and dashes to 134 | # typographically correct entities. 135 | # html_use_smartypants = True 136 | 137 | # Custom sidebar templates, maps document names to template names. 138 | # html_sidebars = {} 139 | 140 | # Additional templates that should be rendered to pages, maps page names to 141 | # template names. 142 | # html_additional_pages = {} 143 | 144 | # If false, no module index is generated. 145 | # html_domain_indices = True 146 | 147 | # If false, no index is generated. 148 | # html_use_index = True 149 | 150 | # If true, the index is split into individual pages for each letter. 151 | # html_split_index = False 152 | 153 | # If true, links to the reST sources are added to the pages. 154 | # html_show_sourcelink = True 155 | 156 | # If true, "Created using Sphinx" is shown in the HTML footer. Default is True. 157 | # html_show_sphinx = True 158 | 159 | # If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. 160 | # html_show_copyright = True 161 | 162 | # If true, an OpenSearch description file will be output, and all pages will 163 | # contain a tag referring to it. The value of this option must be the 164 | # base URL from which the finished HTML is served. 165 | # html_use_opensearch = '' 166 | 167 | # This is the file name suffix for HTML files (e.g. ".xhtml"). 168 | # html_file_suffix = None 169 | 170 | # Output file base name for HTML help builder. 171 | htmlhelp_basename = "route_networkdoc" 172 | 173 | 174 | # -- Options for LaTeX output -------------------------------------------------- 175 | 176 | latex_elements = { 177 | # The paper size ('letterpaper' or 'a4paper'). 178 | #'papersize': 'letterpaper', 179 | # The font size ('10pt', '11pt' or '12pt'). 180 | #'pointsize': '10pt', 181 | # Additional stuff for the LaTeX preamble. 182 | #'preamble': '', 183 | } 184 | 185 | # Grouping the document tree into LaTeX files. List of tuples 186 | # (source start file, target name, title, author, documentclass [howto/manual]). 187 | latex_documents = [ 188 | ( 189 | "index", 190 | "route_network.tex", 191 | "route_network Documentation", 192 | "Jack O'Quin", 193 | "manual", 194 | ), 195 | ] 196 | 197 | # The name of an image file (relative to this directory) to place at the top of 198 | # the title page. 199 | # latex_logo = None 200 | 201 | # For "manual" documents, if this is true, then toplevel headings are parts, 202 | # not chapters. 203 | # latex_use_parts = False 204 | 205 | # If true, show page references after internal links. 206 | # latex_show_pagerefs = False 207 | 208 | # If true, show URL addresses after external links. 209 | # latex_show_urls = False 210 | 211 | # Documents to append as an appendix to all manuals. 212 | # latex_appendices = [] 213 | 214 | # If false, no module index is generated. 215 | # latex_domain_indices = True 216 | 217 | 218 | # -- Options for manual page output -------------------------------------------- 219 | 220 | # One entry per manual page. List of tuples 221 | # (source start file, name, description, authors, manual section). 222 | man_pages = [ 223 | ("index", "route_network", "route_network Documentation", ["Jack O'Quin"], 1) 224 | ] 225 | 226 | # If true, show URL addresses after external links. 227 | # man_show_urls = False 228 | 229 | 230 | # -- Options for Texinfo output ------------------------------------------------ 231 | 232 | # Grouping the document tree into Texinfo files. List of tuples 233 | # (source start file, target name, title, author, 234 | # dir menu entry, description, category) 235 | texinfo_documents = [ 236 | ( 237 | "index", 238 | "route_network", 239 | "route_network Documentation", 240 | "Jack O'Quin", 241 | "route_network", 242 | "Route network graphing.", 243 | "Miscellaneous", 244 | ), 245 | ] 246 | 247 | # Documents to append as an appendix to all manuals. 248 | # texinfo_appendices = [] 249 | 250 | # If false, no module index is generated. 251 | # texinfo_domain_indices = True 252 | 253 | # How to display URL addresses: 'footnote', 'no', or 'inline'. 254 | # texinfo_show_urls = 'footnote' 255 | 256 | 257 | # Example configuration for intersphinx: refer to the Python standard library. 258 | intersphinx_mapping = {"http://docs.python.org/": None} 259 | -------------------------------------------------------------------------------- /osm_cartography/conf.py: -------------------------------------------------------------------------------- 1 | # 2 | # osm_cartography documentation build configuration file, created by 3 | # sphinx-quickstart on Wed Apr 25 16:01:20 2012. 4 | # 5 | # This file is execfile()d with the current directory set to its containing dir. 6 | # 7 | # Note that not all possible configuration values are present in this 8 | # autogenerated file. 9 | # 10 | # All configuration values have a default; values that are commented out 11 | # serve to show the default. 12 | 13 | import sys, os 14 | 15 | # If extensions (or modules to document with autodoc) are in another directory, 16 | # add these directories to sys.path here. If the directory is relative to the 17 | # documentation root, use os.path.abspath to make it absolute, like shown here. 18 | # sys.path.insert(0, os.path.abspath('.')) 19 | 20 | import roslib 21 | 22 | roslib.load_manifest("osm_cartography") 23 | 24 | # -- General configuration ----------------------------------------------------- 25 | 26 | # If your documentation needs a minimal Sphinx version, state it here. 27 | # needs_sphinx = '1.0' 28 | 29 | # Add any Sphinx extension module names here, as strings. They can be extensions 30 | # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. 31 | extensions = [ 32 | "sphinx.ext.autodoc", 33 | "sphinx.ext.intersphinx", 34 | "sphinx.ext.todo", 35 | "sphinx.ext.viewcode", 36 | ] 37 | 38 | # Add any paths that contain templates here, relative to this directory. 39 | templates_path = ["_templates"] 40 | 41 | # The suffix of source filenames. 42 | source_suffix = ".rst" 43 | 44 | # The encoding of source files. 45 | # source_encoding = 'utf-8-sig' 46 | 47 | # The master toctree document. 48 | master_doc = "index" 49 | 50 | # General information about the project. 51 | project = "osm_cartography" 52 | copyright = "2012, Jack O'Quin" 53 | 54 | # The version info for the project you're documenting, acts as replacement for 55 | # |version| and |release|, also used in various other places throughout the 56 | # built documents. 57 | # 58 | # The short X.Y version. 59 | version = "0.2.0" 60 | # The full version, including alpha/beta/rc tags. 61 | release = "0.2.0" 62 | 63 | # The language for content autogenerated by Sphinx. Refer to documentation 64 | # for a list of supported languages. 65 | # language = None 66 | 67 | # There are two options for replacing |today|: either, you set today to some 68 | # non-false value, then it is used: 69 | # today = '' 70 | # Else, today_fmt is used as the format for a strftime call. 71 | # today_fmt = '%B %d, %Y' 72 | 73 | # List of patterns, relative to source directory, that match files and 74 | # directories to ignore when looking for source files. 75 | exclude_patterns = ["_build"] 76 | 77 | # The reST default role (used for this markup: `text`) to use for all documents. 78 | # default_role = None 79 | 80 | # If true, '()' will be appended to :func: etc. cross-reference text. 81 | # add_function_parentheses = True 82 | 83 | # If true, the current module name will be prepended to all description 84 | # unit titles (such as .. function::). 85 | # add_module_names = True 86 | 87 | # If true, sectionauthor and moduleauthor directives will be shown in the 88 | # output. They are ignored by default. 89 | # show_authors = False 90 | 91 | # The name of the Pygments (syntax highlighting) style to use. 92 | pygments_style = "sphinx" 93 | 94 | # A list of ignored prefixes for module index sorting. 95 | # modindex_common_prefix = [] 96 | 97 | 98 | # -- Options for HTML output --------------------------------------------------- 99 | 100 | # The theme to use for HTML and HTML Help pages. See the documentation for 101 | # a list of builtin themes. 102 | html_theme = "default" 103 | 104 | # Theme options are theme-specific and customize the look and feel of a theme 105 | # further. For a list of options available for each theme, see the 106 | # documentation. 107 | # html_theme_options = {} 108 | 109 | # Add any paths that contain custom themes here, relative to this directory. 110 | # html_theme_path = [] 111 | 112 | # The name for this set of Sphinx documents. If None, it defaults to 113 | # " v documentation". 114 | # html_title = None 115 | 116 | # A shorter title for the navigation bar. Default is the same as html_title. 117 | # html_short_title = None 118 | 119 | # The name of an image file (relative to this directory) to place at the top 120 | # of the sidebar. 121 | # html_logo = None 122 | 123 | # The name of an image file (within the static path) to use as favicon of the 124 | # docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 125 | # pixels large. 126 | # html_favicon = None 127 | 128 | # Add any paths that contain custom static files (such as style sheets) here, 129 | # relative to this directory. They are copied after the builtin static files, 130 | # so a file named "default.css" will overwrite the builtin "default.css". 131 | html_static_path = ["_static"] 132 | 133 | # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, 134 | # using the given strftime format. 135 | # html_last_updated_fmt = '%b %d, %Y' 136 | 137 | # If true, SmartyPants will be used to convert quotes and dashes to 138 | # typographically correct entities. 139 | # html_use_smartypants = True 140 | 141 | # Custom sidebar templates, maps document names to template names. 142 | # html_sidebars = {} 143 | 144 | # Additional templates that should be rendered to pages, maps page names to 145 | # template names. 146 | # html_additional_pages = {} 147 | 148 | # If false, no module index is generated. 149 | # html_domain_indices = True 150 | 151 | # If false, no index is generated. 152 | # html_use_index = True 153 | 154 | # If true, the index is split into individual pages for each letter. 155 | # html_split_index = False 156 | 157 | # If true, links to the reST sources are added to the pages. 158 | # html_show_sourcelink = True 159 | 160 | # If true, "Created using Sphinx" is shown in the HTML footer. Default is True. 161 | # html_show_sphinx = True 162 | 163 | # If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. 164 | # html_show_copyright = True 165 | 166 | # If true, an OpenSearch description file will be output, and all pages will 167 | # contain a tag referring to it. The value of this option must be the 168 | # base URL from which the finished HTML is served. 169 | # html_use_opensearch = '' 170 | 171 | # This is the file name suffix for HTML files (e.g. ".xhtml"). 172 | # html_file_suffix = None 173 | 174 | # Output file base name for HTML help builder. 175 | htmlhelp_basename = "osm_cartographydoc" 176 | 177 | 178 | # -- Options for LaTeX output -------------------------------------------------- 179 | 180 | latex_elements = { 181 | # The paper size ('letterpaper' or 'a4paper'). 182 | #'papersize': 'letterpaper', 183 | # The font size ('10pt', '11pt' or '12pt'). 184 | #'pointsize': '10pt', 185 | # Additional stuff for the LaTeX preamble. 186 | #'preamble': '', 187 | } 188 | 189 | # Grouping the document tree into LaTeX files. List of tuples 190 | # (source start file, target name, title, author, documentclass [howto/manual]). 191 | latex_documents = [ 192 | ( 193 | "index", 194 | "osm_cartography.tex", 195 | "osm_cartography Documentation", 196 | "Jack O'Quin", 197 | "manual", 198 | ), 199 | ] 200 | 201 | # The name of an image file (relative to this directory) to place at the top of 202 | # the title page. 203 | # latex_logo = None 204 | 205 | # For "manual" documents, if this is true, then toplevel headings are parts, 206 | # not chapters. 207 | # latex_use_parts = False 208 | 209 | # If true, show page references after internal links. 210 | # latex_show_pagerefs = False 211 | 212 | # If true, show URL addresses after external links. 213 | # latex_show_urls = False 214 | 215 | # Documents to append as an appendix to all manuals. 216 | # latex_appendices = [] 217 | 218 | # If false, no module index is generated. 219 | # latex_domain_indices = True 220 | 221 | 222 | # -- Options for manual page output -------------------------------------------- 223 | 224 | # One entry per manual page. List of tuples 225 | # (source start file, name, description, authors, manual section). 226 | man_pages = [ 227 | ("index", "osm_cartography", "osm_cartography Documentation", ["Jack O'Quin"], 1) 228 | ] 229 | 230 | # If true, show URL addresses after external links. 231 | # man_show_urls = False 232 | 233 | 234 | # -- Options for Texinfo output ------------------------------------------------ 235 | 236 | # Grouping the document tree into Texinfo files. List of tuples 237 | # (source start file, target name, title, author, 238 | # dir menu entry, description, category) 239 | texinfo_documents = [ 240 | ( 241 | "index", 242 | "osm_cartography", 243 | "osm_cartography Documentation", 244 | "Jack O'Quin", 245 | "osm_cartography", 246 | "One line description of project.", 247 | "Miscellaneous", 248 | ), 249 | ] 250 | 251 | # Documents to append as an appendix to all manuals. 252 | # texinfo_appendices = [] 253 | 254 | # If false, no module index is generated. 255 | # texinfo_domain_indices = True 256 | 257 | # How to display URL addresses: 'footnote', 'no', or 'inline'. 258 | # texinfo_show_urls = 'footnote' 259 | 260 | 261 | # Example configuration for intersphinx: refer to the Python standard library. 262 | intersphinx_mapping = {"http://docs.python.org/": None} 263 | -------------------------------------------------------------------------------- /osm_cartography/scripts/viz_osm: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (C) 2012, Jack O'Quin 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the author nor of other contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # Revision $Id$ 35 | 36 | """ 37 | Create rviz markers for geographic information maps from Open Street 38 | Map server. 39 | """ 40 | 41 | 42 | PKG_NAME = "osm_cartography" 43 | import roslib 44 | 45 | roslib.load_manifest(PKG_NAME) 46 | import rospy 47 | 48 | import sys 49 | import itertools 50 | import geodesy.props 51 | import geodesy.utm 52 | import geodesy.wu_point 53 | from geodesy import bounding_box 54 | 55 | from geographic_msgs.msg import GeoPoint 56 | from geographic_msgs.srv import GetGeographicMap 57 | from geometry_msgs.msg import Point 58 | from geometry_msgs.msg import Quaternion 59 | from geometry_msgs.msg import Vector3 60 | from std_msgs.msg import ColorRGBA 61 | from visualization_msgs.msg import Marker 62 | from visualization_msgs.msg import MarkerArray 63 | 64 | # dynamic parameter reconfiguration 65 | from dynamic_reconfigure.server import Server as ReconfigureServer 66 | import osm_cartography.cfg.VizOSMConfig as Config 67 | 68 | 69 | class VizNode: 70 | def __init__(self): 71 | """ROS node to publish visualization markers for a GeographicMap.""" 72 | rospy.init_node("viz_osm") 73 | self.config = None 74 | 75 | # advertise visualization marker topic 76 | self.pub = rospy.Publisher( 77 | "visualization_marker_array", MarkerArray, latch=True, queue_size=10 78 | ) 79 | self.map = None 80 | self.msg = None 81 | rospy.wait_for_service("get_geographic_map") 82 | self.get_map = rospy.ServiceProxy("get_geographic_map", GetGeographicMap) 83 | 84 | # refresh the markers every three seconds, making them last four. 85 | self.timer_interval = rospy.Duration(3) 86 | self.marker_life = self.timer_interval + rospy.Duration(1) 87 | rospy.Timer(self.timer_interval, self.timer_callback) 88 | 89 | # register dynamic reconfigure callback, which runs immediately 90 | self.reconf_server = ReconfigureServer(Config, self.reconfigure) 91 | 92 | def get_markers(self, gmap): 93 | """Get markers for a GeographicMap message. 94 | 95 | :post: self.msg = visualization markers message 96 | """ 97 | self.map = gmap 98 | self.map_points = geodesy.wu_point.WuPointSet(gmap.points) 99 | self.msg = MarkerArray() 100 | self.mark_boundaries(ColorRGBA(r=0.5, g=0.5, b=0.5, a=0.8)) 101 | self.mark_way_points(ColorRGBA(r=1.0, g=1.0, b=0.0, a=0.8)) 102 | 103 | # define arguments for displaying various feature types 104 | road_props = set(["bridge", "highway", "tunnel"]) 105 | fargs = [ 106 | ( 107 | lambda (f): geodesy.props.match(f, road_props), 108 | ColorRGBA(r=8.0, g=0.2, b=0.2, a=0.8), 109 | "roads_osm", 110 | ), 111 | ( 112 | lambda (f): geodesy.props.match(f, set(["building"])), 113 | ColorRGBA(r=0.0, g=0.3, b=0.7, a=0.8), 114 | "buildings_osm", 115 | ), 116 | ( 117 | lambda (f): geodesy.props.match(f, set(["railway"])), 118 | ColorRGBA(r=0.0, g=0.7, b=0.7, a=0.8), 119 | "railroad_osm", 120 | ), 121 | ( 122 | lambda (f): geodesy.props.match(f, set(["amenity", "landuse"])), 123 | ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.5), 124 | "other_osm", 125 | ), 126 | ] 127 | for args in fargs: 128 | self.mark_features(*args) 129 | 130 | def mark_boundaries(self, color): 131 | # draw outline of map boundaries 132 | marker = Marker( 133 | header=self.map.header, 134 | ns="bounds_osm", 135 | id=0, 136 | type=Marker.LINE_STRIP, 137 | action=Marker.ADD, 138 | scale=Vector3(x=2.0), 139 | color=color, 140 | lifetime=self.marker_life, 141 | ) 142 | 143 | # Convert bounds latitudes and longitudes to UTM (no 144 | # altitude), convert UTM points to geometry_msgs/Point 145 | bbox = self.map.bounds 146 | min_lat, min_lon, max_lat, max_lon = bounding_box.getLatLong(bbox) 147 | p0 = geodesy.utm.fromLatLong(min_lat, min_lon).toPoint() 148 | p1 = geodesy.utm.fromLatLong(min_lat, max_lon).toPoint() 149 | p2 = geodesy.utm.fromLatLong(max_lat, max_lon).toPoint() 150 | p3 = geodesy.utm.fromLatLong(max_lat, min_lon).toPoint() 151 | 152 | # add line strips to bounds marker 153 | marker.points.append(p0) 154 | marker.points.append(p1) 155 | marker.points.append(p1) 156 | marker.points.append(p2) 157 | marker.points.append(p2) 158 | marker.points.append(p3) 159 | marker.points.append(p3) 160 | marker.points.append(p0) 161 | self.msg.markers.append(marker) 162 | 163 | def mark_features(self, predicate, color, namespace): 164 | """Create outline for map features 165 | 166 | :param predicate: function to match desired features 167 | :param color: RGBA value 168 | :param namespace: Rviz namespace. 169 | 170 | :todo: differentiate properties for: highway, building, 171 | bridge, tunnel, amenity, etc. 172 | """ 173 | index = 0 174 | for feature in itertools.ifilter(predicate, self.map.features): 175 | marker = Marker( 176 | header=self.map.header, 177 | ns=namespace, 178 | id=index, 179 | type=Marker.LINE_STRIP, 180 | action=Marker.ADD, 181 | scale=Vector3(x=2.0), 182 | color=color, 183 | lifetime=self.marker_life, 184 | ) 185 | index += 1 186 | prev_point = None 187 | for mbr in feature.components: 188 | wu_point = self.map_points.get(mbr.uuid) 189 | if wu_point: # this component is a way point 190 | p = wu_point.toPointXY() 191 | if prev_point: 192 | marker.points.append(prev_point) 193 | marker.points.append(p) 194 | prev_point = p 195 | self.msg.markers.append(marker) 196 | 197 | def mark_way_points(self, color): 198 | """Create slightly transparent disks for way-points. 199 | 200 | :param color: disk RGBA value 201 | """ 202 | index = 0 203 | for wp in self.map_points: 204 | marker = Marker( 205 | header=self.map.header, 206 | ns="waypoints_osm", 207 | id=index, 208 | type=Marker.CYLINDER, 209 | action=Marker.ADD, 210 | scale=Vector3(x=2.0, y=2.0, z=0.2), 211 | color=color, 212 | lifetime=self.marker_life, 213 | ) 214 | index += 1 215 | # use easting and northing coordinates (ignoring altitude) 216 | marker.pose.position = wp.toPointXY() 217 | marker.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) 218 | self.msg.markers.append(marker) 219 | 220 | def reconfigure(self, config, level): 221 | """Dynamic reconfigure callback. 222 | 223 | :param config: New configuration. 224 | :param level: 0x00000001 bit set if URL changed (ignored). 225 | 226 | :returns: New config if valid, old one otherwise. That updates 227 | the dynamic reconfigure GUI window. 228 | """ 229 | if self.config is None: 230 | self.config = config 231 | rospy.loginfo("Map URL: " + str(config["map_url"])) 232 | 233 | try: 234 | resp = self.get_map(config["map_url"], bounding_box.makeGlobal()) 235 | except rospy.ServiceException as e: 236 | rospy.logerr("Service call failed:", str(e)) 237 | # ignore new config, it failed 238 | else: # get_map returned 239 | if resp.success: 240 | self.get_markers(resp.map) 241 | self.config = config # save new URL 242 | # publish visualization markers (on a latched topic) 243 | self.pub.publish(self.msg) 244 | else: 245 | print("get_geographic_map failed, status:", str(resp.status)) 246 | 247 | return self.config 248 | 249 | def timer_callback(self, event): 250 | """ Called periodically to refresh map visualization. """ 251 | if self.msg is not None: 252 | now = rospy.Time() 253 | for m in self.msg.markers: 254 | m.header.stamp = now 255 | self.pub.publish(self.msg) 256 | 257 | 258 | def main(): 259 | viznode = VizNode() 260 | try: 261 | rospy.spin() 262 | except rospy.ROSInterruptException: 263 | pass 264 | 265 | 266 | if __name__ == "__main__": 267 | # run main function and exit 268 | sys.exit(main()) 269 | -------------------------------------------------------------------------------- /route_network/tests/test_planner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | PKG = "route_network" 4 | 5 | import unittest 6 | 7 | import geodesy.props 8 | import unique_id 9 | import geodesy.wu_point 10 | 11 | from geographic_msgs.msg import GeoPoint 12 | from geographic_msgs.msg import RouteNetwork 13 | from geographic_msgs.msg import RoutePath 14 | from geographic_msgs.msg import RouteSegment 15 | from geographic_msgs.msg import WayPoint 16 | from geographic_msgs.srv import GetRoutePlanRequest 17 | from geometry_msgs.msg import Point 18 | from geometry_msgs.msg import Quaternion 19 | 20 | try: 21 | from geographic_msgs.msg import UniqueID 22 | except ImportError: 23 | from uuid_msgs.msg import UniqueID 24 | 25 | # module to test 26 | from route_network.planner import * 27 | 28 | # :todo: place this in a module... 29 | PKG_URL = "http://wiki.ros.org/" + PKG 30 | 31 | 32 | def makeSeg(start, end, oneway=False): 33 | """Make RouteSegment message. 34 | 35 | :param start: Initial UUID. 36 | :param end: Final UUID. 37 | :param oneway: True if segment is one-way. 38 | :returns: RouteSegment message. 39 | """ 40 | uu = unique_id.toMsg(unique_id.fromURL(PKG_URL + "/" + str(start) + "/" + str(end))) 41 | 42 | seg = RouteSegment(id=uu, start=UniqueID(uuid=start), end=UniqueID(uuid=end)) 43 | if oneway: 44 | seg.props.append(KeyValue(key="oneway", value="yes")) 45 | return seg 46 | 47 | 48 | def makeRequest(network, start, goal): 49 | return GetRoutePlanRequest( 50 | UniqueID(uuid=network), UniqueID(uuid=start), UniqueID(uuid=goal) 51 | ) 52 | 53 | 54 | def makeSegment(id, start, end): 55 | return RouteSegment(id=UniqueID(id), start=UniqueID(start), end=UniqueID(end)) 56 | 57 | 58 | def makeWayPoint(id, lat, lon): 59 | w = WayPoint() 60 | w.id = UniqueID(id) 61 | w.position = GeoPoint(latitude=lat, longitude=lon) 62 | return w 63 | 64 | 65 | def tiny_graph(): 66 | "initialize test data: two points with segments between them" 67 | r = RouteNetwork() 68 | r.points.append( 69 | makeWayPoint("da7c242f-2efe-5175-9961-49cc621b80b9", 30.3840168, -97.72821) 70 | ) 71 | r.points.append( 72 | makeWayPoint("812f1c08-a34b-5a21-92b9-18b2b0cf4950", 30.385729, -97.7316754) 73 | ) 74 | r.segments.append( 75 | makeSegment( 76 | "41b7d2be-3c37-5a78-a7ac-248d939af379", 77 | "da7c242f-2efe-5175-9961-49cc621b80b9", 78 | "812f1c08-a34b-5a21-92b9-18b2b0cf4950", 79 | ) 80 | ) 81 | r.segments.append( 82 | makeSegment( 83 | "2df38f2c-202b-5ba5-be73-c6498cb4aafe", 84 | "812f1c08-a34b-5a21-92b9-18b2b0cf4950", 85 | "da7c242f-2efe-5175-9961-49cc621b80b9", 86 | ) 87 | ) 88 | return r 89 | 90 | 91 | def tiny_oneway(): 92 | "initialize test data: two points with one segment from first to second" 93 | r = RouteNetwork() 94 | r.points.append( 95 | makeWayPoint("da7c242f-2efe-5175-9961-49cc621b80b9", 30.3840168, -97.72821) 96 | ) 97 | r.points.append( 98 | makeWayPoint("812f1c08-a34b-5a21-92b9-18b2b0cf4950", 30.385729, -97.7316754) 99 | ) 100 | s = makeSegment( 101 | "41b7d2be-3c37-5a78-a7ac-248d939af379", 102 | "da7c242f-2efe-5175-9961-49cc621b80b9", 103 | "812f1c08-a34b-5a21-92b9-18b2b0cf4950", 104 | ) 105 | geodesy.props.put(s, "oneway", "yes") 106 | r.segments.append(s) 107 | 108 | return r 109 | 110 | 111 | def triangle_graph(): 112 | "initialize test data: three points with one-way segments between them" 113 | r = RouteNetwork() 114 | r.points.append( 115 | makeWayPoint("da7c242f-2efe-5175-9961-49cc621b80b9", 30.3840168, -97.72821) 116 | ) 117 | r.points.append( 118 | makeWayPoint("812f1c08-a34b-5a21-92b9-18b2b0cf4950", 30.385729, -97.7316754) 119 | ) 120 | r.points.append( 121 | makeWayPoint("2b093523-3b39-5c48-a11c-f7c65650c581", 30.3849643, -97.7269564) 122 | ) 123 | s = makeSegment( 124 | "41b7d2be-3c37-5a78-a7ac-248d939af379", 125 | "da7c242f-2efe-5175-9961-49cc621b80b9", 126 | "812f1c08-a34b-5a21-92b9-18b2b0cf4950", 127 | ) 128 | geodesy.props.put(s, "oneway", "yes") 129 | r.segments.append(s) 130 | s = makeSegment( 131 | "2df38f2c-202b-5ba5-be73-c6498cb4aafe", 132 | "812f1c08-a34b-5a21-92b9-18b2b0cf4950", 133 | "2b093523-3b39-5c48-a11c-f7c65650c581", 134 | ) 135 | geodesy.props.put(s, "oneway", "yes") 136 | r.segments.append(s) 137 | s = makeSegment( 138 | "8f2c2df3-be73-5ba5-202b-cb4aafec6498", 139 | "2b093523-3b39-5c48-a11c-f7c65650c581", 140 | "da7c242f-2efe-5175-9961-49cc621b80b9", 141 | ) 142 | geodesy.props.put(s, "oneway", "yes") 143 | r.segments.append(s) 144 | return r 145 | 146 | 147 | def float_range(begin, end, step): 148 | val = begin 149 | while val < end: 150 | yield val 151 | val += step 152 | 153 | 154 | def grid_graph(min_lat, min_lon, max_lat, max_lon, step=0.001): 155 | """Generate a fully-connected rectangular grid. 156 | 157 | :param min_lat: Initial latitude [degrees]. 158 | :param min_lon: Initial longitude [degrees]. 159 | :param max_lat: Latitude limit [degrees]. 160 | :param max_lon: Longitude limit [degrees]. 161 | :param step: Step size [degrees]. 162 | :returns: RouteNetwork message. 163 | """ 164 | nid = unique_id.toMsg(unique_id.fromURL(PKG_URL + "/test_network")) 165 | 166 | r = RouteNetwork(id=nid) 167 | prev_row = None 168 | for latitude in float_range(min_lat, max_lat, step): 169 | prev_col = None 170 | this_row = len(r.points) 171 | for longitude in float_range(min_lon, max_lon, step): 172 | fake_url = "fake://point/" + str(latitude) + "/" + str(longitude) 173 | pt_id = unique_id.fromURL(fake_url) 174 | r.points.append(makeWayPoint(pt_id, latitude, longitude)) 175 | if prev_col is not None: 176 | s = makeSeg(prev_col, pt_id) 177 | r.segments.append(s) 178 | s = makeSeg(pt_id, prev_col) 179 | r.segments.append(s) 180 | prev_col = pt_id 181 | if prev_row is not None: 182 | prev_id = r.points[prev_row].id.uuid 183 | s = makeSeg(prev_id, pt_id) 184 | r.segments.append(s) 185 | s = makeSeg(pt_id, prev_id) 186 | r.segments.append(s) 187 | prev_row += 1 188 | prev_row = this_row 189 | return r 190 | 191 | 192 | class TestPlanner(unittest.TestCase): 193 | """Unit tests for planner module.""" 194 | 195 | def test_empty_route_network(self): 196 | pl = Planner(RouteNetwork()) 197 | self.assertEqual(len(pl.points), 0) 198 | self.assertEqual(len(pl.graph.segments), 0) 199 | 200 | def test_tiny_route_network(self): 201 | pl = Planner(tiny_graph()) 202 | self.assertEqual(len(pl.points), 2) 203 | self.assertEqual(len(pl.graph.segments), 2) 204 | d = 383.272903769 205 | i0 = pl.points.index("da7c242f-2efe-5175-9961-49cc621b80b9") 206 | e0 = pl.edges[i0][0] 207 | self.assertEqual(e0.end, 1) 208 | self.assertAlmostEqual(e0.h, d) 209 | i1 = pl.points.index("812f1c08-a34b-5a21-92b9-18b2b0cf4950") 210 | e1 = pl.edges[i1][0] 211 | self.assertEqual(e1.end, 0) 212 | self.assertAlmostEqual(e1.h, d) 213 | self.assertAlmostEqual(pl.points.distance2D(i0, i1), d) 214 | 215 | # going forward or backward should yield a single route segment 216 | path = pl.planner( 217 | makeRequest( 218 | pl.graph.id.uuid, 219 | "da7c242f-2efe-5175-9961-49cc621b80b9", 220 | "812f1c08-a34b-5a21-92b9-18b2b0cf4950", 221 | ) 222 | ) 223 | self.assertEqual(len(path.segments), 1) 224 | self.assertEqual(path.segments[0].uuid, "41b7d2be-3c37-5a78-a7ac-248d939af379") 225 | path = pl.planner( 226 | makeRequest( 227 | pl.graph.id.uuid, 228 | "812f1c08-a34b-5a21-92b9-18b2b0cf4950", 229 | "da7c242f-2efe-5175-9961-49cc621b80b9", 230 | ) 231 | ) 232 | self.assertEqual(len(path.segments), 1) 233 | self.assertEqual(path.segments[0].uuid, "2df38f2c-202b-5ba5-be73-c6498cb4aafe") 234 | 235 | def test_tiny_oneway_network(self): 236 | pl = Planner(tiny_oneway()) 237 | self.assertEqual(len(pl.points), 2) 238 | self.assertEqual(len(pl.graph.segments), 1) 239 | d = 383.272903769 240 | i0 = pl.points.index("da7c242f-2efe-5175-9961-49cc621b80b9") 241 | e0 = pl.edges[i0][0] 242 | self.assertEqual(e0.end, 1) 243 | self.assertAlmostEqual(e0.h, d) 244 | i1 = pl.points.index("812f1c08-a34b-5a21-92b9-18b2b0cf4950") 245 | self.assertEqual(pl.edges[i1], []) 246 | self.assertAlmostEqual(pl.points.distance2D(i0, i1), d) 247 | 248 | # going forward should yield a single route segment 249 | path = pl.planner( 250 | makeRequest( 251 | pl.graph.id.uuid, 252 | "da7c242f-2efe-5175-9961-49cc621b80b9", 253 | "812f1c08-a34b-5a21-92b9-18b2b0cf4950", 254 | ) 255 | ) 256 | self.assertEqual(len(path.segments), 1) 257 | self.assertEqual(path.segments[0].uuid, "41b7d2be-3c37-5a78-a7ac-248d939af379") 258 | 259 | def test_no_path_to_goal(self): 260 | pl = Planner(tiny_oneway()) 261 | req = makeRequest( 262 | pl.graph.id.uuid, 263 | "812f1c08-a34b-5a21-92b9-18b2b0cf4950", 264 | "da7c242f-2efe-5175-9961-49cc621b80b9", 265 | ) 266 | self.assertRaises(NoPathToGoalError, pl.planner, req) 267 | 268 | def test_starting_at_goal(self): 269 | pl = Planner(tiny_oneway()) 270 | req = makeRequest( 271 | pl.graph.id.uuid, 272 | "da7c242f-2efe-5175-9961-49cc621b80b9", 273 | "da7c242f-2efe-5175-9961-49cc621b80b9", 274 | ) 275 | path = pl.planner(req) 276 | self.assertEqual(len(path.segments), 0) 277 | 278 | def test_bogus_network(self): 279 | pl = Planner(tiny_graph()) 280 | req = makeRequest( 281 | "deadbeef", 282 | "812f1c08-a34b-5a21-92b9-18b2b0cf4950", 283 | "da7c242f-2efe-5175-9961-49cc621b80b9", 284 | ) 285 | self.assertRaises(ValueError, pl.planner, req) 286 | 287 | def test_bogus_start_point(self): 288 | pl = Planner(tiny_graph()) 289 | req = makeRequest( 290 | pl.graph.id.uuid, "deadbeef", "da7c242f-2efe-5175-9961-49cc621b80b9" 291 | ) 292 | self.assertRaises(ValueError, pl.planner, req) 293 | 294 | def test_bogus_goal(self): 295 | pl = Planner(tiny_graph()) 296 | req = makeRequest( 297 | pl.graph.id.uuid, "812f1c08-a34b-5a21-92b9-18b2b0cf4950", "deadbeef" 298 | ) # invalid way point 299 | self.assertRaises(ValueError, pl.planner, req) 300 | 301 | def test_triangle_routes(self): 302 | pl = Planner(triangle_graph()) 303 | 304 | # going forward should yield a single route segment 305 | path = pl.planner( 306 | makeRequest( 307 | pl.graph.id.uuid, 308 | "da7c242f-2efe-5175-9961-49cc621b80b9", 309 | "812f1c08-a34b-5a21-92b9-18b2b0cf4950", 310 | ) 311 | ) 312 | self.assertEqual(len(path.segments), 1) 313 | self.assertEqual(path.segments[0].uuid, "41b7d2be-3c37-5a78-a7ac-248d939af379") 314 | 315 | # going backward should yield two route segments 316 | path = pl.planner( 317 | makeRequest( 318 | pl.graph.id.uuid, 319 | "812f1c08-a34b-5a21-92b9-18b2b0cf4950", 320 | "da7c242f-2efe-5175-9961-49cc621b80b9", 321 | ) 322 | ) 323 | self.assertEqual(len(path.segments), 2) 324 | self.assertEqual(path.segments[0].uuid, "2df38f2c-202b-5ba5-be73-c6498cb4aafe") 325 | self.assertEqual(path.segments[1].uuid, "8f2c2df3-be73-5ba5-202b-cb4aafec6498") 326 | 327 | def test_2x2_grid(self): 328 | # generate a fully-connected 2x2 grid 329 | g = grid_graph(0.0, 0.0, 0.002, 0.002) 330 | pl = Planner(g) 331 | # self.fail(msg=str(pl)) 332 | # all pairs of points should have a valid path 333 | for pt1 in g.points: 334 | for pt2 in g.points: 335 | path = pl.planner(makeRequest(g.id.uuid, pt1.id.uuid, pt2.id.uuid)) 336 | 337 | def test_3x3_grid(self): 338 | # generate a fully-connected 3x3 grid 339 | g = grid_graph(-0.003, -0.003, 0.0, 0.0) 340 | pl = Planner(g) 341 | # all pairs of points should have a valid path 342 | for pt1 in g.points: 343 | for pt2 in g.points: 344 | path = pl.planner(makeRequest(g.id.uuid, pt1.id.uuid, pt2.id.uuid)) 345 | 346 | def test_10x10_grid(self): 347 | # generate a fully-connected 10x10 grid 348 | g = grid_graph(0.0, 0.0, 0.01, 0.01) 349 | pl = Planner(g) 350 | # all pairs of points should have a valid path 351 | for pt1 in g.points: 352 | for pt2 in g.points: 353 | path = pl.planner(makeRequest(g.id.uuid, pt1.id.uuid, pt2.id.uuid)) 354 | 355 | 356 | if __name__ == "__main__": 357 | import rosunit 358 | 359 | rosunit.unitrun(PKG, "test_planner_py", TestPlanner) 360 | -------------------------------------------------------------------------------- /route_network/scripts/rviz_goal: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (C) 2013, Alexander Tiderko 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the author nor of other contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | 35 | import math 36 | import sys 37 | import threading 38 | 39 | from geographic_msgs.msg import GeoPoint 40 | from geographic_msgs.msg import RouteNetwork 41 | from geographic_msgs.msg import RoutePath 42 | from geographic_msgs.srv import GetGeoPath 43 | from geographic_msgs.srv import GetGeoPathResponse 44 | from geographic_msgs.srv import GetRoutePlan 45 | from geographic_msgs.srv import GetRoutePlanResponse 46 | from geometry_msgs.msg import Point 47 | from geometry_msgs.msg import PoseStamped 48 | from geometry_msgs.msg import PoseWithCovarianceStamped 49 | from geometry_msgs.msg import Quaternion 50 | from geometry_msgs.msg import Vector3 51 | from route_network import planner 52 | from sensor_msgs.msg import NavSatFix 53 | from std_msgs.msg import ColorRGBA 54 | from visualization_msgs.msg import Marker 55 | from visualization_msgs.msg import MarkerArray 56 | import geodesy 57 | import rospy 58 | import tf 59 | 60 | 61 | class RvizGoalListener: 62 | """ 63 | ROS node to handle the goal and init positions from rviz and calculate the route. 64 | """ 65 | 66 | def __init__(self): 67 | rospy.init_node("rviz_goal") 68 | self._lock = threading.RLock() 69 | self.graph = None 70 | rospy.loginfo("Waiting for 'get_geo_path' service...") 71 | rospy.wait_for_service("get_geo_path") 72 | self.get_geo_path = rospy.ServiceProxy("get_geo_path", GetGeoPath) 73 | 74 | # Advertise visualization marker topic (display the map on RVIZ) 75 | self.viz_pub = rospy.Publisher( 76 | "visualization_marker_array", MarkerArray, latch=True, queue_size=1 77 | ) 78 | 79 | # Subscribe to sat fix 80 | self._sub_fix = rospy.Subscriber("fix", NavSatFix, self.gps_fix_callback) 81 | 82 | # Subscribe to initialpose: Listen for starting position to plan a route. 83 | self._sub_init = rospy.Subscriber( 84 | "initialpose", PoseWithCovarianceStamped, self.initpose_callback 85 | ) 86 | 87 | # Subscribe to goal: Listen for goal position to plan a route. 88 | self._sub_goal = rospy.Subscriber("goal", PoseStamped, self.goal_callback) 89 | 90 | self._own_geo_point = None 91 | self._own_utmpoint = None 92 | self.listener = tf.TransformListener() 93 | self.gridZone = None 94 | 95 | # Subscribe to route network (Graph on which the planning is carried out). 96 | self._sub_route = rospy.Subscriber( 97 | "route_network", RouteNetwork, self.graph_callback 98 | ) 99 | 100 | def graph_callback(self, graph): 101 | """Callback to initialize to graph to plan on. 102 | 103 | :param graph: The required graph. 104 | :type graph: geographic_msgs/RouteNetwork 105 | """ 106 | if len(graph.points) > 0: 107 | graph.points[0].position 108 | utm = geodesy.utm.fromLatLong( 109 | graph.points[0].position.latitude, 110 | graph.points[0].position.longitude, 111 | graph.points[0].position.altitude, 112 | ) 113 | self.gridZone = utm.gridZone() 114 | self.points = geodesy.wu_point.WuPointSet(graph.points) 115 | self.segment_ids = {} # segments symbol table 116 | for sid in xrange(len(graph.segments)): 117 | self.segment_ids[graph.segments[sid].id.uuid] = sid 118 | self.graph = graph 119 | 120 | def initpose_callback(self, msg): 121 | """Callback to aquire the initial position from RVIZ. 122 | 123 | :param msg: The initial position. 124 | :type msg: geometry_msgs/PoseWithCovarianceStamped 125 | """ 126 | with self._lock: 127 | rospy.loginfo("Init pose received.") 128 | if not self.gridZone is None: 129 | (map_trans, map_rot) = self.listener.lookupTransform( 130 | msg.header.frame_id, "/world", rospy.Time(0) 131 | ) 132 | self._own_utmpoint = geodesy.utm.UTMPoint( 133 | easting=msg.pose.pose.position.x - map_trans[0], 134 | northing=msg.pose.pose.position.y - map_trans[1], 135 | altitude=msg.pose.pose.position.z - map_trans[2], 136 | zone=self.gridZone[0], 137 | band=self.gridZone[1], 138 | ) 139 | self._own_geo_point = self._own_utmpoint.toMsg() 140 | else: 141 | rospy.logerr("Unknown UMT grid zone!") 142 | 143 | def gps_fix_callback(self, msg): 144 | """Callback to acquire the GPS Fix message from a robot. 145 | 146 | :param msg: The GPS position. 147 | :type msg: geometry_msgs/PoseWithCovarianceStamped 148 | """ 149 | with self._lock: 150 | self._own_utmpoint = geodesy.utm.fromLatLong( 151 | msg.latitude, msg.longitude, msg.altitude 152 | ) 153 | self._own_geo_point = GeoPoint(msg.latitude, msg.longitude, msg.altitude) 154 | 155 | def goal_callback(self, msg): 156 | """Callback to acquire the goal position, and calculate the plan towards it. 157 | 158 | :param msg: The goal position. 159 | :type msg: geometry_msgs/PoseStamped 160 | """ 161 | with self._lock: 162 | rospy.loginfo("Goal pose received.") 163 | if not self._own_geo_point is None: 164 | try: 165 | (map_trans, map_rot) = self.listener.lookupTransform( 166 | msg.header.frame_id, "/world", rospy.Time(0) 167 | ) 168 | gridZone = self._own_utmpoint.gridZone() 169 | goal_utm = geodesy.utm.UTMPoint( 170 | easting=msg.pose.position.x - map_trans[0], 171 | northing=msg.pose.position.y - map_trans[1], 172 | altitude=msg.pose.position.z - map_trans[2], 173 | zone=gridZone[0], 174 | band=gridZone[1], 175 | ) 176 | geo_goal = goal_utm.toMsg() 177 | path_plan = self.get_geo_path(self._own_geo_point, geo_goal) 178 | self._pub_viz_path( 179 | [pose.pose.position for pose in path_plan.plan.poses], 180 | self._own_geo_point, 181 | geo_goal, 182 | path_plan.start_seg, 183 | path_plan.goal_seg, 184 | path_plan.distance, 185 | ) 186 | except (tf.Exception, tf.LookupException, tf.ConnectivityException): 187 | import traceback 188 | 189 | print traceback.format_exc() 190 | else: 191 | print "no init pose" 192 | 193 | def _pub_viz_path(self, path, start, goal, start_seg, end_seg, distance): 194 | """Publish visualization markers for a RoutePath. 195 | 196 | :param path: The planned path to be visualized. 197 | :type path: geographic_msgs/GeoPoint[] 198 | :param start: The start position. 199 | :type start: geographic_msgs/GeoPoint 200 | :param goal: The goal position. 201 | :type goal: geographic_msgs/GeoPoint 202 | :param start_seg: The ID of the nearest graph-segment to the starting way point. 203 | :type start_seg: uuid_msgs/UniqueID 204 | :param end_seg: The ID of the nearest segment to the goal way point. 205 | :type end_seg: uuid_msgs/UniqueID 206 | :param distance: Length of the planned route. 207 | :type distance: float64 208 | """ 209 | if self.viz_pub.get_num_connections() > 0 and not self.graph is None: 210 | life_time = rospy.Duration(120) 211 | marks = MarkerArray() 212 | hdr = self.graph.header 213 | hdr.stamp = rospy.Time.now() 214 | index = 0 215 | # add nearest segments 216 | marker = Marker( 217 | header=hdr, 218 | ns="start segment", 219 | id=index, 220 | type=Marker.LINE_STRIP, 221 | action=Marker.ADD, 222 | scale=Vector3(x=3.0), 223 | color=ColorRGBA(r=0.3, g=0.3, b=1.0, a=0.8), 224 | lifetime=life_time, 225 | ) 226 | segment = self.graph.segments[self.segment_ids[start_seg.uuid]] 227 | marker.points.append(self.points[segment.start.uuid].toPointXY()) 228 | marker.points.append(self.points[segment.end.uuid].toPointXY()) 229 | marks.markers.append(marker) 230 | marker = Marker( 231 | header=hdr, 232 | ns="goal segment", 233 | id=index, 234 | type=Marker.LINE_STRIP, 235 | action=Marker.ADD, 236 | scale=Vector3(x=3.0), 237 | color=ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.8), 238 | lifetime=life_time, 239 | ) 240 | segment = self.graph.segments[self.segment_ids[end_seg.uuid]] 241 | marker.points.append(self.points[segment.start.uuid].toPointXY()) 242 | marker.points.append(self.points[segment.end.uuid].toPointXY()) 243 | marks.markers.append(marker) 244 | 245 | # add start point 246 | marker = Marker( 247 | header=hdr, 248 | ns="start", 249 | id=index, 250 | type=Marker.CYLINDER, 251 | action=Marker.ADD, 252 | scale=Vector3(x=6.0, y=4.0, z=10.0), 253 | color=ColorRGBA(r=0.3, g=0.3, b=1.0, a=0.8), 254 | lifetime=life_time, 255 | ) 256 | utm_start = geodesy.utm.fromMsg(start) 257 | marker.pose.position.x = utm_start.easting 258 | marker.pose.position.y = utm_start.northing 259 | marks.markers.append(marker) 260 | # add the route 261 | index += 1 262 | path_marker = Marker( 263 | header=hdr, 264 | ns="path", 265 | id=index, 266 | type=Marker.LINE_STRIP, 267 | action=Marker.ADD, 268 | scale=Vector3(x=3.0), 269 | color=ColorRGBA(r=1.0, g=1.0, b=1.0, a=0.8), 270 | lifetime=life_time, 271 | ) 272 | for geo_point in path: 273 | utm_point = geodesy.utm.fromMsg(geo_point) 274 | path_marker.points.append( 275 | Point(utm_point.easting, utm_point.northing, 0.0) 276 | ) 277 | marks.markers.append(path_marker) 278 | index += 1 279 | # add goal point 280 | marker = Marker( 281 | header=hdr, 282 | ns="goal", 283 | id=index, 284 | type=Marker.CYLINDER, 285 | action=Marker.ADD, 286 | scale=Vector3(x=6.0, y=4.0, z=10.0), 287 | color=ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.8), 288 | lifetime=life_time, 289 | ) 290 | utm_goal = geodesy.utm.fromMsg(goal) 291 | marker.pose.position.x = utm_goal.easting 292 | marker.pose.position.y = utm_goal.northing 293 | marks.markers.append(marker) 294 | 295 | # add distance to the goal 296 | marker = Marker( 297 | header=hdr, 298 | ns="goal distance", 299 | id=index, 300 | text="{:.2f}m".format(distance), 301 | type=Marker.TEXT_VIEW_FACING, 302 | action=Marker.ADD, 303 | scale=Vector3(x=6.0, y=4.0, z=10.0), 304 | color=ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.8), 305 | lifetime=life_time, 306 | ) 307 | utm_goal = geodesy.utm.fromMsg(goal) 308 | marker.pose.position.x = utm_goal.easting 309 | marker.pose.position.y = utm_goal.northing 310 | marker.pose.position.z = 15 311 | marks.markers.append(marker) 312 | self.viz_pub.publish(marks) 313 | 314 | 315 | def main(): 316 | node_class = RvizGoalListener() 317 | try: 318 | rospy.spin() # wait for messages 319 | except rospy.ROSInterruptException: 320 | pass 321 | return 0 322 | 323 | 324 | if __name__ == "__main__": 325 | # run main function and exit 326 | sys.exit(main()) 327 | -------------------------------------------------------------------------------- /route_network/src/route_network/planner.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (C) 2012, Jack O'Quin 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 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of the author nor of other contributors may be 17 | # used to endorse or promote products derived from this software 18 | # without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | # 33 | # Revision $Id$ 34 | 35 | """ 36 | Route network path planner. 37 | 38 | .. _`geographic_msgs/GetRoutePlan`: http://ros.org/doc/api/geographic_msgs/html/srv/GetRoutePlan.html 39 | .. _`geographic_msgs/RouteNetwork`: http://ros.org/doc/api/geographic_msgs/html/msg/RouteNetwork.html 40 | .. _`geographic_msgs/RoutePath`: http://ros.org/doc/api/geographic_msgs/html/msg/RoutePath.html 41 | .. _`uuid_msgs/UniqueID`: http://ros.org/doc/api/uuid_msgs/html/msg/UniqueID.html 42 | 43 | """ 44 | 45 | import numpy 46 | import math 47 | import geodesy.utm 48 | import geodesy.wu_point 49 | import rospy 50 | 51 | from geographic_msgs.msg import RouteNetwork 52 | from geographic_msgs.msg import RoutePath 53 | from geographic_msgs.msg import RouteSegment 54 | from geographic_msgs.srv import GetRoutePlan 55 | from geometry_msgs.msg import Point 56 | from geometry_msgs.msg import Quaternion 57 | 58 | 59 | class PlannerError(Exception): 60 | """Base class for exceptions in this module.""" 61 | 62 | pass 63 | 64 | 65 | class NoPathToGoalError(PlannerError): 66 | """Exception raised when there is no path to the goal.""" 67 | 68 | pass 69 | 70 | 71 | class Edge: 72 | """ 73 | :class:`Edge` stores graph edge data for a way point. 74 | 75 | :param end: Index of ending way point. 76 | :param seg: `uuid_msgs/UniqueID`_ of corresponding RouteSegment. 77 | :param heuristic: Distance heuristic from start to end (must 78 | *not* be an over-estimate). 79 | """ 80 | 81 | def __init__(self, end, seg, heuristic=0.0): 82 | """Constructor. """ 83 | self.end = end 84 | self.seg = seg 85 | self.h = heuristic 86 | 87 | def __str__(self): 88 | return str(self.end) + " " + str(self.seg.uuid) + " (" + str(self.h) + ")" 89 | 90 | 91 | class Planner: 92 | """ 93 | :class:`Planner` plans a route through a RouteNetwork. 94 | 95 | :param graph: `geographic_msgs/RouteNetwork`_ message. 96 | """ 97 | 98 | def __init__(self, graph): 99 | """Constructor. 100 | 101 | Collects relevant information from route network message, 102 | providing convenient access to the data. 103 | """ 104 | self._shift_to_route_within = rospy.get_param("~shift_to_route_within", 7.0) 105 | rospy.loginfo("~shift_to_route_within: %.2f", self._shift_to_route_within) 106 | 107 | self.graph = graph 108 | self.points = geodesy.wu_point.WuPointSet(graph.points) 109 | 110 | # Create empty list of graph edges leaving each map point. 111 | self.edges = [[] for wid in range(len(self.points))] 112 | for seg in self.graph.segments: 113 | index = self.points.index(seg.start.uuid) 114 | if index is not None: 115 | n = self.points.index(seg.end.uuid) 116 | if n is not None: 117 | # use 2D Euclidean distance for the heuristic 118 | dist = self.points.distance2D(index, n) 119 | self.edges[index].append(Edge(n, seg.id, heuristic=dist)) 120 | 121 | # Create a list with utm point for each point 122 | self.utm_points = dict() 123 | for p in self.points: 124 | self.utm_points[self.points.index(p.uuid())] = geodesy.utm.fromMsg( 125 | p.position() 126 | ) 127 | 128 | def __str__(self): 129 | val = "\n" 130 | for i in range(len(self.edges)): 131 | val += str(i) + ":\n" 132 | for k in self.edges[i]: 133 | val += " " + str(k) + "\n" 134 | return val 135 | 136 | def planner(self, req): 137 | """Plan route from start to goal. 138 | 139 | :param req: `geographic_msgs/GetRoutePlan`_ request message. 140 | :returns: `geographic_msgs/RoutePath`_ message. 141 | :raises: :exc:`ValueError` if invalid request. 142 | :raises: :exc:`NoPathToGoalError` if goal not reachable. 143 | """ 144 | # validate request parameters 145 | if req.network.uuid != self.graph.id.uuid: # different route network? 146 | raise ValueError("invalid GetRoutePlan network: " + str(req.network.uuid)) 147 | start_idx = self.points.index(req.start.uuid) 148 | if start_idx is None: 149 | raise ValueError("unknown starting point: " + str(req.start.uuid)) 150 | goal_idx = self.points.index(req.goal.uuid) 151 | if goal_idx is None: 152 | raise ValueError("unknown goal: " + str(req.goal.uuid)) 153 | 154 | # initialize plan 155 | plan = RoutePath(network=self.graph.id) 156 | plan.network = req.network 157 | 158 | # A* shortest path algorithm 159 | opened = [[0.0, start_idx]] 160 | closed = [False for wid in range(len(self.points))] 161 | closed[start_idx] = True 162 | backpath = [None for wid in range(len(self.points))] 163 | while True: 164 | if len(opened) == 0: 165 | raise NoPathToGoalError( 166 | "No path from " + req.start.uuid + " to " + req.goal.uuid 167 | ) 168 | opened.sort() # :todo: make search more efficient 169 | opened.reverse() 170 | h, e = opened.pop() 171 | if e == goal_idx: 172 | break 173 | for edge in self.edges[e]: 174 | e2 = edge.end 175 | if not closed[e2]: 176 | h2 = h + edge.h 177 | opened.append([h2, e2]) 178 | closed[e2] = True 179 | backpath[e2] = [e, edge] 180 | 181 | # generate plan segments from backpath 182 | plan.segments = [] 183 | e = goal_idx 184 | while backpath[e] is not None: 185 | plan.segments.append(backpath[e][1].seg) 186 | e = backpath[e][0] 187 | assert e == start_idx 188 | plan.segments.reverse() 189 | return plan 190 | 191 | def geo_path(self, req): 192 | """Plan the shortest path between a start and a destination geopoint. 193 | 194 | Unlike the 'planner' method, the 'geo_path' method can receive GeoPoints out of the graph, upon such a case, the nearest segments on the OSM map are detected, 195 | and the planning is carried out. 196 | 197 | :pram req: The request message. 198 | :param req: geographic_msgs/GetGeoPath 199 | 200 | :return: The computed path, as well as the ids of the RouteNetwork and the start and end segments, plus the length of the path. The length is set to -1 in case of failure. 201 | :rtype: (geographic_msgs/GeoPoint[], uuid_msgs/UniqueID, uuid_msgs/UniqueID, uuid_msgs/UniqueID, length) 202 | :raises: :exc:`ValueError` if invalid request. 203 | """ 204 | # check for possible errors in request 205 | if math.isnan(req.start.latitude) or math.isnan(req.start.longitude): 206 | raise ValueError("NaN in starting point: " + str(req.start)) 207 | if math.isnan(req.goal.latitude) or math.isnan(req.goal.longitude): 208 | raise ValueError("NaN in starting point: " + str(req.start)) 209 | if math.isnan(req.start.altitude): 210 | req.start.altitude = 0.0 211 | if math.isnan(req.goal.altitude): 212 | req.goal.altitude = 0.0 213 | # get the nearest segment to the start point 214 | (start_seg, start_dist, start_lot) = self.getNearestSegment(req.start) 215 | if start_seg is None: 216 | raise ValueError( 217 | "no nearest segment found for starting way point: " + str(req.start) 218 | ) 219 | # get the nearest segment to the destination point 220 | (goal_seg, goal_dist, goal_lot) = self.getNearestSegment(req.goal) 221 | if goal_seg is None: 222 | raise ValueError( 223 | "no nearest segment found for goal way point: " + str(req.goal) 224 | ) 225 | 226 | # determine the path 227 | result = [] 228 | try: 229 | (route_path, dist) = self._planner_seg( 230 | req.start, start_seg, req.goal, goal_seg 231 | ) 232 | except NoPathToGoalError: 233 | return result, self.graph.id, start_seg.id, goal_seg.id, -1 234 | start_utm = geodesy.utm.fromMsg(req.start) 235 | goal_utm = geodesy.utm.fromMsg(req.goal) 236 | # copy the route to the result list and adds the start and/or goal position to the path 237 | if route_path.segments: 238 | seg = self._getSegment(route_path.segments[0]) 239 | p = self._get_min_point(start_seg, start_lot) 240 | # add the start point if it is not close to the route 241 | dist_first_to_start = self.distance2D(p, start_utm) 242 | if dist_first_to_start > self._shift_to_route_within: 243 | result.append(req.start) 244 | result.append(p.toMsg()) 245 | # add only the endpoints of the segments 246 | for index in range(len(route_path.segments)): 247 | seg_id = route_path.segments[index] 248 | seg = self._getSegment(seg_id) 249 | 250 | if index == 0: 251 | # add the segment start point, if the plan segment and nearest segment are not equal 252 | if not ( 253 | ( 254 | seg.end.uuid == start_seg.start.uuid 255 | and seg.start.uuid == start_seg.end.uuid 256 | ) 257 | or ( 258 | seg.start.uuid == start_seg.start.uuid 259 | and seg.end.uuid == start_seg.end.uuid 260 | ) 261 | ): 262 | result.append(self.points[seg.start.uuid].position()) 263 | if index + 1 == len(route_path.segments): 264 | # add the end point of the last segment, if the plan segment and nearest segment are not equal 265 | if not ( 266 | ( 267 | seg.end.uuid == goal_seg.start.uuid 268 | and seg.start.uuid == goal_seg.end.uuid 269 | ) 270 | or ( 271 | seg.start.uuid == goal_seg.start.uuid 272 | and seg.end.uuid == goal_seg.end.uuid 273 | ) 274 | ): 275 | result.append(self.points[seg.end.uuid].position()) 276 | else: 277 | result.append(self.points[seg.end.uuid].position()) 278 | # add a perpendicular point or the nearest endpoint of the end segment 279 | p = self._get_min_point(goal_seg, goal_lot) 280 | result.append(p.toMsg()) 281 | # add the destination point if it is not close to the route 282 | dist_last_to_goal = self.distance2D(p, goal_utm) 283 | if dist_last_to_goal > self._shift_to_route_within: 284 | result.append(req.goal) 285 | else: 286 | if ( 287 | start_seg.end.uuid == goal_seg.start.uuid 288 | and start_seg.start.uuid == goal_seg.end.uuid 289 | ) or ( 290 | start_seg.start.uuid == goal_seg.start.uuid 291 | and start_seg.end.uuid == goal_seg.end.uuid 292 | ): 293 | # direct connection 294 | result.append(req.start) 295 | result.append(req.goal) 296 | 297 | # calculate the distance 298 | last_utm = None 299 | dist = 0 300 | for point in result: 301 | if last_utm is None: 302 | last_utm = geodesy.utm.fromMsg(point) 303 | else: 304 | new_utm = geodesy.utm.fromMsg(point) 305 | dist += self.distance2D(last_utm, new_utm) 306 | last_utm = new_utm 307 | return result, self.graph.id, start_seg.id, goal_seg.id, dist 308 | 309 | def _planner_seg(self, start_geo_point, start_seg, goal_geo_point, goal_seg): 310 | """Plan route from start to goal. The actual search algorithm to find a path is executed here. 311 | 312 | :param start_geo_point: The start position. 313 | :type start_geo_point: geographic_msgs/GeoPoint 314 | :param start_seg: The nearest segment to the point. 315 | :type start_seg: geographic_msgs/RouteSegment 316 | :param: goal_geo_point: The goal position. 317 | :type goal_geo_point: geographic_msgs/GeoPoint 318 | :param goal_seg: The nearest segment to the point. 319 | :type goal_seg: geographic_msgs/RouteSegment 320 | 321 | :return: The planned path between start and goal, and its length. 322 | :rtype: (geographic_msgs/RoutePath, float) 323 | 324 | :raises: :exc: ValueError if invalid request. 325 | :raises: :exc: NoPathToGoalError if goal not reachable. 326 | """ 327 | # validate request parameters 328 | start__seg_start_idx = self.points.index(start_seg.start.uuid) 329 | start__seg_end_idx = self.points.index(start_seg.end.uuid) 330 | if start__seg_start_idx is None or start__seg_end_idx is None: 331 | raise ValueError("unknown starting segment: " + str(start_seg.id)) 332 | goal__seg_start_idx = self.points.index(goal_seg.start.uuid) 333 | goal__seg_end_idx = self.points.index(goal_seg.end.uuid) 334 | if goal__seg_start_idx is None or goal__seg_end_idx is None: 335 | raise ValueError("unknown goal segment: " + str(goal_seg.id)) 336 | 337 | # initialize plan 338 | plan = RoutePath(network=self.graph.id) 339 | 340 | # A* shortest path algorithm 341 | opened = [] 342 | # add the distance from start point to the start and to the end of the nearest segment 343 | start_utm = geodesy.utm.fromMsg(start_geo_point) 344 | utm_seg_start = self.utm_points[self.points.index(start_seg.start.uuid)] 345 | utm_seg_end = self.utm_points[self.points.index(start_seg.end.uuid)] 346 | length_start2seg_start = self.distance2D(start_utm, utm_seg_start) 347 | length_start2seg_end = self.distance2D(start_utm, utm_seg_end) 348 | 349 | # distance of the last segment to the goal 350 | goal_utm = geodesy.utm.fromMsg(goal_geo_point) 351 | utm_goal_seg_start = self.utm_points[self.points.index(goal_seg.start.uuid)] 352 | utm_goal_seg_end = self.utm_points[self.points.index(goal_seg.end.uuid)] 353 | length_goal2seg_start = self.distance2D(goal_utm, utm_goal_seg_start) 354 | length_goal2seg_end = self.distance2D(goal_utm, utm_goal_seg_end) 355 | 356 | opened.append([length_start2seg_start, start__seg_start_idx]) 357 | 358 | closed = [0 for wid in range(len(self.points))] 359 | closed[start__seg_start_idx] = -1.0 360 | backpath = [None for wid in range(len(self.points))] 361 | reached_goal = None 362 | while True: 363 | if len(opened) == 0: 364 | raise NoPathToGoalError( 365 | "No path from " 366 | + str(start_geo_point) 367 | + " to " 368 | + str(goal_geo_point) 369 | ) 370 | opened.sort() 371 | h, e = opened.pop(0) 372 | if ( 373 | e == goal__seg_start_idx and goal__seg_start_idx != start__seg_start_idx 374 | ) or (e == goal__seg_end_idx and goal__seg_end_idx != start__seg_start_idx): 375 | reached_goal = e 376 | break 377 | for edge in self.edges[e]: 378 | e2 = edge.end 379 | if e2 == start__seg_end_idx: 380 | h2 = length_start2seg_end 381 | elif e2 == start__seg_start_idx: 382 | h2 = length_start2seg_start 383 | else: 384 | h2 = h + edge.h 385 | if e2 == goal__seg_start_idx: 386 | h2 += length_goal2seg_start 387 | elif e2 == goal__seg_end_idx: 388 | h2 += length_goal2seg_end 389 | if closed[e2] == 0 or closed[e2] > h2: 390 | opened.append([h2, e2]) 391 | closed[e2] = h2 392 | backpath[e2] = [e, edge, h2] 393 | 394 | # generate plan segments from backpath 395 | plan.segments = [] 396 | if reached_goal is None: 397 | raise ValueError("no path to target found") 398 | e = reached_goal 399 | dist = backpath[e][2] if backpath[e] is not None else -1 400 | try: 401 | while backpath[e] is not None: 402 | plan.segments.append(backpath[e][1].seg) 403 | e = backpath[e][0] 404 | # :TODO: sometimeswe we have an MemoryError 405 | except Exception as exception: 406 | print("Error, count of segments: ", len(plan.segments)) 407 | raise exception 408 | assert e == start__seg_start_idx or e == start__seg_end_idx 409 | plan.segments.reverse() 410 | 411 | return plan, dist 412 | 413 | def _get_min_point(self, seg, lot): 414 | """Chooses between the orthogonal projection, and the start and end points 415 | of the segment. 416 | 417 | If the given orthogonal projection lies out of the segment, the whether the start 418 | or end point of the segment must be chosen as the minimum point. 419 | 420 | :param seg: The segment. 421 | :type seg: geographic_msgs/RouteSegment 422 | :param lot: The perpendicular point to the segment. 423 | :type lot: geodesy.utm.UTMPoint 424 | 425 | :return: The perpendicular point if it is on the segment, else the start or end 426 | point of the segment. 427 | :rtype: geodesy.utm.UTMPoint 428 | """ 429 | utm_seg_start = self.utm_points[self.points.index(seg.start.uuid)] 430 | utm_seg_end = self.utm_points[self.points.index(seg.end.uuid)] 431 | length_seg = self.distance2D(utm_seg_start, utm_seg_end) 432 | length_lot2start = self.distance2D(lot, utm_seg_start) 433 | length_lot2end = self.distance2D(lot, utm_seg_end) 434 | if length_lot2start <= length_seg and length_lot2end <= length_seg: 435 | return lot 436 | elif length_lot2start < length_lot2end: 437 | return utm_seg_start 438 | else: 439 | return utm_seg_end 440 | 441 | def _getSegment(self, uuid): 442 | """Get the segment that corresponds to the given ID. 443 | 444 | :param uuid: The id of the segment. 445 | :type uuid: uuid_msgs/UniqueID 446 | 447 | :return: The segment for the given uuid. 448 | :rtype: geographic_msgs/RouteSegment if the segment is found, None otherwise. 449 | """ 450 | for seg in self.graph.segments: 451 | if seg.id == uuid: 452 | return seg 453 | return None 454 | 455 | def _getSegmentLength(self, start_point_id, seg_id): 456 | """Searches the segment with given id with given start point in a pre-cached list 457 | and return its length. 458 | 459 | :param start_point_id: The id of start point of the segment. 460 | :type start_point_id: uuid_msgs/UniqueID 461 | :param seg_id: The id of the segment. 462 | :type seg_id: uuid_msgs/UniqueID 463 | 464 | :return: Length of a segment. 465 | :rtype: float if the segment is found, None otherwise. 466 | """ 467 | edges = self.edges[self.points.index(start_point_id.uuid)] 468 | for edge in edges: 469 | if edge.seg == seg_id: 470 | return edge.h 471 | return None 472 | 473 | def getNearestSegment(self, geo_point, max_dist=500.0): 474 | """Determine the nearest segment to the given point. 475 | 476 | :param geo_point: The position. 477 | :type geo_point: geographic_msgs/GeoPoint 478 | :param max_dist: The maximal allowed distance to segment. 479 | :type max_dist: float 480 | 481 | :return: A tuple of the nearest segment, which has the minimum distance to 482 | given point, the distance to the segment and the perpendicular point. 483 | :rtype: (geographic_msgs/RouteSegment, float, geodesy.utm.UTMPoint) or 484 | (None, None, None), if the distance of given point to start or 485 | end of the segment is greater then max_dist 486 | """ 487 | utm_point = geodesy.utm.fromMsg(geo_point) 488 | min_dist = 999999999 489 | result = (None, None, None) 490 | for seg in self.graph.segments: 491 | index = self.points.index(seg.start.uuid) 492 | if index is not None: 493 | n = self.points.index(seg.end.uuid) 494 | if n is not None: 495 | # determine the length of the segment 496 | length_seg = self._getSegmentLength(seg.start, seg.id) 497 | if not length_seg is None: 498 | length_2start = self.distance2D( 499 | utm_point, self.utm_points[index] 500 | ) 501 | length_2end = self.distance2D(utm_point, self.utm_points[n]) 502 | if length_2start < max_dist or length_2end < max_dist: 503 | lot_p = self.getPerpendicularPoint2D( 504 | self.utm_points[index], self.utm_points[n], utm_point 505 | ) 506 | length_lot2p = self.distance2D(utm_point, lot_p) 507 | length_lot2start = self.distance2D( 508 | lot_p, self.utm_points[index] 509 | ) 510 | length_lot2end = self.distance2D(lot_p, self.utm_points[n]) 511 | if ( 512 | length_lot2start <= length_seg 513 | and length_lot2end <= length_seg 514 | ): 515 | cur_min = length_lot2p 516 | if cur_min < min_dist: 517 | min_dist = cur_min 518 | result = (seg, min_dist, lot_p) 519 | else: 520 | cur_min = min(length_2start, length_2end) - 1.0 521 | if cur_min < min_dist: 522 | min_dist = cur_min 523 | result = (seg, min_dist, lot_p) 524 | return result 525 | 526 | def getPerpendicularPoint2D(self, utm_start, utm_end, utm_p): 527 | """Returns the orthongal projection of point utm_p onto a line segment (utm_start -> utm_end) 528 | 529 | :param utm_start: The starting point of the line segment. 530 | :type utm_start: geodesy.utm.UTMPoint 531 | :param utm_end: The ending point of the line segment. 532 | :type utm_end: geodesy.utm.UTMPoint 533 | :param utm_p: The point. 534 | :type utm_p: geodesy.utm.UTMPoint 535 | 536 | :return: The orthogonal projection (cut point) if no errors, None otherwise. 537 | :rtype: geodesy.utm.UTMPoint 538 | """ 539 | s = numpy.array([utm_start.easting, utm_start.northing]) 540 | e = numpy.array([utm_end.easting, utm_end.northing]) 541 | p = numpy.array([utm_p.easting, utm_p.northing]) 542 | rv = e - s # direction vector of the line 543 | rv_2 = (rv * rv).sum() 544 | if rv_2 == 0.0: 545 | raise ValueError("invalid segment length") 546 | try: 547 | lamda = ((p * rv).sum() - (s * rv).sum()) / rv_2 548 | lot_p = s + lamda * rv 549 | except: 550 | import traceback 551 | 552 | print(traceback.format_exc()) 553 | 554 | return geodesy.utm.UTMPoint( 555 | lot_p[0], lot_p[1], zone=utm_p.gridZone()[0], band=utm_p.gridZone()[1] 556 | ) 557 | 558 | @staticmethod 559 | def distance2D(utm1, utm2): 560 | """Compute 2D Euclidean distance between two utm points. 561 | 562 | :param utm1: The first point. 563 | :type utm1: geodesy.utm.UTMPoint 564 | :param utm2: The second point. 565 | :type utm2: geodey.utm.UTMPoint 566 | 567 | :return: Distance in meters within the UTM XY plane. Altitudes are ignored. 568 | :rtype: float64 569 | """ 570 | dx = utm2.easting - utm1.easting 571 | dy = utm2.northing - utm1.northing 572 | return numpy.sqrt(dx * dx + dy * dy) 573 | --------------------------------------------------------------------------------