[ros_bridge] Enable to use impedance controller through ros bridge

hironx_ros_bridge/.gitignore

@@ -0,0 +1,9 @@
+# For rtmbuild
+msg/OpenHRP_*
+srv/OpenHRP_*
+src_gen
+
+# For others
+conf/*.conf
+conf/*.xml
+*.pyc

hironx_ros_bridge/CMakeLists.txt

@@ -1,15 +1,30 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(hironx_ros_bridge)
 
-find_package(catkin REQUIRED COMPONENTS hrpsys_ros_bridge roslib roslint rostest)
+find_package(catkin REQUIRED COMPONENTS hrpsys_ros_bridge roslib roslint rostest rtmbuild)
 find_package(Boost REQUIRED COMPONENTS system)
 
+# catkin_python_setup() must be before generate_messages() included in rtmbuild_init()
+catkin_python_setup()
+
+# for generating bridge for old impedance controller
+include(${PROJECT_SOURCE_DIR}/cmake/rtmbuild_customed.cmake)
+
+# initialize rtmbuild for old impedance controller
+rtmbuild_init()
+
+# call catkin_package, after rtmbuild_init, before rtmbuild_gen*
 catkin_package(
     CATKIN_DEPENDS hrpsys_ros_bridge roslib #
     INCLUDE_DIRS include
+    CFG_EXTRAS rtmbuild_customed.cmake
 )
 
-catkin_python_setup()
+# generate idl for old impedance controller
+rtmbuild_genidl_customed()
+
+# generate bridge for old impedance controller
+rtmbuild_genbridge()
 
 add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/kawada-hironx.dae
   COMMAND ${catkin_EXTRAS_DIR}/test/download_checkmd5.py
@@ -72,6 +87,8 @@ install(DIRECTORY conf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} PATTERN "
 install(DIRECTORY models DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} PATTERN ".svn" EXCLUDE)
 install(DIRECTORY resource DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 install(DIRECTORY test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS PATTERN ".svn" EXCLUDE)
+install(DIRECTORY idl DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS)
+install(DIRECTORY cmake DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS)
 install(FILES rqt_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
 install(CODE "

hironx_ros_bridge/cmake/rtmbuild_customed.cmake

@@ -0,0 +1,149 @@
+# Based on rtmbuild/cmake/rtmbuild.cmake
+cmake_minimum_required(VERSION 2.8.3)
+
+#set(DEBUG_RTMBUILD_CMAKE TRUE)
+
+set(use_catkin TRUE)
+
+# for rosbuild
+if(NOT COMMAND _rtmbuild_genbridge_init)
+  include(${rtmbuild_PACKAGE_PATH}/cmake/servicebridge.cmake)
+  set(use_catkin FALSE)
+endif()
+
+
+##
+## See rtmbuild/cmake/rtmbuild.cmake for GLOBAL VARIABLES
+##
+
+# add_custom_command to compile idl/*.idl file into c++
+# Change points from original rtmbuild_genidl:
+# - don't generate python
+# - don't overwrite lib of CORBA skeleton and stub from other pkgs to avoid compile error in those pkgs
+macro(rtmbuild_genidl_customed)
+  message("[rtmbuild_genidl_customed] add_custom_command for idl files in package ${PROJECT_NAME}")
+
+  set(_autogen "")
+
+  if (use_catkin)
+    set(_output_cpp_dir ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+    # don't overwrite lib of CORBA skeleton and stub from other pkgs to avoid compile error in those pkgs
+    # set(_output_lib_dir ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION})
+    set(_output_lib_dir ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}/${PROJECT_NAME})
+    # don't generate python
+    # set(_output_python_dir ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}/${PROJECT_NAME})
+    unset(_output_python_dir)
+  else()
+    set(_output_dir ${PROJECT_SOURCE_DIR}/idl_gen)
+    set(_output_cpp_dir ${PROJECT_SOURCE_DIR}/idl_gen/cpp/${PROJECT_NAME})
+    set(_output_lib_dir ${PROJECT_SOURCE_DIR}/idl_gen/lib)
+    # don't generate python
+    # set(_output_python_dir ${PROJECT_SOURCE_DIR}/src/${PROJECT_NAME})
+    unset(_output_python_dir)
+    include_directories(${PROJECT_SOURCE_DIR}/idl_gen/cpp/)
+  endif()
+
+  # don't generate python
+  # set(_output_idl_py_files "")
+  unset(_output_idl_py_files)
+  set(_output_idl_hh_files "")
+  file(MAKE_DIRECTORY ${_output_cpp_dir}/idl)
+  file(MAKE_DIRECTORY ${_output_lib_dir})
+  link_directories(${_output_lib_dir})
+
+  message("[rtmbuild_genidl_customed] - _output_cpp_dir : ${_output_cpp_dir}")
+  message("[rtmbuild_genidl_customed] - _output_lib_dir : ${_output_lib_dir}")
+  # don't generate python
+  # message("[rtmbuild_genidl] - _output_python_dir : ${_output_python_dir}")
+
+  ## RTMBUILD_${PROJECT_NAME}_genrpc) depends on each RTMBUILD_${PROJECT_NAME}_${_idl_name}_genrpc)
+  add_custom_target(RTMBUILD_${PROJECT_NAME}_genrpc)
+  if(NOT ${PROJECT_NAME}_idl_files)
+    message(AUTHOR_WARNING "[rtmbuild_genidl_customed] - no idl file is defined")
+  endif()
+  foreach(_idl_file ${${PROJECT_NAME}_idl_files})
+    get_filename_component(_idl_name ${_idl_file} NAME_WE)
+    message("[rtmbuild_genidl_customed] - _idl_file : ${_idl_file}")
+    message("[rtmbuild_genidl_customed] - _idl_name : ${_idl_name}")
+
+    # set(_input_idl ${PROJECT_SOURCE_DIR}/idl/${_idl})
+
+    set(_output_idl_hh ${_output_cpp_dir}/idl/${_idl_name}.hh)
+    # don't generate python
+    # set(_output_idl_py ${_output_python_dir}/${_idl_name}_idl.py)
+    unset(_output_idl_py)
+    set(_output_stub_h ${_output_cpp_dir}/idl/${_idl_name}Stub.h)
+    set(_output_skel_h ${_output_cpp_dir}/idl/${_idl_name}Skel.h)
+    set(_output_stub_cpp ${_output_cpp_dir}/idl/${_idl_name}Stub.cpp)
+    set(_output_skel_cpp ${_output_cpp_dir}/idl/${_idl_name}Skel.cpp)
+    set(_output_stub_lib ${_output_lib_dir}/lib${_idl_name}Stub.so)
+    set(_output_skel_lib ${_output_lib_dir}/lib${_idl_name}Skel.so)
+    list(APPEND ${PROJECT_NAME}_IDLLIBRARY_DIRS lib${_idl_name}Stub.so lib${_idl_name}Skel.so)
+    # call the  rule to compile idl
+    if(DEBUG_RTMBUILD_CMAKE)
+      message("[rtmbuild_genidl_customed] ${_output_idl_hh}\n -> ${_idl_file} ${${_idl}_depends}")
+      message("[rtmbuild_genidl_customed] ${_output_stub_cpp} ${_output_skel_cpp} ${_output_stub_h} ${_output_skel_h}\n -> ${_output_idl_hh}")
+      message("[rtmbuild_genidl_customed] ${_output_stub_lib} ${_output_skel_lib}\n -> ${_output_stub_cpp} ${_output_stub_h} ${_output_skel_cpp} ${_output_skel_h}")
+    endif()
+    # cpp
+    add_custom_command(OUTPUT ${_output_idl_hh}
+      COMMAND ${rtm_idlc} ${rtm_idlflags} -C${_output_cpp_dir}/idl ${_idl_file}
+      DEPENDS ${_idl_file})
+    add_custom_command(OUTPUT ${_output_stub_cpp} ${_output_skel_cpp} ${_output_stub_h} ${_output_skel_h}
+      COMMAND cp ${_idl_file} ${_output_cpp_dir}/idl
+      COMMAND rm -f ${_output_stub_cpp} ${_output_skel_cpp} ${_output_stub_h} ${_output_skel_h}
+      COMMAND ${rtmskel_EXECUTABLE} --include-dir="" --skel-suffix=Skel --stub-suffix=Stub  --idl-file=${_idl_file}
+      WORKING_DIRECTORY ${_output_cpp_dir}/idl
+      DEPENDS ${_output_idl_hh})
+    add_custom_command(OUTPUT ${_output_stub_lib} ${_output_skel_lib}
+      COMMAND ${rtm_cxx} ${rtm_cflags} -I. -shared -o ${_output_stub_lib} ${_output_stub_cpp} ${rtm_libs}
+      COMMAND ${rtm_cxx} ${rtm_cflags} -I. -shared -o ${_output_skel_lib} ${_output_skel_cpp} ${rtm_libs}
+      DEPENDS ${_output_stub_cpp} ${_output_stub_h} ${_output_skel_cpp} ${_output_skel_h})
+    list(APPEND ${PROJECT_NAME}_IDLLIBRARY_DIRS ${_output_stub_lib} ${_output_skel_lib})
+    if(use_catkin)
+      # don't overwrite lib of CORBA skeleton and stub from other pkgs to avoid compile error in those pkgs
+      # install(PROGRAMS ${_output_stub_lib} ${_output_skel_lib} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+      install(PROGRAMS ${_output_stub_lib} ${_output_skel_lib} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}/${PROJECT_NAME})
+    endif()
+    # python
+    # don't generate python
+    # list(APPEND _output_idl_py_files ${_output_idl_py})
+    # cpp
+    list(APPEND _output_idl_hh_files ${_output_idl_hh})
+    #
+    # don't generate python
+    # list(APPEND _autogen ${_output_stub_lib} ${_output_skel_lib} ${_output_idl_py})
+    list(APPEND _autogen ${_output_stub_lib} ${_output_skel_lib})
+
+    # add custom target
+    add_custom_target(RTMBUILD_${PROJECT_NAME}_${_idl_name}_genrpc DEPENDS ${_output_stub_lib} ${_output_skel_lib})
+    add_dependencies(RTMBUILD_${PROJECT_NAME}_genrpc RTMBUILD_${PROJECT_NAME}_${_idl_name}_genrpc)
+    # genrpc may depends on any idl (generate all .hh filesbefore compiling rpc https://github.com/fkanehiro/hrpsys-base/pull/886)
+    add_dependencies(RTMBUILD_${PROJECT_NAME}_${_idl_name}_genrpc RTMBUILD_${PROJECT_NAME}_genhh)
+
+  endforeach(_idl_file)
+  # python
+  # don't generate python
+  # add_custom_target(RTMBUILD_${PROJECT_NAME}_genpy DEPENDS ${_output_idl_py_files})
+  # add_custom_command(OUTPUT ${_output_idl_py_files}
+  #   COMMAND mkdir -p ${_output_python_dir}
+  #   COMMAND echo \"${rtm_idlc} -bpython -I${rtm_idldir} -C${_output_python_dir} ${${PROJECT_NAME}_idl_files}\"
+  #   COMMAND ${rtm_idlc} -bpython -I${rtm_idldir} -C${_output_python_dir} ${${PROJECT_NAME}_idl_files}
+  #   COMMENT "Generating python/idl from ${${PROJECT_NAME}_idl_files}"
+  #   DEPENDS ${${PROJECT_NAME}_idl_files})
+  # add_dependencies(RTMBUILD_${PROJECT_NAME}_genrpc RTMBUILD_${PROJECT_NAME}_genpy)
+  # cpp (generate all .hh filesbefore compiling rpc https://github.com/fkanehiro/hrpsys-base/pull/886)
+  add_custom_target(RTMBUILD_${PROJECT_NAME}_genhh DEPENDS ${_output_idl_hh_files})
+  add_dependencies(RTMBUILD_${PROJECT_NAME}_genrpc RTMBUILD_${PROJECT_NAME}_genhh)
+  ##
+
+  if(_autogen)
+    if(DEBUG_RTMBUILD_CMAKE)
+      message("[rtmbuild_genidl_customed] ADDITIONAL_MAKE_CLEAN_FILES : ${_autogen}")
+    endif()
+    # Also set up to clean the srv_gen directory
+    get_directory_property(_old_clean_files ADDITIONAL_MAKE_CLEAN_FILES)
+    list(APPEND _old_clean_files ${_autogen})
+    set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${_old_clean_files}")
+  endif(_autogen)
+endmacro(rtmbuild_genidl_customed)

hironx_ros_bridge/idl/AbsoluteForceSensorService.idl

@@ -0,0 +1,31 @@
+/**
+ * @file AbsoluteForceSensorService.idl
+ * @brief Services for the absolute force sensor interface
+ */
+module OpenHRP
+{
+  interface AbsoluteForceSensorService
+  {
+    typedef sequence<double, 3> DblSequence3;
+    struct forcemomentOffsetParam {
+      DblSequence3 force_offset;
+      DblSequence3 moment_offset;
+      DblSequence3 link_offset_centroid;
+      double link_offset_mass;
+    };
+
+    /**
+     * @brief set offset parameters.
+     * @param i_param new offset parameters
+     * @return true if set successfully, false otherwise
+     */
+    boolean setForceMomentOffsetParam(in string name, in forcemomentOffsetParam i_param);
+
+    /**
+     * @brief get offset parameters.
+     * @param name is name of the offset parameter set and i_param output offset parameters
+     * @return true if set successfully, false otherwise
+     */
+    boolean getForceMomentOffsetParam(in string name, out forcemomentOffsetParam i_param);
+  };
+};

