MINOR Merge remote-tracking branch 'origin/exploration_integration' i…

…nto hydro-devel

- Add pandora_explorer package
- Add pandora_exploration_msg package
- Rename pandora_discovery metapackage to pandora_exploration
- Fix dependencies and integration issues

README.md

@@ -1,2 +1,2 @@
-# pandora_discovery
+# pandora_exploration
 Modules and algorithms for optimization of space exploration problem for PANDORA's UGV

pandora_discovery/CMakeLists.txt → pandora_exploration/CMakeLists.txt

@@ -1,4 +1,4 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(pandora_discovery)
+project(pandora_exploration)
 find_package(catkin REQUIRED)
-catkin_metapackage()
+catkin_metapackage()

pandora_discovery/package.xml → pandora_exploration/package.xml

@@ -1,22 +1,24 @@
 <package format="2">
- <name>pandora_discovery</name>
+ <name>pandora_exploration</name>
  <version>0.0.0</version>
- <description>Contains pandora discovery packages</description>
+ <description>Contains pandora exploration packages</description>
 
  <maintainer email="[email protected]">Tsirigotis Christos</maintainer>
  <maintainer email="[email protected]">Lykartsis Ioannis</maintainer>
 
  <license>BSD</license>
 
- <url type="bugtracker">https://github.com/pandora-auth-ros-pkg/pandora_discovery/issues</url>
- <url type="repository">https://github.com/pandora-auth-ros-pkg/pandora_discovery.git</url>
+ <url type="bugtracker">https://github.com/pandora-auth-ros-pkg/pandora_exploration/issues</url>
+ <url type="repository">https://github.com/pandora-auth-ros-pkg/pandora_exploration.git</url>
 
  <buildtool_depend>catkin</buildtool_depend>
 
  <exec_depend>pandora_sensor_coverage</exec_depend>
  <exec_depend>pandora_sensor_planner</exec_depend>
+ <exec_depend>pandora_explorer</exec_depend>
+ <exec_depend>pandora_exploration_msgs</exec_depend>
 
  <export>
  <metapackage/>
  </export>
-</package>
+</package>

pandora_exploration_msgs/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(pandora_exploration_msgs)
+find_package(catkin REQUIRED COMPONENTS
+ message_generation
+ std_msgs
+ actionlib_msgs
+ geometry_msgs
+)
+
+catkin_python_setup()
+
+add_action_files(
+ FILES
+ DoExploration.action
+)
+
+generate_messages(
+ DEPENDENCIES
+ std_msgs
+ actionlib_msgs
+ geometry_msgs
+)
+
+catkin_package(
+ CATKIN_DEPENDS
+ message_runtime
+ std_msgs
+ actionlib_msgs
+ geometry_msgs
+)

pandora_exploration_msgs/action/DoExploration.action

@@ -0,0 +1,11 @@
+#goal definition
+uint8 exploration_type
+
+uint8 TYPE_NORMAL=0
+uint8 TYPE_FAST=1
+uint8 TYPE_DEEP=2
+---
+#result definition
+---
+#feedback
+geometry_msgs/PoseStamped base_position

pandora_exploration_msgs/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>pandora_exploration_msgs</name>
+ <version>0.0.0</version>
+ <description>Messages and actions used by navigation packages.</description>
+
+ <maintainer email="[email protected]">Chris Zalidis</maintainer>
+
+ <license>BSD</license>
+
+ <url type="bugtracker">https://github.com/pandora-auth-ros-pkg/pandora_exploration/issues</url>
+ <url type="repository">https://github.com/pandora-auth-ros-pkg/pandora_exploration</url>
+
+ <author>Chris Zalidis</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>message_generation</build_depend>
+ <exec_depend>message_runtime</exec_depend>
+
+ <depend>std_msgs</depend>
+ <depend>actionlib_msgs</depend>
+ <depend>geometry_msgs</depend>
+
+ <export>
+ </export>
+
+</package>

pandora_exploration_msgs/scripts/pandora_exploration/__init__.py

@@ -0,0 +1 @@
+import mocks

