Merge pull request #222 from snozawa/add_robot_init

Add robot init function to create *ri* and robot instance

pr2eus/CMakeLists.txt

@@ -36,6 +36,7 @@ if(CATKIN_ENABLE_TESTING)
   add_rostest(test/robot-no-clock.test)
   add_rostest(test/default-ri-test.test)
   add_rostest(test/speak-test.test)
+  add_rostest(test/robot-init-test.test)
 endif()
 
 generate_eusdoc(robot-interface.l)

pr2eus/package.xml

@@ -35,5 +35,8 @@
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>sound_play</run_depend>
 
+  <export>
+    <pr2eus robot-name="pr2" interface-file="${prefix}/pr2-interface.l" />
+  </export>
 </package>
 

pr2eus/robot-interface.l

@@ -1683,4 +1683,45 @@ send-action [ send message to action server, it means robot will move ]"
     (ros::service-call (format nil "~A/~A/~A/set_parameters" node-name costmap-name inflation-name) req)
     ))
 
+;;
+;; robot-init
+;;
+(defun robot-init (&optional (robot-name (ros::get-param "/robot/type")))
+  "Initialize robot-model and robot-interface instances respectively, such as *pr2* and *ri*.
+   Behavior:
+     1. Without /robot/type and argument, or no interface file is found, robot instance and interface instance are not created and errors are invoked.
+     2. If argument are specified and robot interface file is found, robot instance and interface with given name are created.
+     3. If no argument is specified, /robot/type ROSPARAM is set, and robot interface file is found, robot instance and interface with given name are created.
+   Typical usage:
+     (robot-init) ;; With setting /robot/type ROSPARAM anywhere.
+     (robot-init (or (ros::get-param \"/robot/type\") \"pr2\")) ;; If /robot/type ROSPARAM is specified, use it. Otherwise, use \"pr2\" by default.
+   Configuring user-defined robot:
+     This function searches robot interface file from rospack plugins.
+     If user want to use their own robots from robot-init function,
+     please write export tag in [user_defined_rospackage]/package.xml as pr2eus/package.xml.
+   "
+  (let* (;; Check existence ros::rospack-plugins for robot-name-list and interface-file-list.
+         ;;    For example, old deb environment before ros::rospack-plugins is committed.
+         (robot-name-list
+          (if (fboundp 'ros::rospack-plugins)
+              (mapcar #'cdr (ros::rospack-plugins "pr2eus" "robot-name"))
+            (piped-fork-returns-list "rospack plugins --attrib=robot-name pr2eus | cut -d\\  -f2")))
+         (interface-file-list
+          (if (fboundp 'ros::rospack-plugins)
+              (mapcar #'cdr (ros::rospack-plugins "pr2eus" "interface-file"))
+            (piped-fork-returns-list "rospack plugins --attrib=interface-file pr2eus | cut -d\\  -f2")))
+         (robot-name-interface-file-list
+          (assoc robot-name (mapcar #'(lambda (x y) (list x y))
+                                    robot-name-list interface-file-list)
+                 :test #'string=)))
+    (cond
+     ((or (not robot-name-interface-file-list)
+          (not (probe-file (cadr robot-name-interface-file-list))))
+      (error ";; No such robot interface setting are defined ~A, or interface file found ~A!!~%" robot-name (cadr robot-name-interface-file-list))
+      nil)
+     (t
+      (require (cadr robot-name-interface-file-list))
+      (funcall (eval (read-from-string (format nil "#'~A-init" robot-name))))
+      ))))
+
 (provide :robot-interface "robot-interface.l")

pr2eus/test/robot-init-test.l

@@ -0,0 +1,34 @@
+#!/bin/env roseus
+
+;; robot-init test
+
+(require :unittest "lib/llib/unittest.l")
+
+(load "package://pr2eus/robot-interface.l")
+(ros::roseus "test_robot_init")
+
+(init-unit-test)
+
+;;(deftest test-robot-init-without-param-and-argument ()
+;;  (assert (not (robot-init)) "This should invoke error."))
+
+(deftest test-robot-init-with-argument ()
+  (assert (progn
+            (robot-init "pr2")
+            (and *ri* *pr2*))
+          "This should return pr2 and ri.")
+  )
+
+(deftest test-robot-init-without-argument-with-param ()
+  (makunbound '*ri*)
+  (makunbound '*pr2*)
+  (ros::set-param "/robot/type" "pr2")
+  (assert (progn
+            (robot-init)
+            (and *ri* *pr2*))
+          "This should return pr2 and ri.")
+  )
+
+(run-all-tests)
+(exit)
+

pr2eus/test/robot-init-test.test

@@ -0,0 +1,4 @@
+<launch>
+  <test test-name="robot_init_test_node" pkg="roseus" type="roseus"
+        args="$(find pr2eus)/test/robot-init-test.l" time-limit="600" />
+</launch>