From 570307d87af83a4cf129760f4b5802e5125882e1 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Tue, 1 Sep 2020 15:39:42 +0200 Subject: [PATCH] [joints] started on the joint state monitoring action --- .../cram_joint_states/cram-joint-states.asd | 3 +- .../cram_joint_states/src/monitoring.lisp | 44 +++++++++++++++++++ 2 files changed, 46 insertions(+), 1 deletion(-) create mode 100644 cram_integration/cram_joint_states/src/monitoring.lisp diff --git a/cram_integration/cram_joint_states/cram-joint-states.asd b/cram_integration/cram_joint_states/cram-joint-states.asd index 3975506df3..f48b8fb682 100644 --- a/cram_integration/cram_joint_states/cram-joint-states.asd +++ b/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")))))) diff --git a/cram_integration/cram_joint_states/src/monitoring.lisp b/cram_integration/cram_joint_states/src/monitoring.lisp new file mode 100644 index 0000000000..b044da6fb6 --- /dev/null +++ b/cram_integration/cram_joint_states/src/monitoring.lisp @@ -0,0 +1,44 @@ +;;; +;;; Copyright (c) 2020, Gayane Kazhoyan +;;; 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"))