pandora_exploration_msgs/scripts/pandora_exploration/mocks/explorer.py

@@ -0,0 +1,107 @@
+#!/usr/bin/env python
+# Software License Agreement
+__version__ = "0.0.1"
+__status__ = "Production"
+__license__ = "BSD"
+__copyright__ = "Copyright (c) 2014, P.A.N.D.O.R.A. Team. All rights reserved."
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of P.A.N.D.O.R.A. Team nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+__author__ = "Tsirigotis Christos and Voulgarakis George"
+__maintainer__ = "Tsirigotis Christos"
+__email__ = "[email protected]"
+
+import rospy
+from actionlib import SimpleActionServer
+
+from geometry_msgs.msg import PoseStamped
+from pandora_exploration_msgs.msg import DoExplorationAction, \
+ DoExplorationFeedback, DoExplorationResult
+import tf
+
+class MockExplorer():
+
+ def __init__(self, exploration_topic):
+
+ self.robot_pose_ = PoseStamped()
+ self.listener = tf.TransformListener()
+
+ self.navigation_succedes = True
+ self.reply = False
+ self.preempted = 0
+
+ self.entered_exploration = False
+
+ self.do_exploration_as_ = SimpleActionServer(
+ exploration_topic,
+ DoExplorationAction,
+ execute_cb = self.do_exploration_cb,
+ auto_start = False)
+ self.do_exploration_as_.start()
+
+ def __del__(self):
+
+ self.do_exploration_as_.__del__()
+
+ def do_exploration_cb(self, goal):
+ rospy.loginfo('do_exploration_cb')
+
+ self.entered_exploration = True
+ while not self.reply:
+ rospy.sleep(0.2)
+ (trans, rot) = self.listener.lookupTransform('/map', '/base_footprint', rospy.Time(0))
+ self.robot_pose_.pose.position.x = trans[0]
+ self.robot_pose_.pose.position.y = trans[1]
+ feedback = DoExplorationFeedback()
+ feedback.base_position.pose.position.x = \
+ self.robot_pose_.pose.position.x
+ feedback.base_position.pose.position.y = \
+ self.robot_pose_.pose.position.y
+ self.do_exploration_as_.publish_feedback(feedback)
+ if self.do_exploration_as_.is_preempt_requested():
+ self.preempted += 1
+ rospy.loginfo("Preempted!")
+ self.entered_exploration = False
+ self.do_exploration_as_.set_preempted(DoExplorationResult())
+ return None
+ else:
+ result = DoExplorationResult()
+ self.reply = False
+ self.preempted = 0
+ self.entered_exploration = False
+ if self.navigation_succedes:
+ self.do_exploration_as_.set_succeded(result) 
+ else:
+ self.do_exploration_as_.set_aborted(result)
+
+if __name__ == '__main__':
+
+ rospy.sleep(0.5)
+ rospy.init_node('MockExplorer', anonymous=True)
+ explorer = MockExploration(exploration_topic = '/do_exploration')
+

pandora_exploration_msgs/setup.py

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+ packages=['pandora_exploration'],
+ package_dir={'': 'scripts'}
+)
+
+setup(**d)

pandora_explorer/CMakeLists.txt

