Merge pull request #582 from pazeshun/hironxjsk-add-speech

start-jsk · Feb 8, 2021 · c590aeb · c590aeb
2 parents 95d7d58 + 599ed02
commit c590aeb
Show file tree

Hide file tree

Showing 11 changed files with 368 additions and 25 deletions.
diff --git a/hironx_tutorial/CMakeLists.txt b/hironx_tutorial/CMakeLists.txt
@@ -21,6 +21,31 @@ catkin_package(
 #  DEPENDS system_lib
 )
 
+# Generate HIRONXJSK_nosim.xml
+find_package(hrp2_models QUIET)
+if(${hrp2_models_FOUND})
+  find_package(hrpsys_ros_bridge QUIET)
+  if(${hrpsys_ros_bridge_FOUND})
+    # Based on https://github.com/start-jsk/rtmros_hironx/blob/2.2.0/hironx_ros_bridge/CMakeLists.txt#L25-L48
+    # set OPENHRP3 for configure_file
+    set(PKG_CONFIG_PATH "${openhrp3_PREFIX}/lib/pkgconfig:$ENV{PKG_CONFIG_PATH}") # for openrtm3.1.pc
+    execute_process(
+      COMMAND pkg-config --variable=idl_dir openhrp3.1
+      OUTPUT_VARIABLE OPENHRP_IDL_DIR
+      RESULT_VARIABLE RESULT
+      OUTPUT_STRIP_TRAILING_WHITESPACE)
+    if(NOT RESULT EQUAL 0)
+      set(OPENHRP_FOUND FALSE)
+    endif()
+    # OPENHRP_IDL_DIR = <openhrp3>/share/OpenHRP-3.1/idl/
+    set(OPENHRP_SAMPLE_DIR ${OPENHRP_IDL_DIR}/../../../)
+    set(OPENHRP3 ${OPENHRP_SAMPLE_DIR})  # for longfloor.wrl
+    message("Configure HIRONXJSK_nosim.xml with OPENHRP3=${OPENHRP3}")
+    configure_file(config/HIRONXJSK_nosim.xml.in ${PROJECT_SOURCE_DIR}/config/HIRONXJSK_nosim.xml)
+    add_custom_target(${PROJECT_NAME}_model_files ALL DEPENDS ${PROJECT_SOURCE_DIR}/config/HIRONXJSK_nosim.xml)
+  endif()
+endif()
+
 include_directories(
   ${catkin_INCLUDE_DIRS}
 )

