From 2321d698cf1e7a0c74c08451e0c661e8fb79cf14 Mon Sep 17 00:00:00 2001 From: soonhyo kim Date: Thu, 18 Apr 2024 20:59:02 +0900 Subject: [PATCH] use subcribed msg's stamp and baxter's frame-id once to accelerate the rate --- .../jsk_baxter_lifelog/estimated_endeffector.l | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_lifelog/estimated_endeffector.l b/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_lifelog/estimated_endeffector.l index 6503a7faa4..65b30c1dd7 100644 --- a/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_lifelog/estimated_endeffector.l +++ b/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_lifelog/estimated_endeffector.l @@ -5,22 +5,26 @@ (if (not (boundp '*baxter*)) (setq *baxter* (instance baxter-robot :init))) +(setq frames (make-hash-table)) +(dolist (arm (list :rarm :larm)) + (sethash arm frames (send (send (send *baxter* arm :end-coords) :parent) :name)) + ) -(ros::rate 20) +(ros::rate 100) (defun right-state-cb(msg) - (state-cb "/robot/end_effector/right/wrench" :rarm (send msg :wrench)) + (state-cb "/robot/end_effector/right/wrench" :rarm (send msg :wrench) (send msg :header :stamp)) ) (defun left-state-cb(msg) - (state-cb "/robot/end_effector/left/wrench" :larm (send msg :wrench)) + (state-cb "/robot/end_effector/left/wrench" :larm (send msg :wrench) (send msg :header :stamp)) ) -(defun state-cb(advertise-topic arm wrench-msg) +(defun state-cb(advertise-topic arm wrench-msg stamp) (let ((msg (instance geometry_msgs::WrenchStamped :init))) (send msg :wrench wrench-msg) - (send msg :header :stamp (ros::time-now)) - (send msg :header :frame_id (send (send (send *baxter* arm :end-coords) :parent) :name)) + (send msg :header :stamp stamp) + (send msg :header :frame_id (gethash arm frames)) (ros::publish advertise-topic msg)) ) @@ -28,4 +32,4 @@ (ros::subscribe "/robot/limb/left/endpoint_state" baxter_core_msgs::EndpointState #'left-state-cb) (ros::subscribe "/robot/limb/right/endpoint_state" baxter_core_msgs::EndpointState #'right-state-cb) -(ros::spin) \ No newline at end of file +(ros::spin)