hironx_ros_bridge/idl/ImpedanceControllerService.idl

@@ -0,0 +1,66 @@
+/**
+ * @file ImpedanceControllerService.idl
+ * @brief Services for the impedance interface
+ */
+module OpenHRP
+{
+
+  interface ImpedanceControllerService
+  {
+    typedef sequence<double, 3> DblSequence3;
+
+    struct impedanceParam {
+      string name;
+      string base_name;
+      string target_name;
+      double M_p;
+      double D_p;
+      double K_p;
+      double M_r;
+      double D_r;
+      double K_r;
+      DblSequence3 ref_force;
+      DblSequence3 force_gain;
+      DblSequence3 ref_moment;
+      DblSequence3 moment_gain;
+      double sr_gain;
+      double avoid_gain;
+      double reference_gain;
+      double manipulability_limit;
+    };
+
+    /**
+     * @brief set impedance parameters.
+     * @param i_param new impedance parameters
+     * @return true if set successfully, false otherwise
+     */
+    boolean setImpedanceControllerParam(in impedanceParam i_param);
+
+    /**
+     * @brief get impedance parameters.
+     * @param name is name of the impedance parameter set and i_param output impedance parameters
+     * @return true if set successfully, false otherwise
+     */
+    boolean getImpedanceControllerParam(in string name, out impedanceParam i_param);
+
+    /**
+     * @brief remove impedance parameters.
+     * @param name name of the impedance parameter set
+     * @return true if set successfully, false otherwise
+     */
+    boolean deleteImpedanceController(in string name);
+
+    /**
+     * @brief wait to finish deleting the impedance param object.
+     * @param name name of the impedance parameter set
+     */
+    void waitDeletingImpedanceController(in string name);
+
+    /**
+     * @brief remove impedance parameters and wait to finish deleting the impedance param object.
+     * @param name name of the impedance parameter set
+     * @return true if set successfully, false otherwise
+     */
+    boolean deleteImpedanceControllerAndWait(in string name);
+  };
+};