diff --git a/hironx_tutorial/config/.gitignore b/hironx_tutorial/config/.gitignore
@@ -0,0 +1 @@
+HIRONXJSK_nosim.xml
diff --git a/hironx_tutorial/config/HIRONXJSK_nosim.xml.in b/hironx_tutorial/config/HIRONXJSK_nosim.xml.in
@@ -0,0 +1,180 @@
+<!-- Based on hrpsys_ros_bridge_tutorials/models/HIRONXJSK_nosim.xml (auto-generated file) -->
+<!-- Change robot name from HIRONXJSK to HiroNX (name used in hironx_ros_bridge/conf/nosim.xml.in) in order to reuse HiroNX simulation software -->
+<!-- Use initial robot pose (values of {JOINT_NAME}.angle) of hironx_ros_bridge/conf/nosim.xml.in -->
+<!-- Use longfloor url of hironx_ros_bridge/conf/nosim.xml.in -->
+<!-- Use eyeHomePosition of nextage_ros_bridge/conf/nextage_nosim.xml to show the whole robot initially -->
+<!-- Disable showScale as with nextage_ros_bridge/conf/nextage_nosim.xml because scale is displayed on WAIST plane, not on longfloor -->
+
+<grxui>
+ <mode name="Simulation">
+  <item class="com.generalrobotix.ui.item.GrxSimulationItem" name="simulationItem">
+   <property name="integrate" value="false"/>
+   <property name="timeStep" value="0.005"/>
+   <property name="totalTime" value="2000000.0"/>
+   <property name="method" value="EULER"/>
+  </item>
+  <item class="com.generalrobotix.ui.item.GrxRTSItem" name="HiroNX" select="true">
+   <property name="HiroNX(Robot)0.period" value="0.005"/>
+   <property name="HGcontroller0.period" value="0.005"/>
+   <property name="HGcontroller0.factory" value="HGcontroller"/>
+   <property name="connection" value="HGcontroller0.qOut:HiroNX(Robot)0.qRef"/>
+   <property name="connection" value="HGcontroller0.dqOut:HiroNX(Robot)0.dqRef"/>
+   <property name="connection" value="HGcontroller0.ddqOut:HiroNX(Robot)0.ddqRef"/>
+  </item>
+  <item class="com.generalrobotix.ui.item.GrxModelItem" name="HIRONXJSKmain" url="@hrp2_models_SOURCE_PREFIX@/HIRONXJSK/HIRONXJSKmain.wrl">
+   <property name="rtcName" value="HiroNX(Robot)0"/>
+   <property name="inport" value="qRef:JOINT_VALUE"/>
+   <property name="inport" value="dqRef:JOINT_VELOCITY"/>
+   <property name="inport" value="ddqRef:JOINT_ACCELERATION"/>
+   <property name="inport" value="basePoseRef:WAIST_JOINT:ABS_TRANSFORM"/>
+   <property name="outport" value="q:JOINT_VALUE"/>
+   <property name="outport" value="dq:JOINT_VELOCITY"/>
+   <property name="outport" value="tau:JOINT_TORQUE"/>
+   <property name="outport" value="WAIST_JOINT:WAIST_JOINT:ABS_TRANSFORM"/>
+   <property name="outport" value="lhsensor:lhsensor:FORCE_SENSOR"/>
+   <property name="outport" value="rhsensor:rhsensor:FORCE_SENSOR"/>
+   <property name="WAIST_JOINT.NumOfAABB" value="1"/>
+   <property name="WAIST_JOINT.translation" value="0  0  0"/>
+   <property name="WAIST_JOINT.rotation" value="1 0 0 0"/>
+   <property name="WAIST_JOINT.mode" value="Torque"/>
+   <property name="controller" value="HIRONXJSK_nosim"/>
+   <property name="CHEST_JOINT0.angle" value="0.0"/>
+   <property name="CHEST_JOINT0.mode" value="HighGain"/>
+   <property name="CHEST_JOINT0.NumOfAABB" value="1"/>
+   <property name="HEAD_JOINT0.angle" value="0.0"/>
+   <property name="HEAD_JOINT0.mode" value="HighGain"/>
+   <property name="HEAD_JOINT0.NumOfAABB" value="1"/>
+   <property name="HEAD_JOINT1.angle" value="0.0"/>
+   <property name="HEAD_JOINT1.mode" value="HighGain"/>
+   <property name="HEAD_JOINT1.NumOfAABB" value="1"/>
+   <property name="RARM_JOINT0.angle" value="-0.010472"/>
+   <property name="RARM_JOINT0.mode" value="HighGain"/>
+   <property name="RARM_JOINT0.NumOfAABB" value="1"/>
+   <property name="RARM_JOINT1.angle" value="0.0"/>
+   <property name="RARM_JOINT1.mode" value="HighGain"/>
+   <property name="RARM_JOINT1.NumOfAABB" value="1"/>
+   <property name="RARM_JOINT2.angle" value="-1.74533"/>
+   <property name="RARM_JOINT2.mode" value="HighGain"/>
+   <property name="RARM_JOINT2.NumOfAABB" value="1"/>
+   <property name="RARM_JOINT3.angle" value="0.26529"/>
+   <property name="RARM_JOINT3.mode" value="HighGain"/>
+   <property name="RARM_JOINT3.NumOfAABB" value="1"/>
+   <property name="RARM_JOINT4.angle" value="0.164061"/>
+   <property name="RARM_JOINT4.mode" value="HighGain"/>
+   <property name="RARM_JOINT4.NumOfAABB" value="1"/>
+   <property name="RARM_JOINT5.angle" value="0.055851"/>
+   <property name="RARM_JOINT5.mode" value="HighGain"/>
+   <property name="RARM_JOINT5.NumOfAABB" value="1"/>
+   <property name="LARM_JOINT0.angle" value="0.010472"/>
+   <property name="LARM_JOINT0.mode" value="HighGain"/>
+   <property name="LARM_JOINT0.NumOfAABB" value="1"/>
+   <property name="LARM_JOINT1.angle" value="0.0"/>
+   <property name="LARM_JOINT1.mode" value="HighGain"/>
+   <property name="LARM_JOINT1.NumOfAABB" value="1"/>
+   <property name="LARM_JOINT2.angle" value="-1.74533"/>
+   <property name="LARM_JOINT2.mode" value="HighGain"/>
+   <property name="LARM_JOINT2.NumOfAABB" value="1"/>
+   <property name="LARM_JOINT3.angle" value="-0.26529"/>
+   <property name="LARM_JOINT3.mode" value="HighGain"/>
+   <property name="LARM_JOINT3.NumOfAABB" value="1"/>
+   <property name="LARM_JOINT4.angle" value="0.164061"/>
+   <property name="LARM_JOINT4.mode" value="HighGain"/>
+   <property name="LARM_JOINT4.NumOfAABB" value="1"/>
+   <property name="LARM_JOINT5.angle" value="-0.055851"/>
+   <property name="LARM_JOINT5.mode" value="HighGain"/>
+   <property name="LARM_JOINT5.NumOfAABB" value="1"/>
+   <property name="RHAND_JOINT0.angle" value="0.0"/>
+   <property name="RHAND_JOINT0.mode" value="HighGain"/>
+   <property name="RHAND_JOINT0.NumOfAABB" value="1"/>
+   <property name="RHAND_JOINT1.angle" value="0.0"/>
+   <property name="RHAND_JOINT1.mode" value="HighGain"/>
+   <property name="RHAND_JOINT1.NumOfAABB" value="1"/>
+   <property name="RHAND_JOINT2.angle" value="0.0"/>
+   <property name="RHAND_JOINT2.mode" value="HighGain"/>
+   <property name="RHAND_JOINT2.NumOfAABB" value="1"/>
+   <property name="RHAND_JOINT3.angle" value="0.0"/>
+   <property name="RHAND_JOINT3.mode" value="HighGain"/>
+   <property name="RHAND_JOINT3.NumOfAABB" value="1"/>
+   <property name="LHAND_JOINT0.angle" value="0.0"/>
+   <property name="LHAND_JOINT0.mode" value="HighGain"/>
+   <property name="LHAND_JOINT0.NumOfAABB" value="1"/>
+   <property name="LHAND_JOINT1.angle" value="0.0"/>
+   <property name="LHAND_JOINT1.mode" value="HighGain"/>
+   <property name="LHAND_JOINT1.NumOfAABB" value="1"/>
+   <property name="LHAND_JOINT2.angle" value="0.0"/>
+   <property name="LHAND_JOINT2.mode" value="HighGain"/>
+   <property name="LHAND_JOINT2.NumOfAABB" value="1"/>
+   <property name="LHAND_JOINT3.angle" value="0.0"/>
+   <property name="LHAND_JOINT3.mode" value="HighGain"/>
+   <property name="LHAND_JOINT3.NumOfAABB" value="1"/>
+  </item>
+  <view class="com.generalrobotix.ui.view.GrxRobotHardwareClientView" name="RobotHardware RTC Client">
+   <property name="robotHost" value="localhost"/>
+   <property name="StateHolderRTC" value="StateHolder0"/>
+   <property name="interval" value="100"/>
+   <property name="RobotHardwareServiceRTC" value="RobotHardware0"/>
+   <property name="robotPort" value="2809"/>
+   <property name="ROBOT" value="HiroNX"/>
+  </view>
+  <view class="com.generalrobotix.ui.view.Grx3DView" name="3DView">
+   <property name="view.mode" value="Room"/>
+   <property name="showCoM" value="false"/>
+   <property name="showCoMonFloor" value="false"/>
+   <property name="showDistance" value="false"/>
+   <property name="showIntersection" value="false"/>
+   <property name="eyeHomePosition" value="-0.707107 -0.183013 0.683013 2 0.707107 -0.183013 0.683013 2 0 0.965926 0.258819 0.8 "/>
+   <property name="showCollision" value="true"/>
+   <property name="showActualState" value="true"/>
+   <property name="showScale" value="false"/>
+  </view>
+  <item class="com.generalrobotix.ui.item.GrxRTSItem" name="longfloor" select="true">
+   <property name="longfloor(Robot)0.period" value="0.005"/>
+   <property name="HGcontroller0.period" value="0.005"/>
+   <property name="HGcontroller0.factory" value="HGcontroller"/>
+  </item>
+  <item class="com.generalrobotix.ui.item.GrxModelItem" name="longfloor" url="@OPENHRP3@/share/OpenHRP-3.1/sample/model/longfloor.wrl">
+   <property name="rtcName" value="longfloor(Robot)0"/>
+   <property name="inport" value="qRef:JOINT_VALUE"/>
+   <property name="inport" value="dqRef:JOINT_VELOCITY"/>
+   <property name="inport" value="ddqRef:JOINT_ACCELERATION"/>
+   <property name="inport" value="basePoseRef:WAIST:ABS_TRANSFORM"/>
+   <property name="outport" value="q:JOINT_VALUE"/>
+   <property name="outport" value="dq:JOINT_VELOCITY"/>
+   <property name="outport" value="tau:JOINT_TORQUE"/>
+   <property name="outport" value="WAIST:WAIST:ABS_TRANSFORM"/>
+   <property name="WAIST.NumOfAABB" value="1"/>
+   <property name="WAIST.translation" value="0  0  -0.9"/>  <!-- Place floor under HIRONXJSK base -->
+   <property name="WAIST.rotation" value="1 0 0 0"/>
+  </item>
+  <view class="com.generalrobotix.ui.view.GrxRobotHardwareClientView" name="RobotHardware RTC Client">
+   <property name="robotHost" value="localhost"/>
+   <property name="StateHolderRTC" value="StateHolder0"/>
+   <property name="interval" value="100"/>
+   <property name="RobotHardwareServiceRTC" value="RobotHardware0"/>
+   <property name="robotPort" value="2809"/>
+   <property name="ROBOT" value="longfloor"/>
+  </view>
+  <view class="com.generalrobotix.ui.view.Grx3DView" name="3DView">
+   <property name="view.mode" value="Room"/>
+   <property name="showCoM" value="false"/>
+   <property name="showCoMonFloor" value="false"/>
+   <property name="showDistance" value="false"/>
+   <property name="showIntersection" value="false"/>
+   <property name="eyeHomePosition" value="-0.707107 -0.183013 0.683013 2 0.707107 -0.183013 0.683013 2 0 0.965926 0.258819 0.8 "/>
+   <property name="showCollision" value="true"/>
+   <property name="showActualState" value="true"/>
+   <property name="showScale" value="false"/>
+  </view>
+  <item class="com.generalrobotix.ui.item.GrxCollisionPairItem" name="CP#longfloor_#HIRONXJSKmain_">
+   <property name="springConstant" value="0 0 0 0 0 0"/>
+   <property name="slidingFriction" value="0.5"/>
+   <property name="jointName2" value=""/>
+   <property name="jointName1" value=""/>
+   <property name="damperConstant" value="0 0 0 0 0 0"/>
+   <property name="objectName2" value="longfloor"/>
+   <property name="objectName1" value="HIRONXJSKmain"/>
+   <property name="springDamperModel" value="false"/>
+   <property name="staticFriction" value="0.5"/>
+  </item>
+ </mode>
+</grxui>
diff --git a/hironx_tutorial/doc/images/hironxjsk_hrpsys_viewer.jpg b/hironx_tutorial/doc/images/hironxjsk_hrpsys_viewer.jpg
diff --git a/hironx_tutorial/doc/index.rst b/hironx_tutorial/doc/index.rst
@@ -25,8 +25,31 @@ With the real robot,::
 
 `%HOSTNAME%` is the host name that can be pinged at, `hiro014` for example.
 
