diff --git a/hironx_ros_bridge/.gitignore b/hironx_ros_bridge/.gitignore new file mode 100644 index 00000000..a6c00251 --- /dev/null +++ b/hironx_ros_bridge/.gitignore @@ -0,0 +1,9 @@ +# For rtmbuild +msg/OpenHRP_* +srv/OpenHRP_* +src_gen + +# For others +conf/*.conf +conf/*.xml +*.pyc diff --git a/hironx_ros_bridge/CMakeLists.txt b/hironx_ros_bridge/CMakeLists.txt index 260dbb29..b4790c7f 100644 --- a/hironx_ros_bridge/CMakeLists.txt +++ b/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 " diff --git a/hironx_ros_bridge/cmake/rtmbuild_customed.cmake b/hironx_ros_bridge/cmake/rtmbuild_customed.cmake new file mode 100644 index 00000000..1dc51c59 --- /dev/null +++ b/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) diff --git a/hironx_ros_bridge/idl/AbsoluteForceSensorService.idl b/hironx_ros_bridge/idl/AbsoluteForceSensorService.idl new file mode 100644 index 00000000..227fe601 --- /dev/null +++ b/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 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); + }; +}; diff --git a/hironx_ros_bridge/idl/ImpedanceControllerService.idl b/hironx_ros_bridge/idl/ImpedanceControllerService.idl new file mode 100644 index 00000000..009e7e9e --- /dev/null +++ b/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 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); + }; +}; diff --git a/hironx_ros_bridge/launch/hironx_ros_bridge.launch b/hironx_ros_bridge/launch/hironx_ros_bridge.launch index d1efed4a..80b6548a 100644 --- a/hironx_ros_bridge/launch/hironx_ros_bridge.launch +++ b/hironx_ros_bridge/launch/hironx_ros_bridge.launch @@ -34,12 +34,20 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hironx_ros_bridge/package.xml b/hironx_ros_bridge/package.xml index f4716c6b..27c6d054 100644 --- a/hironx_ros_bridge/package.xml +++ b/hironx_ros_bridge/package.xml @@ -29,8 +29,10 @@ rosbash rosbuild roslang + rtmbuild unzip gnuplot + message_generation gnuplot moveit_commander