Merge pull request #10 from dgerod/develop

Scene Database node to access ROS Server in safe way.

CMakeLists.txt

@@ -1,18 +1,34 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(rosplan_pytools)
 
-find_package(catkin REQUIRED)
-catkin_package(CATKIN_DEPENDS
-    rospy std_msgs mongodb_store
-    rosplan_dispatch_msgs rosplan_knowlege_msgs)
+find_package(catkin REQUIRED COMPONENTS
+    rospy genmsg message_generation actionlib_msgs std_msgs
+    rosplan_dispatch_msgs rosplan_knowledge_msgs
+)
+
+add_service_files(
+    FILES
+    AddElement.srv
+    DiagnosticsDB.srv
+    FindElement.srv
+    RemoveElement.srv
+    ResetDB.srv
+    RetrieveElements.srv
+    UpdateElement.srv
+)
 
 catkin_python_setup()
+
+generate_messages(DEPENDENCIES
+    std_msgs rosplan_dispatch_msgs rosplan_knowledge_msgs
+)
+
 catkin_package(
   LIBRARIES ${PROJECT_NAME}
-  CATKIN_DEPENDS rospy std_msgs mongodb_store
-                 rosplan_dispatch_msgs rosplan_knowlege_msgs
-  DEPENDS
+  CATKIN_DEPENDS rospy genmsg message_generation message_runtime std_msgs
+                 rosplan_dispatch_msgs rosplan_knowledge_msgs
 )
 
 install(DIRECTORY src
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

bin/clear_sdb.py

@@ -7,7 +7,7 @@
 def main():
 
     sdb.initialize()
-    sdb.remove_all_elements()
+    sdb.remove_elements()
 
 
 if __name__ == "__main__":

bin/dump_sdb.py

@@ -10,8 +10,10 @@ def main():
     sdb.initialize()
 
     print("\nElements ---")
-    print(str(sdb.list_elements()) + "\n")
-    for name in sdb.list_elements():
+    elements = sdb.list_elements()
+    print(str(elements) + "\n")
+
+    for name in elements:
         print("[" + name + "] -> [" + str(sdb.get_element(name)[1]) + "]")
 
 

bin/scene_database_node.py

@@ -0,0 +1,18 @@
+#! /usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import rospy
+from rosplan_pytools.controller.nodes import scene_database
+
+
+def main():
+
+    try:
+        scene_database.start_node("scene_database")
+
+    except rospy.ROSInterruptException:
+        pass
+
+
+if __name__ == "__main__":
+    main()

examples/add_elements_to_sdb.py

@@ -3,23 +3,57 @@
 
 from __future__ import print_function
 from rosplan_pytools.controller import scene_database as sdb
-from std_msgs.msg import Empty
+
+from std_msgs.msg import String
 
 
 def main():
 
+    print("nadfkasdjfla")
+
     sdb.initialize()
     sdb.reset()
 
-    print(sdb.num_elements())
-    print(sdb.list_elements())
+    print("num elements: ", sdb.num_elements())
+    print("elements: ", sdb.list_elements())
 
-    sdb.add_element("element_1", sdb.Element(Empty()))
+    raw_input("Add elements. Press Enter to continue...")
 
-    print(sdb.num_elements())
-    print(sdb.list_elements())
+    success = sdb.add_element("element_1", sdb.Element(String("value_1"), "metadata_1"))
+    success = sdb.add_element("element_2", sdb.Element(String("value_2"), "metadata_2"))
+    success = sdb.add_element("element_3", sdb.Element(String("value_3"), "metadata_3"))
 
-    sdb.reset()
+    print("num elements: ", sdb.num_elements())
+    print("elements: ", sdb.list_elements())
+
+    raw_input("Add existing element. Press Enter to continue...")
+
+    success = sdb.add_element("element_1", sdb.Element(String("value_100"), "metadata_100"))
+
+    print("added: ", success)
+    print("num elements: ", sdb.num_elements())
+    print("elements: ", sdb.list_elements())
+
+    raw_input("Remove element. Press Enter to continue...")
+
+    sdb.remove_element("element_2")
+
+    print("num elements: ", sdb.num_elements())
+    print("elements: ", sdb.list_elements())
+
+    raw_input("Get an element. Press Enter to continue...")
+
+    success, element = sdb.get_element("element_3")
+    print("element: ", element)
+    print("num elements: ", sdb.num_elements())
+
+    raw_input("Remove all element. Press Enter to continue...")
+
+    print("num elements: ", sdb.num_elements())
+
+    sdb.remove_elements()
+
+    print("num elements: ", sdb.num_elements())
 
 
 if __name__ == "__main__":

package.xml

@@ -7,20 +7,26 @@
   <maintainer email="dgerod@xyz-lab.org.es">Diego Escudero</maintainer>
 
   <license>TODO</license>
+  <url type="website">https://github.com/dgerod/rosplan_pytools</url>
 
   <buildtool_depend>catkin</buildtool_depend>
+
   <build_depend>rospy</build_depend>
+  <build_depend>genmsg</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>action_lib</build_depend>
   <build_depend>std_msgs</build_depend>
-  <build_depend>mongodb_store</build_depend>
   <build_depend>rosplan_dispatch_msgs</build_depend>
-  <build_depend>rosplan_knowlege_msgs</build_depend>
+  <build_depend>rosplan_knowledge_msgs</build_depend>
 
   <run_depend>rospy</run_depend>
+  <run_depend>genmsg</run_depend>
+  <run_depend>message_generation</run_depend>
+  <run_depend>action_lib</run_depend>
+  <run_depend>message_runtime</run_depend>
   <run_depend>std_msgs</run_depend>
-  <run_depend>mongodb_store</run_depend>
   <run_depend>rosplan_dispatch_msgs</run_depend>
-  <run_depend>rosplan_knowlege_msgs</run_depend>
+  <run_depend>rosplan_knowledge_msgs</run_depend>
 
-  <export>
-  </export>
+  <export></export>
 </package>

src/rosplan_pytools/controller/common/sdb_element.py

@@ -0,0 +1,48 @@
+
+class Element(object):
+
+    @staticmethod
+    def extract_ros_type(message):
+        return message._type
+
+    def __init__(self, value=None, metadata=""):
+
+        self._metadata = metadata
+
+        if value is not None:
+            self._ros_message_value = value
+            self._ros_message_type = self.extract_ros_type(value)
+        else:
+            self._ros_message_value = None
+            self._ros_message_type = ""
+
+    def __str__(self):
+
+        if self._ros_message_value is not None:
+            value = self._ros_message_value
+        else:
+            value = ""
+
+        return "%s, %s, %s" % (self._ros_message_type, value, self._metadata)
+
+    def __eq__(self, other):
+
+        return self._ros_message_type == other._ros_message_type
+
+    def clean(self):
+        self._metadata = ""
+        self._ros_message_value = None
+        self._ros_message_type = ""
+
+    def is_valid(self):
+        return self._ros_message_value is not None
+
+    def metadata(self):
+        return self._metadata
+
+    def value(self):
+        return self._ros_message_value
+
+    def type(self):
+        return self._ros_message_type
+

src/rosplan_pytools/controller/common/sdb_element_converter.py

@@ -0,0 +1,27 @@
+import json
+from rosplan_pytools.common import message_converter
+from rosplan_pytools.controller.common.sdb_element import Element
+
+
+def sdb_element_to_string(element):
+
+    if not isinstance(element, Element):
+        raise TypeError
+
+    dict_ = {'msg_type': element.type(),
+             'msg_value': message_converter.convert_ros_message_to_dictionary(element.value())}
+
+    value = json.dumps(dict_)
+    return element.metadata(), value
+
+
+def string_to_sdb_element(metadata, value):
+
+    if not isinstance(metadata, str) or not isinstance(value, str):
+        raise TypeError
+
+    dict_ = json.loads(value)
+    msg_value = message_converter.convert_dictionary_to_ros_message(dict_['msg_type'],
+                                                                    dict_['msg_value'],
+                                                                    'message')
+    return Element(msg_value, metadata)