merged catkin build into master branch
This commit is contained in:
matlabbe
parent
a4ef95a3bf
commit
84cf6280ac
461
CMakeLists.txt
461
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 "--------------------------------------------")
|
||||
|
||||
27
README.md
27
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.
|
||||
|
||||
9
launch/find_object_2d.launch
Normal file
9
launch/find_object_2d.launch
Normal file
@ -0,0 +1,9 @@
|
||||
<launch>
|
||||
<!-- Nodes -->
|
||||
<node name="find_object_2d" pkg="find_object_2d" type="find_object_2d" output="screen">
|
||||
<remap from="image" to="image"/>
|
||||
<param name="gui" value="false" type="bool"/>
|
||||
<param name="objects_path" value="~/objects" type="str"/>
|
||||
<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/>
|
||||
</node>
|
||||
</launch>
|
||||
9
launch/find_object_2d_gui.launch
Normal file
9
launch/find_object_2d_gui.launch
Normal file
@ -0,0 +1,9 @@
|
||||
<launch>
|
||||
<!-- Nodes -->
|
||||
<node name="find_object_2d" pkg="find_object_2d" type="find_object_2d" output="screen">
|
||||
<remap from="image" to="image"/>
|
||||
<param name="gui" value="true" type="bool"/>
|
||||
<param name="objects_path" value="~/objects" type="str"/>
|
||||
<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/>
|
||||
</node>
|
||||
</launch>
|
||||
29
launch/find_object_3d.launch
Normal file
29
launch/find_object_3d.launch
Normal file
@ -0,0 +1,29 @@
|
||||
<launch>
|
||||
<!-- Example finding 3D poses of the objects detected -->
|
||||
<!-- $roslaunch openni_launch openni.launch depth_registration:=true -->
|
||||
|
||||
<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
|
||||
<param name="gui" value="true" type="bool"/>
|
||||
<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/>
|
||||
<param name="subscribe_depth" value="true" type="bool"/>
|
||||
<param name="objects_path" value="" type="str"/>
|
||||
<param name="object_prefix" value="object" type="str"/>
|
||||
|
||||
<remap from="rgb/image_rect_color" to="camera/rgb/image_rect_color"/>
|
||||
<remap from="depth_registered/image_raw" to="camera/depth_registered/image_raw"/>
|
||||
<remap from="depth_registered/camera_info" to="camera/depth_registered/camera_info"/>
|
||||
</node>
|
||||
|
||||
<!-- Example of tf synchronisation with the objectsStamped message -->
|
||||
<node name="tf_example" pkg="find_object_2d" type="tf_example" output="screen">
|
||||
<param name="map_frame_id" value="/map" type="string"/>
|
||||
<param name="object_prefix" value="object" type="str"/>
|
||||
</node>
|
||||
<!-- fake some tf frames for the example /map -> /odom -> /base_link -> /camera_link -->
|
||||
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
|
||||
args="0.1 0.0 0.3 0.0 0.0 0.0 /base_link /camera_link 100" />
|
||||
<node pkg="tf" type="static_transform_publisher" name="odom_to_base_tf"
|
||||
args="1.0 0.0 0.1 1.5707 0.0 0.0 /odom /base_link 100" />
|
||||
<node pkg="tf" type="static_transform_publisher" name="map_to_odom_tf"
|
||||
args="0.0 0.5 0.0 0.7853 0.0 0.0 /map /odom 100" />
|
||||
</launch>
|
||||
5
msg/ObjectsStamped.msg
Normal file
5
msg/ObjectsStamped.msg
Normal file
@ -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
|
||||
@ -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()
|
||||
|
||||
168
src/ros/CameraROS.cpp
Normal file
168
src/ros/CameraROS.cpp
Normal file
@ -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 <opencv2/imgproc/imgproc.hpp>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
|
||||
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>("ros::Time");
|
||||
qRegisterMetaType<cv::Mat>("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>(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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
90
src/ros/CameraROS.h
Normal file
90
src/ros/CameraROS.h
Normal file
@ -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 <ros/ros.h>
|
||||
#include <ros/spinner.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <image_transport/subscriber_filter.h>
|
||||
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
|
||||
#include "find_object/Camera.h"
|
||||
#include <QtCore/QStringList>
|
||||
|
||||
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<sensor_msgs::CameraInfo> cameraInfoSub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MySyncPolicy;
|
||||
message_filters::Synchronizer<MySyncPolicy> * sync_;
|
||||
};
|
||||
|
||||
#endif /* CAMERAROS_H_ */
|
||||
239
src/ros/FindObjectROS.cpp
Normal file
239
src/ros/FindObjectROS.cpp
Normal file
@ -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 <std_msgs/Float32MultiArray.h>
|
||||
#include "find_object_2d/ObjectsStamped.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
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<std_msgs::Float32MultiArray>("objects", 1);
|
||||
pubStamped_ = nh.advertise<find_object_2d::ObjectsStamped>("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<tf::StampedTransform> transforms;
|
||||
QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
|
||||
for(QMultiMap<int, QTransform>::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<float>(info.objDetected_.size()*12);
|
||||
msgStamped.objects.data = std::vector<float>(info.objDetected_.size()*12);
|
||||
int i=0;
|
||||
QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
|
||||
for(QMultiMap<int, QTransform>::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<depthImage.cols && y >=0 && y<depthImage.rows))
|
||||
{
|
||||
ROS_ERROR("Point must be inside the image (x=%d, y=%d), image size=(%d,%d)",
|
||||
x, y,
|
||||
depthImage.cols, depthImage.rows);
|
||||
return cv::Vec3f(
|
||||
std::numeric_limits<float>::quiet_NaN (),
|
||||
std::numeric_limits<float>::quiet_NaN (),
|
||||
std::numeric_limits<float>::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<float>::quiet_NaN ();
|
||||
|
||||
float depth;
|
||||
bool isValid;
|
||||
if(isInMM)
|
||||
{
|
||||
depth = (float)depthImage.at<uint16_t>(y,x);
|
||||
isValid = depth != 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
depth = depthImage.at<float>(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;
|
||||
}
|
||||
79
src/ros/FindObjectROS.h
Normal file
79
src/ros/FindObjectROS.h
Normal file
@ -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 <ros/ros.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include "find_object/FindObject.h"
|
||||
|
||||
#include <QtCore/QObject>
|
||||
#include <QtCore/QMultiMap>
|
||||
#include <QtCore/QPair>
|
||||
#include <QtCore/QRect>
|
||||
#include <QtGui/QTransform>
|
||||
|
||||
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_ */
|
||||
201
src/ros/find_object_2d_node.cpp
Normal file
201
src/ros/find_object_2d_node.cpp
Normal file
@ -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 <QApplication>
|
||||
#include <QDir>
|
||||
#include "find_object/MainWindow.h"
|
||||
#include "ParametersToolBox.h"
|
||||
#include "find_object/Settings.h"
|
||||
#include <signal.h>
|
||||
|
||||
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(
|
||||
"<qt>Find-Object subscribed to <b>%1</b> topic.<br/>"
|
||||
"You can remap the topic when starting the node: <br/>\"rosrun find_object_2d find_object_2d image:=your/image/topic\".<br/>"
|
||||
"</qt>").arg(topics.first()));
|
||||
}
|
||||
else if(topics.size() == 3)
|
||||
{
|
||||
mainWindow.setSourceImageText(mainWindow.tr(
|
||||
"<qt>Find-Object subscribed to : <br/> <b>%1</b> <br/> <b>%2</b> <br/> <b>%3</b><br/>"
|
||||
"</qt>").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;
|
||||
}
|
||||
115
src/ros/print_objects_detected_node.cpp
Normal file
115
src/ros/print_objects_detected_node.cpp
Normal file
@ -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 <ros/ros.h>
|
||||
#include <std_msgs/Float32MultiArray.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <QTransform>
|
||||
|
||||
/**
|
||||
* 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<msg.data.size(); i+=12)
|
||||
{
|
||||
// get data
|
||||
int id = (int)msg.data[i];
|
||||
float objectWidth = msg.data[i+1];
|
||||
float objectHeight = msg.data[i+2];
|
||||
|
||||
// Find corners Qt
|
||||
QTransform qtHomography(msg.data[i+3], msg.data[i+4], msg.data[i+5],
|
||||
msg.data[i+6], msg.data[i+7], msg.data[i+8],
|
||||
msg.data[i+9], msg.data[i+10], msg.data[i+11]);
|
||||
|
||||
QPointF qtTopLeft = qtHomography.map(QPointF(0,0));
|
||||
QPointF qtTopRight = qtHomography.map(QPointF(objectWidth,0));
|
||||
QPointF qtBottomLeft = qtHomography.map(QPointF(0,objectHeight));
|
||||
QPointF qtBottomRight = qtHomography.map(QPointF(objectWidth,objectHeight));
|
||||
|
||||
printf("Object %d detected, Qt corners at (%f,%f) (%f,%f) (%f,%f) (%f,%f)\n",
|
||||
id,
|
||||
qtTopLeft.x(), qtTopLeft.y(),
|
||||
qtTopRight.x(), qtTopRight.y(),
|
||||
qtBottomLeft.x(), qtBottomLeft.y(),
|
||||
qtBottomRight.x(), qtBottomRight.y());
|
||||
|
||||
// Example with OpenCV
|
||||
if(0)
|
||||
{
|
||||
// Find corners OpenCV
|
||||
cv::Mat cvHomography(3, 3, CV_32F);
|
||||
cvHomography.at<float>(0,0) = msg.data[i+3];
|
||||
cvHomography.at<float>(1,0) = msg.data[i+4];
|
||||
cvHomography.at<float>(2,0) = msg.data[i+5];
|
||||
cvHomography.at<float>(0,1) = msg.data[i+6];
|
||||
cvHomography.at<float>(1,1) = msg.data[i+7];
|
||||
cvHomography.at<float>(2,1) = msg.data[i+8];
|
||||
cvHomography.at<float>(0,2) = msg.data[i+9];
|
||||
cvHomography.at<float>(1,2) = msg.data[i+10];
|
||||
cvHomography.at<float>(2,2) = msg.data[i+11];
|
||||
std::vector<cv::Point2f> 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;
|
||||
}
|
||||
101
src/ros/tf_example_node.cpp
Normal file
101
src/ros/tf_example_node.cpp
Normal file
@ -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 <ros/ros.h>
|
||||
#include <tf/transform_listener.h>
|
||||
#include <find_object_2d/ObjectsStamped.h>
|
||||
#include <QtCore/QString>
|
||||
|
||||
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; i<msg->objects.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();
|
||||
}
|
||||
@ -2,4 +2,6 @@ ADD_SUBDIRECTORY( tcpClient )
|
||||
ADD_SUBDIRECTORY( tcpImagesServer )
|
||||
ADD_SUBDIRECTORY( tcpRequest )
|
||||
ADD_SUBDIRECTORY( tcpService )
|
||||
ADD_SUBDIRECTORY( similarity )
|
||||
IF(NONFREE)
|
||||
ADD_SUBDIRECTORY( similarity )
|
||||
ENDIF(NONFREE)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user