Skip to content

Commit

Permalink
initial commit of ROS wrapper for Astra camera
Browse files Browse the repository at this point in the history
  • Loading branch information
Len Zhong committed May 12, 2016
0 parents commit 361819f
Show file tree
Hide file tree
Showing 83 changed files with 9,510 additions and 0 deletions.
127 changes: 127 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,127 @@
cmake_minimum_required(VERSION 2.8.3)
project(astra_camera)

find_package(catkin REQUIRED camera_info_manager dynamic_reconfigure image_transport nodelet sensor_msgs roscpp message_generation)

find_package(Boost REQUIRED COMPONENTS system thread)


#MACHINE = $(shell uname -m)
execute_process(COMMAND uname -m OUTPUT_VARIABLE MACHINES )
execute_process(COMMAND getconf LONG_BIT OUTPUT_VARIABLE MACHINES_BIT )
MESSAGE(STATUS "ORRBEC Machine : ${MACHINES}")
MESSAGE(STATUS "ORRBEC Machine Bits : ${MACHINES_BIT}")
IF ( (${MACHINES} MATCHES "x86_64") AND (${MACHINES_BIT} MATCHES "64") )
set(HOST_PLATFORM "x64")
# for schroot enviroment
ELSEIF ( (${MACHINES} MATCHES "x86_64") AND (${MACHINES_BIT} MATCHES "32") )
set(HOST_PLATFORM "x86")
ELSEIF ( ${MACHINES} MATCHES "x86" )
ELSEIF ( ${MACHINES} MATCHES "x86" )
set(HOST_PLATFORM "x86")
ELSEIF ( ${MACHINES} MATCHES "i686" )
set(HOST_PLATFORM "x86")
ELSEIF ( ${MACHINES} MATCHES "i386" )
set(HOST_PLATFORM "x86")
ELSEIF ( ${MACHINES} MATCHES "arm" )
set(HOST_PLATFORM "arm")
ELSEIF ( (${MACHINES} MATCHES "aarch64") AND (${MACHINES_BIT} MATCHES "64") )
set(HOST_PLATFORM "arm64")
ELSEIF ( (${MACHINES} MATCHES "aarch64") AND (${MACHINES_BIT} MATCHES "32") )
set(HOST_PLATFORM "arm")
ENDIF ()

MESSAGE(STATUS "ORRBEC : ${HOST_PLATFORM}")

generate_dynamic_reconfigure_options(cfg/Astra.cfg)
add_service_files(FILES
GetSerial.srv)
generate_messages()

catkin_package(
INCLUDE_DIRS include
LIBRARIES astra_wrapper
CATKIN_DEPENDS camera_info_manager dynamic_reconfigure image_transport nodelet sensor_msgs roscpp message_runtime
DEPENDS libastra
)

set(ORRBEC_OPENNI2_REDIST "${CMAKE_CURRENT_SOURCE_DIR}/include/openni2_redist/${HOST_PLATFORM}")
link_directories(${ORRBEC_OPENNI2_REDIST})

#MESSAGE(STATUS "ORRBEC : ${ORRBEC_OPENNI2_REDIST}")

