From 12ab8c3966952345a06d52c69fb1d784a964db11 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Tue, 31 Jul 2018 00:06:45 +0900 Subject: [PATCH 1/4] add roseus program to convert raw sensor value into distance --- .../euslisp/proximity-calibrator.l | 73 +++++++++++++++++++ 1 file changed, 73 insertions(+) create mode 100755 jsk_arc2017_baxter/euslisp/proximity-calibrator.l diff --git a/jsk_arc2017_baxter/euslisp/proximity-calibrator.l b/jsk_arc2017_baxter/euslisp/proximity-calibrator.l new file mode 100755 index 000000000..18dda090c --- /dev/null +++ b/jsk_arc2017_baxter/euslisp/proximity-calibrator.l @@ -0,0 +1,73 @@ +#!/usr/bin/env roseus + +(ros::load-ros-manifest "roseus") +(ros::load-ros-manifest "force_proximity_ros") + +(defclass proximity-calibrator + :super propertied-object + :slots (proximities-calibrated- + proximities-init- + proximities-param-a- + topic-list-)) + +(defmethod proximity-calibrator + (:init (&rest args &key topic-list) + (ros::roseus "proximity-calibrator") + ;; create subscribers and publishers + (ros::subscribe "/set_init_proximities" std_msgs::Empty #'send self :set-init-proximities) + (dolist (topic-name topic-list) + (ros::subscribe topic-name force_proximity_ros::ProximityArray #'send self :cb topic-name) + (ros::advertise (format nil "~A/init" topic-name) std_msgs::Float64MultiArray 1) + (ros::advertise (format nil "~A/calibrated" topic-name) std_msgs::Float64MultiArray 1) + (ros::advertise (format nil "~A/distance" topic-name) std_msgs::Float64MultiArray 1)) + + ;; make hash tables + (setq proximities-calibrated- (make-hash-table :test #'equal)) + (setq proximities-init- (make-hash-table :test #'equal)) + (setq proximities-param-a- (make-hash-table :test #'equal)) + + ;; get default proximity parameters from rosparam + (dolist (topic-name topic-list) + (sethash topic-name + proximities-param-a- + (ros::get-param (format nil "~A/a" topic-name))) + (sethash topic-name + proximities-init- + (ros::get-param (format nil "~A/b" topic-name)))) + (ros::spin-once) + self) + (:cb (topic-name msg) + ;; set calibrated sensor value to hash table + (sethash topic-name + proximities-calibrated- + (mapcar #'- (mapcar #'(lambda (x) (send x :proximity)) (send msg :proximities)) + (gethash topic-name proximities-init-))) + ;; publish distance calculated from proximtiy values + (send self :publish-proximities topic-name) + ) + (:publish-proximities (topic-name) + (let ((float-array (instance std_msgs::Float64MultiArray :init))) + (send float-array :data (gethash topic-name proximities-init-)) + (ros::publish (format nil "~A/init" topic-name) float-array) + (send float-array :data (gethash topic-name proximities-calibrated-)) + (ros::publish (format nil "~A/calibrated" topic-name) float-array) + (send float-array :data + (mapcar #'(lambda (a I-b) + (if (> I-b 0) + (sqrt (/ a I-b)) + *inf*)) + (gethash topic-name proximities-param-a-) + (gethash topic-name proximities-calibrated-))) + (ros::publish (format nil "~A/distance" topic-name) float-array))) + (:set-init-proximities (msg) + (dolist (topic-name topic-list-) + (sethash topic-name + proximities-init- + (gethash topic-name proximities-raw-)))) + ) + + +(defun init (&optional (topic-list (list "/proximity_sensor_topic1" "/proximity_sensor_topic2"))) + (when (not (boundp '*pc*)) + (setq *pc* (instance proximity-calibrator :init :topic-list topic-list))) + (ros::spin-once)) From badf7f6578c47b20e422f9410359885fb93e584b Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Tue, 31 Jul 2018 01:36:19 +0900 Subject: [PATCH 2/4] re-calibrate proximity sensors from service-call --- jsk_arc2017_baxter/euslisp/proximity-calibrator.l | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/jsk_arc2017_baxter/euslisp/proximity-calibrator.l b/jsk_arc2017_baxter/euslisp/proximity-calibrator.l index 18dda090c..4c971e1ec 100755 --- a/jsk_arc2017_baxter/euslisp/proximity-calibrator.l +++ b/jsk_arc2017_baxter/euslisp/proximity-calibrator.l @@ -14,18 +14,16 @@ (:init (&rest args &key topic-list) (ros::roseus "proximity-calibrator") ;; create subscribers and publishers - (ros::subscribe "/set_init_proximities" std_msgs::Empty #'send self :set-init-proximities) + (ros::advertise-service "/set_init_proximities" std_srvs::Empty #'send self :set-init-proximities) (dolist (topic-name topic-list) (ros::subscribe topic-name force_proximity_ros::ProximityArray #'send self :cb topic-name) (ros::advertise (format nil "~A/init" topic-name) std_msgs::Float64MultiArray 1) (ros::advertise (format nil "~A/calibrated" topic-name) std_msgs::Float64MultiArray 1) (ros::advertise (format nil "~A/distance" topic-name) std_msgs::Float64MultiArray 1)) - ;; make hash tables (setq proximities-calibrated- (make-hash-table :test #'equal)) (setq proximities-init- (make-hash-table :test #'equal)) (setq proximities-param-a- (make-hash-table :test #'equal)) - ;; get default proximity parameters from rosparam (dolist (topic-name topic-list) (sethash topic-name @@ -59,11 +57,12 @@ (gethash topic-name proximities-param-a-) (gethash topic-name proximities-calibrated-))) (ros::publish (format nil "~A/distance" topic-name) float-array))) - (:set-init-proximities (msg) + (:set-init-proximities (req) (dolist (topic-name topic-list-) (sethash topic-name proximities-init- - (gethash topic-name proximities-raw-)))) + (gethash topic-name proximities-raw-))) + (instance std_srvs::EmptyResponse :init)) ) From b6804d4966a8083fbc0a3f63d7f8695e2b1367e4 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Tue, 31 Jul 2018 03:43:17 +0900 Subject: [PATCH 3/4] enable to run this node by rosrun --- jsk_arc2017_baxter/euslisp/proximity-calibrator.l | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/jsk_arc2017_baxter/euslisp/proximity-calibrator.l b/jsk_arc2017_baxter/euslisp/proximity-calibrator.l index 4c971e1ec..2ccceae65 100755 --- a/jsk_arc2017_baxter/euslisp/proximity-calibrator.l +++ b/jsk_arc2017_baxter/euslisp/proximity-calibrator.l @@ -70,3 +70,8 @@ (when (not (boundp '*pc*)) (setq *pc* (instance proximity-calibrator :init :topic-list topic-list))) (ros::spin-once)) + + +(init) +(do-until-key + (ros::spin-once)) From d8a55e163474594fe053defd3add4e39555507ee Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Tue, 31 Jul 2018 13:18:28 +0900 Subject: [PATCH 4/4] use (ros::spin) instead of (ros::spin-once) and (sleep) --- jsk_arc2017_baxter/euslisp/proximity-calibrator.l | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/jsk_arc2017_baxter/euslisp/proximity-calibrator.l b/jsk_arc2017_baxter/euslisp/proximity-calibrator.l index 2ccceae65..a5df901bc 100755 --- a/jsk_arc2017_baxter/euslisp/proximity-calibrator.l +++ b/jsk_arc2017_baxter/euslisp/proximity-calibrator.l @@ -73,5 +73,4 @@ (init) -(do-until-key - (ros::spin-once)) +(ros::spin)