hironx_ros_bridge/launch/hironx_ros_bridge.launch

@@ -34,12 +34,20 @@
     <!-- In hrpsys_ros_bridge.launch, Collision check might be turned on -->
     <arg name="USE_COLLISIONCHECK" value="$(arg USE_COLLISIONCHECK)" />
     <arg name="USE_ROBOTHARDWARE" value="$(arg USE_ROBOTHARDWARE)" />
-    <arg name="USE_IMPEDANCECONTROLLER" value="$(arg USE_IMPEDANCECONTROLLER)" />
+    <arg name="USE_IMPEDANCECONTROLLER" value="false" />
     <arg name="USE_SOFTERRORLIMIT" value="false" />
     <arg name="USE_SERVOCONTROLLER" value="$(arg USE_SERVOCONTROLLER)" />
 
     <arg name="corbaport" default="$(arg corbaport)" />
   </include>
+
+  <!-- Launch ros bridge for old impedance controller in HiroNX -->
+  <include file="$(find hironx_ros_bridge)/launch/impedance_controller.launch" if="$(arg USE_IMPEDANCECONTROLLER)">
+    <arg name="corbaport" default="$(arg corbaport)" />
+    <arg name="nameserver" value="$(arg nameserver)" />
+    <arg name="SIMULATOR_NAME" value="$(arg SIMULATOR_NAME)" />
+  </include>
+
 <!--
   <node name="hironx_rviz" pkg="rviz" type="rviz" respawn="true"
   args="-sync -d $(find hrpsys_ros_bridge_tutorials)/launch/hironx.vcg"

