diff --git a/hironx_tutorial/euslisp/hand-joint-state-publisher-real.l b/hironx_tutorial/euslisp/hand-joint-state-publisher-real.l new file mode 100755 index 00000000..f07690cf --- /dev/null +++ b/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) + ) diff --git a/hironx_tutorial/launch/hironxjsk_real.launch b/hironx_tutorial/launch/hironxjsk_real.launch index 09fa9d7d..3675e975 100644 --- a/hironx_tutorial/launch/hironxjsk_real.launch +++ b/hironx_tutorial/launch/hironxjsk_real.launch @@ -1,6 +1,8 @@ + + @@ -17,7 +19,17 @@ + + + + + + publish_rate: $(arg real_hand_joint_state_rate) + + +