+HIRONXJSK Demos
+---------------
+
+Installation
+++++++++++++
+
+To run the following demos, you have to build this package with ``hrp2_models`` (closed package) and ``hrpsys_ros_bridge_tutorials``.
+If you compiled this package or ``hrpsys_ros_bridge_tutorials`` before you download ``hrp2_models``, you have to compile this package and ``hrpsys_ros_bridge_tutorials`` again with ``--force-cmake`` option after you download ``hrp2_models``.
+
+HIRONXJSK hrpsys simulation
++++++++++++++++++++++++++++
+
+.. image:: images/hironxjsk_hrpsys_viewer.jpg
+   :width: 60%
+   :align: center
+
+Execution
+^^^^^^^^^
+
+.. code-block:: bash
+
+  $ rtmlaunch hironx_tutorial hironxjsk_ros_bridge_simulation.launch
+
 HIRONXJSK Picking Demo
-----------------------
+++++++++++++++++++++++
 
 .. image:: images/hironxjsk_picking_demo_gazebo.gif
    :width: 60%
@@ -36,17 +59,20 @@ HIRONXJSK Picking Demo
    :width: 60%
    :align: center
 
-Installation
-++++++++++++
-
-To run this demo, you have to build this package with ``hrp2_models`` (closed package) and ``hrpsys_ros_bridge_tutorials``.
-If you compiled ``hrpsys_ros_bridge_tutorials`` before you download ``hrp2_models``, you have to compile ``hrpsys_ros_bridge_tutorials`` again with ``--force-cmake`` option after you download ``hrp2_models``.
-
-Execution
-+++++++++
+Execution (Gazebo)
+^^^^^^^^^^^^^^^^^^
 
 .. code-block:: bash
 
   $ roslaunch hironx_tutorial hironxjsk_picking_demo.launch
   # Wait until the robot stops moving
   $ rosrun hironx_tutorial hironxjsk-picking-demo.l