hironx_ros_bridge/launch/hironx_ros_bridge_real.launch

@@ -7,7 +7,7 @@
   <env name="RTCTREE_NAMESERVERS" value="$(arg nameserver):$(arg corbaport)" />
   <arg name="USE_ROBOTHARDWARE" default="true" />
   <arg name="USE_SERVOCONTROLLER" default="true" />
-  <arg name="USE_IMPEDANCECONTROLLER" default="false"/>
+  <arg name="USE_IMPEDANCECONTROLLER" default="true"/>
   <arg name="USE_COLLISIONCHECK" default="false"/>
   <arg name="USE_HAND_JOINT_STATE_PUBLISHER" default="true"/>
 

hironx_ros_bridge/launch/impedance_controller.launch

@@ -0,0 +1,23 @@
+<!-- Launch ros bridge for old impedance controller in HiroNX -->
+<launch>
+  <arg name="nameserver" default="localhost" />
+  <arg name="corbaport" default="15005" />
+  <arg name="SIMULATOR_NAME" default="RobotHardware0"/>
+  <env name="SIMULATOR_NAME" value="$(arg SIMULATOR_NAME)" />
+  <env name="RTCTREE_NAMESERVERS" value="$(arg nameserver):$(arg corbaport)" />
+
+  <arg name="omniorb_args" default="-ORBInitRef NameService=corbaloc:iiop:$(arg nameserver):$(arg corbaport)/NameService" />
+  <arg name="openrtm_args" default='-o "corba.nameservers:$(arg nameserver):$(arg corbaport)" -o "naming.formats:%n.rtc" -o "exec_cxt.periodic.type:PeriodicExecutionContext" -o "exec_cxt.periodic.rate:2000" -o "logger.file_name:/tmp/rtc%p.log"' />
+
+  <!-- Launch ImpedanceController and AbsoluteForceSensor -->
+  <node pkg="hironx_ros_bridge" name="ImpedanceControllerServiceROSBridge" type="ImpedanceControllerServiceROSBridgeComp"
+        output="screen" args ="$(arg openrtm_args)" />
+  <node pkg="hironx_ros_bridge" name="AbsoluteForceSensorServiceROSBridge" type="AbsoluteForceSensorServiceROSBridgeComp"
+        output="screen" args="$(arg openrtm_args)" />
+  <rtconnect from="ImpedanceControllerServiceROSBridge.rtc:ImpedanceControllerService" to="ic.rtc:ImpedanceControllerService"  subscription_type="new"/>
+  <rtactivate component="ImpedanceControllerServiceROSBridge.rtc" />
+  <rtconnect from="AbsoluteForceSensorServiceROSBridge.rtc:AbsoluteForceSensorService" to="rmfo.rtc:AbsoluteForceSensorService"  subscription_type="new"/>
+  <rtactivate component="AbsoluteForceSensorServiceROSBridge.rtc" />
+  <node name="rtmlaunch_impedance_controller" pkg="openrtm_tools" type="rtmlaunch.py"
+        args="$(find hironx_ros_bridge)/launch/impedance_controller.launch" />
+</launch>

hironx_ros_bridge/package.xml

@@ -29,6 +29,7 @@
   <build_depend>rosbash</build_depend>
   <build_depend>rosbuild</build_depend>
   <build_depend>roslang</build_depend>
+  <build_depend>rtmbuild</build_depend>
   <build_depend>unzip</build_depend>
   <build_depend>gnuplot</build_depend>