begin making classifier ROS server

CMakeLists.txt

@@ -8,7 +8,9 @@ find_package(catkin REQUIRED COMPONENTS eigenpy pinocchio urdf hpp-fcl
 	roscpp
 	sensor_msgs
 	tf
-	rviz)
+	rviz
+	std_msgs
+	message_generation)
 
 add_definitions(-Wno-deprecated-declarations)
 set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/Modules)
@@ -18,6 +20,18 @@ find_package(Eigen3 3.2 REQUIRED)
 find_package(Boost 1.58 REQUIRED COMPONENTS system)
 find_package(YamlCpp 0.5.2 REQUIRED) # Using a custom FindYamlcpp.cmake
 
+
+add_service_files(
+  FILES
+  AddTwoInts.srv
+  BinaryClassifierQuery.srv
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
 ###################################
 ## catkin specific configuration ##
 ###################################

package.xml

@@ -18,4 +18,7 @@
   <depend>sensor_msgs</depend>
   <depend>rviz</depend>
 
+  <depend>message_generation</depend>
+  <depend>message_runtime</depend>
+
 </package>

src/python_src/add_ints_client.py

@@ -0,0 +1,27 @@
+#!/usr/bin/env python
+
+import sys
+import rospy
+from avatar_locomanipulation.srv import *
+
+def add_two_ints_client(x, y):
+    rospy.wait_for_service('add_two_ints')
+    try:
+        add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)
+        resp1 = add_two_ints(x, y)
+        return resp1.sum
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
+
+def usage():
+    return "%s [x y]"%sys.argv[0]
+
+if __name__ == "__main__":
+    if len(sys.argv) == 3:
+        x = int(sys.argv[1])
+        y = int(sys.argv[2])
+    else:
+        print usage()
+        sys.exit(1)
+    print "Requesting %s+%s"%(x, y)
+    print "%s + %s = %s"%(x, y, add_two_ints_client(x, y))

src/python_src/add_ints_service.py

@@ -0,0 +1,17 @@
+#!/usr/bin/env python
+
+from avatar_locomanipulation.srv import *
+import rospy
+
+def handle_add_two_ints(req):
+    print "Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b))
+    return AddTwoIntsResponse(req.a + req.b)
+
+def add_two_ints_server():
+    rospy.init_node('add_two_ints_server')
+    s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints)
+    print "Ready to add two ints."
+    rospy.spin()
+
+if __name__ == "__main__":
+    add_two_ints_server()

src/python_src/locomanipulation_feasibility_classifier_service.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+
+from avatar_locomanipulation.srv import *
+import rospy
+
+import numpy as np
+import tensorflow as tf
+
+class LocomanipulationFeasibilityClassifier:
+	def __init__(self):
+		self.x = []
+
+	def perform_binary_classification(self, req):
+		print "Input Vector:", np.array(req.x)
+		res = BinaryClassifierQueryResponse()
+		res.y = 0.75
+		print "Returning:", res.y
+		return res
+
+	def start_classifier_server(self):
+	    rospy.init_node('locomanipulation_feasibility_classifier_server')
+	    s = rospy.Service('locomanipulation_feasibility_classifier', BinaryClassifierQuery, self.perform_binary_classification)
+	    print "Locomanipulation Feasibility Classifier Server Started."
+	    rospy.spin()
+
+if __name__ == "__main__":
+	classifier = LocomanipulationFeasibilityClassifier()
+	classifier.start_classifier_server()

srv/AddTwoInts.srv

@@ -0,0 +1,4 @@
+int64 a
+int64 b
+---
+int64 sum

srv/BinaryClassifierQuery.srv

@@ -0,0 +1,3 @@
+float64[] x
+---
+float64 y