@@ -0,0 +1,104 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(pandora_explorer)
+
+find_package(catkin REQUIRED
+ COMPONENTS
+ roscpp
+ visualization_msgs
+ actionlib
+ pandora_exploration_msgs
+ geometry_msgs
+ move_base_msgs
+ costmap_2d
+ nav_msgs
+ pluginlib
+ nav_core
+ angles
+ tf
+ roslint
+)
+
+
+catkin_package(
+# DEPENDS
+ CATKIN_DEPENDS
+ roscpp
+ visualization_msgs
+ actionlib
+ pandora_exploration_msgs
+ geometry_msgs
+ move_base_msgs
+ costmap_2d
+ nav_msgs
+ pluginlib
+ nav_core
+ angles
+ tf
+# INCLUDE_DIRS
+# include
+# LIBRARIES
+)
+
+include_directories(
+ include
+ ${catkin_INCLUDE_DIRS}
+)
+
+
+add_library(frontier_searchers src/map_frontier_search.cpp)
+target_link_libraries(frontier_searchers ${catkin_LIBRARIES})
+
+add_library(frontier_path_generators
+ src/navfn_frontier_path_generator.cpp
+ src/navfn_service_frontier_path_generator.cpp
+)
+target_link_libraries(frontier_path_generators ${catkin_LIBRARIES})
+
+add_library(frontier_cost_functions
+ src/cost_functions/distance_cost_function.cpp
+ src/cost_functions/size_cost_function.cpp
+ src/cost_functions/alignment_cost_function.cpp
+ src/cost_functions/visited_cost_function.cpp
+ )
+target_link_libraries(frontier_cost_functions ${catkin_LIBRARIES})
+
+add_library(frontier_goal_selector src/frontier_goal_selector.cpp)
+target_link_libraries(frontier_goal_selector
+ ${catkin_LIBRARIES}
+ frontier_searchers
+ frontier_path_generators
+ frontier_cost_functions
+)
+
+add_executable(exploration_controller
+ src/exploration_controller.cpp
+ src/exploration_controller_node.cpp)
+add_dependencies(exploration_controller ${catkin_EXPORTED_TARGETS})
+target_link_libraries(exploration_controller
+ ${catkin_LIBRARIES}
+ frontier_goal_selector
+)
+
+## Roslint
+set(ROSLINT_CPP_OPTS
+ "--filter=-build/include_what_you_use, -whitespace/comments, -whitespace/blank_line, -whitespace/braces")
+file(GLOB_RECURSE ${PROJECT_NAME}_LINT_SRCS
+ RELATIVE ${PROJECT_SOURCE_DIR}
+ include/pandora_explorer/*.h
+ include/pandora_explorer/cost_functions/*.h
+ src/*.cpp
+ src/cost_functions/*.cpp
+ )
+list(REMOVE_ITEM ${PROJECT_NAME}_LINT_SRCS
+ RELATIVE ${PROJECT_SOURCE_DIR}
+ include/pandora_explorer/geometry_tools.h
+ include/pandora_explorer/costmap_tools.h
+ include/pandora_explorer/map_frontier_search.h
+ include/pandora_explorer/frontier.h
+ src/map_frontier_search.cpp
+ )
+roslint_cpp(${${PROJECT_NAME}_LINT_SRCS})
+
+if(CATKIN_ENABLE_TESTING)
+ add_subdirectory(test)
+endif()

pandora_explorer/config/coverage_costmap_params.yaml

@@ -0,0 +1,32 @@
+footprint: [[-0.27, -0.21], [-0.27, 0.21], [0.27, 0.21], [0.27, -0.21]]
+footprint_padding: 0.01
+
+transform_tolerance: 0.5
+update_frequency: 1.0
+publish_frequency: 1.0
+
+global_frame: /map
+robot_base_frame: /base_footprint
+resolution: 0.05
+
+static_map: true
+track_unknown_space: true
+
+plugins:
+ - {name: static, type: "costmap_2d::MapLayer"}
+ - {name: coverage, type: "costmap_2d::CoverageLayer"}
+ - {name: inflation, type: "costmap_2d::InflationLayer"}
+
+static:
+ map_topic: /slam/map
+ # map_topic: move_base/global_costmap/costmap
+ subscribe_to_updates: false
+ unknown_cost_value: 51
+ lethal_cost_threshold: 61
+
+coverage:
+ map_topic: /data_fusion/sensor_coverage/kinect_space
+ subscribe_to_updates: false
+
+inflation:
+ inflation_radius: 0.25

pandora_explorer/config/exploration_controller_params.yaml

@@ -0,0 +1,11 @@
+# try that many times, in a row, to find a goal
+max_goal_searches: 12
+
+# robot has that many seconds to reach a goal
+goal_timeout: 3.0
+
+# how far from a goal the robot has to be
+# to considered visited (in meters)
+goal_reached_dist: 0.6
+
+use_coverage: false