+
+Execution (Real Robot)
+^^^^^^^^^^^^^^^^^^^^^^
+
+.. code-block:: bash
+
+  # First, start up robot. Then...
+  $ roslaunch hironx_tutorial hironxjsk_picking_demo.launch gazebo:=false
+  $ rosrun hironx_tutorial hironxjsk-picking-demo.l
diff --git a/hironx_tutorial/launch/hironxjsk_real.launch b/hironx_tutorial/launch/hironxjsk_real.launch
@@ -0,0 +1,63 @@
+<launch>
+
+  <arg name="launch_sound_play" default="true" />
+
+  <!-- Start Camera -->
+  <include file="$(find openni2_launch)/launch/openni2.launch">
+    <arg name="camera" value="head_camera" />
+    <arg name="publish_tf" value="false" />
+  </include>
+  <!-- FIXME: When subscribing compressedDepth, hz of image_raw drops if png_level is more than 5. -->
+  <!--        Usage of CPU processing camera may be too high (not 100%, though)                    -->
+  <!--        CPU: Intel(R) Xeon(R) CPU W5580 @ 3.20GHz                                            -->
+  <param name="head_camera/depth/image_raw/compressedDepth/png_level" value="5" />
+
+  <!-- Start Robot -->
+  <include file="$(find hironx_ros_bridge)/launch/hironx_ros_bridge_real.launch">
+    <arg name="COLLADA_FILE" value="$(find hrpsys_ros_bridge_tutorials)/models/HIRONXJSK_SENSORS.urdf" />
+    <arg name="nameserver" value="hiro014" />
+    <arg name="USE_IMPEDANCECONTROLLER" value="true" />
+    <arg name="USE_COLLISIONCHECK" value="true" />
+  </include>
+
+  <!-- Publish odom tf -->
+  <node name="odom_tf_publisher"
+        pkg="tf" type="static_transform_publisher"
+        args="0 0 0.8 0 0 0 odom WAIST 20" />
+
+  <!-- Start Sound -->
+  <group if="$(arg launch_sound_play)">
+
+    <!-- English speach node -->
+    <node name="sound_play" pkg="sound_play" type="soundplay_node.py" respawn="true">
+      <remap from="sound_play" to="robotsound" />
+    </node>
+
+    <!-- Japanese speech node -->
+    <include file="$(find voice_text)/launch/voice_text.launch">
+      <arg name="sound_play_respawn" value="true" />
+    </include>
+
+  </group>
+
+  <!-- Speech Recognition -->
+  <node name="respeaker_transformer" pkg="tf" type="static_transform_publisher"
+        args="0 0 0.2 0 0 0 HEAD_JOINT1_Link respeaker_base 100"/>
+  <include file="$(find respeaker_ros)/launch/sample_respeaker.launch">
+    <arg name="publish_tf" default="false"/>
+    <arg name="launch_soundplay" default="false"/>
+    <arg name="audio" value="speech_audio"/>
+    <arg name="language" value="ja-JP"/>
+  </include>
+  <!-- Set speak action server names -->
+  <!-- This parameter is for speech_to_text node in respeaker_ros -->
+  <!-- https://github.com/jsk-ros-pkg/jsk_3rdparty/pull/168 -->
+  <group ns="speech_to_text">
+    <rosparam>
+       tts_action_names:
+         - robotsound
+         - robotsound_jp
+    </rosparam>
+  </group>
+
+</launch>
diff --git a/hironx_tutorial/launch/hironxjsk_ros_bridge_simulation.launch b/hironx_tutorial/launch/hironxjsk_ros_bridge_simulation.launch
@@ -0,0 +1,42 @@
+<launch>
+
+  <!-- Based on hironx_ros_bridge/launch/hironx_ros_bridge_simulation.launch -->
+
+  <arg name="GUI" default="true" />
+  <arg name="corbaport" default="15005" />
+  <arg name="PROJECT_FILE" default="$(find hironx_tutorial)/config/HIRONXJSK_nosim.xml" />
+  <arg name="MODEL_FILE" default="$(find hrp2_models)/HIRONXJSK/HIRONXJSKmain.wrl" />
+  <arg name="COLLADA_FILE" default="$(find hrpsys_ros_bridge_tutorials)/models/HIRONXJSK_SENSORS.urdf" />
+  <arg name="CONF_FILE" default="$(find hrpsys_ros_bridge_tutorials)/models/HIRONXJSK.conf" />
+  <arg name="CONF_FILE_COLLISIONDETECT" default="$(find hrpsys_ros_bridge_tutorials)/models/HIRONXJSK.conf" />
+  <arg name="DEBUG_HRPSYS" default="false" />
+  <arg name="USE_COLLISIONCHECK" default="true" />
+
+  <!-- Set model file to parameter server  -->
+  <param name="hironx/collada_model_filepath" value="$(arg MODEL_FILE)" />
+
+  <include file="$(find hironx_ros_bridge)/launch/hironx_startup.launch" >
+    <arg name="GUI" default="$(arg GUI)" />
+    <arg name="PROJECT_FILE" value="$(arg PROJECT_FILE)" />
+    <arg name="MODEL_FILE" value="$(arg MODEL_FILE)" />
+    <arg name="corbaport" default="$(arg corbaport)" />
+    <arg name="CONF_FILE" value="$(arg CONF_FILE)" />
+    <arg name="DEBUG_HRPSYS" value="$(arg DEBUG_HRPSYS)" />
+  </include>
+  <include file="$(find hironx_ros_bridge)/launch/hironx_ros_bridge.launch" >
+    <arg name="nameserver" value="localhost" />
+    <arg name="MODEL_FILE" value="$(arg MODEL_FILE)" />
+    <arg name="COLLADA_FILE" value="$(arg COLLADA_FILE)" />
+    <arg name="SIMULATOR_NAME" value="HiroNX(Robot)0" />
+    <arg name="corbaport" default="$(arg corbaport)" />
+    <arg name="CONF_FILE_COLLISIONDETECT" value="$(arg CONF_FILE_COLLISIONDETECT)" />
+    <arg name="open_rqt_gui" default="false" />
+    <arg name="USE_COLLISIONCHECK" value="$(arg USE_COLLISIONCHECK)"/>
+  </include>
+
+  <!-- Publish odom tf -->
+  <node name="odom_tf_publisher"
+        pkg="tf" type="static_transform_publisher"
+        args="0 0 0.8 0 0 0 odom WAIST 20" />
+
+</launch>
diff --git a/hironx_tutorial/package.xml b/hironx_tutorial/package.xml
@@ -20,6 +20,11 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <!-- Start of HIRONXJSK demo dependency -->
+  <build_depend>hrp2_models</build_depend> <!-- only jsk people have this package, so if you do not find them, please comment out this -->
+  <build_depend>hrpsys_ros_bridge</build_depend>  <!-- Even if hrpsys_ros_bridge is not installed, build succeeds -->
+  <!-- End of HIRONXJSK demo dependency -->
+
   <run_depend>hironx_ros_bridge</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>jsk_tools</run_depend>