diff --git a/CMakeLists.txt b/CMakeLists.txt index 97161582..c8508518 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,87 +1,33 @@ -# Top-Level CmakeLists.txt -cmake_minimum_required(VERSION 2.8.2) -PROJECT( Find-Object ) -SET(PROJECT_PREFIX find_object) +cmake_minimum_required(VERSION 2.8.3) -ADD_DEFINITIONS(-DPROJECT_PREFIX="${PROJECT_PREFIX}") -ADD_DEFINITIONS(-DPROJECT_NAME="${PROJECT_NAME}") -IF(WIN32 AND NOT MINGW) - ADD_DEFINITIONS("-wd4251") -ELSE () - ADD_DEFINITIONS( "-Wall" ) +IF(NOT CATKIN_BUILD) + #Standalone build + PROJECT( Find-Object ) +ELSE() + #Standalone build + PROJECT( find_object_2d ) ENDIF() -#ADD_DEFINITIONS("-DUNICODE") # to test with UNICODE projects -####### local cmake modules ####### -SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") +OPTION(CATKIN_BUILD "Set to ON to build in a Catkin workspace (ROS)" OFF) ####################### # VERSION ####################### -SET(PROJECT_VERSION "0.5.1") -ADD_DEFINITIONS(-DPROJECT_VERSION="${PROJECT_VERSION}") +SET(PROJECT_VERSION “0.5.1”) +SET(PROJECT_PREFIX find_object) STRING(REGEX MATCHALL "[0-9]" PROJECT_VERSION_PARTS "${PROJECT_VERSION}") - LIST(GET PROJECT_VERSION_PARTS 0 PROJECT_VERSION_MAJOR) LIST(GET PROJECT_VERSION_PARTS 1 PROJECT_VERSION_MINOR) LIST(GET PROJECT_VERSION_PARTS 2 PROJECT_VERSION_PATCH) -SET(PROJECT_SOVERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}") - -####### COMPILATION PARAMS ####### -# In case of Makefiles if the user does not setup CMAKE_BUILD_TYPE, assume it's Release: -IF(${CMAKE_GENERATOR} MATCHES ".*Makefiles") - IF("${CMAKE_BUILD_TYPE}" STREQUAL "") - set(CMAKE_BUILD_TYPE Release) - ENDIF("${CMAKE_BUILD_TYPE}" STREQUAL "") -ENDIF(${CMAKE_GENERATOR} MATCHES ".*Makefiles") - -SET(CMAKE_DEBUG_POSTFIX "d") - -####### Build libraries as shared or static ####### -OPTION( BUILD_SHARED_LIBS "Set to OFF to build static libraries" ON ) - -####### SET RPATH ######### -# When RPATH is activated (supported on most UNIX systems), -# the user doesn't need to change LD_LIBRARY_PATH - -# use, i.e. don't skip the full RPATH for the build tree -SET(CMAKE_SKIP_BUILD_RPATH FALSE) - -# when building, don't use the install RPATH already -# (but later on when installing) -SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) - -# the RPATH to be used when installing -SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_PREFIX}-${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}") - -# add the automatically determined parts of the RPATH -# which point to directories outside the build tree to the install RPATH -SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) - -####### OUTPUT DIR ####### -SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) -SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) -SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) - -####### INSTALL DIR ####### -# Offer the user the choice of overriding the installation directories -set(INSTALL_LIB_DIR lib/${PROJECT_PREFIX}-${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} CACHE PATH "Installation directory for libraries") -set(INSTALL_BIN_DIR bin CACHE PATH "Installation directory for executables") -set(INSTALL_INCLUDE_DIR include/${PROJECT_PREFIX}-${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} CACHE PATH - "Installation directory for header files") -if(WIN32 AND NOT CYGWIN) - set(DEF_INSTALL_CMAKE_DIR CMake) -else() - set(DEF_INSTALL_CMAKE_DIR lib/${PROJECT_PREFIX}-${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}) -endif() -set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR} CACHE PATH - "Installation directory for CMake files") +ADD_DEFINITIONS(-DPROJECT_PREFIX="${PROJECT_PREFIX}") +ADD_DEFINITIONS(-DPROJECT_VERSION="${PROJECT_VERSION}") +ADD_DEFINITIONS(-DPROJECT_NAME="${PROJECT_NAME}") ####### DEPENDENCIES ####### FIND_PACKAGE(OpenCV REQUIRED) # tested on 2.3.1 -FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtNetwork REQUIRED) # tested on Qt4.8 +FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtNetwork REQUIRED) ADD_DEFINITIONS(-DQT_NO_KEYWORDS) # To avoid conflicts with boost signals used in ROS IF(OPENCV_NONFREE_FOUND) @@ -91,146 +37,271 @@ ELSE() ENDIF() CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/include/${PROJECT_PREFIX}/Version.h) -####### OSX BUNDLE CMAKE_INSTALL_PREFIX ####### -IF(APPLE) - OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF) -ENDIF(APPLE) -IF(APPLE AND BUILD_AS_BUNDLE) - # Required when packaging, and set CMAKE_INSTALL_PREFIX to "/". - SET(CPACK_SET_DESTDIR TRUE) - - SET(CMAKE_BUNDLE_NAME - "${PROJECT_NAME}") - SET(CMAKE_BUNDLE_LOCATION "/") - - # make sure CMAKE_INSTALL_PREFIX ends in / - SET(CMAKE_INSTALL_PREFIX - "/${CMAKE_BUNDLE_NAME}.app/Contents") -ENDIF(APPLE AND BUILD_AS_BUNDLE) +IF(NOT CATKIN_BUILD) + #Standalone build + IF(WIN32 AND NOT MINGW) + ADD_DEFINITIONS("-wd4251") + ELSE () + ADD_DEFINITIONS( "-Wall" ) + ENDIF() + #ADD_DEFINITIONS("-DUNICODE") # to test with UNICODE projects -####### SOURCES (Projects) ####### -ADD_SUBDIRECTORY( src ) -ADD_SUBDIRECTORY( app ) -ADD_SUBDIRECTORY( example ) -ADD_SUBDIRECTORY( tools ) + ####### COMPILATION PARAMS ####### + # In case of Makefiles if the user does not setup CMAKE_BUILD_TYPE, assume it's Release: + IF(${CMAKE_GENERATOR} MATCHES ".*Makefiles") + IF("${CMAKE_BUILD_TYPE}" STREQUAL "") + set(CMAKE_BUILD_TYPE Release) + ENDIF("${CMAKE_BUILD_TYPE}" STREQUAL "") + ENDIF(${CMAKE_GENERATOR} MATCHES ".*Makefiles") + SET(CMAKE_DEBUG_POSTFIX "d") + ####### Build libraries as shared or static ####### + OPTION( BUILD_SHARED_LIBS "Set to OFF to build static libraries" ON ) -####################### -# Uninstall target, for "make uninstall" -####################### -CONFIGURE_FILE( - "${CMAKE_CURRENT_SOURCE_DIR}/cmake_uninstall.cmake.in" - "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" - IMMEDIATE @ONLY) + ####### SET RPATH ######### + # When RPATH is activated (supported on most UNIX systems), + # the user doesn't need to change LD_LIBRARY_PATH -ADD_CUSTOM_TARGET(uninstall - "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake") + # use, i.e. don't skip the full RPATH for the build tree + SET(CMAKE_SKIP_BUILD_RPATH FALSE) -####################### -# Setup FindObjectConfig.cmake -####################### -# Create the FindObjectConfig.cmake and FindObjectConfigVersion files -file(RELATIVE_PATH REL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${INSTALL_INCLUDE_DIR}") -file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${INSTALL_LIB_DIR}") + # when building, don't use the install RPATH already + # (but later on when installing) + SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) + + # the RPATH to be used when installing + SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_PREFIX}-${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}") + + # add the automatically determined parts of the RPATH + # which point to directories outside the build tree to the install RPATH + SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + + ####### OUTPUT DIR ####### + SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) + SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) + SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) + + ####### INSTALL DIR ####### + # Offer the user the choice of overriding the installation directories + set(INSTALL_LIB_DIR lib/${PROJECT_PREFIX}-${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} CACHE PATH "Installation directory for libraries") + set(INSTALL_BIN_DIR bin CACHE PATH "Installation directory for executables") + set(INSTALL_INCLUDE_DIR include/${PROJECT_PREFIX}-${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} CACHE PATH + "Installation directory for header files") + if(WIN32 AND NOT CYGWIN) + set(DEF_INSTALL_CMAKE_DIR CMake) + else() + set(DEF_INSTALL_CMAKE_DIR lib/${PROJECT_PREFIX}-${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}) + endif() + set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR} CACHE PATH + "Installation directory for CMake files") -# ... for the build tree -set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/include") -set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}") -configure_file(FindObjectConfig.cmake.in - "${PROJECT_BINARY_DIR}/FindObjectConfig.cmake" @ONLY) - -# ... for the install tree -set(CONF_INCLUDE_DIRS "\${FindObject_CMAKE_DIR}/${REL_INCLUDE_DIR}") -set(CONF_LIB_DIR "\${FindObject_CMAKE_DIR}/${REL_LIB_DIR}") -configure_file(FindObjectConfig.cmake.in - "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/FindObjectConfig.cmake" @ONLY) - -# ... for both -configure_file(FindObjectConfigVersion.cmake.in - "${PROJECT_BINARY_DIR}/FindObjectConfigVersion.cmake" @ONLY) -# Install the FindObjectConfig.cmake and FindObjectConfigVersion.cmake -install(FILES - "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/FindObjectConfig.cmake" - "${PROJECT_BINARY_DIR}/FindObjectConfigVersion.cmake" - DESTINATION "${INSTALL_CMAKE_DIR}" COMPONENT devel) -#### + ####### OSX BUNDLE CMAKE_INSTALL_PREFIX ####### + IF(APPLE) + OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF) + ENDIF(APPLE) + IF(APPLE AND BUILD_AS_BUNDLE) + # Required when packaging, and set CMAKE_INSTALL_PREFIX to "/". + SET(CPACK_SET_DESTDIR TRUE) + + SET(CMAKE_BUNDLE_NAME + "${PROJECT_NAME}") + SET(CMAKE_BUNDLE_LOCATION "/") + + # make sure CMAKE_INSTALL_PREFIX ends in / + SET(CMAKE_INSTALL_PREFIX + "/${CMAKE_BUNDLE_NAME}.app/Contents") + ENDIF(APPLE AND BUILD_AS_BUNDLE) -####################### -# CPACK (Packaging) -####################### -INCLUDE(InstallRequiredSystemLibraries) + ####### SOURCES (Projects) ####### + ADD_SUBDIRECTORY( src ) + ADD_SUBDIRECTORY( app ) + ADD_SUBDIRECTORY( tools ) + IF(NONFREE) + ADD_SUBDIRECTORY( example ) + ENDIF(NONFREE) -SET(CPACK_PACKAGE_NAME "${PROJECT_NAME}") -SET(CPACK_PACKAGE_VENDOR "${PROJECT_NAME} project") -SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Find-Object") -SET(CPACK_PACKAGE_VERSION_MAJOR "${PROJECT_VERSION_MAJOR}") -SET(CPACK_PACKAGE_VERSION_MINOR "${PROJECT_VERSION_MINOR}") -SET(CPACK_PACKAGE_VERSION_PATCH "${PROJECT_VERSION_PATCH}") -SET(CPACK_PACKAGE_CONTACT "matlabbe@gmail.com") + ####################### + # Uninstall target, for "make uninstall" + ####################### + CONFIGURE_FILE( + "${CMAKE_CURRENT_SOURCE_DIR}/cmake_uninstall.cmake.in" + "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" + IMMEDIATE @ONLY) -set(CPACK_SOURCE_IGNORE_FILES - "\\\\.svn/" - "${PROJECT_SOURCE_DIR}/build/[a-zA-Z0-9_]+" - "~$" - "${PROJECT_SOURCE_DIR}/bin/.*${PROJECT_PREFIX}" - "${PROJECT_SOURCE_DIR}/bin/.*${PROJECT_NAME}" - "\\\\.DS_Store" -) + ADD_CUSTOM_TARGET(uninstall + "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake") -IF(WIN32) - SET(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_SOURCE_DIR}/LICENSE") - IF(CMAKE_CL_64) - SET(CPACK_NSIS_INSTALL_ROOT "$PROGRAMFILES64") - ELSE() - SET(CPACK_NSIS_INSTALL_ROOT "$PROGRAMFILES") - ENDIF() - SET(CPACK_GENERATOR "ZIP;NSIS") - SET(CPACK_SOURCE_GENERATOR "ZIP") - SET(CPACK_NSIS_PACKAGE_NAME "${PROJECT_NAME}") - SET(ICON_PATH "${PROJECT_SOURCE_DIR}/app/${PROJECT_NAME}.ico") - SET(CPACK_NSIS_MUI_ICON ${ICON_PATH}) - SET(CPACK_NSIS_MUI_UNIICON ${ICON_PATH}) - SET(CPACK_NSIS_DISPLAY_NAME "${PROJECT_NAME}") - SET(CPACK_NSIS_CONTACT ${CPACK_PACKAGE_CONTACT}) - # Set the icon used for the Windows "Add or Remove Programs" tool. - SET(CPACK_NSIS_INSTALLED_ICON_NAME bin\\\\${PROJECT_NAME}.exe) - SET(CPACK_PACKAGE_EXECUTABLES "${PROJECT_NAME}" "${PROJECT_NAME}" ${CPACK_PACKAGE_EXECUTABLES}) - SET(CPACK_CREATE_DESKTOP_LINKS "${PROJECT_NAME}" ${CPACK_CREATE_DESKTOP_LINKS}) - SET(CPACK_PACKAGE_INSTALL_DIRECTORY "${PROJECT_NAME}") -ELSEIF(APPLE) - IF(BUILD_AS_BUNDLE) - # On APPLE and if BUILD_AS_BUNDLE=ON, the project is created as a bundle - # over the main app (see ./src).Here we package only this bundle. Note - # that we set CMAKE_INSTALL_PREFIX to "/" when packaging to DragNDrop... - SET(CPACK_GENERATOR "DragNDrop") - ELSE() - SET(CPACK_GENERATOR "PackageMaker;TBZ2") - ENDIF() - - SET(CPACK_SOURCE_GENERATOR "TBZ2") + ####################### + # Setup FindObjectConfig.cmake + ####################### + # Create the FindObjectConfig.cmake and FindObjectConfigVersion files + file(RELATIVE_PATH REL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${INSTALL_INCLUDE_DIR}") + file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${INSTALL_LIB_DIR}") + + # ... for the build tree + set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/include") + set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}") + configure_file(FindObjectConfig.cmake.in + "${PROJECT_BINARY_DIR}/FindObjectConfig.cmake" @ONLY) + + # ... for the install tree + set(CONF_INCLUDE_DIRS "\${FindObject_CMAKE_DIR}/${REL_INCLUDE_DIR}") + set(CONF_LIB_DIR "\${FindObject_CMAKE_DIR}/${REL_LIB_DIR}") + configure_file(FindObjectConfig.cmake.in + "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/FindObjectConfig.cmake" @ONLY) + + # ... for both + configure_file(FindObjectConfigVersion.cmake.in + "${PROJECT_BINARY_DIR}/FindObjectConfigVersion.cmake" @ONLY) + + # Install the FindObjectConfig.cmake and FindObjectConfigVersion.cmake + install(FILES + "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/FindObjectConfig.cmake" + "${PROJECT_BINARY_DIR}/FindObjectConfigVersion.cmake" + DESTINATION "${INSTALL_CMAKE_DIR}" COMPONENT devel) + #### + + ####################### + # CPACK (Packaging) + ####################### + INCLUDE(InstallRequiredSystemLibraries) + + SET(CPACK_PACKAGE_NAME "${PROJECT_NAME}") + SET(CPACK_PACKAGE_VENDOR "${PROJECT_NAME} project") + SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Find-Object") + SET(CPACK_PACKAGE_VERSION_MAJOR "${PROJECT_VERSION_MAJOR}") + SET(CPACK_PACKAGE_VERSION_MINOR "${PROJECT_VERSION_MINOR}") + SET(CPACK_PACKAGE_VERSION_PATCH "${PROJECT_VERSION_PATCH}") + SET(CPACK_PACKAGE_CONTACT "matlabbe@gmail.com") + + set(CPACK_SOURCE_IGNORE_FILES + "\\\\.svn/" + "${PROJECT_SOURCE_DIR}/build/[a-zA-Z0-9_]+" + "~$" + "${PROJECT_SOURCE_DIR}/bin/.*${PROJECT_PREFIX}" + "${PROJECT_SOURCE_DIR}/bin/.*${PROJECT_NAME}" + "\\\\.DS_Store" + ) + + IF(WIN32) + SET(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_SOURCE_DIR}/LICENSE") + IF(CMAKE_CL_64) + SET(CPACK_NSIS_INSTALL_ROOT "$PROGRAMFILES64") + ELSE() + SET(CPACK_NSIS_INSTALL_ROOT "$PROGRAMFILES") + ENDIF() + SET(CPACK_GENERATOR "ZIP;NSIS") + SET(CPACK_SOURCE_GENERATOR "ZIP") + SET(CPACK_NSIS_PACKAGE_NAME "${PROJECT_NAME}") + SET(ICON_PATH "${PROJECT_SOURCE_DIR}/app/${PROJECT_NAME}.ico") + SET(CPACK_NSIS_MUI_ICON ${ICON_PATH}) + SET(CPACK_NSIS_MUI_UNIICON ${ICON_PATH}) + SET(CPACK_NSIS_DISPLAY_NAME "${PROJECT_NAME}") + SET(CPACK_NSIS_CONTACT ${CPACK_PACKAGE_CONTACT}) + # Set the icon used for the Windows "Add or Remove Programs" tool. + SET(CPACK_NSIS_INSTALLED_ICON_NAME bin\\\\${PROJECT_NAME}.exe) + SET(CPACK_PACKAGE_EXECUTABLES "${PROJECT_NAME}" "${PROJECT_NAME}" ${CPACK_PACKAGE_EXECUTABLES}) + SET(CPACK_CREATE_DESKTOP_LINKS "${PROJECT_NAME}" ${CPACK_CREATE_DESKTOP_LINKS}) + SET(CPACK_PACKAGE_INSTALL_DIRECTORY "${PROJECT_NAME}") + ELSEIF(APPLE) + IF(BUILD_AS_BUNDLE) + # On APPLE and if BUILD_AS_BUNDLE=ON, the project is created as a bundle + # over the main app (see ./src).Here we package only this bundle. Note + # that we set CMAKE_INSTALL_PREFIX to "/" when packaging to DragNDrop... + SET(CPACK_GENERATOR "DragNDrop") + ELSE() + SET(CPACK_GENERATOR "PackageMaker;TBZ2") + ENDIF() + + SET(CPACK_SOURCE_GENERATOR "TBZ2") + + SET(CPACK_PACKAGE_ICON "${PROJECT_SOURCE_DIR}/app/${PROJECT_NAME}.icns") + ELSE() + SET(CPACK_SOURCE_GENERATOR "ZIP") + ENDIF() + + INCLUDE(CPack) + + ####################### + # OUTPUT INFO + ####################### + MESSAGE(STATUS "--------------------------------------------") + MESSAGE(STATUS "Info :") + MESSAGE(STATUS " CMAKE_INSTALL_PREFIX = ${CMAKE_INSTALL_PREFIX}") + MESSAGE(STATUS " CMAKE_BUILD_TYPE = ${CMAKE_BUILD_TYPE}") + IF(OPENCV_NONFREE_FOUND) + MESSAGE(STATUS " With OpenCV nonfree module (SIFT/SURF) = YES") + ELSE() + MESSAGE(STATUS " With OpenCV nonfree module (SIFT/SURF) = NO (not found)") + ENDIF() + IF(APPLE) + MESSAGE(STATUS " BUILD_AS_BUNDLE = ${BUILD_AS_BUNDLE}") + ENDIF(APPLE) + MESSAGE(STATUS "--------------------------------------------") - SET(CPACK_PACKAGE_ICON "${PROJECT_SOURCE_DIR}/app/${PROJECT_NAME}.icns") ELSE() - SET(CPACK_SOURCE_GENERATOR "ZIP") + #ROS Catkin build + + ## Find catkin macros and libraries + ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) + ## is used, also find other catkin packages + find_package(catkin REQUIRED COMPONENTS + cv_bridge roscpp rospy sensor_msgs std_msgs image_transport genmsg message_filters pcl_ros tf + ) + + ## Generate messages in the 'msg' folder + add_message_files( + FILES + ObjectsStamped.msg + ) + + ## Generate added messages and services with any dependencies listed here + generate_messages( + DEPENDENCIES + std_msgs + sensor_msgs + ) + + ################################### + ## catkin specific configuration ## + ################################### + ## The catkin_package macro generates cmake config files for your package + ## Declare things to be passed to dependent projects + ## INCLUDE_DIRS: uncomment this if you package contains header files + ## LIBRARIES: libraries you create in this project that dependent projects also need + ## CATKIN_DEPENDS: catkin_packages dependent projects also need + ## DEPENDS: system dependencies of this project that dependent projects also need + catkin_package( + CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs image_transport message_filters pcl_ros tf + ) + + ########### + ## Build ## + ########### + ADD_SUBDIRECTORY( src ) + + ############# + ## Install ## + ############# + ## Mark executables and/or libraries for installation + install(TARGETS + find_object + find_object_2d + print_objects_detected + tf_example + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + ## Mark other files for installation (e.g. launch and bag files, etc.) + install(FILES + launch/find_object_2d_gui.launch + launch/find_object_2d.launch + launch/find_object_3d.launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + ) ENDIF() -INCLUDE(CPack) - -####################### -# OUTPUT INFO -####################### -MESSAGE(STATUS "--------------------------------------------") -MESSAGE(STATUS "Info :") -MESSAGE(STATUS " CMAKE_INSTALL_PREFIX = ${CMAKE_INSTALL_PREFIX}") -MESSAGE(STATUS " CMAKE_BUILD_TYPE = ${CMAKE_BUILD_TYPE}") -IF(OPENCV_NONFREE_FOUND) -MESSAGE(STATUS " With OpenCV nonfree module (SIFT/SURF) = YES") -ELSE() -MESSAGE(STATUS " With OpenCV nonfree module (SIFT/SURF) = NO (not found)") -ENDIF() -IF(APPLE) -MESSAGE(STATUS " BUILD_AS_BUNDLE = ${BUILD_AS_BUNDLE}") -ENDIF(APPLE) -MESSAGE(STATUS "--------------------------------------------") diff --git a/README.md b/README.md index 1b2b549f..039f416c 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,25 @@ -# find-object -Find-Object project, visit the [home page](http://introlab.github.io/find-object/) for more information. For the ROS package, select the find_object_2d branch. +## find-object (standalone) +Find-Object project, visit the [home page](http://introlab.github.io/find-object/) for more information. + +## find_object_2d (ROS package) + +### Install +```bash +# Install ROS Groovy/Hydro/Indigo/Jade (catkin build): + $ cd ~/catkin_ws + $ git clone -b find_object_2d https://github.com/introlab/find-object.git src/find_object_2d + $ catkin_make + +# Install ROS Fuerte (in a directory of your "ROS_PACKAGE_PATH"): + $ svn checkout -r176 http://find-object.googlecode.com/svn/trunk/ros-pkg/find_object_2d + $ rosmake find_object_2d +``` + +### Run +```bash + $ roscore & + # Launch your preferred ubs camera driver + $ rosrun uvc_camera uvc_camera_node & + $ rosrun find_object_2d find_object_2d image:=image_raw +``` +See [find_object_2d](http://wiki.ros.org/find_object_2d) for more information. diff --git a/launch/find_object_2d.launch b/launch/find_object_2d.launch new file mode 100644 index 00000000..3d7b815b --- /dev/null +++ b/launch/find_object_2d.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/launch/find_object_2d_gui.launch b/launch/find_object_2d_gui.launch new file mode 100644 index 00000000..49c3c4de --- /dev/null +++ b/launch/find_object_2d_gui.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/launch/find_object_3d.launch b/launch/find_object_3d.launch new file mode 100644 index 00000000..660a56f0 --- /dev/null +++ b/launch/find_object_3d.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/msg/ObjectsStamped.msg b/msg/ObjectsStamped.msg new file mode 100644 index 00000000..fc34a83f --- /dev/null +++ b/msg/ObjectsStamped.msg @@ -0,0 +1,5 @@ +# objects format: +# [ObjectId1, objectWidth, objectHeight, h11, h12, h13, h21, h22, h23, h31, h32, h33, ObjectId2...] +# where h## is a 3x3 homography matrix (h31 = dx and h32 = dy, see QTransform) +Header header +std_msgs/Float32MultiArray objects \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 92f7b0f8..b240a1d5 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -2,29 +2,36 @@ ### Qt Gui stuff ### SET(headers_ui - ../include/${PROJECT_PREFIX}/MainWindow.h - ../include/${PROJECT_PREFIX}/FindObject.h - ../include/${PROJECT_PREFIX}/Camera.h - ../include/${PROJECT_PREFIX}/TcpServer.h - ../include/${PROJECT_PREFIX}/ObjWidget.h - ./AddObjectDialog.h - ./CameraTcpServer.h - ./ParametersToolBox.h - ./AboutDialog.h - ./RectItem.h - ./ImageDropWidget.h - ./rtabmap/PdfPlot.h - ./utilite/UPlot.h + ../include/${PROJECT_PREFIX}/MainWindow.h + ../include/${PROJECT_PREFIX}/FindObject.h + ../include/${PROJECT_PREFIX}/Camera.h + ../include/${PROJECT_PREFIX}/TcpServer.h + ../include/${PROJECT_PREFIX}/ObjWidget.h + ./AddObjectDialog.h + ./CameraTcpServer.h + ./ParametersToolBox.h + ./AboutDialog.h + ./RectItem.h + ./ImageDropWidget.h + ./rtabmap/PdfPlot.h + ./utilite/UPlot.h ) +IF(CATKIN_BUILD) + SET(headers_ui + ${headers_ui} + ./ros/CameraROS.h + ./ros/FindObjectROS.h + ) +ENDIF(CATKIN_BUILD) SET(uis - ./ui/mainWindow.ui - ./ui/addObjectDialog.ui - ./ui/aboutDialog.ui + ./ui/mainWindow.ui + ./ui/addObjectDialog.ui + ./ui/aboutDialog.ui ) SET(qrc - ./resources.qrc + ./resources.qrc ) # generate rules for building source files from the resources @@ -38,47 +45,68 @@ QT4_WRAP_CPP(moc_srcs ${headers_ui}) ### Qt Gui stuff end### SET(SRC_FILES - ./MainWindow.cpp - ./AddObjectDialog.cpp - ./KeypointItem.cpp - ./RectItem.cpp - ./QtOpenCV.cpp - ./Camera.cpp - ./CameraTcpServer.cpp - ./ParametersToolBox.cpp - ./Settings.cpp - ./ObjWidget.cpp - ./ImageDropWidget.cpp - ./FindObject.cpp - ./AboutDialog.cpp - ./TcpServer.cpp - ./Vocabulary.cpp - ./JsonWriter.cpp - ./utilite/ULogger.cpp - ./utilite/UPlot.cpp - ./utilite/UDirectory.cpp - ./utilite/UFile.cpp - ./utilite/UConversion.cpp - ./rtabmap/PdfPlot.cpp - ./json/jsoncpp.cpp - ${moc_srcs} - ${moc_uis} - ${srcs_qrc} + ./MainWindow.cpp + ./AddObjectDialog.cpp + ./KeypointItem.cpp + ./RectItem.cpp + ./QtOpenCV.cpp + ./Camera.cpp + ./CameraTcpServer.cpp + ./ParametersToolBox.cpp + ./Settings.cpp + ./ObjWidget.cpp + ./ImageDropWidget.cpp + ./FindObject.cpp + ./AboutDialog.cpp + ./TcpServer.cpp + ./Vocabulary.cpp + ./JsonWriter.cpp + ./utilite/ULogger.cpp + ./utilite/UPlot.cpp + ./utilite/UDirectory.cpp + ./utilite/UFile.cpp + ./utilite/UConversion.cpp + ./rtabmap/PdfPlot.cpp + ./json/jsoncpp.cpp + ${moc_srcs} + ${moc_uis} + ${srcs_qrc} ) +IF(CATKIN_BUILD) + SET(SRC_FILES + ${SRC_FILES} + ./ros/CameraROS.cpp + ./ros/FindObjectROS.cpp + ) +ENDIF(CATKIN_BUILD) + + SET(INCLUDE_DIRS - ${CMAKE_CURRENT_SOURCE_DIR}/../include - ${CMAKE_CURRENT_SOURCE_DIR} - ${OpenCV_INCLUDE_DIRS} - ${CMAKE_CURRENT_BINARY_DIR} # for qt ui generated in binary dir + ${CMAKE_CURRENT_SOURCE_DIR}/../include + ${CMAKE_CURRENT_SOURCE_DIR} + ${OpenCV_INCLUDE_DIRS} + ${CMAKE_CURRENT_BINARY_DIR} # for qt ui generated in binary dir ) +IF(CATKIN_BUILD) + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ) +ENDIF(CATKIN_BUILD) INCLUDE(${QT_USE_FILE}) SET(LIBRARIES - ${QT_LIBRARIES} - ${OpenCV_LIBS} + ${QT_LIBRARIES} + ${OpenCV_LIBS} ) +IF(CATKIN_BUILD) + SET(LIBRARIES + ${LIBRARIES} + ${catkin_LIBRARIES} + ) +ENDIF(CATKIN_BUILD) #include files INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) @@ -87,10 +115,27 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) ADD_LIBRARY(find_object ${SRC_FILES}) # Linking with Qt libraries TARGET_LINK_LIBRARIES(find_object ${LIBRARIES}) +IF(CATKIN_BUILD) + set_target_properties(find_object PROPERTIES OUTPUT_NAME find_object_2d) +ENDIF(CATKIN_BUILD) -INSTALL(TARGETS find_object - RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT runtime - LIBRARY DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel - ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel) - -install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION "${INSTALL_INCLUDE_DIR}" COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE) +IF(NOT CATKIN_BUILD) + INSTALL(TARGETS find_object + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT runtime + LIBRARY DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel + ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel) + + install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION "${INSTALL_INCLUDE_DIR}" COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE) +ELSE() + add_executable(find_object_2d ros/find_object_2d_node.cpp) + target_link_libraries(find_object_2d find_object ${LIBRARIES}) + add_dependencies(find_object_2d find_object_2d_generate_messages_cpp) + + add_executable(print_objects_detected ros/print_objects_detected_node.cpp) + target_link_libraries(print_objects_detected ${LIBRARIES}) + add_dependencies(print_objects_detected find_object_2d_generate_messages_cpp) + + add_executable(tf_example ros/tf_example_node.cpp) + target_link_libraries(tf_example ${LIBRARIES}) + add_dependencies(tf_example find_object_2d_generate_messages_cpp) +ENDIF() diff --git a/src/ros/CameraROS.cpp b/src/ros/CameraROS.cpp new file mode 100644 index 00000000..daadfa46 --- /dev/null +++ b/src/ros/CameraROS.cpp @@ -0,0 +1,168 @@ +/* +Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "CameraROS.h" +#include "find_object/Settings.h" + +#include +#include + +using namespace find_object; + +CameraROS::CameraROS(bool subscribeDepth, QObject * parent) : + Camera(parent), + subscribeDepth_(subscribeDepth) +{ + ros::NodeHandle nh; // public + ros::NodeHandle pnh("~"); // private + + qRegisterMetaType("ros::Time"); + qRegisterMetaType("cv::Mat"); + + if(!subscribeDepth_) + { + image_transport::ImageTransport it(nh); + imageSub_ = it.subscribe(nh.resolveName("image"), 1, &CameraROS::imgReceivedCallback, this); + } + else + { + int queueSize = 10; + pnh.param("queue_size", queueSize, queueSize); + ROS_INFO("find_object_ros: queue_size = %d", queueSize); + + ros::NodeHandle rgb_nh(nh, "rgb"); + ros::NodeHandle rgb_pnh(pnh, "rgb"); + ros::NodeHandle depth_nh(nh, "depth_registered"); + ros::NodeHandle depth_pnh(pnh, "depth_registered"); + image_transport::ImageTransport rgb_it(rgb_nh); + image_transport::ImageTransport depth_it(depth_nh); + image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); + image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); + + rgbSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect_color"), 1, hintsRgb); + depthSub_.subscribe(depth_it, depth_nh.resolveName("image_raw"), 1, hintsDepth); + cameraInfoSub_.subscribe(depth_nh, "camera_info", 1); + sync_ = new message_filters::Synchronizer(MySyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_); + sync_->registerCallback(boost::bind(&CameraROS::imgDepthReceivedCallback, this, _1, _2, _3)); + + } +} + +QStringList CameraROS::subscribedTopics() const +{ + QStringList topics; + if(subscribeDepth_) + { + topics.append(rgbSub_.getTopic().c_str()); + topics.append(depthSub_.getTopic().c_str()); + topics.append(cameraInfoSub_.getTopic().c_str()); + } + else + { + topics.append(imageSub_.getTopic().c_str()); + } + return topics; +} + +bool CameraROS::start() +{ + this->startTimer(); + return true; +} + +void CameraROS::stop() +{ + this->stopTimer(); +} + +void CameraROS::takeImage() +{ + ros::spinOnce(); +} + +void CameraROS::imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg) +{ + if(msg->data.size()) + { + cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg); + if(msg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0) + { + cv::Mat cpy = ptr->image.clone(); + Q_EMIT rosDataReceived(msg->header.frame_id, msg->header.stamp, cv::Mat(), 0.0f); + Q_EMIT imageReceived(cpy); + } + else if(msg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) + { + cv::Mat bgr; + cv::cvtColor(ptr->image, bgr, cv::COLOR_RGB2BGR); + Q_EMIT rosDataReceived(msg->header.frame_id, msg->header.stamp, cv::Mat(), 0.0f); + Q_EMIT imageReceived(bgr); + } + else + { + ROS_ERROR("find_object_ros: Encoding \"%s\" detected. Supported image encodings are bgr8 and rgb8...", msg->encoding.c_str()); + } + } +} + +void CameraROS::imgDepthReceivedCallback( + const sensor_msgs::ImageConstPtr& rgbMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) +{ + if(!(rgbMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + rgbMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + rgbMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) && + (depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 || + depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0)) + { + ROS_ERROR("find_object_ros: Input type must be rgb=mono8,rgb8,bgr8 and depth=32FC1,16UC1"); + return; + } + + if(rgbMsg->data.size()) + { + cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(rgbMsg); + cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg); + float depthConstant = 1.0f/cameraInfoMsg->K[4]; + if(rgbMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0) + { + cv::Mat cpy = ptr->image.clone(); + Q_EMIT rosDataReceived(rgbMsg->header.frame_id, rgbMsg->header.stamp, ptrDepth->image, depthConstant); + Q_EMIT imageReceived(cpy); + } + else if(rgbMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) + { + cv::Mat bgr; + cv::cvtColor(ptr->image, bgr, cv::COLOR_RGB2BGR); + Q_EMIT rosDataReceived(rgbMsg->header.frame_id, rgbMsg->header.stamp, ptrDepth->image, depthConstant); + Q_EMIT imageReceived(bgr); + } + } + + +} diff --git a/src/ros/CameraROS.h b/src/ros/CameraROS.h new file mode 100644 index 00000000..4094d8b8 --- /dev/null +++ b/src/ros/CameraROS.h @@ -0,0 +1,90 @@ +/* +Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef CAMERAROS_H_ +#define CAMERAROS_H_ + +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include "find_object/Camera.h" +#include + +class CameraROS : public find_object::Camera { + Q_OBJECT +public: + CameraROS(bool subscribeDepth, QObject * parent = 0); + virtual ~CameraROS() {} + + virtual bool start(); + virtual void stop(); + + QStringList subscribedTopics() const; + +Q_SIGNALS: + void rosDataReceived(const std::string & frameId, + const ros::Time & stamp, + const cv::Mat & depth, + float depthConstant); + +private Q_SLOTS: + virtual void takeImage(); + +private: + void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg); + void imgDepthReceivedCallback( + const sensor_msgs::ImageConstPtr& rgbMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg); + +private: + bool subscribeDepth_; + image_transport::Subscriber imageSub_; + + image_transport::SubscriberFilter rgbSub_; + image_transport::SubscriberFilter depthSub_; + message_filters::Subscriber cameraInfoSub_; + + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo> MySyncPolicy; + message_filters::Synchronizer * sync_; +}; + +#endif /* CAMERAROS_H_ */ diff --git a/src/ros/FindObjectROS.cpp b/src/ros/FindObjectROS.cpp new file mode 100644 index 00000000..36677405 --- /dev/null +++ b/src/ros/FindObjectROS.cpp @@ -0,0 +1,239 @@ +/* +Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "FindObjectROS.h" + +#include +#include "find_object_2d/ObjectsStamped.h" + +#include + +using namespace find_object; + +FindObjectROS::FindObjectROS(const std::string & objFramePrefix, QObject * parent) : + FindObject(parent), + objFramePrefix_("object") +{ + ros::NodeHandle pnh("~"); // public + pnh.param("object_prefix", objFramePrefix_, objFramePrefix_); + + ros::NodeHandle nh; // public + + pub_ = nh.advertise("objects", 1); + pubStamped_ = nh.advertise("objectsStamped", 1); + + this->connect(this, SIGNAL(objectsFound(find_object::DetectionInfo)), this, SLOT(publish(find_object::DetectionInfo))); +} + +void FindObjectROS::publish(const find_object::DetectionInfo & info) +{ + // send tf before the message + if(info.objDetected_.size() && !depth_.empty() && depthConstant_ != 0.0f) + { + std::vector transforms; + QMultiMap::const_iterator iterSizes=info.objDetectedSizes_.constBegin(); + for(QMultiMap::const_iterator iter=info.objDetected_.constBegin(); + iter!=info.objDetected_.constEnd(); + ++iter, ++iterSizes) + { + // get data + int id = iter.key(); + float objectWidth = iterSizes->width(); + float objectHeight = iterSizes->height(); + + // Find center of the object + QPointF center = iter->map(QPointF(objectWidth/2, objectHeight/2)); + QPointF xAxis = iter->map(QPointF(3*objectWidth/4, objectHeight/2)); + QPointF yAxis = iter->map(QPointF(objectWidth/2, 3*objectHeight/4)); + + cv::Vec3f center3D = this->getDepth(depth_, + center.x()+0.5f, center.y()+0.5f, + float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f, + 1.0f/depthConstant_, 1.0f/depthConstant_); + + cv::Vec3f axisEndX = this->getDepth(depth_, + xAxis.x()+0.5f, xAxis.y()+0.5f, + float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f, + 1.0f/depthConstant_, 1.0f/depthConstant_); + + cv::Vec3f axisEndY = this->getDepth(depth_, + yAxis.x()+0.5f, yAxis.y()+0.5f, + float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f, + 1.0f/depthConstant_, 1.0f/depthConstant_); + + if(std::isfinite(center3D.val[0]) && std::isfinite(center3D.val[1]) && std::isfinite(center3D.val[2]) && + std::isfinite(axisEndX.val[0]) && std::isfinite(axisEndX.val[1]) && std::isfinite(axisEndX.val[2]) && + std::isfinite(axisEndY.val[0]) && std::isfinite(axisEndY.val[1]) && std::isfinite(axisEndY.val[2])) + { + tf::StampedTransform transform; + transform.setIdentity(); + transform.child_frame_id_ = QString("%1_%2").arg(objFramePrefix_.c_str()).arg(id).toStdString(); + transform.frame_id_ = frameId_; + transform.stamp_ = stamp_; + transform.setOrigin(tf::Vector3(center3D.val[0], center3D.val[1], center3D.val[2])); + + //set rotation (y inverted) + tf::Vector3 xAxis(axisEndX.val[0] - center3D.val[0], axisEndX.val[1] - center3D.val[1], axisEndX.val[2] - center3D.val[2]); + xAxis.normalize(); + tf::Vector3 yAxis(axisEndY.val[0] - center3D.val[0], axisEndY.val[1] - center3D.val[1], axisEndY.val[2] - center3D.val[2]); + yAxis.normalize(); + tf::Vector3 zAxis = xAxis*yAxis; + tf::Matrix3x3 rotationMatrix( + xAxis.x(), yAxis.x() ,zAxis.x(), + xAxis.y(), yAxis.y(), zAxis.y(), + xAxis.z(), yAxis.z(), zAxis.z()); + tf::Quaternion q; + rotationMatrix.getRotation(q); + // set x axis going front of the object, with z up and z left + q *= tf::createQuaternionFromRPY(CV_PI/2.0, CV_PI/2.0, 0); + transform.setRotation(q.normalized()); + + transforms.push_back(transform); + } + else + { + ROS_WARN("Object %d detected, center 2D at (%f,%f), but invalid depth, cannot set frame \"%s\"! " + "(maybe object is too near of the camera or bad depth image)\n", + id, + center.x(), center.y(), + QString("%1_%2").arg(objFramePrefix_.c_str()).arg(id).toStdString().c_str()); + } + } + if(transforms.size()) + { + tfBroadcaster_.sendTransform(transforms); + } + } + + if(pub_.getNumSubscribers() || pubStamped_.getNumSubscribers()) + { + std_msgs::Float32MultiArray msg; + find_object_2d::ObjectsStamped msgStamped; + msg.data = std::vector(info.objDetected_.size()*12); + msgStamped.objects.data = std::vector(info.objDetected_.size()*12); + int i=0; + QMultiMap::const_iterator iterSizes=info.objDetectedSizes_.constBegin(); + for(QMultiMap::const_iterator iter=info.objDetected_.constBegin(); + iter!=info.objDetected_.constEnd(); + ++iter, ++iterSizes) + { + msg.data[i] = msgStamped.objects.data[i] = iter.key(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iterSizes->width(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iterSizes->height(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iter->m11(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iter->m12(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iter->m13(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iter->m21(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iter->m22(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iter->m23(); ++i; + msg.data[i] = msgStamped.objects.data[i] = iter->m31(); ++i;// dx + msg.data[i] = msgStamped.objects.data[i] = iter->m32(); ++i;// dy + msg.data[i] = msgStamped.objects.data[i] = iter->m33(); ++i; + } + if(pub_.getNumSubscribers()) + { + pub_.publish(msg); + } + if(pubStamped_.getNumSubscribers()) + { + // use same header as the input image (for synchronization and frame reference) + msgStamped.header.frame_id = frameId_; + msgStamped.header.stamp = stamp_; + pubStamped_.publish(msgStamped); + } + } +} + +void FindObjectROS::setDepthData(const std::string & frameId, + const ros::Time & stamp, + const cv::Mat & depth, + float depthConstant) +{ + frameId_ = frameId; + stamp_ = stamp; + depth_ = depth; + depthConstant_ = depthConstant; +} + +cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage, + int x, int y, + float cx, float cy, + float fx, float fy) +{ + if(!(x >=0 && x=0 && y::quiet_NaN (), + std::numeric_limits::quiet_NaN (), + std::numeric_limits::quiet_NaN ()); + } + + + cv::Vec3f pt; + + // Use correct principal point from calibration + float center_x = cx; //cameraInfo.K.at(2) + float center_y = cy; //cameraInfo.K.at(5) + + bool isInMM = depthImage.type() == CV_16UC1; // is in mm? + + // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) + float unit_scaling = isInMM?0.001f:1.0f; + float constant_x = unit_scaling / fx; //cameraInfo.K.at(0) + float constant_y = unit_scaling / fy; //cameraInfo.K.at(4) + float bad_point = std::numeric_limits::quiet_NaN (); + + float depth; + bool isValid; + if(isInMM) + { + depth = (float)depthImage.at(y,x); + isValid = depth != 0.0f; + } + else + { + depth = depthImage.at(y,x); + isValid = std::isfinite(depth); + } + + // Check for invalid measurements + if (!isValid) + { + pt.val[0] = pt.val[1] = pt.val[2] = bad_point; + } + else + { + // Fill in XYZ + pt.val[0] = (float(x) - center_x) * depth * constant_x; + pt.val[1] = (float(y) - center_y) * depth * constant_y; + pt.val[2] = depth*unit_scaling; + } + return pt; +} diff --git a/src/ros/FindObjectROS.h b/src/ros/FindObjectROS.h new file mode 100644 index 00000000..0e875794 --- /dev/null +++ b/src/ros/FindObjectROS.h @@ -0,0 +1,79 @@ +/* +Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef FINDOBJECTROS_H_ +#define FINDOBJECTROS_H_ + +#include +#include +#include +#include "find_object/FindObject.h" + +#include +#include +#include +#include +#include + +class FindObjectROS : public find_object::FindObject +{ + Q_OBJECT; + +public: + FindObjectROS(const std::string & objPrefix, QObject * parent = 0); + virtual ~FindObjectROS() {} + +public Q_SLOTS: + void publish(const find_object::DetectionInfo & info); + + void setDepthData(const std::string & frameId, + const ros::Time & stamp, + const cv::Mat & depth, + float depthConstant); + +private: + cv::Vec3f getDepth(const cv::Mat & depthImage, + int x, int y, + float cx, float cy, + float fx, float fy); + + +private: + ros::Publisher pub_; + ros::Publisher pubStamped_; + + std::string frameId_; + ros::Time stamp_; + cv::Mat depth_; + float depthConstant_; + + std::string objFramePrefix_; + tf::TransformBroadcaster tfBroadcaster_; + +}; + +#endif /* FINDOBJECTROS_H_ */ diff --git a/src/ros/find_object_2d_node.cpp b/src/ros/find_object_2d_node.cpp new file mode 100644 index 00000000..eefec798 --- /dev/null +++ b/src/ros/find_object_2d_node.cpp @@ -0,0 +1,201 @@ +/* +Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "CameraROS.h" +#include "FindObjectROS.h" + +#include +#include +#include "find_object/MainWindow.h" +#include "ParametersToolBox.h" +#include "find_object/Settings.h" +#include + +using namespace find_object; + +bool gui; +std::string settingsPath; + +void my_handler_gui(int s){ + QApplication::closeAllWindows(); + QApplication::quit(); +} +void my_handler(int s){ + QCoreApplication::quit(); +} + +void setupQuitSignal(bool gui) +{ + // Catch ctrl-c to close the gui + struct sigaction sigIntHandler; + if(gui) + { + sigIntHandler.sa_handler = my_handler_gui; + } + else + { + sigIntHandler.sa_handler = my_handler; + } + sigemptyset(&sigIntHandler.sa_mask); + sigIntHandler.sa_flags = 0; + sigaction(SIGINT, &sigIntHandler, NULL); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "find_object_2d"); + + gui = true; + std::string objectsPath; + std::string sessionPath; + settingsPath = QDir::homePath().append("/.ros/find_object_2d.ini").toStdString(); + bool subscribeDepth = false; + std::string objFramePrefix = "object"; + + ros::NodeHandle nh("~"); + + nh.param("gui", gui, gui); + nh.param("objects_path", objectsPath, objectsPath); + nh.param("session_path", sessionPath, sessionPath); + nh.param("settings_path", settingsPath, settingsPath); + nh.param("subscribe_depth", subscribeDepth, subscribeDepth); + nh.param("obj_frame_prefix", objFramePrefix, objFramePrefix); + + ROS_INFO("gui=%d", (int)gui); + ROS_INFO("objects_path=%s", objectsPath.c_str()); + ROS_INFO("session_path=%s", sessionPath.c_str()); + ROS_INFO("settings_path=%s", settingsPath.c_str()); + ROS_INFO("subscribe_depth = %s", subscribeDepth?"true":"false"); + ROS_INFO("obj_frame_prefix = %s", objFramePrefix.c_str()); + + if(settingsPath.empty()) + { + settingsPath = QDir::homePath().append("/.ros/find_object_2d.ini").toStdString(); + } + else + { + if(!sessionPath.empty()) + { + ROS_WARN("\"settings_path\" parameter is ignored when \"session_path\" is set."); + } + + QString path = settingsPath.c_str(); + if(path.contains('~')) + { + path.replace('~', QDir::homePath()); + settingsPath = path.toStdString(); + } + } + + // Load settings, should be loaded before creating other objects + Settings::init(settingsPath.c_str()); + + FindObjectROS * findObjectROS = new FindObjectROS(objFramePrefix); + if(!sessionPath.empty()) + { + if(!objectsPath.empty()) + { + ROS_WARN("\"objects_path\" parameter is ignored when \"session_path\" is set."); + } + if(!findObjectROS->loadSession(sessionPath.c_str())) + { + ROS_ERROR("Failed to load session \"%s\"", sessionPath.c_str()); + } + } + else if(!objectsPath.empty()) + { + QString path = objectsPath.c_str(); + if(path.contains('~')) + { + path.replace('~', QDir::homePath()); + } + if(!findObjectROS->loadObjects(path)) + { + ROS_ERROR("No objects loaded from path \"%s\"", path.toStdString().c_str()); + } + } + + CameraROS * camera = new CameraROS(subscribeDepth); + + QObject::connect( + camera, + SIGNAL(rosDataReceived(const std::string &, const ros::Time &, const cv::Mat &, float)), + findObjectROS, + SLOT(setDepthData(const std::string &, const ros::Time &, const cv::Mat &, float))); + + // Catch ctrl-c to close the gui + setupQuitSignal(gui); + + if(gui) + { + QApplication app(argc, argv); + MainWindow mainWindow(findObjectROS, camera); // take ownership + + QObject::connect( + &mainWindow, + SIGNAL(objectsFound(const find_object::DetectionInfo &)), + findObjectROS, + SLOT(publish(const find_object::DetectionInfo &))); + + QStringList topics = camera->subscribedTopics(); + if(topics.size() == 1) + { + mainWindow.setSourceImageText(mainWindow.tr( + "Find-Object subscribed to %1 topic.
" + "You can remap the topic when starting the node:
\"rosrun find_object_2d find_object_2d image:=your/image/topic\".
" + "").arg(topics.first())); + } + else if(topics.size() == 3) + { + mainWindow.setSourceImageText(mainWindow.tr( + "Find-Object subscribed to :
%1
%2
%3
" + "").arg(topics.at(0)).arg(topics.at(1)).arg(topics.at(2))); + } + mainWindow.show(); + app.connect( &app, SIGNAL( lastWindowClosed() ), &app, SLOT( quit() ) ); + + // loop + mainWindow.startProcessing(); + app.exec(); + Settings::saveSettings(); + } + else + { + QCoreApplication app(argc, argv); + + // connect stuff: + QObject::connect(camera, SIGNAL(imageReceived(const cv::Mat &)), findObjectROS, SLOT(detect(const cv::Mat &))); + + //loop + camera->start(); + app.exec(); + + delete camera; + delete findObjectROS; + } + return 0; +} diff --git a/src/ros/print_objects_detected_node.cpp b/src/ros/print_objects_detected_node.cpp new file mode 100644 index 00000000..4af9168a --- /dev/null +++ b/src/ros/print_objects_detected_node.cpp @@ -0,0 +1,115 @@ +/* +Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include +#include +#include + +/** + * IMPORTANT : + * Parameter General/MirrorView must be false + * Parameter Homography/homographyComputed must be true + */ +void objectsDetectedCallback(const std_msgs::Float32MultiArray & msg) +{ + printf("---\n"); + if(msg.data.size()) + { + for(unsigned int i=0; i(0,0) = msg.data[i+3]; + cvHomography.at(1,0) = msg.data[i+4]; + cvHomography.at(2,0) = msg.data[i+5]; + cvHomography.at(0,1) = msg.data[i+6]; + cvHomography.at(1,1) = msg.data[i+7]; + cvHomography.at(2,1) = msg.data[i+8]; + cvHomography.at(0,2) = msg.data[i+9]; + cvHomography.at(1,2) = msg.data[i+10]; + cvHomography.at(2,2) = msg.data[i+11]; + std::vector inPts, outPts; + inPts.push_back(cv::Point2f(0,0)); + inPts.push_back(cv::Point2f(objectWidth,0)); + inPts.push_back(cv::Point2f(0,objectHeight)); + inPts.push_back(cv::Point2f(objectWidth,objectHeight)); + cv::perspectiveTransform(inPts, outPts, cvHomography); + + printf("Object %d detected, CV corners at (%f,%f) (%f,%f) (%f,%f) (%f,%f)\n", + id, + outPts.at(0).x, outPts.at(0).y, + outPts.at(1).x, outPts.at(1).y, + outPts.at(2).x, outPts.at(2).y, + outPts.at(3).x, outPts.at(3).y); + } + } + } + else + { + printf("No objects detected.\n"); + } +} + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "objects_detected"); + + ros::NodeHandle nh; + ros::Subscriber subs; + subs = nh.subscribe("objects", 1, objectsDetectedCallback); + + ros::spin(); + + return 0; +} diff --git a/src/ros/tf_example_node.cpp b/src/ros/tf_example_node.cpp new file mode 100644 index 00000000..3962adc9 --- /dev/null +++ b/src/ros/tf_example_node.cpp @@ -0,0 +1,101 @@ +/* +Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include +#include +#include + +class TfExample +{ +public: + TfExample() : + mapFrameId_("/map"), + objFramePrefix_("object") + { + ros::NodeHandle pnh("~"); + pnh.param("map_frame_id", mapFrameId_, mapFrameId_); + pnh.param("object_prefix", objFramePrefix_, objFramePrefix_); + + ros::NodeHandle nh; + subs_ = nh.subscribe("objectsStamped", 1, &TfExample::objectsDetectedCallback, this); + } + + // Here I synchronize with the ObjectsStamped topic to + // know when the TF is ready and for which objects + void objectsDetectedCallback(const find_object_2d::ObjectsStampedConstPtr & msg) + { + if(msg->objects.data.size()) + { + for(unsigned int i=0; iobjects.data.size(); i+=12) + { + // get data + int id = (int)msg->objects.data[i]; + std::string objectFrameId = QString("%1_%2").arg(objFramePrefix_.c_str()).arg(id).toStdString(); // "object_1", "object_2" + + tf::StampedTransform pose; + tf::StampedTransform poseCam; + try + { + // Get transformation from "object_#" frame to target frame "map" + // The timestamp matches the one sent over TF + tfListener_.lookupTransform(mapFrameId_, objectFrameId, msg->header.stamp, pose); + tfListener_.lookupTransform(msg->header.frame_id, objectFrameId, msg->header.stamp, poseCam); + } + catch(tf::TransformException & ex) + { + ROS_WARN("%s",ex.what()); + continue; + } + + // Here "pose" is the position of the object "id" in "/map" frame. + ROS_INFO("Object_%d [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]", + id, mapFrameId_.c_str(), + pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z(), + pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w()); + ROS_INFO("Object_%d [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]", + id, msg->header.frame_id.c_str(), + poseCam.getOrigin().x(), poseCam.getOrigin().y(), poseCam.getOrigin().z(), + poseCam.getRotation().x(), poseCam.getRotation().y(), poseCam.getRotation().z(), poseCam.getRotation().w()); + } + } + } + +private: + std::string mapFrameId_; + std::string objFramePrefix_; + ros::Subscriber subs_; + tf::TransformListener tfListener_; +}; + +int main(int argc, char * argv[]) +{ + ros::init(argc, argv, "tf_example_node"); + + TfExample sync; + ros::spin(); +} diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index 7ca893a2..ebdc846d 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -2,4 +2,6 @@ ADD_SUBDIRECTORY( tcpClient ) ADD_SUBDIRECTORY( tcpImagesServer ) ADD_SUBDIRECTORY( tcpRequest ) ADD_SUBDIRECTORY( tcpService ) -ADD_SUBDIRECTORY( similarity ) \ No newline at end of file +IF(NONFREE) +ADD_SUBDIRECTORY( similarity ) +ENDIF(NONFREE)