publish real hand joint state

hironx_tutorial/euslisp/hand-joint-state-publisher-real.l

@@ -0,0 +1,18 @@
+#!/usr/bin/env roseus
+
+(load "package://hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l")
+
+(hironxjsk-init)
+
+(setq pub-rate (ros::get-param "~publish_rate" 5))
+
+(ros::advertise "joint_states" sensor_msgs::JointState 1)
+(ros::rate pub-rate)
+(while (ros::ok)
+  (setq msg (instance sensor_msgs::JointState :init))
+  (send msg :header :stamp (ros::time-now))
+  (send msg :name (list "RHAND_JOINT0" "RHAND_JOINT1" "RHAND_JOINT2" "RHAND_JOINT3" "LHAND_JOINT0" "LHAND_JOINT1" "LHAND_JOINT2" "LHAND_JOINT3"))
+  (send msg :position (map float-vector #'deg2rad (send *ri* :hand-angle-vector :hands)))
+  (ros::publish "joint_states" msg)
+  (ros::sleep)
+  )

hironx_tutorial/launch/hironxjsk_real.launch

@@ -1,6 +1,8 @@
 <launch>
 
   <arg name="launch_sound_play" default="true" />
+  <arg name="publish_real_hand_joint_state" default="false" />
+  <arg name="real_hand_joint_state_rate" default="5" />
 
   <!-- Start Camera -->
   <include file="$(find openni2_launch)/launch/openni2.launch">
@@ -17,7 +19,17 @@
     <arg name="nameserver" value="hiro014" />
     <arg name="USE_IMPEDANCECONTROLLER" value="true" />
     <arg name="USE_COLLISIONCHECK" value="true" />
+    <arg name="USE_HAND_JOINT_STATE_PUBLISHER" value="true" unless="$(arg publish_real_hand_joint_state)" />
+    <arg name="USE_HAND_JOINT_STATE_PUBLISHER" value="false" if="$(arg publish_real_hand_joint_state)" />
   </include>
+  <group if="$(arg publish_real_hand_joint_state)">
+    <node name="hand_joint_state_publisher_real"
+          pkg="hironx_tutorial" type="hand-joint-state-publisher-real.l">
+      <rosparam subst_value="True">
+        publish_rate: $(arg real_hand_joint_state_rate)
+      </rosparam>
+    </node>
+  </group>
 
   <!-- Publish odom tf -->
   <node name="odom_tf_publisher"