add tray monitor package

cob_tray_monitor/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})

cob_tray_monitor/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

cob_tray_monitor/mainpage.dox

@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b cob_tray_monitor is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

cob_tray_monitor/manifest.xml

@@ -0,0 +1,16 @@
+<package>
+  <description brief="cob_tray_monitor">
+
+     cob_tray_monitor
+
+  </description>
+  <author>Florian Weisshardt</author>
+  <license>LGPL</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/cob_tray_monitor</url>
+
+  <depend package="rospy"/>
+  <depend package="sensor_msgs"/>
+  <depend package="cob_srvs"/>
+
+</package>

cob_tray_monitor/ros/launch/tray_monitor.launch

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<launch>
+
+	<rosparam ns="tray_monitor" file="$(find cob_tray_monitor)/config/monitor_params.yaml" command="load" />
+
+	<node ns="tray_monitor" pkg="cob_tray_monitor" type="tray_monitor.py" name="tray_monitor" respawn="false" output="screen"/>
+
+</launch>
+

cob_tray_monitor/ros/src/tray_monitor.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python
+import roslib; roslib.load_manifest('cob_tray_monitor')
+import rospy
+import math
+from std_msgs.msg import *
+from sensor_msgs.msg import *
+from cob_srvs.srv import *
+
+class Monitor():
+
+    def __init__(self):        
+        self.pub = rospy.Publisher("occupied", Bool)
+        rospy.Subscriber("/tray_sensors/range_1", Range, self.range1_callback)
+        rospy.Subscriber("/tray_sensors/range_2", Range, self.range2_callback)
+        rospy.Subscriber("/tray_sensors/range_3", Range, self.range3_callback)
+        rospy.Subscriber("/tray_sensors/range_4", Range, self.range4_callback)
+        rospy.Service('occupied', Trigger, self.srv_callback)
+        self.range1 = Range()
+        self.range2 = Range()
+        self.range3 = Range()
+        self.range4 = Range()
+        self.occupied = True
+
+        if not rospy.has_param('distance_limit'):
+            rospy.logerr("no distance_limit specified")
+            exit(1)
+
+        self.limit = rospy.get_param('distance_limit')
+
+    def srv_callback(self,req):
+        res = TriggerResponse()
+        res.success.data = self.occupied
+        return res
+
+    def range1_callback(self,msg):
+        self.range1 = msg
+
+    def range2_callback(self,msg):
+        self.range2 = msg
+
+    def range3_callback(self,msg):
+        self.range3 = msg
+
+    def range4_callback(self,msg):
+        self.range4 = msg
+
+    def publish_state(self):
+        msg = Bool()
+        if self.range1.range <= self.limit:
+            msg.data = True;
+        if self.range2.range <= self.limit:
+            msg.data = True;
+        if self.range3.range <= self.limit:
+            msg.data = True;
+        if self.range4.range <= self.limit:
+            msg.data = True;
+
+        self.pub.publish(msg)
+
+if __name__ == "__main__":
+    rospy.init_node('tactile_sensors')
+    rospy.sleep(0.5)
+
+    m = Monitor()
+    rospy.loginfo("tray monitor running")
+
+    r = rospy.Rate(10)
+    while not rospy.is_shutdown():
+        m.publish_state()
+        r.sleep()