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..9a964966 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_from_dir.cmake) + +# initialize rtmbuild for old impedance controller +rtmbuild_init_from_dir(idl_315_1_9) + +# call catkin_package, after rtmbuild_init, before rtmbuild_gen* catkin_package( CATKIN_DEPENDS hrpsys_ros_bridge roslib # INCLUDE_DIRS include + CFG_EXTRAS rtmbuild_from_dir.cmake ) -catkin_python_setup() +# generate idl for old impedance controller +rtmbuild_genidl_from_dir(idl_315_1_9 315_1_9) + +# 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_315_1_9 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_from_dir.cmake b/hironx_ros_bridge/cmake/rtmbuild_from_dir.cmake new file mode 100644 index 00000000..465e5854 --- /dev/null +++ b/hironx_ros_bridge/cmake/rtmbuild_from_dir.cmake @@ -0,0 +1,345 @@ +# 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() + + +## +## GLOBAL VARIABLES +## +## openrtm_aist_INCLUDE_DIRS +## openrtm_aist_LIBRARIES +## openhrp3_INCLUDE_DIRS +## openhrp3_LIBRARIES +## idl2srv_EXECUTABLE +## rtmskel_EXECUTABLE +## ${PROJECT_NAME}_idl_files +## ${PROJECT_NAME}_autogen_files +## ${PROJECT_NAME}_autogen_msg_files +## ${PROJECT_NAME}_autogen_srv_files +## ${PROJECT_NAME}_autogen_interfaces +## rtm_idlc, rtm_idlflags, rtm_idldir +## rtm_cxx, rtm_cflags +## hrp_idldir + + +# Based on _rtmbuild_get_idls in rtmbuild/cmake/servicebridge.cmake +# Change points from original _rtmbuild_get_idls: +# - get idl dir name as argument +# get idl dir name as argument +# macro(_rtmbuild_get_idls) +macro(_rtmbuild_get_idls_from_dir _idl_dir) + # get idl dir name as argument + # file(GLOB _idl_files "${PROJECT_SOURCE_DIR}/idl/*.idl") ## get full path + file(GLOB _idl_files "${PROJECT_SOURCE_DIR}/${_idl_dir}/*.idl") ## get full path + foreach(_idl_file ${_idl_files}) + # copy from rosbuild_get_msgs to avoid .#Foo.idl, by emacs + if(${_idl_file} MATCHES "/[^\\.]+\\.idl$") + list(APPEND ${PROJECT_NAME}_idl_files ${_idl_file}) + endif() + endforeach(_idl_file) +endmacro(_rtmbuild_get_idls_from_dir) + +# +# setup global variables +# +# Change points from original rtmbuild_init: +# - get idl dir name as argument +# - use idl2srv.py in hironx_ros_bridge +# - use _rtmbuild_get_idls_from_dir instead of _rtmbuild_get_idls +# get idl dir name as argument +# macro(rtmbuild_init) +macro(rtmbuild_init_from_dir _idl_dir) + # get idl dir name as argument + # set(_extra_message_dependencies ${ARGV0}) + set(_extra_message_dependencies ${ARGV1}) + if(NOT use_catkin) ## rosbuild_init cleans all variable so we first defind project and call rosbuild_init later with ROSBUILD_DONT_REDEFINE_PROJECT=TRUE + get_filename_component(_project ${CMAKE_SOURCE_DIR} NAME) + project(${_project}) + + rosbuild_find_ros_package(openrtm_aist) + rosbuild_find_ros_package(openhrp3) + set(ENV{PKG_CONFIG_PATH} ${openrtm_aist_PACKAGE_PATH}/lib/pkgconfig:${openhrp3_PACKAGE_PATH}/lib/pkgconfig:$ENV{PKG_CONFIG_PATH}) + message("[rtmbuild_init] - ENV{PKG_CONFIG_PATH} - > $ENV{PKG_CONFIG_PATH}") + endif() + # + # use pkg-config to set --cflags --libs plus rtm-related flags + # + find_package(PkgConfig) + pkg_check_modules(openrtm_aist openrtm-aist REQUIRED) + pkg_check_modules(openhrp3 openhrp3.1) + message("[rtmbuild_init] Building package ${CMAKE_SOURCE_DIR} ${PROJECT_NAME}") + message("[rtmbuild_init] - CATKIN_TOPLEVEL = ${CATKIN_TOPLEVEL}") + if(DEBUG_RTMBUILD_CMAKE) + message("[rtmbuild_init] - openrtm_aist_INCLUDE_DIRS -> ${openrtm_aist_INCLUDE_DIRS}") + message("[rtmbuild_init] - openrtm_aist_LIBRARIES -> ${openrtm_aist_LIBRARIES}") + message("[rtmbuild_init] - openhrp3_INCLUDE_DIRS -> ${openhrp3_INCLUDE_DIRS}") + message("[rtmbuild_init] - openhrp3_LIBRARIES -> ${openhrp3_LIBRARIES}") + endif() + + # use idl2srv.py in hironx_ros_bridge + # if(EXISTS ${rtmbuild_SOURCE_PREFIX}) # catkin + # set(idl2srv_EXECUTABLE ${rtmbuild_SOURCE_PREFIX}/scripts/idl2srv.py) + # elseif(EXISTS ${rtmbuild_PACKAGE_PATH}) ## for rosbuild + # set(idl2srv_EXECUTABLE ${rtmbuild_PACKAGE_PATH}/scripts/idl2srv.py) + # else() + # pkg_check_modules(rtmbuild rtmbuild REQUIRED) + # set(idl2srv_EXECUTABLE ${rtmbuild_PREFIX}/share/rtmbuild/scripts/idl2srv.py) + # endif() + set(idl2srv_EXECUTABLE ${PROJECT_SOURCE_DIR}/scripts/idl2srv.py) + message("[rtmbuild_init] - idl2srv_EXECUTABLE -> ${idl2srv_EXECUTABLE}") + + execute_process(COMMAND pkg-config openrtm-aist --variable=prefix OUTPUT_VARIABLE rtm_prefix OUTPUT_STRIP_TRAILING_WHITESPACE) + if(EXISTS ${rtm_prefix}/bin/rtm-skelwrapper) + set(_rtm_exe_path ${rtm_prefix}/bin) + else() + set(_rtm_exe_path ${rtm_prefix}/lib/openrtm_aist/bin) + endif() + set(rtmskel_EXECUTABLE PATH=${_rtm_exe_path}:$ENV{PATH} PYTHONPATH=${openrtm_aist_PREFIX}/lib/openrtm-1.1/py_helper:$ENV{PYTHONPATH} ${_rtm_exe_path}/rtm-skelwrapper) + message("[rtmbuild_init] - rtmskel_EXECUTABLE -> ${rtmskel_EXECUTABLE}") + + execute_process(COMMAND pkg-config openrtm-aist --variable=rtm_idlc OUTPUT_VARIABLE rtm_idlc OUTPUT_STRIP_TRAILING_WHITESPACE) + execute_process(COMMAND pkg-config openrtm-aist --variable=rtm_idlflags OUTPUT_VARIABLE rtm_idlflags OUTPUT_STRIP_TRAILING_WHITESPACE) + set(rtm_idlflags "${rtm_idlflags} -Wbuse_quotes") # IDLs in hrpsys-base needs this option because of https://github.com/start-jsk/rtmros_common/issues/861. We can remove this after openrtm-aist.pc is updated. + execute_process(COMMAND pkg-config openrtm-aist --variable=rtm_idldir OUTPUT_VARIABLE rtm_idldir OUTPUT_STRIP_TRAILING_WHITESPACE) + execute_process(COMMAND pkg-config openrtm-aist --variable=rtm_cxx OUTPUT_VARIABLE rtm_cxx OUTPUT_STRIP_TRAILING_WHITESPACE) + execute_process(COMMAND pkg-config openrtm-aist --variable=rtm_cflags OUTPUT_VARIABLE rtm_cflags OUTPUT_STRIP_TRAILING_WHITESPACE) + execute_process(COMMAND pkg-config openrtm-aist --variable=rtm_libs OUTPUT_VARIABLE rtm_libs OUTPUT_STRIP_TRAILING_WHITESPACE) + execute_process(COMMAND pkg-config openhrp3.1 --variable=idl_dir OUTPUT_VARIABLE hrp_idldir OUTPUT_STRIP_TRAILING_WHITESPACE) + separate_arguments(rtm_idlflags) + separate_arguments(rtm_cflags) + separate_arguments(rtm_libs) + set(rtm_cxx "c++") ## openrtm-aist --variable=rtm_cxx sometimes returns /usr/lib/ccache/c++ + message("[rtmbuild_init] - rtm_idlc -> ${rtm_idlc}") + message("[rtmbuild_init] - rtm_idlflags -> ${rtm_idlflags}") + message("[rtmbuild_init] - rtm_idldir -> ${rtm_idldir}") + message("[rtmbuild_init] - rtm_cxx -> ${rtm_cxx}") + message("[rtmbuild_init] - rtm_cflags -> ${rtm_cflags}") + message("[rtmbuild_init] - rtm_libs -> ${rtm_libs}") + message("[rtmbuild_init] - hrp_idldir -> ${hrp_idldir}") + + ## + ## get idl files and store to _idl_list + message("[rtmbuild_init] Generating bridge compornents from ${PROJECT_SOURCE_DIR}/idl") + set(${PROJECT_NAME}_idl_files "") + # use _rtmbuild_get_idls_from_dir instead of _rtmbuild_get_idls + # _rtmbuild_get_idls() ## set ${PROJECT_NAME}_idl_files + _rtmbuild_get_idls_from_dir(${_idl_dir}) ## set ${PROJECT_NAME}_idl_files + message("[rtmbuild_init] - ${PROJECT_NAME}_idl_files : ${${PROJECT_NAME}_idl_files}") + if(NOT ${PROJECT_NAME}_idl_files) + message(AUTHOR_WARNING "[rtmbuild_init] - no idl file is defined") + endif() + + ## generate msg/srv/cpp from idl + set(${PROJECT_NAME}_autogen_msg_files "") + set(${PROJECT_NAME}_autogen_srv_files "") + _rtmbuild_genbridge_init() + message("[rtmbuild_init] - ${PROJECT_NAME}_autogen_msg_files : ${${PROJECT_NAME}_autogen_msg_files}") + message("[rtmbuild_init] - ${PROJECT_NAME}_autogen_srv_files : ${${PROJECT_NAME}_autogen_srv_files}") + message("[rtmbuild_init] - ${PROJECT_NAME}_autogen_interfaces : ${${PROJECT_NAME}_autogen_interfaces}") + set(rtmbuild_${PROJECT_NAME}_autogen_msg_files ${${PROJECT_NAME}_autogen_msg_files}) + + ## + ## rosbulid_init for rosbuild + if(NOT use_catkin) + set(ROSBUILD_DONT_REDEFINE_PROJECT TRUE) + rosbuild_init() + endif() + + if(use_catkin) + add_message_files(DIRECTORY msg FILES "${${PROJECT_NAME}_autogen_msg_files}") + add_service_files(DIRECTORY srv FILES "${${PROJECT_NAME}_autogen_srv_files}") + generate_messages(DEPENDENCIES std_msgs ${_extra_message_dependencies}) + else() + rosbuild_genmsg() + rosbuild_gensrv() + endif() + + include_directories(${catkin_INCLUDE_DIRS} ${openrtm_aist_INCLUDE_DIRS} ${openhrp3_INCLUDE_DIRS}) + link_directories(${catkin_LIBRARY_DIRS} ${openrtm_aist_LIBRARY_DIRS} ${openhrp3_LIBRARY_DIRS}) + +endmacro(rtmbuild_init_from_dir) + +# add_custom_command to compile idl/*.idl file into c++ +# Change points from original rtmbuild_genidl: +# - use idl dir name given as argument +# - add suffix given as argument to lib of CORBA skeleton and stub +# - generate idl python in dir given as argument +# use idl dir name given as argument +# add suffix given as argument to lib of CORBA skeleton and stub +# generate idl python in dir given as argument +# macro(rtmbuild_genidl) +macro(rtmbuild_genidl_from_dir _idl_dir _lib_suffix) + message("[rtmbuild_genidl_from_dir] 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}) + set(_output_lib_dir ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}) + # generate idl python in dir given as argument + # set(_output_python_dir ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}/${PROJECT_NAME}) + set(_output_idl_py_dir ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}/${PROJECT_NAME}/${_idl_dir}) + else() + # use idl dir name given as argument + # 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) + set(_output_dir ${PROJECT_SOURCE_DIR}/${_idl_dir}_gen) + set(_output_cpp_dir ${PROJECT_SOURCE_DIR}/${_idl_dir}_gen/cpp/${PROJECT_NAME}) + set(_output_lib_dir ${PROJECT_SOURCE_DIR}/${_idl_dir}_gen/lib) + # generate idl python in dir given as argument + # set(_output_python_dir ${PROJECT_SOURCE_DIR}/src/${PROJECT_NAME}) + set(_output_idl_py_dir ${PROJECT_SOURCE_DIR}/src/${PROJECT_NAME}/${_idl_dir}) + # use idl dir name given as argument + # include_directories(${PROJECT_SOURCE_DIR}/idl_gen/cpp/) + include_directories(${PROJECT_SOURCE_DIR}/${_idl_dir}_gen/cpp/) + endif() + + set(_output_idl_py_files "") + set(_output_idl_hh_files "") + # use idl dir name given as argument + # file(MAKE_DIRECTORY ${_output_cpp_dir}/idl) + file(MAKE_DIRECTORY ${_output_cpp_dir}/${_idl_dir}) + file(MAKE_DIRECTORY ${_output_lib_dir}) + link_directories(${_output_lib_dir}) + + message("[rtmbuild_genidl_from_dir] - _output_cpp_dir : ${_output_cpp_dir}") + message("[rtmbuild_genidl_from_dir] - _output_lib_dir : ${_output_lib_dir}") + # generate idl python in dir given as argument + # message("[rtmbuild_genidl] - _output_python_dir : ${_output_python_dir}") + message("[rtmbuild_genidl_from_dir] - _output_idl_py_dir: ${_output_idl_py_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_from_dir] - 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_from_dir] - _idl_file : ${_idl_file}") + message("[rtmbuild_genidl_from_dir] - _idl_name : ${_idl_name}") + + ## use idl dir name given as argument + ## set(_input_idl ${PROJECT_SOURCE_DIR}/idl/${_idl}) + # set(_input_idl ${PROJECT_SOURCE_DIR}/${_idl_dir}/${_idl}) + + # use idl dir name given as argument + # set(_output_idl_hh ${_output_cpp_dir}/idl/${_idl_name}.hh) + set(_output_idl_hh ${_output_cpp_dir}/${_idl_dir}/${_idl_name}.hh) + # generate idl python in dir given as argument + # set(_output_idl_py ${_output_python_dir}/${_idl_name}_idl.py) + set(_output_idl_py ${_output_idl_py_dir}/${_idl_name}_idl.py) + # use idl dir name given as argument + # 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_h ${_output_cpp_dir}/${_idl_dir}/${_idl_name}Stub.h) + set(_output_skel_h ${_output_cpp_dir}/${_idl_dir}/${_idl_name}Skel.h) + set(_output_stub_cpp ${_output_cpp_dir}/${_idl_dir}/${_idl_name}Stub.cpp) + set(_output_skel_cpp ${_output_cpp_dir}/${_idl_dir}/${_idl_name}Skel.cpp) + # add suffix given as argument to lib of CORBA skeleton and stub + # 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) + set(_output_stub_lib ${_output_lib_dir}/lib${_idl_name}Stub_${_lib_suffix}.so) + set(_output_skel_lib ${_output_lib_dir}/lib${_idl_name}Skel_${_lib_suffix}.so) + list(APPEND ${PROJECT_NAME}_IDLLIBRARY_DIRS lib${_idl_name}Stub_${_lib_suffix}.so lib${_idl_name}Skel_${_lib_suffix}.so) + # call the rule to compile idl + if(DEBUG_RTMBUILD_CMAKE) + message("[rtmbuild_genidl_from_dir] ${_output_idl_hh}\n -> ${_idl_file} ${${_idl}_depends}") + message("[rtmbuild_genidl_from_dir] ${_output_stub_cpp} ${_output_skel_cpp} ${_output_stub_h} ${_output_skel_h}\n -> ${_output_idl_hh}") + message("[rtmbuild_genidl_from_dir] ${_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} + # use idl dir name given as argument + # COMMAND ${rtm_idlc} ${rtm_idlflags} -C${_output_cpp_dir}/idl ${_idl_file} + COMMAND ${rtm_idlc} ${rtm_idlflags} -C${_output_cpp_dir}/${_idl_dir} ${_idl_file} + DEPENDS ${_idl_file}) + add_custom_command(OUTPUT ${_output_stub_cpp} ${_output_skel_cpp} ${_output_stub_h} ${_output_skel_h} + # use idl dir name given as argument + # COMMAND cp ${_idl_file} ${_output_cpp_dir}/idl + COMMAND cp ${_idl_file} ${_output_cpp_dir}/${_idl_dir} + 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} + # use idl dir name given as argument + # WORKING_DIRECTORY ${_output_cpp_dir}/idl + WORKING_DIRECTORY ${_output_cpp_dir}/${_idl_dir} + 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) + install(PROGRAMS ${_output_stub_lib} ${_output_skel_lib} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + endif() + # python + list(APPEND _output_idl_py_files ${_output_idl_py}) + # cpp + list(APPEND _output_idl_hh_files ${_output_idl_hh}) + # + list(APPEND _autogen ${_output_stub_lib} ${_output_skel_lib} ${_output_idl_py}) + + # 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 + # generate idl python in dir given as argument + # 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) + string(RANDOM _rand_str) + set(_rtm_idlc_py_dir /tmp/rtm_idlc_py_${PROJECT_NAME}_${_rand_str}) + set(_rtm_idlc_idl_py_files "") + foreach(_output_idl_py ${_output_idl_py_files}) + string(REPLACE ${_output_idl_py_dir} ${_rtm_idlc_py_dir} _rtm_idlc_idl_py ${_output_idl_py}) + list(APPEND _rtm_idlc_idl_py_files ${_rtm_idlc_idl_py}) + endforeach(_output_idl_py) + set(_output_idl_py_dir_init "${_output_idl_py_dir}/__init__.py") + add_custom_target(RTMBUILD_${PROJECT_NAME}_genpy DEPENDS ${_output_idl_py_files} ${_output_idl_py_dir_init}) + add_custom_command(OUTPUT ${_output_idl_py_files} ${_output_idl_py_dir_init} + COMMAND mkdir -p ${_rtm_idlc_py_dir} + COMMAND echo \"${rtm_idlc} -bpython -I${rtm_idldir} -C${_rtm_idlc_py_dir} ${${PROJECT_NAME}_idl_files}\" + COMMAND ${rtm_idlc} -bpython -I${rtm_idldir} -C${_rtm_idlc_py_dir} ${${PROJECT_NAME}_idl_files} + COMMAND mkdir -p ${_output_idl_py_dir} + COMMAND touch ${_output_idl_py_dir_init} + COMMAND cp ${_rtm_idlc_idl_py_files} ${_output_idl_py_dir} + COMMAND rm -rf ${_rtm_idlc_py_dir} + 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_from_dir] 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_from_dir) diff --git a/hironx_ros_bridge/idl_315_1_9/AbsoluteForceSensorService.idl b/hironx_ros_bridge/idl_315_1_9/AbsoluteForceSensorService.idl new file mode 100644 index 00000000..227fe601 --- /dev/null +++ b/hironx_ros_bridge/idl_315_1_9/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_315_1_9/ImpedanceControllerService.idl b/hironx_ros_bridge/idl_315_1_9/ImpedanceControllerService.idl new file mode 100644 index 00000000..009e7e9e --- /dev/null +++ b/hironx_ros_bridge/idl_315_1_9/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..087729dc 100644 --- a/hironx_ros_bridge/package.xml +++ b/hironx_ros_bridge/package.xml @@ -29,6 +29,7 @@ rosbash rosbuild roslang + rtmbuild unzip gnuplot diff --git a/hironx_ros_bridge/scripts/idl2srv.py b/hironx_ros_bridge/scripts/idl2srv.py new file mode 100755 index 00000000..477285b0 --- /dev/null +++ b/hironx_ros_bridge/scripts/idl2srv.py @@ -0,0 +1,715 @@ +#!/usr/bin/env python + +# Based on rtmbuild/scripts/idl2srv.py +# Change points from original idl2srv.py: +# - allow idl dir name with version + +from optparse import OptionParser +import os, os.path, sys, string, re + +# resolve library path, copied from omniidl (ubuntu 11.10) +# Try a path based on the installation prefix, customised for Debian +sppath = "/usr/lib/omniidl" +if os.path.isdir(sppath): + sys.path.append(sppath) + +from omniidl import idlast, idlvisitor, idlutil, idltype +from omniidl_be import cxx +import _omniidl + +# TODO +# not generate unused msgs +# how to manipulate namespace -> under score concat + +TypeNameMap = { # for ROS msg/srv + idltype.tk_boolean: 'bool', + idltype.tk_char: 'int8', + idltype.tk_octet: 'uint8', + idltype.tk_wchar: 'int16', + idltype.tk_short: 'int16', + idltype.tk_ushort: 'uint16', + idltype.tk_long: 'int32', + idltype.tk_ulong: 'uint32', + idltype.tk_longlong: 'int64', + idltype.tk_ulonglong: 'uint64', + idltype.tk_float: 'float32', + idltype.tk_double: 'float64', + idltype.tk_string: 'string', + idltype.tk_wstring: 'string', + idltype.tk_any: 'string', # ?? + idltype.tk_TypeCode: 'uint64', # ?? + idltype.tk_enum: 'uint64'} + +MultiArrayTypeNameMap = { # for ROS msg/srv + idltype.tk_char: 'std_msgs/Int8MultiArray', + idltype.tk_octet: 'std_msgs/Uint8MultiArray', + idltype.tk_wchar: 'std_msgs/Int16MultiArray', + idltype.tk_short: 'std_msgs/Int16MultiArray', + idltype.tk_ushort: 'std_msgs/Uint16MultiArray', + idltype.tk_long: 'std_msgs/Int32MultiArray', + idltype.tk_ulong: 'std_msgs/Uint32MultiArray', + idltype.tk_longlong: 'std_msgs/Int64MultiArray', + idltype.tk_ulonglong:'std_msgs/Uint64MultiArray', + idltype.tk_float: 'std_msgs/Float32MultiArray', + idltype.tk_double: 'std_msgs/Float64MultiArray', + idltype.tk_string: 'openrtm_ros_bridge/StringMultiArray'} + +# convert functions for IDL/ROS +# _CORBA_String_element type is env depend ?? +convert_functions = """\n +template +void genseq(S& s, int size, _CORBA_Unbounded_Sequence* p){ + s = S(size,size,S::allocbuf(size), 1);} +template +void genseq(S& s, int size, _CORBA_Bounded_Sequence* p){ + s = S(size,S::allocbuf(size), 1);} +template inline +void convert(T1& in, T2& out){ out = static_cast(in); } +template +void convert(T& in, std::string& out){ out = std::string(in); } +template +void convert(std::string& in, T& out){ out = static_cast(in.c_str()); } +void convert(_CORBA_String_element in, std::string& out){ out = std::string(in); } +void convert(std::string& in, _CORBA_String_element out){ out = (const char*)in.c_str(); } +template +void convert(S& s, std::vector& v){ + int size = s.length(); + v = std::vector(s.length()); + for(int i=0; i +void convert(std::vector& v, S& s){ + int size = v.size(); + s = S(size, size, S::allocbuf(size), 1); + for(int i=0; i +void convert(S& s, boost::array& v){ + for(std::size_t i=0; i +void convert(boost::array& v, S& s){ + s = S(n, S::allocbuf(n), 1); + for(std::size_t i=0; i +void convert(boost::array& v, S (&s)[n]){ + for(std::size_t i=0; i void convert(T& in, RTC::LightweightRTObject_var out){ std::cerr << "convert from RTC::LightweightRTObject_var is not supported" << std::endl; } +""" + +multiarray_conversion = """ +template // convert multi-dimensional array +void convert(S& s, %s& v){ + if(v.layout.dim.size()==0 || v.layout.dim[v.layout.dim.size()-1].size==0) { + int level = v.layout.dim.size(); + v.layout.dim.push_back(std_msgs::MultiArrayDimension()); + v.layout.dim[level].stride = 1; + for(uint i=0; i +void convert(%s& s, %s& v){ v.data.push_back(s); } +template +void convert(%s& v, S& s){ + int level, size; + for(level=0; (size=v.layout.dim[level].size)==0; level++); + genseq(s, size, &s); + v.layout.dim[level].size = 0; + for(uint i=0; i +void convert(%s& v, %s& s){ convert(v.data[v.layout.data_offset++], s); } +""" # % (std_msgs::Float64MultiArray, double, std_msgs::Float64MultiArray) + (std_msgs::Float64MultiArray, std_msgs::Float64MultiArray, double) + +# symbol type constant +NOT_FULL = 0 +ROS_FULL = 1 +CPP_FULL = 2 + +# visitor for generate msg/srv/cpp/h for bridge compornents +class ServiceVisitor (idlvisitor.AstVisitor): + + def __init__(self): + self.generated_msgs = [] # type objects + + def visitAST(self, node): + for n in node.declarations(): + n.accept(self) + def visitModule(self, node): + for n in node.definitions(): + n.accept(self) + def visitInterface(self, node): + self.outputMsg(node) + for c in node.contents(): + c.accept(self) + if node.mainFile(): + self.genBridgeComponent(node) + + def visitOperation(self, node): + if node.mainFile(): + self.outputSrv(node) + for n in node.parameters(): + self.outputMsg(n.paramType()) + n.accept(self) + self.outputMsg(node.returnType()) +## +## +## + def getCppTypeText(self, typ, out=False, full=NOT_FULL): + if isinstance(typ, idltype.Base): + return cxx.types.basic_map[typ.kind()] + if isinstance(typ, idltype.String): + return ('char*' if out else 'const char*') # ?? + if isinstance(typ, idltype.Declared): + postfix = ('*' if out and cxx.types.variableDecl(typ.decl()) else '') + return self.getCppTypeText(typ.decl(), False, full) + postfix + + if isinstance(typ, idlast.Struct): + if full == CPP_FULL: + name = idlutil.ccolonName(typ.scopedName()) + elif full == ROS_FULL: + return '_'.join(typ.scopedName()) # return + else: + name = typ.identifier() + return name + ('*' if out and cxx.types.variableDecl(typ) else '') + if isinstance(typ, idlast.Enum) or \ + isinstance(typ, idlast.Interface) or \ + isinstance(typ, idlast.Operation): + if full == CPP_FULL: + if ( idlutil.ccolonName(typ.scopedName()) == "RTC::LightweightRTObject") : + return idlutil.ccolonName(typ.scopedName())+"_var" + else: + return idlutil.ccolonName(typ.scopedName()) + elif full == ROS_FULL: + return '_'.join(typ.scopedName()) + else: + return typ.identifier() + if isinstance(typ, idlast.Typedef): + if full == CPP_FULL: + return idlutil.ccolonName(typ.declarators()[0].scopedName()) + elif full == ROS_FULL: + return '_'.join(typ.declarators()[0].scopedName()) + else: + return typ.declarators()[0].identifier() + if isinstance(typ, idlast.Declarator): + return self.getCppTypeText(typ.alias(), out, full) + + return 'undefined' + + + def getROSTypeText(self, typ): + if isinstance(typ, idltype.Base): + return TypeNameMap[typ.kind()] + if isinstance(typ, idltype.String) or isinstance(typ, idltype.WString): + return 'string' # ?? + if isinstance(typ, idltype.Sequence): + etype = typ.seqType() # n-dimensional array -> 1-dimensional + size = typ.bound() + dim = 1 + while not (isinstance(etype, idltype.Base) or isinstance(etype, idltype.String) or isinstance(etype, idltype.WString) or isinstance(etype, idlast.Struct) or isinstance(etype, idlast.Interface) or isinstance(etype, idlast.Enum) or isinstance(etype, idlast.Forward)): + if isinstance(etype, idltype.Declared): + etype = etype.decl() + elif isinstance(etype, idltype.Sequence): + dim += 1 + size *= etype.bound() + etype = etype.seqType() + elif isinstance(etype, idlast.Typedef): + arrsize = [size] + etype.declarators()[0].sizes() + if(len(arrsize) != 1): dim += 1 + size = reduce(lambda a,b: a*b, arrsize) + etype = etype.aliasType() + elif isinstance(etype, idlast.Declarator): + etype = etype.alias() +# elif isinstance(etype, idlast.Forward): +# etype = etype.fullDecl() + else: + return 'undefined' + if( 1 < dim ): + return MultiArrayTypeNameMap[etype.kind()] + return self.getROSTypeText(etype) + ('[]' if size==0 else '[%d]' % size) + if isinstance(typ, idltype.Declared): + return self.getROSTypeText(typ.decl()) + + if isinstance(typ, idlast.Interface): + return '_'.join(typ.scopedName()) + if isinstance(typ, idlast.Struct): + return '_'.join(typ.scopedName()) + if isinstance(typ, idlast.Const): + return TypeNameMap[typ.constKind()] + if isinstance(typ, idlast.Enum): + return TypeNameMap[idltype.tk_longlong] # enum is int64 ?? + if isinstance(typ, idlast.Union): + return TypeNameMap[idltype.tk_double] # union is not in ROS + if isinstance(typ, idlast.Typedef): + arraysize = typ.declarators()[0].sizes() + if 0 < len(arraysize): + return self.getROSTypeText(typ.aliasType()) + ('[%d]' % reduce(lambda a,b: a*b, arraysize)) + return self.getROSTypeText(typ.aliasType()) + if isinstance(typ, idlast.Declarator): + return self.getROSTypeText(typ.alias()) + if isinstance(typ, idlast.Forward): + return self.getROSTypeText(typ.fullDecl()) + + return 'undefined' + + # output .msg file defined in .idl + def outputMsg(self, typ): + if typ in self.generated_msgs: + return + elif isinstance(typ, idlast.Struct): + for mem in typ.members(): + self.outputMsg(mem.memberType()) + self.generated_msgs += [typ] + elif isinstance(typ, idlast.Enum) or \ + isinstance(typ, idlast.Interface): + self.generated_msgs += [typ] + elif isinstance(typ, idltype.Sequence): + return self.outputMsg(typ.seqType()) + elif isinstance(typ, idltype.Declared): + return self.outputMsg(typ.decl()) + elif isinstance(typ, idlast.Typedef): + return self.outputMsg(typ.aliasType()) + elif isinstance(typ, idlast.Declarator): + return self.outputMsg(typ.alias()) + elif isinstance(typ, idlast.Forward): + return self.outputMsg(typ.fullDecl()) + else: + return + + msgfile = basedir + "/msg/" + self.getCppTypeText(typ,full=ROS_FULL) + ".msg" + if not os.path.exists(basedir): + return + print msgfile + if options.filenames: + return + + if os.path.exists(msgfile) and (os.stat(msgfile).st_mtime > os.stat(idlfile).st_mtime) and not options.overwrite: + return # do not overwrite + + os.system('mkdir -p %s/msg' % basedir) + print >>sys.stderr,"[idl2srv] writing "+msgfile+"...." + fd = open(msgfile, 'w') + if isinstance(typ, idlast.Enum): + for val in typ.enumerators(): + fd.write("%s %s=%d\n" % (self.getROSTypeText(typ), val.identifier(), val.value())) + elif isinstance(typ, idlast.Struct): + for mem in typ.members(): + fd.write(self.getROSTypeText(mem.memberType()) + " " + mem.declarators()[0].identifier() + "\n") + elif isinstance(typ, idlast.Interface): + for mem in typ.contents(): + if isinstance(mem, idlast.Const): + fd.write("%s %s=%s\n" % (self.getROSTypeText(mem), mem.identifier(), mem.value())) + fd.close() + + # output .srv file defined in .idl + def outputSrv(self, op): + + srvfile = basedir + "/srv/" + self.getCppTypeText(op,full=ROS_FULL) + ".srv" + if not os.path.exists(basedir): + return + print srvfile + if options.filenames: + return + + if os.path.exists(srvfile) and (os.stat(srvfile).st_mtime > os.stat(idlfile).st_mtime) and not options.overwrite: + return # do not overwrite + os.system('mkdir -p %s/srv' % basedir) + args = op.parameters() + + print >>sys.stderr, "[idl2srv] writing "+srvfile+"...." + fd = open(srvfile, 'w') + for arg in [arg for arg in args if arg.is_in()]: + fd.write("%s %s\n" % (self.getROSTypeText(arg.paramType()), arg.identifier())) + fd.write("---\n") + if not op.oneway() and op.returnType().kind() != idltype.tk_void: + fd.write("%s operation_return\n" % self.getROSTypeText(op.returnType())) + for arg in [arg for arg in args if arg.is_out()]: + fd.write("%s %s\n" % (self.getROSTypeText(arg.paramType()), arg.identifier())) + fd.close() + + def convertFunctionCode(self, interface): + visitor = DependencyVisitor() + interface.accept(visitor) + code = '' + + for typ in visitor.multiarray: + msg = MultiArrayTypeNameMap[typ.kind()].replace('/','::') + cpp = cxx.types.basic_map[typ.kind()] + code += multiarray_conversion % (msg,cpp,msg,msg,msg,cpp) + + for typ in visitor.allmsg: + if not isinstance(typ, idlast.Struct): + continue + + code += 'template<> void convert(%s& in, %s::%s& out){\n' % (self.getCppTypeText(typ, full=CPP_FULL), pkgname, self.getCppTypeText(typ,full=ROS_FULL)) + for mem in typ.members(): + var = mem.declarators()[0].identifier() + code += ' convert(in.%s, out.%s);\n' % (var, var) + code += '}\n' + + code += 'template<> void convert(%s::%s& in, %s& out){\n' % (pkgname, self.getCppTypeText(typ,full=ROS_FULL), self.getCppTypeText(typ, full=CPP_FULL)) + for mem in typ.members(): + var = mem.declarators()[0].identifier() + code += ' convert(in.%s, out.%s);\n' % (var, var) + code += '}\n' + + return code + + def ServiceBridgeFunction(self, op, ifname, pkgname): + code = req_code = res_code = '' + params = [] + for par in op.parameters(): + is_out = par.is_out() + ptype = par.paramType() + var = par.identifier() + # temporary variables + if isinstance(ptype.unalias(), idltype.Base) or \ + isinstance(ptype.unalias(), idltype.String) or \ + isinstance(ptype.unalias(), idltype.Sequence) or \ + isinstance(ptype.unalias(), idltype.Declared): + code += ' %s %s;\n' % (self.getCppTypeText(ptype, out=is_out, full=CPP_FULL), var) + params += [var] + if isinstance(ptype.unalias(), idltype.Base) or \ + isinstance(ptype.unalias(), idltype.String): + if is_out: + res_code += ' convert(%s, res.%s);\n' % (var, var) + else: + req_code += ' convert(req.%s, %s);\n' % (var, var) + if isinstance(ptype.unalias(), idltype.Sequence): + if is_out: + res_code += ' convert(*%s, res.%s);\n' % (var, var) + else: + req_code += ' convert(req.%s, %s);\n' % (var, var) + if isinstance(ptype.unalias(), idltype.Declared): + if is_out: + ptr = ('*' if cxx.types.variableDecl(ptype.decl()) else '') + res_code += ' convert(%s%s, res.%s);\n' % (ptr, var, var) + if cxx.types.variableDecl(ptype.decl()): + res_code += ' delete %s;\n' % (var) + + else: + req_code += ' convert(req.%s, %s);\n' % (var, var) + + code += '\n' + code += ' ROS_INFO_STREAM("%s::%s() called");\n' % (ifname, op.identifier()) + + code += '\n' + req_code + '\n' + + code += ' try {\n' + + params = ', '.join(params) + + if op.oneway() or op.returnType().kind() == idltype.tk_void: + code += ' m_service0->%s(%s);\n' % (op.identifier(), params) + elif isinstance(op.returnType().unalias(), idltype.Base): + code += ' res.operation_return = m_service0->%s(%s);\n' % (op.identifier(), params) + else: + rtype = op.returnType() + if isinstance(rtype.unalias(), idltype.String): + ptr = '' + elif isinstance(rtype.unalias(), idltype.Sequence): + ptr = '*' + elif isinstance(rtype.unalias(), idltype.Declared): + ptr = ('*' if cxx.types.variableDecl(rtype.decl()) else '') + else: ptr = '' + code += ' %s operation_return;\n' % self.getCppTypeText(rtype, out=True, full=CPP_FULL) + code += ' operation_return = m_service0->%s(%s);\n' % (op.identifier(), params) + code += ' convert(%soperation_return, res.operation_return);\n' % ptr + connected_RTC_name = ifname[0:ifname.rindex("ServiceROSBridge")] + code += ' } catch(CORBA::COMM_FAILURE& ex) {\n' + code += ' ROS_ERROR_STREAM("%s::%s : Caught system exception COMM_FAILURE -- unable to contact the object [minor code = " << ex.minor() << "]. One possibility is IDL version mismatch between %s and %s. So please check IDL versions are consistent. Another possibility is that Service-Port function, %s of %s, called but %s died becase of it. So please check %s is still alive.");\n' % (ifname, op.identifier(), ifname, connected_RTC_name, op.identifier(), connected_RTC_name, connected_RTC_name, connected_RTC_name) + code += ' return false;\n' + code += ' } catch(CORBA::MARSHAL& ex) {\n' + code += ' ROS_ERROR_STREAM("%s::%s : Caught CORBA::SystemException::MARSHAL [minor code = " << ex.minor() << "]. One possibility is IDL version mismatch between %s and %s. So please check IDL versions are consistent.");\n' % (ifname, op.identifier(), ifname, connected_RTC_name) + code += ' return false;\n' + code += ' } catch(CORBA::BAD_PARAM& ex) {\n' + code += ' ROS_ERROR_STREAM("%s::%s : Caught CORBA::SystemException::BAD_PARAM [minor code = " << ex.minor() << "]. One possibility is that Service-Port function, %s of %s, called and some problem has occurred within it and %s keeps alive. So please check %s of %s is correctly finished.");\n' % (ifname, op.identifier(), op.identifier(), connected_RTC_name, connected_RTC_name, op.identifier(), connected_RTC_name) + code += ' return false;\n' + code += ' } catch(CORBA::OBJECT_NOT_EXIST& ex) {\n' + code += ' ROS_ERROR_STREAM("%s::%s : Caught CORBA::SystemException::OBJECT_NOT_EXIST [minor code = " << ex.minor() << "]. One possibility is that %s RTC is not found. So please check %s is still alive.");\n' % (ifname, op.identifier(), connected_RTC_name, connected_RTC_name) + code += ' return false;\n' + code += ' } catch(CORBA::SystemException& ex) {\n' + code += ' ROS_ERROR_STREAM("%s::%s : Caught CORBA::SystemException [minor code = " << ex.minor() << "].");\n' % (ifname, op.identifier()) + code += ' return false;\n' + code += ' } catch(CORBA::Exception&) {\n' + code += ' ROS_ERROR_STREAM("%s::%s : Caught CORBA::Exception.");\n' % (ifname, op.identifier()) + code += ' return false;\n' + code += ' } catch(omniORB::fatalException& fe) {\n' + code += ' ROS_ERROR_STREAM("%s::%s : Caught omniORB::fatalException:");\n' % (ifname, op.identifier()) + code += ' ROS_ERROR_STREAM(" file: " << fe.file());\n' + code += ' ROS_ERROR_STREAM(" line: " << fe.line());\n' + code += ' ROS_ERROR_STREAM(" mesg: " << fe.errmsg());\n' + code += ' return false;\n' + code += ' }\n' + code += ' catch(...) {\n' + code += ' ROS_ERROR_STREAM("%s::%s : Caught unknown exception.");\n' % (ifname, op.identifier()) + code += ' return false;\n' + code += ' }\n' + + code += res_code + + code += ' ROS_INFO_STREAM("%s::%s() succeeded");\n' % (ifname, op.identifier()) + + return """bool %s::%s(%s::%s::Request &req, %s::%s::Response &res){\n%s\n return true;\n};\n\n""" % (ifname, op.identifier(), pkgname, self.getCppTypeText(op,full=ROS_FULL), pkgname, self.getCppTypeText(op,full=ROS_FULL), code) + + # generate cpp source to bridge RTM/ROS + def genBridgeComponent(self, interface): + idlfile = interface.file() + module_name = '%sROSBridge' % interface.identifier() + #service_name = idlutil.ccolonName(interface.scopedName()) + service_name = interface.identifier() + idl_name = os.path.split(idlfile)[1] + wd = basedir + '/src_gen' + + Comp_cpp = wd + '/' + module_name + 'Comp.cpp' + mod_cpp = wd + '/' + module_name + '.cpp' + mod_h = wd + '/' + module_name + '.h' + print Comp_cpp + print mod_cpp + print mod_h + if options.filenames: + return + + if all([ os.path.exists(x) and os.stat(x).st_mtime > os.stat(idlfile).st_mtime for x in [Comp_cpp, mod_cpp, mod_h]]) and not options.overwrite: + return # do not overwrite + + # check if rtc-template exists under `rospack find openrtm_aist`/bin, otherwise use openrtm_aist_PREFIX/lib/openrtm_aist/bin + from subprocess import check_output, Popen, PIPE + openrtm_path = Popen(['rospack','find','openrtm_aist'], stdout=PIPE).communicate()[0].rstrip() # use Popen, not check_output, since catkin_make can not found rospack find + if not os.path.exists(os.path.join(openrtm_path, "bin")) : + openrtm_path = os.path.join(check_output(['pkg-config','openrtm-aist','--variable=prefix']).rstrip(),"lib/openrtm_aist") + command = "PATH=%s/bin:$PATH rtc-template -bcxx --module-name=%s --consumer=%s:service0:'%s' --consumer-idl=%s --idl-include=%s" % (openrtm_path, module_name, service_name, service_name, idlfile, idldir) + #command = "rosrun openrtm_aist rtc-template -bcxx --module-name=%s --consumer=%s:service0:'%s' --consumer-idl=%s --idl-include=%s" % (module_name, service_name, service_name, idlfile, idldir) + os.system("mkdir -p %s" % tmpdir) + os.system("mkdir -p %s" % wd) + os.system("cd %s; yes 2> /dev/null | %s > /dev/null" % (tmpdir, command)) + + # TODO: ignore attribute read/write operators + operations = [o for o in interface.callables() if isinstance(o, idlast.Operation)] + + def addline(src, dest, ref): + idx = dest.find(ref) + idx = dest.find('\n',idx) + return dest[0:idx] + '\n' + src + dest[idx:] + + def replaceline(src, dest, ref): + idx1 = dest.find(ref) + idx2 = dest.find('\n',idx1) + return dest[0:idx1] + src + dest[idx2:] + + # Comp.cpp + # use ros node name as rtm component name + # call ros::init in Comp.cpp + compsrc = open(tmpdir + '/' + module_name + 'Comp.cpp').read() + compsrc = addline(' ros::init(argc, argv, "' + module_name + '", ros::init_options::NoSigintHandler);', compsrc, 'RTC::Manager::init(argc, argv);') + compsrc = replaceline(' comp = manager->createComponent(std::string("'+module_name+'?instance_name="+ros::this_node::getName().substr(1)).c_str()); // skip root name space for OpenRTM instance name', compsrc, ' comp = manager->createComponent("'+module_name+'");') + open(wd + '/' + module_name + 'Comp.cpp', 'w').write(compsrc) + + #.cpp + # make ROS service in onInitialize + # make ROS bridge functions in .cpp + compsrc = open(tmpdir + '/' + module_name + '.cpp').read() + + port_name_src = """ nh = ros::NodeHandle("~"); + std::string port_name = "service0"; + nh.getParam("service_port", port_name);""" + compsrc = addline(port_name_src, compsrc, 'Set service consumers to Ports') + compsrc = compsrc.replace('registerConsumer("service0"','registerConsumer(port_name.c_str()') + + compsrc += """ +RTC::ReturnCode_t %s::onExecute(RTC::UniqueId ec_id) { + ros::spinOnce(); + return RTC::RTC_OK; +}\n\n""" % module_name + + compsrc += "RTC::ReturnCode_t %s::onActivated(RTC::UniqueId ec_id) {\n" % module_name + for i in range(len(operations)): + name = operations[i].identifier() + compsrc += ' _srv%d = nh.advertiseService("%s", &%s::%s, this);\n' % (i, name, module_name, name) + compsrc +=" return RTC::RTC_OK;\n}\n\n""" + + compsrc += "RTC::ReturnCode_t %s::onDeactivated(RTC::UniqueId ec_id) {\n" % module_name + for i in range(len(operations)): + name = operations[i].identifier() + srvinst = ' _srv%d.shutdown();' % i + compsrc = addline(srvinst, compsrc, 'Unadvertise service') + compsrc +=" return RTC::RTC_OK;\n}\n\n""" + + compsrc += convert_functions + self.convertFunctionCode(interface) + + for op in operations: + compsrc += self.ServiceBridgeFunction(op, module_name, pkgname) + open(wd + '/' + module_name + '.cpp', 'w').write(compsrc) + + #.h + # add ros headers, service server functions, uncomment onExecute + compsrc = open(tmpdir + '/' + module_name + '.h').read() + # allow idl dir name with version + # compsrc = re.sub(basedir+"/idl/(.+).h", pkgname+r'/idl/\1.h', compsrc) + compsrc = re.sub(basedir+"/idl(.*)/(.+).h", pkgname+r'/idl\1/\2.h', compsrc) + + compsrc = compsrc.replace('<%s>'%service_name, '<%s>'%idlutil.ccolonName(interface.scopedName())) + + incs = ['', '// ROS', '#include '] + incs += ['#include <%s/%s.h>' % (pkgname, self.getCppTypeText(op,full=ROS_FULL)) for op in operations] + incs = '\n'.join(incs) + compsrc = addline(incs, compsrc, '#define') + + compsrc = '\n'.join([ (a.replace('//','') if ('RTC::ReturnCode_t onExecute' in a) else a) for a in compsrc.split('\n')]) + compsrc = '\n'.join([ (a.replace('//','') if ('RTC::ReturnCode_t onActivated' in a) else a) for a in compsrc.split('\n')]) + compsrc = '\n'.join([ (a.replace('//','') if ('RTC::ReturnCode_t onDeactivated' in a) else a) for a in compsrc.split('\n')]) + + srvfunc = [' bool %s(%s::%s::Request &req, %s::%s::Response &res);' % (op.identifier(), pkgname, self.getCppTypeText(op,full=ROS_FULL), pkgname, self.getCppTypeText(op,full=ROS_FULL)) for op in operations] + srvfunc = '\n'.join(srvfunc) + compsrc = addline(srvfunc, compsrc, 'public:') + + defsrv = " ros::NodeHandle nh;\n"; + defsrv += " ros::ServiceServer " + ', '.join(['_srv%d' % i for i in range(len(operations))]) + ';' + compsrc = addline(defsrv, compsrc, 'private:') + + open(wd + '/' + module_name + '.h', 'w').write(compsrc) + + # finialize + ## os.system("rm -f %s" % tmpdir) remove tmpdir in rtmbuild.cmake + return + +# all types depended by a interface +class DependencyVisitor (idlvisitor.AstVisitor): + def __init__(self): + self.allmsg = [] + self.multiarray = [] + self.checked = [] + + def visitInterface(self, node): + if node.mainFile(): + for n in node.callables(): + n.accept(self) + + def checkBasicType(self, node): + classes = [idltype.Base, idltype.String, idltype.WString] + classes += [idlast.Const, idlast.Enum, idlast.Union] + if not (any([isinstance(node, cls) for cls in classes]) or node in self.checked): + self.checked += [node] + node.accept(self) + + def visitOperation(self, node): + types = [p.paramType() for p in node.parameters()] + types += [node.returnType()] + for n in types: + self.checkBasicType(n) + + def visitStruct(self, node): + for mem in node.members(): + self.checkBasicType(mem.memberType()) + if not node in self.allmsg: # add self after all members + self.allmsg += [node] + + def visitSequenceType(self, node): + etype = node.seqType() + dim = 1 + while not (isinstance(etype, idltype.Base) or isinstance(etype, idltype.String) or isinstance(etype, idltype.WString) or isinstance(etype, idlast.Struct) or isinstance(etype, idlast.Interface) or isinstance(etype, idlast.Enum) or isinstance(etype, idlast.Forward)): + if isinstance(etype, idltype.Sequence): + dim += 1 + etype = etype.seqType() + elif isinstance(etype, idltype.Declared): + etype = etype.decl() + elif isinstance(etype, idlast.Typedef): + if len(etype.declarators()[0].sizes()) != 0 : dim += 1 + etype = etype.aliasType() + elif isinstance(etype, idlast.Declarator): + etype = etype.alias() +# elif isinstance(etype, idlast.Forward): +# etype = etype.fullDecl() + if 1 < dim and isinstance(etype, idltype.Base) and (not etype in self.multiarray) : + self.multiarray += [etype] + self.checkBasicType(etype) + def visitDeclaredType(self, node): + self.checkBasicType(node.decl()) + def visitTypedef(self, node): + self.checkBasicType(node.aliasType()) + def visitDeclarator(self, node): + self.checkBasicType(node.alias()) + def visitForward(self, node): + self.checkBasicType(node.fullDecl()) + + +# visitor only prints interface names +class InterfaceNameVisitor (idlvisitor.AstVisitor): + def visitAST(self, node): + for n in node.declarations(): + n.accept(self) + def visitModule(self, node): + for n in node.definitions(): + n.accept(self) + def visitInterface(self, node): + if node.mainFile(): + print node.identifier() + if options.interfaces: + pass + + +if __name__ == '__main__': + global options, basedir, pkgname, tmpdir + + parser = OptionParser() + parser.add_option("-i", "--idl", dest="idlfile", + help="target idl file", metavar="FILE") + parser.add_option("-I", "--include-dirs", dest="idlpath", metavar="PATHLIST", + help="list of directories to check idl include") + parser.add_option("-o", "--overwrite", action="store_true", + dest="overwrite", default=False, + help="overwrite all generate files") + parser.add_option("--filenames", action="store_true", + dest="filenames", default=False, + help="print filenames to generate") + parser.add_option("--interfaces", action="store_true", + dest="interfaces", default=False, + help="print interface names") + parser.add_option("--package-name", action="store", type="string", + dest="package_name", default=False, + help="overwrite package name") + parser.add_option("--tmpdir", action="store", type="string", + dest="tmpdir", default="/tmp/idl2srv", + help="tmporary directory") + (options, args) = parser.parse_args() + + idlfile = options.idlfile + if not os.path.exists(idlfile): + exit + tmpdir = options.tmpdir + idlfile = os.path.abspath(idlfile) + idldir = os.path.split(idlfile)[0] + basedir = os.path.split(idldir)[0] + if options.package_name: + pkgname = options.package_name + else: + pkgname = os.path.split(basedir)[1] # global var + + # preproccess and compile idl + if options.idlpath: + pathlist = options.idlpath.strip('"') + option = ' '.join(['-I'+d for d in filter(None, pathlist.split(' '))]) + else: + option = '' + + fd = os.popen('/usr/bin/omnicpp %s "%s"' % (option, idlfile), 'r') + try: + tree = _omniidl.compile(fd, idlfile) # newer version API + except: + tree = _omniidl.compile(fd) # old version API + + # output msg/srv and bridge component src + if options.interfaces: + tree.accept(InterfaceNameVisitor()) + else: + tree.accept(ServiceVisitor()) + tree.accept(InterfaceNameVisitor()) + diff --git a/hironx_ros_bridge/setup.py b/hironx_ros_bridge/setup.py index 78630d35..de9f3df1 100644 --- a/hironx_ros_bridge/setup.py +++ b/hironx_ros_bridge/setup.py @@ -5,7 +5,7 @@ # fetch values from package.xml setup_args = generate_distutils_setup( - packages=['hironx_ros_bridge', 'hironx_ros_bridge.testutil', 'hironx_ros_bridge.hrpsys_315_1_9.hrpsys'], + packages=['hironx_ros_bridge', 'hironx_ros_bridge.testutil'], package_dir={'': 'src'}) setup(**setup_args) diff --git a/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py b/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py index 8fb58583..0c9204ca 100644 --- a/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py +++ b/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py @@ -344,10 +344,9 @@ def init(self, robotname="HiroNX(Robot)0", url=""): if self.ms and self.ms.ref and len(self.ms.ref.get_component_profiles()) > 0: print self.configurator_name, " version = ", self.ms.ref.get_component_profiles()[0].version if self.ms and self.ms.ref and len(self.ms.ref.get_component_profiles()) > 0 and StrictVersion(self.ms.ref.get_component_profiles()[0].version) < StrictVersion('315.2.0'): - sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), 'hrpsys_315_1_9/hrpsys')) delete_module('ImpedanceControllerService_idl') - import AbsoluteForceSensorService_idl - import ImpedanceControllerService_idl + import hironx_ros_bridge.idl_315_1_9.AbsoluteForceSensorService_idl + import hironx_ros_bridge.idl_315_1_9.ImpedanceControllerService_idl # HrpsysConfigurator.init(self, robotname=robotname, url=url) self.sensors = self.getSensors(url) diff --git a/hironx_ros_bridge/src/hironx_ros_bridge/hrpsys_315_1_9/hrpsys/AbsoluteForceSensorService_idl.py b/hironx_ros_bridge/src/hironx_ros_bridge/hrpsys_315_1_9/hrpsys/AbsoluteForceSensorService_idl.py deleted file mode 100644 index ff4881b8..00000000 --- a/hironx_ros_bridge/src/hironx_ros_bridge/hrpsys_315_1_9/hrpsys/AbsoluteForceSensorService_idl.py +++ /dev/null @@ -1,107 +0,0 @@ -# Python stubs generated by omniidl from /home/k-okada/catkin_ws/ws_old_hrpsys/build/hrpsys/build/hrpsys-base-source/idl/AbsoluteForceSensorService.idl - -import omniORB, _omnipy -from omniORB import CORBA, PortableServer -_0_CORBA = CORBA - -_omnipy.checkVersion(3,0, __file__) - - -# -# Start of module "OpenHRP" -# -__name__ = "OpenHRP" -_0_OpenHRP = omniORB.openModule("OpenHRP", r"/home/k-okada/catkin_ws/ws_old_hrpsys/build/hrpsys/build/hrpsys-base-source/idl/AbsoluteForceSensorService.idl") -_0_OpenHRP__POA = omniORB.openModule("OpenHRP__POA", r"/home/k-okada/catkin_ws/ws_old_hrpsys/build/hrpsys/build/hrpsys-base-source/idl/AbsoluteForceSensorService.idl") - - -# interface AbsoluteForceSensorService -_0_OpenHRP._d_AbsoluteForceSensorService = (omniORB.tcInternal.tv_objref, "IDL:OpenHRP/AbsoluteForceSensorService:1.0", "AbsoluteForceSensorService") -omniORB.typeMapping["IDL:OpenHRP/AbsoluteForceSensorService:1.0"] = _0_OpenHRP._d_AbsoluteForceSensorService -_0_OpenHRP.AbsoluteForceSensorService = omniORB.newEmptyClass() -class AbsoluteForceSensorService : - _NP_RepositoryId = _0_OpenHRP._d_AbsoluteForceSensorService[1] - - def __init__(self, *args, **kw): - raise RuntimeError("Cannot construct objects of this type.") - - _nil = CORBA.Object._nil - - - # typedef ... DblSequence3 - class DblSequence3: - _NP_RepositoryId = "IDL:OpenHRP/AbsoluteForceSensorService/DblSequence3:1.0" - def __init__(self, *args, **kw): - raise RuntimeError("Cannot construct objects of this type.") - _d_DblSequence3 = (omniORB.tcInternal.tv_sequence, omniORB.tcInternal.tv_double, 3) - _ad_DblSequence3 = (omniORB.tcInternal.tv_alias, DblSequence3._NP_RepositoryId, "DblSequence3", (omniORB.tcInternal.tv_sequence, omniORB.tcInternal.tv_double, 3)) - _tc_DblSequence3 = omniORB.tcInternal.createTypeCode(_ad_DblSequence3) - omniORB.registerType(DblSequence3._NP_RepositoryId, _ad_DblSequence3, _tc_DblSequence3) - - # struct forcemomentOffsetParam - _0_OpenHRP.AbsoluteForceSensorService.forcemomentOffsetParam = omniORB.newEmptyClass() - class forcemomentOffsetParam (omniORB.StructBase): - _NP_RepositoryId = "IDL:OpenHRP/AbsoluteForceSensorService/forcemomentOffsetParam:1.0" - - _NP_ClassName = "OpenHRP.AbsoluteForceSensorService.forcemomentOffsetParam" - - def __init__(self, force_offset, moment_offset, link_offset_centroid, link_offset_mass): - self.force_offset = force_offset - self.moment_offset = moment_offset - self.link_offset_centroid = link_offset_centroid - self.link_offset_mass = link_offset_mass - - _d_forcemomentOffsetParam = _0_OpenHRP.AbsoluteForceSensorService._d_forcemomentOffsetParam = (omniORB.tcInternal.tv_struct, forcemomentOffsetParam, forcemomentOffsetParam._NP_RepositoryId, "forcemomentOffsetParam", "force_offset", omniORB.typeMapping["IDL:OpenHRP/AbsoluteForceSensorService/DblSequence3:1.0"], "moment_offset", omniORB.typeMapping["IDL:OpenHRP/AbsoluteForceSensorService/DblSequence3:1.0"], "link_offset_centroid", omniORB.typeMapping["IDL:OpenHRP/AbsoluteForceSensorService/DblSequence3:1.0"], "link_offset_mass", omniORB.tcInternal.tv_double) - _tc_forcemomentOffsetParam = omniORB.tcInternal.createTypeCode(_d_forcemomentOffsetParam) - omniORB.registerType(forcemomentOffsetParam._NP_RepositoryId, _d_forcemomentOffsetParam, _tc_forcemomentOffsetParam) - - -_0_OpenHRP.AbsoluteForceSensorService = AbsoluteForceSensorService -_0_OpenHRP._tc_AbsoluteForceSensorService = omniORB.tcInternal.createTypeCode(_0_OpenHRP._d_AbsoluteForceSensorService) -omniORB.registerType(AbsoluteForceSensorService._NP_RepositoryId, _0_OpenHRP._d_AbsoluteForceSensorService, _0_OpenHRP._tc_AbsoluteForceSensorService) - -# AbsoluteForceSensorService operations and attributes -AbsoluteForceSensorService._d_setForceMomentOffsetParam = (((omniORB.tcInternal.tv_string,0), omniORB.typeMapping["IDL:OpenHRP/AbsoluteForceSensorService/forcemomentOffsetParam:1.0"]), (omniORB.tcInternal.tv_boolean, ), None) -AbsoluteForceSensorService._d_getForceMomentOffsetParam = (((omniORB.tcInternal.tv_string,0), ), (omniORB.tcInternal.tv_boolean, omniORB.typeMapping["IDL:OpenHRP/AbsoluteForceSensorService/forcemomentOffsetParam:1.0"]), None) - -# AbsoluteForceSensorService object reference -class _objref_AbsoluteForceSensorService (CORBA.Object): - _NP_RepositoryId = AbsoluteForceSensorService._NP_RepositoryId - - def __init__(self): - CORBA.Object.__init__(self) - - def setForceMomentOffsetParam(self, *args): - return _omnipy.invoke(self, "setForceMomentOffsetParam", _0_OpenHRP.AbsoluteForceSensorService._d_setForceMomentOffsetParam, args) - - def getForceMomentOffsetParam(self, *args): - return _omnipy.invoke(self, "getForceMomentOffsetParam", _0_OpenHRP.AbsoluteForceSensorService._d_getForceMomentOffsetParam, args) - - __methods__ = ["setForceMomentOffsetParam", "getForceMomentOffsetParam"] + CORBA.Object.__methods__ - -omniORB.registerObjref(AbsoluteForceSensorService._NP_RepositoryId, _objref_AbsoluteForceSensorService) -_0_OpenHRP._objref_AbsoluteForceSensorService = _objref_AbsoluteForceSensorService -del AbsoluteForceSensorService, _objref_AbsoluteForceSensorService - -# AbsoluteForceSensorService skeleton -__name__ = "OpenHRP__POA" -class AbsoluteForceSensorService (PortableServer.Servant): - _NP_RepositoryId = _0_OpenHRP.AbsoluteForceSensorService._NP_RepositoryId - - - _omni_op_d = {"setForceMomentOffsetParam": _0_OpenHRP.AbsoluteForceSensorService._d_setForceMomentOffsetParam, "getForceMomentOffsetParam": _0_OpenHRP.AbsoluteForceSensorService._d_getForceMomentOffsetParam} - -AbsoluteForceSensorService._omni_skeleton = AbsoluteForceSensorService -_0_OpenHRP__POA.AbsoluteForceSensorService = AbsoluteForceSensorService -omniORB.registerSkeleton(AbsoluteForceSensorService._NP_RepositoryId, AbsoluteForceSensorService) -del AbsoluteForceSensorService -__name__ = "OpenHRP" - -# -# End of module "OpenHRP" -# -__name__ = "AbsoluteForceSensorService_idl" - -_exported_modules = ( "OpenHRP", ) - -# The end. diff --git a/hironx_ros_bridge/src/hironx_ros_bridge/hrpsys_315_1_9/hrpsys/ImpedanceControllerService_idl.py b/hironx_ros_bridge/src/hironx_ros_bridge/hrpsys_315_1_9/hrpsys/ImpedanceControllerService_idl.py deleted file mode 100644 index a593e268..00000000 --- a/hironx_ros_bridge/src/hironx_ros_bridge/hrpsys_315_1_9/hrpsys/ImpedanceControllerService_idl.py +++ /dev/null @@ -1,136 +0,0 @@ -# Python stubs generated by omniidl from /home/k-okada/catkin_ws/ws_old_hrpsys/build_isolated/hrpsys/build/hrpsys-base-source/idl/ImpedanceControllerService.idl - -print ";;" -print ";; Loading ImpedanceController < 315.1.9" -print ";;" - -import omniORB, _omnipy -from omniORB import CORBA, PortableServer -_0_CORBA = CORBA - -_omnipy.checkVersion(3,0, __file__) - - -# -# Start of module "OpenHRP" -# -__name__ = "OpenHRP" -_0_OpenHRP = omniORB.openModule("OpenHRP", r"/home/k-okada/catkin_ws/ws_old_hrpsys/build_isolated/hrpsys/build/hrpsys-base-source/idl/ImpedanceControllerService.idl") -_0_OpenHRP__POA = omniORB.openModule("OpenHRP__POA", r"/home/k-okada/catkin_ws/ws_old_hrpsys/build_isolated/hrpsys/build/hrpsys-base-source/idl/ImpedanceControllerService.idl") - - -# interface ImpedanceControllerService -_0_OpenHRP._d_ImpedanceControllerService = (omniORB.tcInternal.tv_objref, "IDL:OpenHRP/ImpedanceControllerService:1.0", "ImpedanceControllerService") -omniORB.typeMapping["IDL:OpenHRP/ImpedanceControllerService:1.0"] = _0_OpenHRP._d_ImpedanceControllerService -_0_OpenHRP.ImpedanceControllerService = omniORB.newEmptyClass() -class ImpedanceControllerService : - _NP_RepositoryId = _0_OpenHRP._d_ImpedanceControllerService[1] - - def __init__(self, *args, **kw): - raise RuntimeError("Cannot construct objects of this type.") - - _nil = CORBA.Object._nil - - - # typedef ... DblSequence3 - class DblSequence3: - _NP_RepositoryId = "IDL:OpenHRP/ImpedanceControllerService/DblSequence3:1.0" - def __init__(self, *args, **kw): - raise RuntimeError("Cannot construct objects of this type.") - _d_DblSequence3 = (omniORB.tcInternal.tv_sequence, omniORB.tcInternal.tv_double, 3) - _ad_DblSequence3 = (omniORB.tcInternal.tv_alias, DblSequence3._NP_RepositoryId, "DblSequence3", (omniORB.tcInternal.tv_sequence, omniORB.tcInternal.tv_double, 3)) - _tc_DblSequence3 = omniORB.tcInternal.createTypeCode(_ad_DblSequence3) - omniORB.registerType(DblSequence3._NP_RepositoryId, _ad_DblSequence3, _tc_DblSequence3) - - # struct impedanceParam - _0_OpenHRP.ImpedanceControllerService.impedanceParam = omniORB.newEmptyClass() - class impedanceParam (omniORB.StructBase): - _NP_RepositoryId = "IDL:OpenHRP/ImpedanceControllerService/impedanceParam:1.0" - - _NP_ClassName = "OpenHRP.ImpedanceControllerService.impedanceParam" - - def __init__(self, name, base_name, target_name, M_p, D_p, K_p, M_r, D_r, K_r, ref_force, force_gain, ref_moment, moment_gain, sr_gain, avoid_gain, reference_gain, manipulability_limit): - self.name = name - self.base_name = base_name - self.target_name = target_name - self.M_p = M_p - self.D_p = D_p - self.K_p = K_p - self.M_r = M_r - self.D_r = D_r - self.K_r = K_r - self.ref_force = ref_force - self.force_gain = force_gain - self.ref_moment = ref_moment - self.moment_gain = moment_gain - self.sr_gain = sr_gain - self.avoid_gain = avoid_gain - self.reference_gain = reference_gain - self.manipulability_limit = manipulability_limit - - _d_impedanceParam = _0_OpenHRP.ImpedanceControllerService._d_impedanceParam = (omniORB.tcInternal.tv_struct, impedanceParam, impedanceParam._NP_RepositoryId, "impedanceParam", "name", (omniORB.tcInternal.tv_string,0), "base_name", (omniORB.tcInternal.tv_string,0), "target_name", (omniORB.tcInternal.tv_string,0), "M_p", omniORB.tcInternal.tv_double, "D_p", omniORB.tcInternal.tv_double, "K_p", omniORB.tcInternal.tv_double, "M_r", omniORB.tcInternal.tv_double, "D_r", omniORB.tcInternal.tv_double, "K_r", omniORB.tcInternal.tv_double, "ref_force", omniORB.typeMapping["IDL:OpenHRP/ImpedanceControllerService/DblSequence3:1.0"], "force_gain", omniORB.typeMapping["IDL:OpenHRP/ImpedanceControllerService/DblSequence3:1.0"], "ref_moment", omniORB.typeMapping["IDL:OpenHRP/ImpedanceControllerService/DblSequence3:1.0"], "moment_gain", omniORB.typeMapping["IDL:OpenHRP/ImpedanceControllerService/DblSequence3:1.0"], "sr_gain", omniORB.tcInternal.tv_double, "avoid_gain", omniORB.tcInternal.tv_double, "reference_gain", omniORB.tcInternal.tv_double, "manipulability_limit", omniORB.tcInternal.tv_double) - _tc_impedanceParam = omniORB.tcInternal.createTypeCode(_d_impedanceParam) - omniORB.registerType(impedanceParam._NP_RepositoryId, _d_impedanceParam, _tc_impedanceParam) - - -_0_OpenHRP.ImpedanceControllerService = ImpedanceControllerService -_0_OpenHRP._tc_ImpedanceControllerService = omniORB.tcInternal.createTypeCode(_0_OpenHRP._d_ImpedanceControllerService) -omniORB.registerType(ImpedanceControllerService._NP_RepositoryId, _0_OpenHRP._d_ImpedanceControllerService, _0_OpenHRP._tc_ImpedanceControllerService) - -# ImpedanceControllerService operations and attributes -ImpedanceControllerService._d_setImpedanceControllerParam = ((omniORB.typeMapping["IDL:OpenHRP/ImpedanceControllerService/impedanceParam:1.0"], ), (omniORB.tcInternal.tv_boolean, ), None) -ImpedanceControllerService._d_getImpedanceControllerParam = (((omniORB.tcInternal.tv_string,0), ), (omniORB.tcInternal.tv_boolean, omniORB.typeMapping["IDL:OpenHRP/ImpedanceControllerService/impedanceParam:1.0"]), None) -ImpedanceControllerService._d_deleteImpedanceController = (((omniORB.tcInternal.tv_string,0), ), (omniORB.tcInternal.tv_boolean, ), None) -ImpedanceControllerService._d_waitDeletingImpedanceController = (((omniORB.tcInternal.tv_string,0), ), (), None) -ImpedanceControllerService._d_deleteImpedanceControllerAndWait = (((omniORB.tcInternal.tv_string,0), ), (omniORB.tcInternal.tv_boolean, ), None) - -# ImpedanceControllerService object reference -class _objref_ImpedanceControllerService (CORBA.Object): - _NP_RepositoryId = ImpedanceControllerService._NP_RepositoryId - - def __init__(self): - CORBA.Object.__init__(self) - - def setImpedanceControllerParam(self, *args): - return _omnipy.invoke(self, "setImpedanceControllerParam", _0_OpenHRP.ImpedanceControllerService._d_setImpedanceControllerParam, args) - - def getImpedanceControllerParam(self, *args): - return _omnipy.invoke(self, "getImpedanceControllerParam", _0_OpenHRP.ImpedanceControllerService._d_getImpedanceControllerParam, args) - - def deleteImpedanceController(self, *args): - return _omnipy.invoke(self, "deleteImpedanceController", _0_OpenHRP.ImpedanceControllerService._d_deleteImpedanceController, args) - - def waitDeletingImpedanceController(self, *args): - return _omnipy.invoke(self, "waitDeletingImpedanceController", _0_OpenHRP.ImpedanceControllerService._d_waitDeletingImpedanceController, args) - - def deleteImpedanceControllerAndWait(self, *args): - return _omnipy.invoke(self, "deleteImpedanceControllerAndWait", _0_OpenHRP.ImpedanceControllerService._d_deleteImpedanceControllerAndWait, args) - - __methods__ = ["setImpedanceControllerParam", "getImpedanceControllerParam", "deleteImpedanceController", "waitDeletingImpedanceController", "deleteImpedanceControllerAndWait"] + CORBA.Object.__methods__ - -omniORB.registerObjref(ImpedanceControllerService._NP_RepositoryId, _objref_ImpedanceControllerService) -_0_OpenHRP._objref_ImpedanceControllerService = _objref_ImpedanceControllerService -del ImpedanceControllerService, _objref_ImpedanceControllerService - -# ImpedanceControllerService skeleton -__name__ = "OpenHRP__POA" -class ImpedanceControllerService (PortableServer.Servant): - _NP_RepositoryId = _0_OpenHRP.ImpedanceControllerService._NP_RepositoryId - - - _omni_op_d = {"setImpedanceControllerParam": _0_OpenHRP.ImpedanceControllerService._d_setImpedanceControllerParam, "getImpedanceControllerParam": _0_OpenHRP.ImpedanceControllerService._d_getImpedanceControllerParam, "deleteImpedanceController": _0_OpenHRP.ImpedanceControllerService._d_deleteImpedanceController, "waitDeletingImpedanceController": _0_OpenHRP.ImpedanceControllerService._d_waitDeletingImpedanceController, "deleteImpedanceControllerAndWait": _0_OpenHRP.ImpedanceControllerService._d_deleteImpedanceControllerAndWait} - -ImpedanceControllerService._omni_skeleton = ImpedanceControllerService -_0_OpenHRP__POA.ImpedanceControllerService = ImpedanceControllerService -omniORB.registerSkeleton(ImpedanceControllerService._NP_RepositoryId, ImpedanceControllerService) -del ImpedanceControllerService -__name__ = "OpenHRP" - -# -# End of module "OpenHRP" -# -__name__ = "ImpedanceControllerService_idl" - -_exported_modules = ( "OpenHRP", ) - -# The end. diff --git a/hironx_ros_bridge/src/hironx_ros_bridge/hrpsys_315_1_9/hrpsys/__init__.py b/hironx_ros_bridge/src/hironx_ros_bridge/hrpsys_315_1_9/hrpsys/__init__.py deleted file mode 100644 index cdb66cdd..00000000 --- a/hironx_ros_bridge/src/hironx_ros_bridge/hrpsys_315_1_9/hrpsys/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -import sys, os -sys.path.append(os.path.dirname(__file__))