fix for gripper topic name: jsk-enshu#292

mmurooka · Oct 25, 2019 · 035bc82 · 035bc82
1 parent 494e057
commit 035bc82
Show file tree

Hide file tree

Showing 2 changed files with 40 additions and 17 deletions.
diff --git a/dynamixel_7dof_arm/euslisp/old_examples/test-dynamixel-motor.l b/dynamixel_7dof_arm/euslisp/old_examples/test-dynamixel-motor.l
@@ -17,20 +17,30 @@
 ;;     具体的には/arm_j0_controllder/stateなどをsubscribeすることになる
 (defun dxl-state-cb
   (msg)
-  (setf (elt *state-list* (1- (read-from-string (string-left-trim "arm_joint" (send msg :name))))) msg))
+  (setf
+   (elt *state-list*
+	(if (string= (send msg :name) "gripper_joint")
+	    6 (1- (read-from-string (string-left-trim "arm_joint" (send msg :name))))))
+   msg))
 
 ;;   subscriberの登録
 (dotimes (i *arm-dof*)
-  (ros::subscribe (format nil "/arm_j~d_controller/state" (1+ i)) ;; subscribeしたいtopic名
-                  dynamixel_msgs::JointState ;; topicの型名
-                  #'dxl-state-cb)) ;; コールバック関数の設定, #'で指定するのがEuslispのポイント
+  (ros::subscribe
+   (if (eq i 6) ;; subscribeしたいtopic名
+       (format nil "/gripper_joint_controller/state")
+     (format nil "/arm_j~d_controller/state" (1+ i)))
+   dynamixel_msgs::JointState ;; topicの型名
+   #'dxl-state-cb)) ;; コールバック関数の設定, #'で指定するのがEuslispのポイント
 
 ;; advertiseの設定
 ;;  どのtopicをpublishするか (指令値などの送信用)
 (dotimes (i *arm-dof*)
-  (ros::advertise (format nil "/arm_j~d_controller/command" (1+ i)) ;; topic名
-                  std_msgs::Float64 ;; topicの型
-                  1)) ;; キューの数
+  (ros::advertise
+   (if (eq i 6) ;; topic名
+       (format nil "/gripper_joint_controller/command")
+     (format nil "/arm_j~d_controller/command" (1+ i)))
+   std_msgs::Float64 ;; topicの型
+   1)) ;; キューの数
 
 
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
@@ -50,8 +60,11 @@
     (dotimes (i *arm-dof*)
       (format t ";; send command position to arm ~d~%;; press enter~%" (1+ i))
       (read-line)
-      (ros::publish (format nil "/arm_j~d_controller/command" (1+ i)) ;; topic名
-                    (instance std_msgs::Float64 :init :data (deg2rad (+ 10 (elt current-av i))))) ;; 指令値をstd_msgs::Float64に変換して送信
+      (ros::publish
+       (if (eq i 6) ;; topic名
+	   (format nil "/gripper_joint_controller/command")
+	 (format nil "/arm_j~d_controller/command" (1+ i)))
+       (instance std_msgs::Float64 :init :data (deg2rad (+ 10 (elt current-av i))))) ;; 指令値をstd_msgs::Float64に変換して送信
       )
     t))
 (warn ";; (test-send-angle-vector-once) ~%")
@@ -63,10 +76,13 @@
     (do-until-key
      (dotimes (i *arm-dof*)
        (format t ";; send for arm ~d~%;; press enter~%" (1+ i))
-       (ros::publish (format nil "/arm_j~d_controller/command" (1+ i))
-                     (if move
-                         (instance std_msgs::Float64 :init :data (deg2rad (+ (* (- (mod ii 3) 1) 5) (elt current-av i))))
-                       (instance std_msgs::Float64 :init :data (deg2rad (+ 0 (elt current-av i))))))
+       (ros::publish
+	(if (eq i 6) ;; topic名
+	    (format nil "/gripper_joint_controller/command")
+	  (format nil "/arm_j~d_controller/command" (1+ i)))
+	(if move
+	    (instance std_msgs::Float64 :init :data (deg2rad (+ (* (- (mod ii 3) 1) 5) (elt current-av i))))
+	  (instance std_msgs::Float64 :init :data (deg2rad (+ 0 (elt current-av i))))))
        )
      (format t ";; temps ~A~%" (send-all *state-list* :motor_temps))
      (unix:usleep 1000000) ;; 1秒まつ

diff --git a/dynamixel_7dof_arm/euslisp/thermo-speaker.l b/dynamixel_7dof_arm/euslisp/thermo-speaker.l
@@ -9,12 +9,19 @@
 
 (defun state-cb
   (msg)
-  (setf (elt *state-list* (1- (read-from-string (string-left-trim "arm_joint" (send msg :name))))) msg))
+  (setf
+   (elt *state-list*
+	(if (string= (send msg :name) "gripper_joint")
+	    6 (1- (read-from-string (string-left-trim "arm_joint" (send msg :name))))))
+   msg))
 
 (dotimes (i *arm-dof*)
-  (ros::subscribe (format nil "/arm_j~d_controller/state" (1+ i))
-                  dynamixel_msgs::JointState
-                  #'state-cb))
+  (ros::subscribe
+   (if (eq i 6)
+       (format nil "/gripper_joint_controller/state")
+     (format nil "/arm_j~d_controller/state" (1+ i)))
+   dynamixel_msgs::JointState
+   #'state-cb))
 
 (defun main () ;; [deg]
   (ros::rate 0.1)