[joints] started on the joint state monitoring action

cram_integration/cram_joint_states/cram-joint-states.asd

@@ -41,4 +41,5 @@
   ((:module "src"
     :components
     ((:file "package")
-     (:file "joint-states" :depends-on ("package"))))))
+     (:file "joint-states" :depends-on ("package"))
+     (:file "monitoring" :depends-on ("package" "joint-states"))))))

cram_integration/cram_joint_states/src/monitoring.lisp

@@ -0,0 +1,44 @@
+;;;
+;;; Copyright (c) 2020, Gayane Kazhoyan <[email protected]>
+;;; All rights reserved.
+;;;
+;;; Redistribution and use in source and binary forms, with or without
+;;; modification, are permitted provided that the following conditions are met:
+;;;
+;;;     * Redistributions of source code must retain the above copyright
+;;;       notice, this list of conditions and the following disclaimer.
+;;;     * Redistributions in binary form must reproduce the above copyright
+;;;       notice, this list of conditions and the following disclaimer in the
+;;;       documentation and/or other materials provided with the distribution.
+;;;     * Neither the name of the Institute for Artificial Intelligence/
+;;;       Universitaet Bremen nor the names of its contributors may be used to
+;;;       endorse or promote products derived from this software without
+;;;       specific prior written permission.
+;;;
+;;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+;;; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+;;; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+;;; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+;;; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+;;; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+;;; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+;;; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+;;; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+;;; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+;;; POSSIBILITY OF SUCH DAMAGE.
+
+(in-package :joints)
+
+(defun monitor-joint-state (joint-name comparison-function)
+  (unless joint-name
+    (setf joint-name "r_gripper_joint"))
+  (unless comparison-function
+    (setf comparison-function (lambda (joint-angle) (< joint-angle 0.002))))
+  (cpl:wait-for
+   (cpl:fl-funcall (lambda (joint-state-msg-fluent)
+                     (funcall comparison-function
+                              (car (joints:joint-positions
+                                    (list joint-name)
+                                    joint-state-msg-fluent))))
+                   *robot-joint-states-msg*))
+  (print "DONE"))