Added scoring interface (topics and services)

fsuarez6 · Apr 22, 2014 · ae9f618 · ae9f618
1 parent f0eb0b7
commit ae9f618
Show file tree

Hide file tree

Showing 15 changed files with 216 additions and 41 deletions.
diff --git a/labrob_control/CHANGELOG.rst b/labrob_control/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package labrob_control
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+Forthcoming
+-----------
+* Added scoring interface (topics and services)
+* Contributors: Alex, fsuarez6
+
 0.1.0 (2014-04-07)
 ------------------
 * Initial release

diff --git a/labrob_description/CHANGELOG.rst b/labrob_description/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package labrob_description
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+Forthcoming
+-----------
+* Added scoring interface (topics and services)
+* Contributors: fsuarez6
+
 0.1.0 (2014-04-07)
 ------------------
 * Initial release

diff --git a/labrob_gazebo/CHANGELOG.rst b/labrob_gazebo/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package labrob_gazebo
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+Forthcoming
+-----------
+* Added scoring interface (topics and services)
+* Contributors: fsuarez6
+
 0.1.0 (2014-04-07)
 ------------------
 * Initial release

diff --git a/labrob_msgs/CHANGELOG.rst b/labrob_msgs/CHANGELOG.rst
@@ -0,0 +1,8 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package labrob_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Forthcoming
+-----------
+* Added scoring interface (topics and services)
+* Contributors: fsuarez6
diff --git a/labrob_msgs/CMakeLists.txt b/labrob_msgs/CMakeLists.txt
@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(labrob_msgs)
+
+find_package(catkin REQUIRED COMPONENTS 
+  message_generation nav_msgs)
+
+add_service_files(
+    FILES
+    GetMapPercentage.srv
+)
+
+generate_messages(DEPENDENCIES nav_msgs std_msgs)
+
+catkin_package(
+CATKIN_DEPENDS 
+  message_runtime
+)
+
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
diff --git a/labrob_msgs/package.xml b/labrob_msgs/package.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<package>
+  <name>labrob_msgs</name>
+  <version>0.1.0</version>
+  <description>The labrob_msgs package</description>
+
+  <maintainer email="[email protected]">Francisco Suarez-Ruiz</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.romin.upm.es/wiki/</url>
+  <url type="repository">https://github.com/fsuarez6/labrob</url>
+  <author>Francisco Suarez-Ruiz</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>message_runtime</run_depend> 
+
+
+</package>
diff --git a/labrob_msgs/srv/GetMapPercentage.srv b/labrob_msgs/srv/GetMapPercentage.srv
@@ -0,0 +1,3 @@
+nav_msgs/OccupancyGrid map
+---
+float32 percentage_covered
diff --git a/labrob_worlds/CHANGELOG.rst b/labrob_worlds/CHANGELOG.rst
@@ -0,0 +1,8 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package labrob_worlds
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Forthcoming
+-----------
+* Added scoring interface (topics and services)
+* Contributors: fsuarez6
diff --git a/labrob_worlds/CMakeLists.txt b/labrob_worlds/CMakeLists.txt
@@ -5,15 +5,21 @@ find_package(catkin REQUIRED COMPONENTS
   gazebo_ros
   roscpp
   rospy
+  labrob_msgs
+  nav_msgs
 )
 
 
 catkin_package(
 CATKIN_DEPENDS 
   roscpp
   gazebo_ros
+  labrob_msgs
+  nav_msgs
 )
 
+catkin_python_setup()
+
 find_package(gazebo REQUIRED)
 
 link_directories(${GAZEBO_LIBRARY_DIRS})

diff --git a/labrob_worlds/launch/crazy_maze.launch b/labrob_worlds/launch/crazy_maze.launch
@@ -12,16 +12,11 @@
     <arg name="debug" value="false"/>  
     <arg name="gui" value="true"/>
   </include>
-
-<!-- For proposes of testing the world NOT the robot
- <include file="$(find gazebo_ros)/launch/empty_world.launch">
-    <arg name="world_name" value="$(find labrob_worlds)/worlds/crazy_maze.world" />
-    <arg name="paused" value="false" />
-    <arg name="debug" value="false" />
-    <arg name="gui" value="true" />    
-    <arg name="use_sim_time" value="true" />
-    <arg name="headless" value="false" />
-  </include>    
--->
+
+  <!-- Start the node that handles the map exploration percentage -->
+  <node name="challenge_map_server" pkg="labrob_worlds" type="challenge_map_server.py" output="log">
+    <param name="map_size_m2" value="400.0"/>
+    <param name="map_resolution" value="0.05"/>
+  </node>
 
 </launch>
diff --git a/labrob_worlds/models/crazy_coins/model.sdf b/labrob_worlds/models/crazy_coins/model.sdf
@@ -266,7 +266,9 @@
     </link>
 
     <plugin name="coins_manager_plugin" filename="libcoins_manager_plugin.so" >
+      <goal_coin>coin_2</goal_coin>
       <robot_name>labrob</robot_name>
+      <update_rate>10.0</update_rate>
       <spin_velocity>1.570796327</spin_velocity>
     </plugin>
 

diff --git a/labrob_worlds/package.xml b/labrob_worlds/package.xml
@@ -16,12 +16,14 @@
   <build_depend>gazebo_ros</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
-  <build_depend>geometry_msgs</build_depend>
+  <build_depend>labrob_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
 
   <run_depend>gazebo_ros</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>rospy</run_depend>
-  <run_depend>geometry_msgs</run_depend>
+  <run_depend>labrob_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
 
   <export>
     <gazebo_ros plugin_path="${prefix}/lib" gazebo_media_path="${prefix}" />

diff --git a/labrob_worlds/scripts/challenge_map_server.py b/labrob_worlds/scripts/challenge_map_server.py
@@ -0,0 +1,34 @@
+#!/usr/bin/env python
+import rospy, math
+from labrob_msgs.srv import GetMapPercentage
+
+from nav_msgs.msg import OccupancyGrid
+
+class ChallengeMapServer:
+  def __init__(self):
+    rospy.init_node('challenge_map_server')
+
+    # Initial values
+    size_m2 = rospy.get_param('/map_size_m2', 400.0)
+    resolution = rospy.get_param('/map_resolution', 0.05)
+    self.size_map = size_m2/resolution
+
+    # Advertise the service
+    s = rospy.Service('get_percent_map_completed', GetMapPercentage, self.handle_map)
+
+    rospy.loginfo('Challenge begun. Map size: %d squares' % int(self.size_map));
+
+  def handle_map(self, req):
+    rospy.loginfo('Received map. Calculating Percentage Completed...');
+
+    n_squares_mapped = sum(1 for x in req.map.data if x >= 0)
+    percentage = float(n_squares_mapped)/float(self.size_map)
+
+    rospy.loginfo('Map Complete: '+str(100.0 * percentage)+'%')
+
+    return percentage
+
+
+if __name__ == "__main__":
+  ChallengeMapServer()
+  rospy.spin()
diff --git a/labrob_worlds/setup.py b/labrob_worlds/setup.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(
+          packages=['labrob_worlds'],
+              package_dir={'': 'scripts'})
+
+setup(**setup_args)