include_directories(include
${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
${CMAKE_CURRENT_SOURCE_DIR}/include/openni2
)


add_library(astra_wrapper
src/astra_convert.cpp
src/astra_device.cpp
src/astra_device_info.cpp
src/astra_timer_filter.cpp
src/astra_frame_listener.cpp
src/astra_device_manager.cpp
src/astra_exception.cpp
src/astra_video_mode.cpp
)
target_link_libraries(astra_wrapper ${catkin_LIBRARIES} -lOpenNI2 -L${ORRBEC_OPENNI2_REDIST}
${Boost_LIBRARIES} )

add_library(astra_driver_lib
src/astra_driver.cpp
)
target_link_libraries(astra_driver_lib astra_wrapper ${catkin_LIBRARIES} ${Boost_LIBRARIES} )
add_dependencies(astra_driver_lib ${PROJECT_NAME}_gencfg)

add_library(astra_camera_nodelet
ros/astra_camera_nodelet.cpp
)
target_link_libraries(astra_camera_nodelet astra_driver_lib ${catkin_LIBRARIES} ${Boost_LIBRARIES} )
add_dependencies(astra_camera_nodelet ${PROJECT_NAME}_gencfg)

add_executable(astra_camera_node
ros/astra_camera_node.cpp
)
target_link_libraries(astra_camera_node astra_driver_lib ${catkin_LIBRARIES} ${Boost_LIBRARIES} )
add_dependencies(astra_camera_node ${PROJECT_NAME}_gencfg ${PROJECT_NAME}_generate_messages_cpp)

add_executable(astra_list_devices
src/list_devices.cpp
)
target_link_libraries(astra_list_devices astra_wrapper)

add_executable(astra_test_wrapper test/test_wrapper.cpp )
target_link_libraries(astra_test_wrapper astra_wrapper ${Boost_LIBRARIES})
if (UNIX AND NOT APPLE)
add_executable(astra_usb_reset src/usb_reset.c)
set(ADDITIONAL_EXECUTABLES "astra_usb_reset")
endif()

install(TARGETS astra_wrapper astra_camera_nodelet astra_camera_node astra_list_devices astra_driver_lib ${ADDITIONAL_EXECUTABLES}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)

# add xml file
install(FILES astra_nodelets.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

#copy runtime file to bin, not support wildcard *.*
#execute_process(COMMAND cp -R ${CMAKE_CURRENT_SOURCE_DIR}/include/openni2_redist/OpenNI2 ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/
# COMMAND cp -R ${CMAKE_CURRENT_SOURCE_DIR}/include/openni2_redist/libOpenNI2.jni.so ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/
# COMMAND cp -R ${CMAKE_CURRENT_SOURCE_DIR}/include/openni2_redist/libOpenNI2.so ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/
# COMMAND cp -R ${CMAKE_CURRENT_SOURCE_DIR}/include/openni2_redist/OpenNI.ini ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/
# COMMAND cp -R ${CMAKE_CURRENT_SOURCE_DIR}/include/openni2_redist/org.openni.jar ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/
#)



33 changes: 33 additions & 0 deletions COPYRIGHT.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
This project includes source code derived from openni2_camera project by Willow Garage.
The original license for openni2_camera project is included below:

/*
* Copyright (c) 2013, Willow Garage, Inc.
* 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 Willow Garage, Inc. 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 OWNER 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.
*
* Author: Julius Kammerl ([email protected])
*/
9 changes: 9 additions & 0 deletions astra_nodelets.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
<library path="lib/libastra_camera_nodelet">

<class name="astra_camera/AstraDriverNodelet" type="astra_camera::AstraDriverNodelet" base_class_type="nodelet::Nodelet">
<description>
Astra camera driver nodelet.
</description>
</class>

</library>
49 changes: 49 additions & 0 deletions cfg/Astra.cfg
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
#! /usr/bin/env python

PACKAGE='astra_camera'

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

# TODO Only offer modes supported by known hardware
output_mode_enum = gen.enum([ gen.const( "SXGA_30Hz", int_t, 1, "1280x1024@30Hz"),
gen.const( "SXGA_15Hz", int_t, 2, "1280x1024@15Hz"),
gen.const( "XGA_30Hz", int_t, 3, "1280x720@30Hz"),
gen.const( "XGA_15Hz", int_t, 4, "1280x720@15Hz"),
gen.const( "VGA_30Hz", int_t, 5, "640x480@30Hz"),
gen.const( "VGA_25Hz", int_t, 6, "640x480@25Hz"),
gen.const( "QVGA_25Hz", int_t, 7, "320x240@25Hz"),
gen.const( "QVGA_30Hz", int_t, 8, "320x240@30Hz"),
gen.const( "QVGA_60Hz", int_t, 9, "320x240@60Hz"),
gen.const( "QQVGA_25Hz", int_t, 10, "160x120@25Hz"),
gen.const( "QQVGA_30Hz", int_t, 11, "160x120@30Hz"),
gen.const( "QQVGA_60Hz", int_t, 12, "160x120@60Hz")],
"output mode")


gen.add("ir_mode", int_t, 0, "Video mode for IR camera", 5, 1, 12, edit_method = output_mode_enum)
gen.add("color_mode", int_t, 0, "Video mode for color camera", 5, 1, 12, edit_method = output_mode_enum)
gen.add("depth_mode", int_t, 0, "Video mode for depth camera", 5, 1, 12, edit_method = output_mode_enum)

gen.add("depth_registration", bool_t, 0, "Depth data registration", True)
gen.add("color_depth_synchronization", bool_t, 0, "Synchronization of color and depth camera", False)
gen.add("auto_exposure", bool_t, 0, "Auto-Exposure", True)
gen.add("auto_white_balance", bool_t, 0, "Auto-White-Balance", True)

gen.add("data_skip", int_t, 0, "Skip N images for every image published (rgb/depth/depth_registered/ir)", 0, 0, 10)

gen.add("ir_time_offset", double_t, 0, "ir image time offset in seconds", -0.033, -1.0, 1.0 );
gen.add("color_time_offset", double_t, 0, "color image time offset in seconds", -0.033, -1.0, 1.0 );
gen.add("depth_time_offset", double_t, 0, "depth image time offset in seconds", -0.033, -1.0, 1.0 );

gen.add("depth_ir_offset_x", double_t, 0, "X offset between IR and depth images", 5.0, -10.0, 10.0)
gen.add("depth_ir_offset_y", double_t, 0, "Y offset between IR and depth images", 4.0, -10.0, 10.0)

gen.add("z_offset_mm", int_t, 0, "Z offset in mm", 0, -200, 200)
gen.add("z_scaling", double_t, 0, "Scaling factor for depth values", 1.0, 0.5, 1.5)

gen.add("use_device_time", bool_t, 0, "Use internal timer of OpenNI device", True)

exit(gen.generate(PACKAGE, "Astra", "Astra"))

54 changes: 54 additions & 0 deletions include/astra_camera/astra_convert.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,54 @@
/*
* Copyright (c) 2016, Orbbec Ltd.
* 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 Orbbec Ltd. 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 OWNER 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.
*
* Author: Tim Liu ([email protected])
*/


#ifndef ASTRA_CONVERT_H_
#define ASTRA_CONVERT_H_

#include "astra_camera/astra_device_info.h"
#include "astra_camera/astra_video_mode.h"

#include "openni2/OpenNI.h"

#include <vector>

namespace astra_wrapper
{

const AstraDeviceInfo astra_convert(const openni::DeviceInfo* pInfo);

const AstraVideoMode astra_convert(const openni::VideoMode& input);
const openni::VideoMode astra_convert(const AstraVideoMode& input);

const std::vector<AstraVideoMode> astra_convert(const openni::Array<openni::VideoMode>& input);
}

#endif
Loading

0 comments on commit 361819f

Please sign in to comment.