├── openni2_launch ├── CATKIN_IGNORE ├── src │ └── openni2_launch │ │ ├── __init__.py │ │ └── wtf_plugin.py ├── rosdoc.yaml ├── setup.py ├── CMakeLists.txt ├── launch │ ├── includes │ │ ├── manager.launch │ │ ├── ir.launch │ │ ├── color.launch │ │ ├── processing.launch │ │ ├── depth.launch │ │ ├── pointclouds.launch │ │ ├── device.launch │ │ └── device.launch.xml │ ├── kinect_frames.launch │ ├── openni2_tf_prefix.launch │ └── openni2.launch ├── package.xml ├── CHANGELOG.rst └── doc │ ├── index.rst │ ├── Makefile │ └── conf.py ├── openni2_camera ├── srv │ └── GetSerial.srv ├── package.xml ├── launch │ ├── tfs.launch.py │ ├── camera_only.launch.py │ ├── camera_with_cloud_norgb.launch.py │ └── camera_with_cloud.launch.py ├── include │ └── openni2_camera │ │ ├── openni2_device_info.h │ │ ├── openni2_timer_filter.h │ │ ├── openni2_convert.h │ │ ├── openni2_frame_listener.h │ │ ├── openni2_video_mode.h │ │ ├── openni2_device_manager.h │ │ ├── openni2_exception.h │ │ ├── openni2_device.h │ │ └── openni2_driver.h ├── src │ ├── openni2_device_info.cpp │ ├── usb_reset.c │ ├── list_devices.cpp │ ├── openni2_timer_filter.cpp │ ├── openni2_exception.cpp │ ├── openni2_convert.cpp │ ├── openni2_video_mode.cpp │ ├── openni2_frame_listener.cpp │ ├── openni2_device_manager.cpp │ └── openni2_device.cpp ├── CMakeLists.txt ├── test │ └── test_wrapper.cpp └── CHANGELOG.rst ├── .github └── workflows │ └── ci.yml ├── LICENSE └── README.md /openni2_launch/CATKIN_IGNORE: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /openni2_launch/src/openni2_launch/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /openni2_camera/srv/GetSerial.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string serial 3 | -------------------------------------------------------------------------------- /openni2_launch/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | sphinx_root_dir: doc 3 | -------------------------------------------------------------------------------- /openni2_launch/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=["openni2_launch"], 9 | package_dir={'': 'src'}) 10 | 11 | setup(**setup_args) 12 | -------------------------------------------------------------------------------- /openni2_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(openni2_launch) 3 | 4 | find_package(catkin) 5 | catkin_python_setup() 6 | catkin_package() 7 | 8 | install(DIRECTORY launch 9 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ 10 | ) 11 | 12 | if (CATKIN_ENABLE_TESTING) 13 | find_package(roslaunch REQUIRED) 14 | roslaunch_add_file_check(launch) 15 | # roslaunch_add_file_check(launch/includes) # 20181003 this fails. 16 | endif() 17 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | industrial_ci: 7 | name: ROS2 (${{ matrix.env.ROS_DISTRO }}) 8 | strategy: 9 | fail-fast: false 10 | matrix: 11 | env: 12 | - {ROS_DISTRO: rolling, ROS_REPO: ros, ABICHECK_URL: "github:ros-drivers/openni2_camera#ros2"} 13 | runs-on: ubuntu-22.04 14 | steps: 15 | - uses: actions/checkout@v2 16 | - uses: 'ros-industrial/industrial_ci@master' 17 | env: ${{ matrix.env }} 18 | -------------------------------------------------------------------------------- /openni2_launch/launch/includes/manager.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /openni2_launch/launch/kinect_frames.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /openni2_launch/launch/includes/ir.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /openni2_launch/launch/includes/color.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /openni2_launch/launch/includes/processing.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /openni2_launch/launch/includes/depth.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /openni2_launch/launch/includes/pointclouds.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /openni2_camera/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | openni2_camera 4 | 2.3.0 5 | Drivers for the Asus Xtion and Primesense Devices. For using a kinect 6 | with ROS, try the freenect stack 7 | 8 | Julius Kammerl 9 | BSD 10 | 11 | Michael Ferguson 12 | Isaac I. Y. Saito 13 | 14 | ament_cmake 15 | rosidl_default_generators 16 | 17 | pkg-config 18 | builtin_interfaces 19 | camera_info_manager 20 | libopenni2-dev 21 | sensor_msgs 22 | rclcpp 23 | rclcpp_components 24 | image_transport 25 | depth_image_proc 26 | rosidl_default_runtime 27 | 28 | rosidl_interface_packages 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /openni2_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | openni2_launch 4 | 1.6.0 5 | 6 | Launch files to start the openni2_camera drivers using rgbd_launch. 7 | 8 | Isaac I. Y. Saito 9 | BSD 10 | 11 | Julius Kammerl 12 | Michael Ferguson 13 | 14 | catkin 15 | python-catkin-pkg 16 | python3-catkin-pkg 17 | roslaunch 18 | rgbd_launch 19 | depth_image_proc 20 | image_proc 21 | nodelet 22 | openni2_camera 23 | rospy 24 | roswtf 25 | tf 26 | usbutils 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /openni2_launch/launch/includes/device.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2013, Willow Garage, Inc. 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | 3. Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /openni2_camera/launch/tfs.launch.py: -------------------------------------------------------------------------------- 1 | import launch 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.substitutions import LaunchConfiguration 4 | from launch_ros.actions import Node 5 | 6 | 7 | def generate_launch_description(): 8 | 9 | namespace_param_name = "namespace" 10 | namespace = LaunchConfiguration(namespace_param_name) 11 | namespace_launch_arg = DeclareLaunchArgument(namespace_param_name) 12 | 13 | tf_prefix_param_name = "tf_prefix" 14 | tf_prefix = LaunchConfiguration(tf_prefix_param_name) 15 | tf_prefix_launch_arg = DeclareLaunchArgument(tf_prefix_param_name) 16 | 17 | tf_args = [ 18 | ["--frame-id", [tf_prefix,"/",namespace,"_link"], 19 | "--child-frame-id", [tf_prefix,"/",namespace,"_depth_frame"], 20 | "--y", "-0.02"], 21 | ["--frame-id", [tf_prefix,"/",namespace,"_link"], 22 | "--child-frame-id", [tf_prefix,"/",namespace,"_rgb_frame"], 23 | "--y", "-0.045"], 24 | ["--frame-id", [tf_prefix,"/",namespace,"_depth_frame"], 25 | "--child-frame-id", [tf_prefix,"/",namespace,"_depth_optical_frame"], 26 | "--roll", "-1.5707963267948966", "--yaw", "-1.5707963267948966"], 27 | ["--frame-id", [tf_prefix,"/",namespace,"_rgb_frame"], 28 | "--child-frame-id", [tf_prefix,"/",namespace,"_rgb_optical_frame"], 29 | "--roll", "-1.5707963267948966", "--yaw", "-1.5707963267948966"], 30 | ] 31 | 32 | tf_nodes = [Node(package='tf2_ros', executable='static_transform_publisher', output='screen', arguments=args) for args in tf_args] 33 | 34 | return launch.LaunchDescription([namespace_launch_arg, tf_prefix_launch_arg] + tf_nodes) 35 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # openni2_camera 2 | 3 | ## Introduction 4 | ROS wrapper for openni 2.0 5 | 6 | Note: openni2_camera supports xtion devices, but not kinects. For using a kinect with ROS, try the freenect stack: http://www.ros.org/wiki/freenect_stack 7 | 8 | ## Contribution 9 | 10 | Branching: 11 | - ROS2: 12 | - The [ros2](https://github.com/ros-drivers/openni2_camera/tree/ros2) branch supports Rolling 13 | - The [jazzy](https://github.com/ros-drivers/openni2_camera/tree/jazzy) branch supports Jazzy and Kilted 14 | - The [iron](https://github.com/ros-drivers/openni2_camera/tree/iron) branch supports Humble to Iron 15 | - openni2_launch has NOT been ported yet 16 | - ROS1: [ros1](https://github.com/ros-drivers/openni2_camera/tree/ros1) branch (no longer maintained) 17 | 18 | ## Developer document 19 | - [docs.ros.org/openni2_launch](http://docs.ros.org/en/melodic/api/openni2_launch/html/) 20 | - Source of the doc: [openni2_launch/doc](./openni2_launch/doc/) 21 | 22 | ## Running ROS2 Driver 23 | 24 | An example launch exists that loads just the camera component: 25 | 26 | ``` 27 | ros2 launch openni2_camera camera_only.launch.py 28 | ``` 29 | 30 | If you want to get a PointCloud2, use: 31 | 32 | ``` 33 | ros2 launch openni2_camera camera_with_cloud.launch.py 34 | ``` 35 | 36 | ## Migration from ROS1 37 | 38 | * The rgb/image topic has been renamed to rgb/image_raw for consistency. 39 | * The nodelet has been refactored into an rclcpp component called 40 | "openni2_wrapper::OpenNI2Driver". See the launch folder for an example 41 | of how to start this. 42 | * rgbd_launch and openni2_launch have not yet been ported (although it 43 | is now possible since lazy pub/sub is implemented). It is recommended 44 | to create a launch file with the specific pipeline you want. 45 | See the launch folder for an example. 46 | 47 | ## Known Issues 48 | 49 | * Using "use_device_time" is currently broken. 50 | -------------------------------------------------------------------------------- /openni2_camera/include/openni2_camera/openni2_device_info.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | 32 | #ifndef OPENNI2_DEVICE_INFO_H_ 33 | #define OPENNI2_DEVICE_INFO_H_ 34 | 35 | #include 36 | #include 37 | 38 | namespace openni2_wrapper 39 | { 40 | 41 | struct OpenNI2DeviceInfo 42 | { 43 | std::string uri_; 44 | std::string vendor_; 45 | std::string name_; 46 | uint16_t vendor_id_; 47 | uint16_t product_id_; 48 | }; 49 | 50 | std::ostream& operator << (std::ostream& stream, const OpenNI2DeviceInfo& device_info); 51 | 52 | } 53 | 54 | #endif /* DRIVER_H_ */ 55 | -------------------------------------------------------------------------------- /openni2_camera/include/openni2_camera/openni2_timer_filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | 32 | #ifndef OPENNI2_TIME_FILTER_H_ 33 | #define OPENNI2_TIME_FILTER_H_ 34 | 35 | #include 36 | 37 | #include "OpenNI.h" 38 | 39 | namespace openni2_wrapper 40 | { 41 | 42 | class OpenNI2TimerFilter 43 | { 44 | public: 45 | OpenNI2TimerFilter(std::size_t filter_len); 46 | virtual ~OpenNI2TimerFilter(); 47 | 48 | void addSample(double sample); 49 | 50 | double getMedian(); 51 | double getMovingAvg(); 52 | 53 | void clear(); 54 | 55 | private: 56 | std::size_t filter_len_; 57 | 58 | std::deque buffer_; 59 | }; 60 | 61 | } 62 | 63 | #endif 64 | -------------------------------------------------------------------------------- /openni2_camera/include/openni2_camera/openni2_convert.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | 32 | 33 | #ifndef OPENNI2_CONVERT_H_ 34 | #define OPENNI2_CONVERT_H_ 35 | 36 | #include "openni2_camera/openni2_device_info.h" 37 | #include "openni2_camera/openni2_video_mode.h" 38 | 39 | #include "OpenNI.h" 40 | 41 | #include 42 | 43 | namespace openni2_wrapper 44 | { 45 | 46 | const OpenNI2DeviceInfo openni2_convert(const openni::DeviceInfo* pInfo); 47 | 48 | const OpenNI2VideoMode openni2_convert(const openni::VideoMode& input); 49 | const openni::VideoMode openni2_convert(const OpenNI2VideoMode& input); 50 | 51 | const std::vector openni2_convert(const openni::Array& input); 52 | } 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /openni2_camera/src/openni2_device_info.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | 32 | #include "openni2_camera/openni2_device_info.h" 33 | 34 | namespace openni2_wrapper 35 | { 36 | 37 | 38 | std::ostream& operator << (std::ostream& stream, const OpenNI2DeviceInfo& device_info) { 39 | stream << "Uri: " << device_info.uri_ << " (Vendor: " << device_info.vendor_ << 40 | ", Name: " << device_info.name_ << 41 | ", Vendor ID: " << std::hex << device_info.vendor_id_ << 42 | ", Product ID: " << std::hex << device_info.product_id_ << 43 | ")" << std::endl; 44 | return stream; 45 | } 46 | 47 | 48 | 49 | } //namespace openni2_wrapper 50 | -------------------------------------------------------------------------------- /openni2_camera/include/openni2_camera/openni2_frame_listener.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | 32 | #ifndef OPENNI2_FRAME_LISTENER_H_ 33 | #define OPENNI2_FRAME_LISTENER_H_ 34 | 35 | #include "openni2_camera/openni2_device.h" 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | #include "OpenNI.h" 43 | 44 | namespace openni2_wrapper 45 | { 46 | 47 | class OpenNI2TimerFilter; 48 | 49 | class OpenNI2FrameListener : public openni::VideoStream::NewFrameListener 50 | { 51 | public: 52 | OpenNI2FrameListener(rclcpp::Node* node); 53 | 54 | virtual ~OpenNI2FrameListener() 55 | { }; 56 | 57 | void onNewFrame(openni::VideoStream& stream); 58 | 59 | void setCallback(FrameCallbackFunction& callback) 60 | { 61 | callback_ = callback; 62 | } 63 | 64 | void setUseDeviceTimer(bool enable); 65 | 66 | private: 67 | openni::VideoFrameRef m_frame; 68 | 69 | FrameCallbackFunction callback_; 70 | 71 | bool use_device_timer_; 72 | std::shared_ptr timer_filter_; 73 | 74 | rclcpp::Node* node_; // needed for time 75 | rclcpp::Time prev_time_stamp_; 76 | }; 77 | 78 | } 79 | 80 | #endif 81 | -------------------------------------------------------------------------------- /openni2_camera/src/usb_reset.c: -------------------------------------------------------------------------------- 1 | /* usbreset -- send a USB port reset to a USB device */ 2 | 3 | /* 4 | * Copyright (c) 2014, JSK Robotics Lab, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Willow Garage, Inc. nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | * Author: Ryohei Ueda (ueda@jsk.t.u-tokyo.ac.jp) 32 | */ 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | 43 | int main(int argc, char **argv) 44 | { 45 | const char *filename; 46 | int fd; 47 | int rc; 48 | 49 | if (argc != 2) { 50 | fprintf(stderr, "Usage: usb_reset device-filename\n"); 51 | fprintf(stderr, "\n"); 52 | fprintf(stderr, "For instance if lsusb shows 'Bus 002 Device 013' for your camera\n"); 53 | fprintf(stderr, "You would call with:\n"); 54 | fprintf(stderr, " ros2 run openni2_camera usb_reset /dev/bus/usb/002/013\n"); 55 | return 1; 56 | } 57 | filename = argv[1]; 58 | 59 | fd = open(filename, O_WRONLY); 60 | if (fd < 0) { 61 | perror("Error opening output file"); 62 | return 1; 63 | } 64 | 65 | printf("Resetting USB device %s\n", filename); 66 | rc = ioctl(fd, USBDEVFS_RESET, 0); 67 | if (rc < 0) { 68 | perror("Error in ioctl"); 69 | return 1; 70 | } 71 | printf("Reset successful\n"); 72 | 73 | close(fd); 74 | return 0; 75 | } 76 | -------------------------------------------------------------------------------- /openni2_camera/include/openni2_camera/openni2_video_mode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | 32 | #ifndef OPENNI2_VIDEO_MODE_H_ 33 | #define OPENNI2_VIDEO_MODE_H_ 34 | 35 | #include 36 | #include 37 | 38 | namespace openni2_wrapper 39 | { 40 | 41 | // copied from OniEnums.h 42 | typedef enum 43 | { 44 | // Depth 45 | PIXEL_FORMAT_DEPTH_1_MM = 100, 46 | PIXEL_FORMAT_DEPTH_100_UM = 101, 47 | PIXEL_FORMAT_SHIFT_9_2 = 102, 48 | PIXEL_FORMAT_SHIFT_9_3 = 103, 49 | 50 | // Color 51 | PIXEL_FORMAT_RGB888 = 200, 52 | PIXEL_FORMAT_YUV422 = 201, 53 | PIXEL_FORMAT_GRAY8 = 202, 54 | PIXEL_FORMAT_GRAY16 = 203, 55 | PIXEL_FORMAT_JPEG = 204, 56 | } PixelFormat; 57 | 58 | struct OpenNI2VideoMode 59 | { 60 | std::size_t x_resolution_; 61 | std::size_t y_resolution_; 62 | double frame_rate_; 63 | PixelFormat pixel_format_; 64 | }; 65 | 66 | std::ostream& operator << (std::ostream& stream, const OpenNI2VideoMode& video_mode); 67 | 68 | bool operator==(const OpenNI2VideoMode& video_mode_a, const OpenNI2VideoMode& video_mode_b); 69 | bool operator!=(const OpenNI2VideoMode& video_mode_a, const OpenNI2VideoMode& video_mode_b); 70 | 71 | } 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /openni2_launch/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package openni2_launch 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.6.0 (2022-04-12) 6 | ------------------ 7 | * [capability] Add depth_registered and depth_registered_filtered arguments `#111 `_ 8 | 9 | 1.5.1 (2021-02-01) 10 | ------------------ 11 | * Default branch update for ROS1 development. 12 | * [CI] Add GitHub Actions configs 13 | 14 | 1.5.0 (2021-01-12) 15 | ------------------ 16 | * [capability] Use serialnumber in camera names `#101 `_ 17 | * Release for Noetic `#98 `_ 18 | 19 | 1.4.2 (2020-06-03) 20 | ------------------ 21 | * update package.xml for noetic support 22 | * Contributors: Michael Ferguson 23 | 24 | 0.4.2 (2018-12-01) 25 | ------------------ 26 | * Fix roswtf plugin dependency `#92 `_ 27 | * Contributors: Isaac I.Y. Saito 28 | 29 | 0.4.1 (2018-10-20) 30 | ------------------ 31 | * [fix] [roswtf plugin] Accept vendor and product IDs (fix `#88 `_). 32 | * [improve] Remove unused ROS Parameters. `#76 `_ 33 | * [improve] Add roslaunch check. 34 | * Contributors: Isaac I.Y. Saito 35 | 36 | 0.4.0 (2018-07-17) 37 | ------------------ 38 | * Add a simple roswtf plugin. `#80 `_ 39 | * Contributors: Isaac I.Y. Saito, PlusOne Robotics Inc. 40 | 41 | 0.3.0 (2017-11-03) 42 | ------------------ 43 | * Move openni2_launch package into openni2_camera repository `#55 `_ 44 | * Add rosdoc-based document. 45 | * Contributors: Isaac I.Y. Saito 46 | 47 | 0.2.2 (2015-01-23) 48 | ------------------ 49 | * add tf_prefix version, as per `#16 `_ 50 | * fix `#19 `_, reverts `#16 `_ 51 | * Contributors: Michael Ferguson 52 | 53 | 0.2.1 (2014-05-22) 54 | ------------------ 55 | * Force device_id to string type 56 | * Contributors: Dariush Forouher, Michael Ferguson 57 | 58 | 0.2.0 (2014-05-20) 59 | ------------------ 60 | * Remove machine arg, as it is not necessary. 61 | * Add tf_prefix same as openni 62 | * Defaults for depth processing are set apropriately for both hardware and software registration. 63 | * Contributors: Libor Wagner, Mark Pitchless, Michael Ferguson, Piyush Khandelwal 64 | 65 | 0.1.2 (2013-09-30) 66 | ------------------ 67 | * Expose driver parameters. Note: depth_registration is now off by default. 68 | 69 | 0.1.1 (2013-09-25) 70 | ------------------ 71 | * Properly namespace the nodelet manager 72 | 73 | 0.1.0 (2013-09-10) 74 | ------------------ 75 | * First release 76 | * This package is a thin wrapper around rgbd_launch 77 | -------------------------------------------------------------------------------- /openni2_camera/src/list_devices.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Savioke, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Stephan Wirth (wirth@savioke.com) 30 | */ 31 | 32 | /** 33 | * Small executable that creates a device manager to print the information of all devices including their 34 | * serial number. 35 | */ 36 | 37 | #include 38 | #include "openni2_camera/openni2_device_manager.h" 39 | #include "openni2_camera/openni2_exception.h" 40 | 41 | using openni2_wrapper::OpenNI2DeviceManager; 42 | using openni2_wrapper::OpenNI2DeviceInfo; 43 | using openni2_wrapper::OpenNI2Exception; 44 | 45 | int main(int arc, char** argv) 46 | { 47 | openni2_wrapper::OpenNI2DeviceManager manager; 48 | std::shared_ptr > device_infos = manager.getConnectedDeviceInfos(); 49 | std::cout << "Found " << device_infos->size() << " devices:" << std::endl << std::endl; 50 | for (size_t i = 0; i < device_infos->size(); ++i) 51 | { 52 | std::cout << "Device #" << i << ":" << std::endl; 53 | std::cout << device_infos->at(i) << std::endl; 54 | try { 55 | std::string serial = manager.getSerial(device_infos->at(i).uri_); 56 | std::cout << "Serial number: " << serial << std::endl; 57 | } 58 | catch (const OpenNI2Exception& exception) 59 | { 60 | std::cerr << "Could not retrieve serial number: " << exception.what() << std::endl; 61 | } 62 | } 63 | return 0; 64 | } 65 | 66 | -------------------------------------------------------------------------------- /openni2_camera/include/openni2_camera/openni2_device_manager.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | 32 | #ifndef OPENNI2_DEVICE_MANAGER_H_ 33 | #define OPENNI2_DEVICE_MANAGER_H_ 34 | 35 | #include "openni2_camera/openni2_device_info.h" 36 | 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | namespace openni2_wrapper 44 | { 45 | 46 | class OpenNI2DeviceListener; 47 | class OpenNI2Device; 48 | 49 | class OpenNI2DeviceManager 50 | { 51 | public: 52 | OpenNI2DeviceManager(); 53 | virtual ~OpenNI2DeviceManager(); 54 | 55 | static std::shared_ptr getSingelton(); 56 | 57 | std::shared_ptr > getConnectedDeviceInfos() const; 58 | std::shared_ptr > getConnectedDeviceURIs() const; 59 | std::size_t getNumOfConnectedDevices() const; 60 | 61 | std::shared_ptr getAnyDevice(rclcpp::Node* node); 62 | std::shared_ptr getDevice(const std::string& device_URI, rclcpp::Node* node); 63 | 64 | std::string getSerial(const std::string& device_URI) const; 65 | 66 | protected: 67 | std::shared_ptr device_listener_; 68 | 69 | static std::shared_ptr singelton_; 70 | }; 71 | 72 | 73 | std::ostream& operator <<(std::ostream& stream, const OpenNI2DeviceManager& device_manager); 74 | 75 | } 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /openni2_camera/src/openni2_timer_filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | 32 | #include "openni2_camera/openni2_timer_filter.h" 33 | #include 34 | 35 | 36 | namespace openni2_wrapper 37 | { 38 | 39 | OpenNI2TimerFilter::OpenNI2TimerFilter(std::size_t filter_len): 40 | filter_len_(filter_len) 41 | { 42 | } 43 | 44 | OpenNI2TimerFilter::~OpenNI2TimerFilter() 45 | { 46 | } 47 | 48 | void OpenNI2TimerFilter::addSample(double sample) 49 | { 50 | buffer_.push_back(sample); 51 | if (buffer_.size()>filter_len_) 52 | buffer_.pop_front(); 53 | } 54 | 55 | double OpenNI2TimerFilter::getMedian() 56 | { 57 | if (buffer_.size()>0) 58 | { 59 | std::deque sort_buffer = buffer_; 60 | 61 | std::sort(sort_buffer.begin(), sort_buffer.end()); 62 | 63 | return sort_buffer[sort_buffer.size()/2]; 64 | } else 65 | return 0.0; 66 | } 67 | 68 | double OpenNI2TimerFilter::getMovingAvg() 69 | { 70 | if (buffer_.size() > 0) 71 | { 72 | double sum = 0; 73 | 74 | std::deque::const_iterator it = buffer_.begin(); 75 | std::deque::const_iterator it_end = buffer_.end(); 76 | 77 | while (it != it_end) 78 | { 79 | sum += *(it++); 80 | } 81 | 82 | return sum / static_cast(buffer_.size()); 83 | } else 84 | return 0.0; 85 | } 86 | 87 | 88 | void OpenNI2TimerFilter::clear() 89 | { 90 | buffer_.clear(); 91 | } 92 | 93 | 94 | } //namespace openni2_wrapper 95 | -------------------------------------------------------------------------------- /openni2_camera/src/openni2_exception.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011 2011 Willow Garage, Inc. 5 | * Suat Gedikli 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of Willow Garage, Inc. nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | #include "openni2_camera/openni2_exception.h" 38 | #include 39 | 40 | namespace openni2_wrapper 41 | { 42 | 43 | OpenNI2Exception::OpenNI2Exception (const std::string& function_name, const std::string& file_name, unsigned line_number, const std::string& message) throw () 44 | : function_name_ (function_name) 45 | , file_name_ (file_name) 46 | , line_number_ (line_number) 47 | , message_ (message) 48 | { 49 | std::stringstream sstream; 50 | sstream << function_name_ << " @ " << file_name_ << " @ " << line_number_ << " : " << message_; 51 | message_long_ = sstream.str(); 52 | } 53 | 54 | OpenNI2Exception::~OpenNI2Exception () throw () 55 | { 56 | } 57 | 58 | OpenNI2Exception& OpenNI2Exception::operator = (const OpenNI2Exception& exception) throw () 59 | { 60 | message_ = exception.message_; 61 | return *this; 62 | } 63 | 64 | const char* OpenNI2Exception::what () const throw () 65 | { 66 | return message_long_.c_str(); 67 | } 68 | 69 | const std::string& OpenNI2Exception::getFunctionName () const throw () 70 | { 71 | return function_name_; 72 | } 73 | 74 | const std::string& OpenNI2Exception::getFileName () const throw () 75 | { 76 | return file_name_; 77 | } 78 | 79 | unsigned OpenNI2Exception::getLineNumber () const throw () 80 | { 81 | return line_number_; 82 | } 83 | 84 | } //namespace openni_camera 85 | -------------------------------------------------------------------------------- /openni2_launch/launch/includes/device.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 33 | 34 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /openni2_camera/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(openni2_camera) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif() 7 | 8 | find_package(ament_cmake REQUIRED) 9 | find_package(builtin_interfaces REQUIRED) 10 | find_package(camera_info_manager REQUIRED) 11 | find_package(image_transport REQUIRED) 12 | find_package(sensor_msgs REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(rclcpp_components REQUIRED) 15 | find_package(rosidl_default_generators REQUIRED) 16 | 17 | find_package(PkgConfig) 18 | pkg_check_modules(PC_OPENNI2 libopenni2) 19 | if (NOT PC_OPENNI2_FOUND) 20 | pkg_check_modules(PC_OPENNI2 REQUIRED openni2) 21 | endif() 22 | 23 | rosidl_generate_interfaces(${PROJECT_NAME} 24 | "srv/GetSerial.srv" 25 | DEPENDENCIES builtin_interfaces 26 | ) 27 | 28 | include_directories( 29 | include 30 | ${PC_OPENNI2_INCLUDE_DIRS} 31 | ) 32 | 33 | link_directories(${PC_OPENNI2_LIBRARY_DIRS}) 34 | 35 | add_library(openni2_wrapper SHARED 36 | src/openni2_convert.cpp 37 | src/openni2_device.cpp 38 | src/openni2_device_info.cpp 39 | src/openni2_timer_filter.cpp 40 | src/openni2_frame_listener.cpp 41 | src/openni2_device_manager.cpp 42 | src/openni2_exception.cpp 43 | src/openni2_video_mode.cpp 44 | ) 45 | target_link_libraries(openni2_wrapper 46 | ${PC_OPENNI2_LIBRARIES} 47 | ) 48 | target_link_libraries(openni2_wrapper 49 | ${builtin_interfaces_TARGETS} 50 | camera_info_manager::camera_info_manager 51 | image_transport::image_transport 52 | ${sensor_msgs_TARGETS} 53 | rclcpp::rclcpp 54 | rclcpp_components::component 55 | ) 56 | 57 | add_executable(test_wrapper test/test_wrapper.cpp) 58 | target_link_libraries(test_wrapper openni2_wrapper) 59 | 60 | add_library(openni2_camera_lib SHARED 61 | src/openni2_driver.cpp 62 | ) 63 | target_link_libraries(openni2_camera_lib 64 | openni2_wrapper 65 | ) 66 | target_link_libraries(openni2_camera_lib 67 | camera_info_manager::camera_info_manager 68 | image_transport::image_transport 69 | ${sensor_msgs_TARGETS} 70 | rclcpp::rclcpp 71 | ) 72 | rosidl_get_typesupport_target(cpp_typesupport_target 73 | ${PROJECT_NAME} "rosidl_typesupport_cpp") 74 | target_link_libraries(openni2_camera_lib "${cpp_typesupport_target}") 75 | 76 | rclcpp_components_register_node(openni2_camera_lib 77 | PLUGIN "openni2_wrapper::OpenNI2Driver" 78 | EXECUTABLE openni2_camera_driver 79 | ) 80 | 81 | add_executable(list_devices 82 | src/list_devices.cpp 83 | ) 84 | target_link_libraries(list_devices 85 | openni2_wrapper 86 | ) 87 | 88 | if (UNIX AND NOT APPLE) 89 | add_executable(usb_reset src/usb_reset.c) 90 | set(ADDITIONAL_EXECUTABLES "usb_reset") 91 | endif() 92 | 93 | install( 94 | TARGETS 95 | list_devices 96 | openni2_camera_lib 97 | openni2_wrapper 98 | ${ADDITIONAL_EXECUTABLES} 99 | ARCHIVE DESTINATION lib 100 | LIBRARY DESTINATION lib 101 | RUNTIME DESTINATION lib/${PROJECT_NAME} 102 | ) 103 | 104 | install( 105 | DIRECTORY include/ 106 | DESTINATION include 107 | ) 108 | 109 | install( 110 | DIRECTORY launch 111 | DESTINATION share/${PROJECT_NAME}/ 112 | ) 113 | 114 | ament_export_include_directories(include) 115 | ament_export_libraries(openni2_wrapper) 116 | ament_export_dependencies( 117 | camera_info_manager 118 | image_transport 119 | sensor_msgs 120 | rclcpp 121 | ) 122 | ament_package() 123 | -------------------------------------------------------------------------------- /openni2_camera/src/openni2_convert.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | 32 | #include "openni2_camera/openni2_convert.h" 33 | #include "openni2_camera/openni2_exception.h" 34 | 35 | #include 36 | 37 | namespace openni2_wrapper 38 | { 39 | 40 | const OpenNI2DeviceInfo openni2_convert(const openni::DeviceInfo* pInfo) 41 | { 42 | if (!pInfo) 43 | THROW_OPENNI_EXCEPTION("openni2_convert called with zero pointer\n"); 44 | 45 | OpenNI2DeviceInfo output; 46 | 47 | output.name_ = pInfo->getName(); 48 | output.uri_ = pInfo->getUri(); 49 | output.vendor_ = pInfo->getVendor(); 50 | output.product_id_ = pInfo->getUsbProductId(); 51 | output.vendor_id_ = pInfo->getUsbVendorId(); 52 | 53 | return output; 54 | } 55 | 56 | 57 | const OpenNI2VideoMode openni2_convert(const openni::VideoMode& input) 58 | { 59 | OpenNI2VideoMode output; 60 | 61 | output.x_resolution_ = input.getResolutionX(); 62 | output.y_resolution_ = input.getResolutionY(); 63 | output.frame_rate_ = input.getFps(); 64 | output.pixel_format_ = static_cast(input.getPixelFormat()); 65 | 66 | return output; 67 | } 68 | 69 | const openni::VideoMode openni2_convert(const OpenNI2VideoMode& input) 70 | { 71 | 72 | openni::VideoMode output; 73 | 74 | output.setResolution(input.x_resolution_, input.y_resolution_); 75 | output.setFps(input.frame_rate_); 76 | output.setPixelFormat(static_cast(input.pixel_format_)); 77 | 78 | return output; 79 | } 80 | 81 | 82 | const std::vector openni2_convert(const openni::Array& input) 83 | { 84 | std::vector output; 85 | 86 | int size = input.getSize(); 87 | 88 | output.reserve(size); 89 | 90 | for (int i=0; i 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of Willow Garage, Inc. nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | #ifndef __OPENNI2_EXCEPTION__ 38 | #define __OPENNI2_EXCEPTION__ 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #if defined _WIN32 && defined _MSC_VER 46 | # define __PRETTY_FUNCTION__ __FUNCTION__ 47 | #endif 48 | #define THROW_OPENNI_EXCEPTION(format,...) throwOpenNIException( __PRETTY_FUNCTION__, __FILE__, __LINE__, format , ##__VA_ARGS__ ) 49 | 50 | namespace openni2_wrapper 51 | { 52 | /** 53 | * @brief General exception class 54 | * @author Suat Gedikli 55 | * @date 02.january 2011 56 | */ 57 | class OpenNI2Exception : public std::exception 58 | { 59 | public: 60 | OpenNI2Exception(const std::string& function_name, 61 | const std::string& file_name, 62 | unsigned line_number, 63 | const std::string& message) throw (); 64 | 65 | virtual ~OpenNI2Exception() throw (); 66 | OpenNI2Exception & operator=(const OpenNI2Exception& exception) throw (); 67 | virtual const char* what() const throw (); 68 | 69 | const std::string& getFunctionName() const throw (); 70 | const std::string& getFileName() const throw (); 71 | unsigned getLineNumber() const throw (); 72 | 73 | protected: 74 | std::string function_name_; 75 | std::string file_name_; 76 | unsigned line_number_; 77 | std::string message_; 78 | std::string message_long_; 79 | }; 80 | 81 | inline void throwOpenNIException(const char* function, const char* file, unsigned line, const char* format, ...) 82 | { 83 | static char msg[1024]; 84 | va_list args; 85 | va_start(args, format); 86 | vsprintf(msg, format, args); 87 | throw OpenNI2Exception(function, file, line, msg); 88 | } 89 | } // namespace openni_camera 90 | #endif 91 | -------------------------------------------------------------------------------- /openni2_camera/src/openni2_video_mode.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | 32 | #include "openni2_camera/openni2_video_mode.h" 33 | 34 | namespace openni2_wrapper 35 | { 36 | 37 | 38 | std::ostream& operator << (std::ostream& stream, const OpenNI2VideoMode& video_mode) { 39 | stream << "Resolution: " << (int)video_mode.x_resolution_ << "x" << (int)video_mode.y_resolution_ << 40 | "@" << video_mode.frame_rate_ << 41 | "Hz Format: "; 42 | 43 | switch (video_mode.pixel_format_) 44 | { 45 | case PIXEL_FORMAT_DEPTH_1_MM: 46 | stream << "Depth 1mm"; 47 | break; 48 | case PIXEL_FORMAT_DEPTH_100_UM: 49 | stream << "Depth 100um"; 50 | break; 51 | case PIXEL_FORMAT_SHIFT_9_2: 52 | stream << "Shift 9/2"; 53 | break; 54 | case PIXEL_FORMAT_SHIFT_9_3: 55 | stream << "Shift 9/3"; 56 | break; 57 | case PIXEL_FORMAT_RGB888: 58 | stream << "RGB888"; 59 | break; 60 | case PIXEL_FORMAT_YUV422: 61 | stream << "YUV422"; 62 | break; 63 | case PIXEL_FORMAT_GRAY8: 64 | stream << "Gray8"; 65 | break; 66 | case PIXEL_FORMAT_GRAY16: 67 | stream << "Gray16"; 68 | break; 69 | case PIXEL_FORMAT_JPEG: 70 | stream << "JPEG"; 71 | break; 72 | 73 | default: 74 | break; 75 | } 76 | 77 | return stream; 78 | } 79 | 80 | bool operator==(const OpenNI2VideoMode& video_mode_a, const OpenNI2VideoMode& video_mode_b) 81 | { 82 | return (video_mode_a.x_resolution_==video_mode_b.x_resolution_) && 83 | (video_mode_a.y_resolution_==video_mode_b.y_resolution_) && 84 | (video_mode_a.frame_rate_ ==video_mode_b.frame_rate_) && 85 | (video_mode_a.pixel_format_==video_mode_b.pixel_format_); 86 | } 87 | 88 | bool operator!=(const OpenNI2VideoMode& video_mode_a, const OpenNI2VideoMode& video_mode_b) 89 | { 90 | return !(video_mode_a==video_mode_b); 91 | } 92 | 93 | } //namespace openni2_wrapper 94 | -------------------------------------------------------------------------------- /openni2_camera/test/test_wrapper.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | 32 | #include "openni2_camera/openni2_device_manager.h" 33 | #include "openni2_camera/openni2_device.h" 34 | 35 | #include 36 | 37 | #include 38 | 39 | using namespace std; 40 | using namespace openni2_wrapper; 41 | 42 | int ir_counter_ = 0; 43 | int color_counter_ = 0; 44 | int depth_counter_ = 0; 45 | 46 | void IRCallback(sensor_msgs::msg::Image::SharedPtr image) 47 | { 48 | ++ir_counter_; 49 | } 50 | 51 | void ColorCallback(sensor_msgs::msg::Image::SharedPtr image) 52 | { 53 | ++color_counter_; 54 | } 55 | 56 | void DepthCallback(sensor_msgs::msg::Image::SharedPtr image) 57 | { 58 | ++depth_counter_; 59 | } 60 | 61 | int main(int argc, char **argv) 62 | { 63 | rclcpp::init(argc, argv); 64 | rclcpp::Node node("test_wrapper"); 65 | 66 | OpenNI2DeviceManager device_manager; 67 | 68 | std::cout << device_manager; 69 | 70 | std::shared_ptr > device_uris = device_manager.getConnectedDeviceURIs(); 71 | 72 | for (const std::string& uri : *device_uris) 73 | { 74 | std::shared_ptr device = device_manager.getDevice(uri, &node); 75 | 76 | std::cout << *device; 77 | 78 | device->setIRFrameCallback(std::bind(&IRCallback, std::placeholders::_1)); 79 | device->setColorFrameCallback(std::bind(&ColorCallback, std::placeholders::_1)); 80 | device->setDepthFrameCallback(std::bind(&DepthCallback, std::placeholders::_1)); 81 | 82 | ir_counter_ = 0; 83 | color_counter_ = 0; 84 | depth_counter_ = 0; 85 | 86 | device->startColorStream(); 87 | device->startDepthStream(); 88 | 89 | std::this_thread::sleep_for(1000ms); 90 | 91 | device->stopAllStreams(); 92 | 93 | std::cout<`_. 52 | Change the number of devices to expect by setting the number in ROS parameter "``openni2_num_sensors_expected``". 53 | 54 | Example: :: 55 | 56 | $ lsusb 57 | : 58 | Bus 005 Device 002: ID 1d27:0601 ASUS 59 | Bus 003 Device 002: ID 1d27:0601 ASUS 60 | 61 | term-1$ roscore 62 | 63 | term-2$ roswtf 64 | 65 | Loaded plugin tf.tfwtf 66 | Loaded plugin openni2_launch.wtf_plugin 67 | No package or stack in context 68 | ================================================================================ 69 | Static checks summary: 70 | 71 | Found 1 error(s). 72 | 73 | ERROR Different number of openni2 sensors found. 74 | * 2 openni2 sensors found (expected: 1). 75 | 76 | ================================================================================ 77 | Beginning tests of your ROS graph. These may take awhile... 78 | analyzing graph... 79 | ... done analyzing graph 80 | running graph rules... 81 | ... done running graph rules 82 | 83 | Online checks summary: 84 | 85 | Found 1 warning(s). 86 | Warnings are things that may be just fine, but are sometimes at fault 87 | 88 | WARNING The following node subscriptions are unconnected: 89 | * /rosout: 90 | * /rosout 91 | 92 | After setting `openni2_num_sensors_expected` param with 2, no error occurs. :: 93 | 94 | term-2$ rosparam set openni2_num_sensors_expected 2 95 | 96 | $ roswtf 97 | Loaded plugin tf.tfwtf 98 | Loaded plugin openni2_launch.wtf_plugin 99 | No package or stack in context 100 | ================================================================================ 101 | Static checks summary: 102 | 103 | No errors or warnings 104 | ================================================================================ 105 | Beginning tests of your ROS graph. These may take awhile... 106 | analyzing graph... 107 | ... done analyzing graph 108 | running graph rules... 109 | ... done running graph rules 110 | 111 | Online checks summary: 112 | 113 | Found 1 warning(s). 114 | Warnings are things that may be just fine, but are sometimes at fault 115 | 116 | WARNING The following node subscriptions are unconnected: 117 | * /rosout: 118 | * /rosout 119 | 120 | Indices and tables 121 | ================== 122 | 123 | * :ref:`genindex` 124 | * :ref:`modindex` 125 | * :ref:`search` 126 | -------------------------------------------------------------------------------- /openni2_camera/launch/camera_with_cloud_norgb.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2020-2023, Michael Ferguson 4 | # All rights reserved. 5 | # 6 | # Software License Agreement (BSD License 2.0) 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions 10 | # are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above 15 | # copyright notice, this list of conditions and the following 16 | # disclaimer in the documentation and/or other materials provided 17 | # with the distribution. 18 | # * Neither the name of the copyright holder nor the names of its 19 | # contributors may be used to endorse or promote products derived 20 | # from this software without specific prior written permission. 21 | # 22 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | 35 | import launch 36 | import launch_ros.actions 37 | import launch_ros.descriptions 38 | from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PathJoinSubstitution 39 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 40 | from launch.launch_description_sources import PythonLaunchDescriptionSource 41 | 42 | 43 | def generate_launch_description(): 44 | 45 | namespace_param_name = "namespace" 46 | namespace = LaunchConfiguration(namespace_param_name) 47 | namespace_launch_arg = DeclareLaunchArgument(namespace_param_name, default_value='camera') 48 | 49 | tf_prefix_param_name = "tf_prefix" 50 | tf_prefix = LaunchConfiguration(tf_prefix_param_name) 51 | tf_prefix_launch_arg = DeclareLaunchArgument(tf_prefix_param_name, default_value='') 52 | 53 | container = launch_ros.actions.ComposableNodeContainer( 54 | name='container', 55 | namespace=namespace, 56 | package='rclcpp_components', 57 | executable='component_container', 58 | composable_node_descriptions=[ 59 | # Driver 60 | launch_ros.descriptions.ComposableNode( 61 | package='openni2_camera', 62 | plugin='openni2_wrapper::OpenNI2Driver', 63 | name='driver', 64 | namespace=namespace, 65 | parameters=[{'depth_registration': False}, 66 | {'use_device_time': True}, 67 | {'rgb_frame_id': [namespace,"_rgb_optical_frame"]}, 68 | {'depth_frame_id': [namespace,"_depth_optical_frame"]}, 69 | {'ir_frame_id': [namespace,"_ir_optical_frame"]},], 70 | remappings=[('depth/image', 'depth_registered/image_raw')], 71 | ), 72 | # Create XYZ point cloud 73 | launch_ros.descriptions.ComposableNode( 74 | package='depth_image_proc', 75 | plugin='depth_image_proc::PointCloudXyzNode', 76 | name='points_xyz', 77 | namespace=namespace, 78 | parameters=[{'queue_size': 10}], 79 | remappings=[('image_rect', 'depth/image_raw'), 80 | ('camera_info', 'depth/camera_info'), 81 | ('points', 'depth/points')], 82 | ), 83 | ], 84 | output='screen', 85 | ) 86 | 87 | tfs = IncludeLaunchDescription( 88 | PythonLaunchDescriptionSource(PathJoinSubstitution([ThisLaunchFileDir(), "tfs.launch.py"])), 89 | launch_arguments={namespace_param_name: namespace, tf_prefix_param_name: tf_prefix}.items(), 90 | ) 91 | 92 | return launch.LaunchDescription([ 93 | namespace_launch_arg, 94 | tf_prefix_launch_arg, 95 | container, 96 | tfs, 97 | ]) 98 | -------------------------------------------------------------------------------- /openni2_camera/launch/camera_with_cloud.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2020-2023, Michael Ferguson 4 | # All rights reserved. 5 | # 6 | # Software License Agreement (BSD License 2.0) 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions 10 | # are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above 15 | # copyright notice, this list of conditions and the following 16 | # disclaimer in the documentation and/or other materials provided 17 | # with the distribution. 18 | # * Neither the name of the copyright holder nor the names of its 19 | # contributors may be used to endorse or promote products derived 20 | # from this software without specific prior written permission. 21 | # 22 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | 35 | import launch 36 | import launch_ros.actions 37 | import launch_ros.descriptions 38 | from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PathJoinSubstitution 39 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 40 | from launch.launch_description_sources import PythonLaunchDescriptionSource 41 | 42 | 43 | def generate_launch_description(): 44 | 45 | namespace_param_name = "namespace" 46 | namespace = LaunchConfiguration(namespace_param_name) 47 | namespace_launch_arg = DeclareLaunchArgument(namespace_param_name, default_value='camera') 48 | 49 | tf_prefix_param_name = "tf_prefix" 50 | tf_prefix = LaunchConfiguration(tf_prefix_param_name) 51 | tf_prefix_launch_arg = DeclareLaunchArgument(tf_prefix_param_name, default_value='') 52 | 53 | container = launch_ros.actions.ComposableNodeContainer( 54 | name='container', 55 | namespace=namespace, 56 | package='rclcpp_components', 57 | executable='component_container', 58 | composable_node_descriptions=[ 59 | # Driver 60 | launch_ros.descriptions.ComposableNode( 61 | package='openni2_camera', 62 | plugin='openni2_wrapper::OpenNI2Driver', 63 | name='driver', 64 | namespace=namespace, 65 | parameters=[{'depth_registration': True}, 66 | {'use_device_time': True}, 67 | {'rgb_frame_id': [namespace,"_rgb_optical_frame"]}, 68 | {'depth_frame_id': [namespace,"_depth_optical_frame"]}, 69 | {'ir_frame_id': [namespace,"_ir_optical_frame"]},], 70 | remappings=[('depth/image', 'depth_registered/image_raw')], 71 | ), 72 | # Create XYZRGB point cloud 73 | launch_ros.descriptions.ComposableNode( 74 | package='depth_image_proc', 75 | plugin='depth_image_proc::PointCloudXyzrgbNode', 76 | name='points_xyzrgb', 77 | namespace=namespace, 78 | parameters=[{'queue_size': 10}], 79 | remappings=[('rgb/image_rect_color', 'rgb/image_raw'), 80 | ('rgb/camera_info', 'rgb/camera_info'), 81 | ('depth_registered/image_rect', 'depth_registered/image_raw'), 82 | ('points', 'depth_registered/points'), ], 83 | ), 84 | ], 85 | output='screen', 86 | ) 87 | 88 | tfs = IncludeLaunchDescription( 89 | PythonLaunchDescriptionSource(PathJoinSubstitution([ThisLaunchFileDir(), "tfs.launch.py"])), 90 | launch_arguments={namespace_param_name: namespace, tf_prefix_param_name: tf_prefix}.items(), 91 | ) 92 | 93 | return launch.LaunchDescription([ 94 | namespace_launch_arg, 95 | tf_prefix_launch_arg, 96 | container, 97 | tfs, 98 | ]) 99 | -------------------------------------------------------------------------------- /openni2_launch/src/openni2_launch/wtf_plugin.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2018, PlusOne Robotics, Inc. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # * Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # * Redistributions in binary form must reproduce the above 12 | # copyright notice, this list of conditions and the following 13 | # disclaimer in the documentation and/or other materials provided 14 | # with the distribution. 15 | # * Neither the name of Plus One Robotics, Inc. nor the names of its 16 | # contributors may be used to endorse or promote products derived 17 | # from this software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | 32 | from __future__ import print_function 33 | import logging 34 | import re 35 | import subprocess 36 | 37 | import rospy 38 | from roswtf.rules import warning_rule, error_rule 39 | 40 | 41 | def _device_notfound_subproc(id_manufacturer, id_product): 42 | """ 43 | @rtype: [dict] 44 | @return: Example: 45 | 46 | [{'device': '/dev/bus/usb/002/004', 'tag': 'Lenovo ', 'id': '17ef:305a'}, 47 | {'device': '/dev/bus/usb/002/001', 'tag': 'Linux Foundation 3.0 root hub', 'id': '1d6b:0003'}, 48 | {'device': '/dev/bus/usb/001/006', 'tag': 'Validity Sensors, Inc. ', 'id': '138a:0090'},,,] 49 | 50 | @note: This method depends on Linux command (via subprocess), which makes 51 | this command platform-dependent. Ubuntu Xenial onward, a Python module 52 | that encapsulate platform operation becomes available so this method 53 | can be wiped out. See https://github.com/ros-drivers/openni2_camera/pull/80#discussion_r193295442 54 | """ 55 | device_re = re.compile("Bus\s+(?P\d+)\s+Device\s+(?P\d+).+ID\s(?P\w+:\w+)\s(?P.+)$", re.I) 56 | df = subprocess.check_output("lsusb") 57 | devices = [] 58 | for i in df.split('\n'): 59 | if i: 60 | info = device_re.match(i) 61 | if info: 62 | dinfo = info.groupdict() 63 | logging.debug("dinfo: {}, dinfo.id: {}".format(dinfo, dinfo["id"])) 64 | if dinfo["id"] == "{}:{}".format(id_manufacturer, id_product): 65 | dinfo['device'] = "/dev/bus/usb/{}/{}".format(dinfo.pop('bus'), dinfo.pop('device')) 66 | devices.append(dinfo) 67 | logging.info("#devices: {}\ndevices: {}".format(len(devices), devices)) 68 | return devices 69 | 70 | 71 | def sensor_notfound(ctx): 72 | """ 73 | @summary: Check if expected number of sensors are found. 74 | Expected number of sensors can be set by 75 | ROS Parameter 'openni2_num_sensors_expected'. 76 | @note: Technically this can be static check, but because of the 77 | need for connecting to ROS Param server, this needs 78 | to be online check. 79 | """ 80 | errors = [] 81 | num_sensors_expected = rospy.get_param("openni2_num_sensors_expected", 1) 82 | # The set of manufacture id and prod id. Default: Asus Xtion. 83 | id_manufacturer = rospy.get_param("id_manufacturer", "1d27") 84 | id_product = rospy.get_param("id_product", "0601") 85 | devices = _device_notfound_subproc( 86 | id_manufacturer=id_manufacturer, id_product=id_product) 87 | num_sensors = len(devices) 88 | if num_sensors != num_sensors_expected: 89 | errors.append("{} openni2 sensors found (expected: {}).".format( 90 | num_sensors, num_sensors_expected)) 91 | return errors 92 | 93 | 94 | # app_warnings and app_errors declare the rules that we actually check 95 | app_warnings_online = [ 96 | ] 97 | 98 | app_warnings_static = [ 99 | ] 100 | 101 | app_errors_online = [ 102 | (sensor_notfound, "Different number of openni2 sensors found."), 103 | ] 104 | 105 | app_errors_static = [ 106 | ] 107 | 108 | 109 | # roswtf entry point for online checks 110 | def roswtf_plugin_online(ctx): 111 | for r in app_warnings_online: 112 | warning_rule(r, r[0](ctx), ctx) 113 | for r in app_errors_online: 114 | error_rule(r, r[0](ctx), ctx) 115 | 116 | 117 | def roswtf_plugin_static(ctx): 118 | for r in app_warnings_static: 119 | warning_rule(r, r[0](ctx), ctx) 120 | for r in app_errors_static: 121 | error_rule(r, r[0](ctx), ctx) 122 | -------------------------------------------------------------------------------- /openni2_camera/include/openni2_camera/openni2_device.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | 32 | #ifndef OPENNI2_DEVICE_H 33 | #define OPENNI2_DEVICE_H 34 | 35 | #include "openni2_camera/openni2_video_mode.h" 36 | 37 | #include "openni2_camera/openni2_exception.h" 38 | 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | 47 | namespace openni 48 | { 49 | class Device; 50 | class DeviceInfo; 51 | class VideoStream; 52 | class SensorInfo; 53 | } 54 | 55 | namespace openni2_wrapper 56 | { 57 | 58 | typedef std::function FrameCallbackFunction; 59 | 60 | class OpenNI2FrameListener; 61 | 62 | class OpenNI2Device 63 | { 64 | public: 65 | OpenNI2Device(const std::string& device_URI, 66 | rclcpp::Node* node); 67 | virtual ~OpenNI2Device(); 68 | 69 | const std::string getUri() const; 70 | const std::string getVendor() const; 71 | const std::string getName() const; 72 | uint16_t getUsbVendorId() const; 73 | uint16_t getUsbProductId() const; 74 | 75 | const std::string getStringID() const; 76 | 77 | bool isValid() const; 78 | 79 | bool hasIRSensor() const; 80 | bool hasColorSensor() const; 81 | bool hasDepthSensor() const; 82 | 83 | void startIRStream(); 84 | void startColorStream(); 85 | void startDepthStream(); 86 | 87 | void stopAllStreams(); 88 | 89 | void stopIRStream(); 90 | void stopColorStream(); 91 | void stopDepthStream(); 92 | 93 | bool isIRStreamStarted(); 94 | bool isColorStreamStarted(); 95 | bool isDepthStreamStarted(); 96 | 97 | bool isImageRegistrationModeSupported() const; 98 | void setImageRegistrationMode(bool enabled); 99 | void setDepthColorSync(bool enabled); 100 | 101 | const OpenNI2VideoMode getIRVideoMode(); 102 | const OpenNI2VideoMode getColorVideoMode(); 103 | const OpenNI2VideoMode getDepthVideoMode(); 104 | 105 | const std::vector& getSupportedIRVideoModes() const; 106 | const std::vector& getSupportedColorVideoModes() const; 107 | const std::vector& getSupportedDepthVideoModes() const; 108 | 109 | bool isIRVideoModeSupported(const OpenNI2VideoMode& video_mode) const; 110 | bool isColorVideoModeSupported(const OpenNI2VideoMode& video_mode) const; 111 | bool isDepthVideoModeSupported(const OpenNI2VideoMode& video_mode) const; 112 | 113 | void setIRVideoMode(const OpenNI2VideoMode& video_mode); 114 | void setColorVideoMode(const OpenNI2VideoMode& video_mode); 115 | void setDepthVideoMode(const OpenNI2VideoMode& video_mode); 116 | 117 | void setIRFrameCallback(FrameCallbackFunction callback); 118 | void setColorFrameCallback(FrameCallbackFunction callback); 119 | void setDepthFrameCallback(FrameCallbackFunction callback); 120 | 121 | float getIRFocalLength (int output_y_resolution) const; 122 | float getColorFocalLength (int output_y_resolution) const; 123 | float getDepthFocalLength (int output_y_resolution) const; 124 | float getBaseline () const; 125 | 126 | void setAutoExposure(bool enable); 127 | void setAutoWhiteBalance(bool enable); 128 | void setExposure(int exposure); 129 | 130 | bool getAutoExposure() const; 131 | bool getAutoWhiteBalance() const; 132 | int getExposure() const; 133 | 134 | void setUseDeviceTimer(bool enable); 135 | 136 | protected: 137 | void shutdown(); 138 | 139 | std::shared_ptr getIRVideoStream() const; 140 | std::shared_ptr getColorVideoStream() const; 141 | std::shared_ptr getDepthVideoStream() const; 142 | 143 | std::shared_ptr openni_device_; 144 | std::shared_ptr device_info_; 145 | 146 | std::shared_ptr ir_frame_listener; 147 | std::shared_ptr color_frame_listener; 148 | std::shared_ptr depth_frame_listener; 149 | 150 | mutable std::shared_ptr ir_video_stream_; 151 | mutable std::shared_ptr color_video_stream_; 152 | mutable std::shared_ptr depth_video_stream_; 153 | 154 | mutable std::vector ir_video_modes_; 155 | mutable std::vector color_video_modes_; 156 | mutable std::vector depth_video_modes_; 157 | 158 | bool ir_video_started_; 159 | bool color_video_started_; 160 | bool depth_video_started_; 161 | 162 | bool image_registration_activated_; 163 | }; 164 | 165 | std::ostream& operator << (std::ostream& stream, const OpenNI2Device& device); 166 | 167 | } 168 | 169 | #endif /* OPENNI_DEVICE_H */ 170 | -------------------------------------------------------------------------------- /openni2_camera/src/openni2_frame_listener.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | #include "OpenNI.h" 32 | 33 | #include "openni2_camera/openni2_frame_listener.h" 34 | #include "openni2_camera/openni2_timer_filter.h" 35 | 36 | #include 37 | 38 | #include 39 | 40 | #define TIME_FILTER_LENGTH 15 41 | 42 | namespace openni2_wrapper 43 | { 44 | 45 | OpenNI2FrameListener::OpenNI2FrameListener(rclcpp::Node* node) : 46 | callback_(0), 47 | use_device_timer_(false), 48 | timer_filter_(new OpenNI2TimerFilter(TIME_FILTER_LENGTH)), 49 | node_(node), 50 | prev_time_stamp_(node_->now()) 51 | { 52 | 53 | } 54 | 55 | void OpenNI2FrameListener::setUseDeviceTimer(bool enable) 56 | { 57 | use_device_timer_ = enable; 58 | 59 | if (use_device_timer_) 60 | { 61 | timer_filter_->clear(); 62 | } 63 | } 64 | 65 | void OpenNI2FrameListener::onNewFrame(openni::VideoStream& stream) 66 | { 67 | stream.readFrame(&m_frame); 68 | 69 | if (m_frame.isValid() && callback_) 70 | { 71 | sensor_msgs::msg::Image::SharedPtr image(new sensor_msgs::msg::Image); 72 | 73 | rclcpp::Time ros_now = node_->now(); 74 | 75 | if (!use_device_timer_) 76 | { 77 | image->header.stamp = ros_now; 78 | 79 | RCLCPP_DEBUG(rclcpp::get_logger("openni2"), "Time interval between frames: %.4f ms", 80 | (float)((ros_now.nanoseconds() - prev_time_stamp_.nanoseconds()) / 1e6)); 81 | 82 | prev_time_stamp_ = ros_now; 83 | } 84 | else 85 | { 86 | // Get device time in microseconds 87 | uint64_t device_time = m_frame.getTimestamp(); 88 | 89 | // Convert both device time and ros time into floating point seconds 90 | double device_time_in_sec = static_cast(device_time) / 1e6; 91 | double ros_time_in_sec = ros_now.seconds(); 92 | 93 | // Then compute difference and filter 94 | double time_diff = ros_time_in_sec - device_time_in_sec; 95 | timer_filter_->addSample(time_diff); 96 | 97 | // Synchronize the device time with the system time 98 | double filtered_time_diff = timer_filter_->getMedian(); 99 | device_time_in_sec += filtered_time_diff; 100 | 101 | // Update the image header with correc time 102 | rclcpp::Time corrected_timestamp(static_cast(device_time_in_sec * 1e9)); 103 | image->header.stamp = corrected_timestamp; 104 | 105 | RCLCPP_DEBUG(rclcpp::get_logger("openni2"), "Time interval between frames: %.4f ms", 106 | (float)((corrected_timestamp.nanoseconds() - prev_time_stamp_.nanoseconds()) / 1e6)); 107 | 108 | prev_time_stamp_ = corrected_timestamp; 109 | } 110 | 111 | image->width = m_frame.getWidth(); 112 | image->height = m_frame.getHeight(); 113 | 114 | std::size_t data_size = m_frame.getDataSize(); 115 | 116 | image->data.resize(data_size); 117 | memcpy(&image->data[0], m_frame.getData(), data_size); 118 | 119 | image->is_bigendian = 0; 120 | 121 | const openni::VideoMode& video_mode = m_frame.getVideoMode(); 122 | switch (video_mode.getPixelFormat()) 123 | { 124 | case openni::PIXEL_FORMAT_DEPTH_1_MM: 125 | image->encoding = sensor_msgs::image_encodings::TYPE_16UC1; 126 | image->step = sizeof(unsigned char) * 2 * image->width; 127 | break; 128 | case openni::PIXEL_FORMAT_DEPTH_100_UM: 129 | image->encoding = sensor_msgs::image_encodings::TYPE_16UC1; 130 | image->step = sizeof(unsigned char) * 2 * image->width; 131 | break; 132 | case openni::PIXEL_FORMAT_SHIFT_9_2: 133 | image->encoding = sensor_msgs::image_encodings::TYPE_16UC1; 134 | image->step = sizeof(unsigned char) * 2 * image->width; 135 | break; 136 | case openni::PIXEL_FORMAT_SHIFT_9_3: 137 | image->encoding = sensor_msgs::image_encodings::TYPE_16UC1; 138 | image->step = sizeof(unsigned char) * 2 * image->width; 139 | break; 140 | 141 | case openni::PIXEL_FORMAT_RGB888: 142 | image->encoding = sensor_msgs::image_encodings::RGB8; 143 | image->step = sizeof(unsigned char) * 3 * image->width; 144 | break; 145 | case openni::PIXEL_FORMAT_YUV422: 146 | image->encoding = sensor_msgs::image_encodings::YUV422; 147 | image->step = sizeof(unsigned char) * 4 * image->width; 148 | break; 149 | case openni::PIXEL_FORMAT_GRAY8: 150 | image->encoding = sensor_msgs::image_encodings::MONO8; 151 | image->step = sizeof(unsigned char) * 1 * image->width; 152 | break; 153 | case openni::PIXEL_FORMAT_GRAY16: 154 | image->encoding = sensor_msgs::image_encodings::MONO16; 155 | image->step = sizeof(unsigned char) * 2 * image->width; 156 | break; 157 | case openni::PIXEL_FORMAT_JPEG: 158 | default: 159 | RCLCPP_ERROR(rclcpp::get_logger("openni2"), "Invalid image encoding"); 160 | break; 161 | } 162 | 163 | callback_(image); 164 | } 165 | 166 | } 167 | 168 | } 169 | 170 | -------------------------------------------------------------------------------- /openni2_camera/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package openni2_camera 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.3.0 (2025-05-21) 6 | ------------------ 7 | * Replace ament_target_dependencies with target_link_libraries (`#145 `_) 8 | `ament_target_dependencies` is deprecated, it will require a release on 9 | `rolling` 10 | * Contributors: Alejandro Hernández Cordero 11 | 12 | 2.2.2 (2024-11-03) 13 | ------------------ 14 | * add dynamic parameters (`#141 `_) 15 | In ROS 1, a number of things were dynamic - this PR re-adds them using 16 | the new ROS 2 paradigm for parameter updating. I've tested this with a 17 | Primesense device and am able to disable auto_exposure and 18 | auto_white_balance. 19 | * Contributors: Michael Ferguson 20 | 21 | 2.2.1 (2024-08-11) 22 | ------------------ 23 | * rename depth/image_raw to depth_raw/image (`#140 `_) 24 | Without this change, both depth/image and depth/image_raw publish the 25 | same camera_info topic (depth/camera_info) - this has several issues: 26 | * subscribing to either image causes both to be published along with 27 | their respective camera_info 28 | * remapping either camera_info, remaps both 29 | * In Jazzy, it appears that the double camera_info also causes minor 30 | issues with message_filters in downstream packages 31 | * Contributors: Michael Ferguson 32 | 33 | 2.2.0 (2024-03-14) 34 | ------------------ 35 | * fix build on Ubuntu Noble (24.04) (`#138 `_) 36 | add missing header 37 | * Contributors: Michael Ferguson 38 | 39 | 2.1.0 (2024-02-14) 40 | ------------------ 41 | * implement proper lazy subscribers (`#136 `_) 42 | Fixes `#119 `_, works in Jazzy and later 43 | * publish tfs and set matching frame names (`#135 `_) 44 | This PR ports the TFs from original 45 | [`kinect_frames.launch`](https://github.com/ros-drivers/rgbd_launch/blob/noetic-devel/launch/kinect_frames.launch) 46 | to the ROS 2 launch files and sets the image frame names appropriately. 47 | * Depth-only point cloud launch file for Non-RGB PrimeSense device. (`#132 `_) 48 | * Contributors: Christian Rauch, Michael Ferguson, TinLethax 49 | 50 | 2.0.2 (2023-04-26) 51 | ------------------ 52 | * add depth_image_proc exec dep (`#131 `_) 53 | Needed when starting the camera with point cloud processing. 54 | * Contributors: Mario Prats 55 | 56 | 2.0.1 (2023-03-02) 57 | ------------------ 58 | * fix device time (`#129 `_) 59 | * add build depend on pkg-config (`#128 `_) 60 | * Contributors: Michael Ferguson 61 | 62 | 2.0.0 (2023-03-01) 63 | ------------------ 64 | * revive the test wrapper (`#127 `_) 65 | resolves `#123 `_ 66 | * remove boost and unused files (`#122 `_) 67 | * replace boost functionality by standard C++ 68 | * remove ROS nodelet metadata 69 | * store parameter callback 70 | * remove ROS1 node 71 | * remove unused os import 72 | Co-authored-by: Christian Rauch 73 | * fix build, target only humble and later (`#118 `_) 74 | * Capability: [CI] Add GitHub Action (`#116 `_) 75 | * Capability: [CI] Add GitHub Action 76 | * Maintenance: Update maintaner contact 77 | * initial port to ROS2 78 | * Contributors: Isaac I.Y. Saito, Michael Ferguson 79 | 80 | 1.6.0 (2022-04-12) 81 | ------------------ 82 | 83 | 1.5.1 (2021-02-01) 84 | ------------------ 85 | 86 | 1.5.0 (2021-01-12) 87 | ------------------ 88 | * [capability] Use serialnumber in camera names `#101 `_ 89 | * Release for Noetic `#98 `_ 90 | 91 | 1.4.2 (2020-06-03) 92 | ------------------ 93 | 94 | 0.4.2 (2018-12-01) 95 | ------------------ 96 | 97 | 0.4.1 (2018-10-20) 98 | ------------------ 99 | 100 | 0.4.0 (2018-07-17) 101 | ------------------ 102 | 103 | 0.3.0 (2017-11-03) 104 | ------------------ 105 | 106 | 107 | 0.2.9 (2017-10-25) 108 | ------------------ 109 | * [fix] Device re-connection `#53 `_ 110 | * [fix] Publish projector/camera_info (fixes disparity img) `#48 `_ 111 | * Contributors: Isaac I.Y. Saito, Martin Guenther, Shaun Edwards 112 | 113 | 0.2.8 (2017-08-17) 114 | ------------------ 115 | * [capability] Add exposure time control `#46 `_ 116 | * [fix] device URI formatting `#47 `_ 117 | * Contributors: Martin Guenther, Michael Ferguson, Sergey Alexandrov 118 | 119 | 0.2.7 (2016-06-07) 120 | ------------------ 121 | * Merge pull request `#44 `_ from jacquelinekay/fix_kinetic_build 122 | fix compile on kinetic 123 | * Contributors: Jackie Kay, Michael Ferguson 124 | 125 | 0.2.6 (2016-05-05) 126 | ------------------ 127 | * [fix] Compile for OSX `#30 `_ 128 | * [fix] Crash on OSX and warning fixes. 129 | * Contributors: Hans Gaiser, Isaac I.Y. Saito, Michael Ferguson 130 | 131 | 0.2.5 (2015-10-15) 132 | ------------------ 133 | * Merge pull request `#34 `_ from Intermodalics/feature/get_serial_service 134 | added get_serial service 135 | * Contributors: Michael Ferguson, Ruben Smits 136 | 137 | 0.2.4 (2015-04-06) 138 | ------------------ 139 | * proper usage of device_id parameter in resolveDeviceURI, resolve unique parts of device URIs, too 140 | * print vendor id and product id as hex value (like in lsusb) 141 | * Contributors: Michael Ferguson, Stephan Wirth 142 | 143 | 0.2.3 (2015-01-16) 144 | ------------------ 145 | * Explicitly ask for serial number when trying to resolve device URI instead of doing it once on device connected, fixes `#24 `_ 146 | * Contributors: Michael Ferguson, Stephan Wirth 147 | 148 | 0.2.2 (2014-10-06) 149 | ------------------ 150 | * Add usb_reset 151 | * Contributors: Kei Okada, Michael Ferguson 152 | 153 | 0.2.1 (2014-08-22) 154 | ------------------ 155 | * Fixed a bug that prevents depth only sensors from properly calculating the point cloud due to incorrect focal length 156 | * Updated cmakelists for OSX 157 | * Contributors: Colin Lea, Michael Ferguson, Tarek Taha 158 | 159 | 0.2.0 (2014-05-22) 160 | ------------------ 161 | * device_id: find camera by serial number 162 | * Make freenect_stack link a real link for wiki. 163 | * Contributors: Dariush Forouher, Michael Ferguson 164 | 165 | 0.1.2 (2014-02-03) 166 | ------------------ 167 | * Fix CMake error. 168 | * Contributors: Benjamin Chretien, Michael Ferguson 169 | 170 | 0.1.1 (2013-11-13) 171 | ------------------ 172 | * Fixed default value of ir_mode. Thanks @nxdefiant 173 | https://github.com/ros-drivers/openni2_camera/issues/16 174 | 175 | 0.1.0 (2013-08-28) 176 | ------------------ 177 | * initial release 178 | -------------------------------------------------------------------------------- /openni2_launch/launch/openni2_tf_prefix.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 10 | 11 | 12 | 16 | 17 | 19 | 21 | 22 | 26 | 27 | 28 | 30 | 31 | 32 | 33 | 35 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 63 | 64 | 66 | 67 | 69 | 70 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 129 | 130 | 131 | 132 | 133 | 134 | -------------------------------------------------------------------------------- /openni2_camera/include/openni2_camera/openni2_driver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | 32 | #ifndef OPENNI2_DRIVER_H 33 | #define OPENNI2_DRIVER_H 34 | 35 | #include 36 | 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | #include "openni2_camera/openni2_device_manager.h" 46 | #include "openni2_camera/openni2_device.h" 47 | #include "openni2_camera/openni2_video_mode.h" 48 | #include "openni2_camera/srv/get_serial.hpp" 49 | 50 | #include 51 | 52 | namespace openni2_wrapper 53 | { 54 | 55 | class OpenNI2Driver : public rclcpp::Node 56 | { 57 | public: 58 | OpenNI2Driver(const rclcpp::NodeOptions & node_options); 59 | 60 | private: 61 | void newIRFrameCallback(sensor_msgs::msg::Image::SharedPtr image); 62 | void newColorFrameCallback(sensor_msgs::msg::Image::SharedPtr image); 63 | void newDepthFrameCallback(sensor_msgs::msg::Image::SharedPtr image); 64 | 65 | // Methods to get calibration parameters for the various cameras 66 | sensor_msgs::msg::CameraInfo::SharedPtr getDefaultCameraInfo(int width, int height, double f) const; 67 | sensor_msgs::msg::CameraInfo::SharedPtr getColorCameraInfo(int width, int height, rclcpp::Time time) const; 68 | sensor_msgs::msg::CameraInfo::SharedPtr getIRCameraInfo(int width, int height, rclcpp::Time time) const; 69 | sensor_msgs::msg::CameraInfo::SharedPtr getDepthCameraInfo(int width, int height, rclcpp::Time time) const; 70 | sensor_msgs::msg::CameraInfo::SharedPtr getProjectorCameraInfo(int width, int height, rclcpp::Time time) const; 71 | 72 | // resolves non-URI device IDs to URIs, e.g. '#1' is resolved to the URI of the first device 73 | std::string resolveDeviceURI(const std::string& device_id); 74 | void initDevice(); 75 | 76 | void advertiseROSTopics(); 77 | 78 | // TODO: this is hack around two issues 79 | // First, subscription callbacks do not yet exist in ROS2 80 | // Second, we can't initialize topics in the constructor (shared_from_this doesn't work yet) 81 | void periodic(); 82 | bool initialized_; 83 | 84 | void monitorConnection(); 85 | void colorConnectCb(); 86 | void depthConnectCb(); 87 | void irConnectCb(); 88 | 89 | void getSerialCb(const std::shared_ptr request, 90 | std::shared_ptr response); 91 | 92 | rcl_interfaces::msg::SetParametersResult paramCb(const std::vector parameters); 93 | 94 | void applyConfigToOpenNIDevice(); 95 | 96 | void genVideoModeTableMap(); 97 | bool lookupVideoMode(const std::string& mode, OpenNI2VideoMode& video_mode); 98 | 99 | sensor_msgs::msg::Image::ConstSharedPtr rawToFloatingPointConversion(sensor_msgs::msg::Image::ConstSharedPtr raw_image); 100 | 101 | void setIRVideoMode(const OpenNI2VideoMode& ir_video_mode); 102 | void setColorVideoMode(const OpenNI2VideoMode& color_video_mode); 103 | void setDepthVideoMode(const OpenNI2VideoMode& depth_video_mode); 104 | 105 | int extractBusID(const std::string& uri) const; 106 | bool isConnected() const; 107 | 108 | void forceSetExposure(); 109 | 110 | std::shared_ptr device_manager_; 111 | std::shared_ptr device_; 112 | 113 | std::string device_id_; 114 | int bus_id_; 115 | 116 | /** \brief indicates if reconnect logic is enabled. */ 117 | bool enable_reconnect_; 118 | 119 | /** \brief indicates if the serialnumber is used in the camera names. 120 | * Default is false. The name is based on the device and vendor name. 121 | * This produces non-unique camera names, if multiple camaras of the 122 | * same type are used on the same machine. 123 | * Set to true, to get names with serialnumber. The names will always 124 | * be unique. This matches the 'openni' behaviour. 125 | */ 126 | bool serialnumber_as_name_; 127 | 128 | /** \brief get_serial server*/ 129 | rclcpp::Service::SharedPtr get_serial_server; 130 | 131 | std::mutex connect_mutex_; 132 | // published topics 133 | image_transport::CameraPublisher pub_color_; 134 | image_transport::CameraPublisher pub_depth_; 135 | image_transport::CameraPublisher pub_depth_raw_; 136 | image_transport::CameraPublisher pub_ir_; 137 | rclcpp::Publisher::SharedPtr pub_projector_info_; 138 | 139 | /** \brief timer for connection monitoring thread */ 140 | rclcpp::TimerBase::SharedPtr timer_; 141 | 142 | /** \brief Camera info manager objects. */ 143 | std::shared_ptr color_info_manager_, ir_info_manager_; 144 | 145 | OnSetParametersCallbackHandle::SharedPtr parameters_callback_; 146 | 147 | OpenNI2VideoMode ir_video_mode_; 148 | OpenNI2VideoMode color_video_mode_; 149 | OpenNI2VideoMode depth_video_mode_; 150 | 151 | std::string ir_frame_id_; 152 | std::string color_frame_id_; 153 | std::string depth_frame_id_ ; 154 | 155 | std::string color_info_url_, ir_info_url_; 156 | 157 | bool color_depth_synchronization_; 158 | bool depth_registration_; 159 | 160 | std::map video_modes_lookup_; 161 | 162 | // dynamic reconfigure config 163 | double depth_ir_offset_x_; 164 | double depth_ir_offset_y_; 165 | int z_offset_mm_; 166 | double z_scaling_; 167 | 168 | double ir_time_offset_; 169 | double color_time_offset_; 170 | double depth_time_offset_; 171 | 172 | int data_skip_; 173 | 174 | int data_skip_ir_counter_; 175 | int data_skip_color_counter_; 176 | int data_skip_depth_counter_; 177 | 178 | bool auto_exposure_; 179 | bool auto_white_balance_; 180 | int exposure_; 181 | 182 | bool ir_subscribers_; 183 | bool color_subscribers_; 184 | bool depth_subscribers_; 185 | bool depth_raw_subscribers_; 186 | bool projector_info_subscribers_; 187 | 188 | bool use_device_time_; 189 | }; 190 | 191 | } 192 | 193 | #endif 194 | -------------------------------------------------------------------------------- /openni2_launch/launch/openni2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 10 | 11 | 15 | 16 | 18 | 20 | 21 | 25 | 26 | 27 | 29 | 30 | 31 | 32 | 34 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 64 | 65 | 67 | 68 | 70 | 71 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 132 | 133 | 134 | 135 | 136 | -------------------------------------------------------------------------------- /openni2_launch/doc/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | PAPER = 8 | BUILDDIR = .build 9 | 10 | # User-friendly check for sphinx-build 11 | ifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1) 12 | $(error The '$(SPHINXBUILD)' command was not found. Make sure you have Sphinx installed, then set the SPHINXBUILD environment variable to point to the full path of the '$(SPHINXBUILD)' executable. Alternatively you can add the directory with the executable to your PATH. If you don't have Sphinx installed, grab it from http://sphinx-doc.org/) 13 | endif 14 | 15 | # Internal variables. 16 | PAPEROPT_a4 = -D latex_paper_size=a4 17 | PAPEROPT_letter = -D latex_paper_size=letter 18 | ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . 19 | # the i18n builder cannot share the environment and doctrees with the others 20 | I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . 21 | 22 | .PHONY: help 23 | help: 24 | @echo "Please use \`make ' where is one of" 25 | @echo " html to make standalone HTML files" 26 | @echo " dirhtml to make HTML files named index.html in directories" 27 | @echo " singlehtml to make a single large HTML file" 28 | @echo " pickle to make pickle files" 29 | @echo " json to make JSON files" 30 | @echo " htmlhelp to make HTML files and a HTML help project" 31 | @echo " qthelp to make HTML files and a qthelp project" 32 | @echo " applehelp to make an Apple Help Book" 33 | @echo " devhelp to make HTML files and a Devhelp project" 34 | @echo " epub to make an epub" 35 | @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" 36 | @echo " latexpdf to make LaTeX files and run them through pdflatex" 37 | @echo " latexpdfja to make LaTeX files and run them through platex/dvipdfmx" 38 | @echo " text to make text files" 39 | @echo " man to make manual pages" 40 | @echo " texinfo to make Texinfo files" 41 | @echo " info to make Texinfo files and run them through makeinfo" 42 | @echo " gettext to make PO message catalogs" 43 | @echo " changes to make an overview of all changed/added/deprecated items" 44 | @echo " xml to make Docutils-native XML files" 45 | @echo " pseudoxml to make pseudoxml-XML files for display purposes" 46 | @echo " linkcheck to check all external links for integrity" 47 | @echo " doctest to run all doctests embedded in the documentation (if enabled)" 48 | @echo " coverage to run coverage check of the documentation (if enabled)" 49 | 50 | .PHONY: clean 51 | clean: 52 | rm -rf $(BUILDDIR)/* 53 | 54 | .PHONY: html 55 | html: 56 | $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html 57 | @echo 58 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." 59 | 60 | .PHONY: dirhtml 61 | dirhtml: 62 | $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml 63 | @echo 64 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." 65 | 66 | .PHONY: singlehtml 67 | singlehtml: 68 | $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml 69 | @echo 70 | @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." 71 | 72 | .PHONY: pickle 73 | pickle: 74 | $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle 75 | @echo 76 | @echo "Build finished; now you can process the pickle files." 77 | 78 | .PHONY: json 79 | json: 80 | $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json 81 | @echo 82 | @echo "Build finished; now you can process the JSON files." 83 | 84 | .PHONY: htmlhelp 85 | htmlhelp: 86 | $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp 87 | @echo 88 | @echo "Build finished; now you can run HTML Help Workshop with the" \ 89 | ".hhp project file in $(BUILDDIR)/htmlhelp." 90 | 91 | .PHONY: qthelp 92 | qthelp: 93 | $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp 94 | @echo 95 | @echo "Build finished; now you can run "qcollectiongenerator" with the" \ 96 | ".qhcp project file in $(BUILDDIR)/qthelp, like this:" 97 | @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/openni2_launch.qhcp" 98 | @echo "To view the help file:" 99 | @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/openni2_launch.qhc" 100 | 101 | .PHONY: applehelp 102 | applehelp: 103 | $(SPHINXBUILD) -b applehelp $(ALLSPHINXOPTS) $(BUILDDIR)/applehelp 104 | @echo 105 | @echo "Build finished. The help book is in $(BUILDDIR)/applehelp." 106 | @echo "N.B. You won't be able to view it unless you put it in" \ 107 | "~/Library/Documentation/Help or install it in your application" \ 108 | "bundle." 109 | 110 | .PHONY: devhelp 111 | devhelp: 112 | $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp 113 | @echo 114 | @echo "Build finished." 115 | @echo "To view the help file:" 116 | @echo "# mkdir -p $$HOME/.local/share/devhelp/openni2_launch" 117 | @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/openni2_launch" 118 | @echo "# devhelp" 119 | 120 | .PHONY: epub 121 | epub: 122 | $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub 123 | @echo 124 | @echo "Build finished. The epub file is in $(BUILDDIR)/epub." 125 | 126 | .PHONY: latex 127 | latex: 128 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 129 | @echo 130 | @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." 131 | @echo "Run \`make' in that directory to run these through (pdf)latex" \ 132 | "(use \`make latexpdf' here to do that automatically)." 133 | 134 | .PHONY: latexpdf 135 | latexpdf: 136 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 137 | @echo "Running LaTeX files through pdflatex..." 138 | $(MAKE) -C $(BUILDDIR)/latex all-pdf 139 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 140 | 141 | .PHONY: latexpdfja 142 | latexpdfja: 143 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 144 | @echo "Running LaTeX files through platex and dvipdfmx..." 145 | $(MAKE) -C $(BUILDDIR)/latex all-pdf-ja 146 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 147 | 148 | .PHONY: text 149 | text: 150 | $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text 151 | @echo 152 | @echo "Build finished. The text files are in $(BUILDDIR)/text." 153 | 154 | .PHONY: man 155 | man: 156 | $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man 157 | @echo 158 | @echo "Build finished. The manual pages are in $(BUILDDIR)/man." 159 | 160 | .PHONY: texinfo 161 | texinfo: 162 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 163 | @echo 164 | @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." 165 | @echo "Run \`make' in that directory to run these through makeinfo" \ 166 | "(use \`make info' here to do that automatically)." 167 | 168 | .PHONY: info 169 | info: 170 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 171 | @echo "Running Texinfo files through makeinfo..." 172 | make -C $(BUILDDIR)/texinfo info 173 | @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." 174 | 175 | .PHONY: gettext 176 | gettext: 177 | $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale 178 | @echo 179 | @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." 180 | 181 | .PHONY: changes 182 | changes: 183 | $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes 184 | @echo 185 | @echo "The overview file is in $(BUILDDIR)/changes." 186 | 187 | .PHONY: linkcheck 188 | linkcheck: 189 | $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck 190 | @echo 191 | @echo "Link check complete; look for any errors in the above output " \ 192 | "or in $(BUILDDIR)/linkcheck/output.txt." 193 | 194 | .PHONY: doctest 195 | doctest: 196 | $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest 197 | @echo "Testing of doctests in the sources finished, look at the " \ 198 | "results in $(BUILDDIR)/doctest/output.txt." 199 | 200 | .PHONY: coverage 201 | coverage: 202 | $(SPHINXBUILD) -b coverage $(ALLSPHINXOPTS) $(BUILDDIR)/coverage 203 | @echo "Testing of coverage in the sources finished, look at the " \ 204 | "results in $(BUILDDIR)/coverage/python.txt." 205 | 206 | .PHONY: xml 207 | xml: 208 | $(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml 209 | @echo 210 | @echo "Build finished. The XML files are in $(BUILDDIR)/xml." 211 | 212 | .PHONY: pseudoxml 213 | pseudoxml: 214 | $(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml 215 | @echo 216 | @echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml." 217 | -------------------------------------------------------------------------------- /openni2_camera/src/openni2_device_manager.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | 32 | #include "openni2_camera/openni2_device_manager.h" 33 | #include "openni2_camera/openni2_convert.h" 34 | #include "openni2_camera/openni2_device.h" 35 | #include "openni2_camera/openni2_exception.h" 36 | 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | #include "OpenNI.h" 43 | 44 | namespace openni2_wrapper 45 | { 46 | 47 | class OpenNI2DeviceInfoComparator 48 | { 49 | public: 50 | bool operator()(const OpenNI2DeviceInfo& di1, const OpenNI2DeviceInfo& di2) const 51 | { 52 | return (di1.uri_.compare(di2.uri_) < 0); 53 | } 54 | }; 55 | 56 | typedef std::set DeviceSet; 57 | 58 | class OpenNI2DeviceListener : public openni::OpenNI::DeviceConnectedListener, 59 | public openni::OpenNI::DeviceDisconnectedListener, 60 | public openni::OpenNI::DeviceStateChangedListener 61 | { 62 | public: 63 | OpenNI2DeviceListener() : 64 | openni::OpenNI::DeviceConnectedListener(), 65 | openni::OpenNI::DeviceDisconnectedListener(), 66 | openni::OpenNI::DeviceStateChangedListener() 67 | { 68 | openni::OpenNI::addDeviceConnectedListener(this); 69 | openni::OpenNI::addDeviceDisconnectedListener(this); 70 | openni::OpenNI::addDeviceStateChangedListener(this); 71 | 72 | // get list of currently connected devices 73 | openni::Array device_info_list; 74 | openni::OpenNI::enumerateDevices(&device_info_list); 75 | 76 | for (int i = 0; i < device_info_list.getSize(); ++i) 77 | { 78 | onDeviceConnected(&device_info_list[i]); 79 | } 80 | } 81 | 82 | ~OpenNI2DeviceListener() 83 | { 84 | openni::OpenNI::removeDeviceConnectedListener(this); 85 | openni::OpenNI::removeDeviceDisconnectedListener(this); 86 | openni::OpenNI::removeDeviceStateChangedListener(this); 87 | } 88 | 89 | virtual void onDeviceStateChanged(const openni::DeviceInfo* pInfo, openni::DeviceState state) 90 | { 91 | RCLCPP_INFO(rclcpp::get_logger("openni2"), "Device \"%s\" error state changed to %d\n", pInfo->getUri(), state); 92 | 93 | switch (state) 94 | { 95 | case openni::DEVICE_STATE_OK: 96 | onDeviceConnected(pInfo); 97 | break; 98 | case openni::DEVICE_STATE_ERROR: 99 | case openni::DEVICE_STATE_NOT_READY: 100 | case openni::DEVICE_STATE_EOF: 101 | default: 102 | onDeviceDisconnected(pInfo); 103 | break; 104 | } 105 | } 106 | 107 | virtual void onDeviceConnected(const openni::DeviceInfo* pInfo) 108 | { 109 | std::lock_guard l(device_mutex_); 110 | 111 | const OpenNI2DeviceInfo device_info_wrapped = openni2_convert(pInfo); 112 | 113 | RCLCPP_INFO(rclcpp::get_logger("openni2"), "Device \"%s\" found.", pInfo->getUri()); 114 | 115 | // make sure it does not exist in set before inserting 116 | device_set_.erase(device_info_wrapped); 117 | device_set_.insert(device_info_wrapped); 118 | } 119 | 120 | 121 | virtual void onDeviceDisconnected(const openni::DeviceInfo* pInfo) 122 | { 123 | std::lock_guard l(device_mutex_); 124 | 125 | RCLCPP_WARN(rclcpp::get_logger("openni2"), "Device \"%s\" disconnected\n", pInfo->getUri()); 126 | 127 | const OpenNI2DeviceInfo device_info_wrapped = openni2_convert(pInfo); 128 | device_set_.erase(device_info_wrapped); 129 | } 130 | 131 | std::shared_ptr > getConnectedDeviceURIs() 132 | { 133 | std::lock_guard l(device_mutex_); 134 | 135 | std::shared_ptr > result = std::make_shared >(); 136 | 137 | result->reserve(device_set_.size()); 138 | 139 | std::set::const_iterator it; 140 | std::set::const_iterator it_end = device_set_.end(); 141 | 142 | for (it = device_set_.begin(); it != it_end; ++it) 143 | result->push_back(it->uri_); 144 | 145 | return result; 146 | } 147 | 148 | std::shared_ptr > getConnectedDeviceInfos() 149 | { 150 | std::lock_guard l(device_mutex_); 151 | 152 | std::shared_ptr > result = std::make_shared >(); 153 | 154 | result->reserve(device_set_.size()); 155 | 156 | DeviceSet::const_iterator it; 157 | DeviceSet::const_iterator it_end = device_set_.end(); 158 | 159 | for (it = device_set_.begin(); it != it_end; ++it) 160 | result->push_back(*it); 161 | 162 | return result; 163 | } 164 | 165 | std::size_t getNumOfConnectedDevices() 166 | { 167 | std::lock_guard l(device_mutex_); 168 | 169 | return device_set_.size(); 170 | } 171 | 172 | std::mutex device_mutex_; 173 | DeviceSet device_set_; 174 | }; 175 | 176 | ////////////////////////////////////////////////////////////////////////// 177 | 178 | std::shared_ptr OpenNI2DeviceManager::singelton_; 179 | 180 | OpenNI2DeviceManager::OpenNI2DeviceManager() 181 | { 182 | openni::Status rc = openni::OpenNI::initialize(); 183 | if (rc != openni::STATUS_OK) 184 | THROW_OPENNI_EXCEPTION("Initialize failed\n%s\n", openni::OpenNI::getExtendedError()); 185 | 186 | device_listener_ = std::make_shared(); 187 | } 188 | 189 | OpenNI2DeviceManager::~OpenNI2DeviceManager() 190 | { 191 | } 192 | 193 | std::shared_ptr OpenNI2DeviceManager::getSingelton() 194 | { 195 | if (singelton_.get()==0) 196 | singelton_ = std::make_shared(); 197 | 198 | return singelton_; 199 | } 200 | 201 | std::shared_ptr > OpenNI2DeviceManager::getConnectedDeviceInfos() const 202 | { 203 | return device_listener_->getConnectedDeviceInfos(); 204 | } 205 | 206 | std::shared_ptr > OpenNI2DeviceManager::getConnectedDeviceURIs() const 207 | { 208 | return device_listener_->getConnectedDeviceURIs(); 209 | } 210 | 211 | std::size_t OpenNI2DeviceManager::getNumOfConnectedDevices() const 212 | { 213 | return device_listener_->getNumOfConnectedDevices(); 214 | } 215 | 216 | std::string OpenNI2DeviceManager::getSerial(const std::string& Uri) const 217 | { 218 | openni::Device openni_device; 219 | std::string ret; 220 | 221 | // we need to open the device to query the serial number 222 | if (Uri.length() > 0 && openni_device.open(Uri.c_str()) == openni::STATUS_OK) 223 | { 224 | int serial_len = 100; 225 | char serial[serial_len]; 226 | 227 | openni::Status rc = openni_device.getProperty(openni::DEVICE_PROPERTY_SERIAL_NUMBER, serial, &serial_len); 228 | if (rc == openni::STATUS_OK) 229 | ret = serial; 230 | else 231 | { 232 | THROW_OPENNI_EXCEPTION("Serial number query failed: %s", openni::OpenNI::getExtendedError()); 233 | } 234 | // close the device again 235 | openni_device.close(); 236 | } 237 | else 238 | { 239 | THROW_OPENNI_EXCEPTION("Device open failed: %s", openni::OpenNI::getExtendedError()); 240 | } 241 | return ret; 242 | } 243 | 244 | std::shared_ptr OpenNI2DeviceManager::getAnyDevice(rclcpp::Node* node) 245 | { 246 | return std::make_shared("", node); 247 | } 248 | std::shared_ptr OpenNI2DeviceManager::getDevice(const std::string& device_URI, rclcpp::Node* node) 249 | { 250 | return std::make_shared(device_URI, node); 251 | } 252 | 253 | 254 | std::ostream& operator << (std::ostream& stream, const OpenNI2DeviceManager& device_manager) { 255 | 256 | std::shared_ptr > device_info = device_manager.getConnectedDeviceInfos(); 257 | 258 | std::vector::const_iterator it; 259 | std::vector::const_iterator it_end = device_info->end(); 260 | 261 | for (it = device_info->begin(); it != it_end; ++it) 262 | { 263 | stream << "Uri: " << it->uri_ << " (Vendor: " << it->vendor_ << 264 | ", Name: " << it->name_ << 265 | ", Vendor ID: " << it->vendor_id_ << 266 | ", Product ID: " << it->product_id_ << 267 | ")" << std::endl; 268 | } 269 | 270 | return stream; 271 | } 272 | 273 | 274 | } //namespace openni2_wrapper 275 | -------------------------------------------------------------------------------- /openni2_launch/doc/conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # openni2_launch documentation build configuration file, created by 4 | # sphinx-quickstart on Fri Nov 3 15:35:05 2017. 5 | # 6 | # This file is execfile()d with the current directory set to its 7 | # containing dir. 8 | # 9 | # Note that not all possible configuration values are present in this 10 | # autogenerated file. 11 | # 12 | # All configuration values have a default; values that are commented out 13 | # serve to show the default. 14 | 15 | import sys 16 | import os 17 | 18 | import catkin_pkg.package 19 | catkin_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) 20 | catkin_package = catkin_pkg.package.parse_package(os.path.join(catkin_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME)) 21 | 22 | # If extensions (or modules to document with autodoc) are in another directory, 23 | # add these directories to sys.path here. If the directory is relative to the 24 | # documentation root, use os.path.abspath to make it absolute, like shown here. 25 | #sys.path.insert(0, os.path.abspath('.')) 26 | 27 | # -- General configuration ------------------------------------------------ 28 | 29 | # If your documentation needs a minimal Sphinx version, state it here. 30 | #needs_sphinx = '1.0' 31 | 32 | # Add any Sphinx extension module names here, as strings. They can be 33 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 34 | # ones. 35 | extensions = [ 36 | 'sphinx.ext.autodoc', 37 | 'sphinx.ext.doctest', 38 | 'sphinx.ext.intersphinx', 39 | 'sphinx.ext.todo', 40 | 'sphinx.ext.coverage', 41 | 'sphinx.ext.pngmath', 42 | 'sphinx.ext.ifconfig', 43 | 'sphinx.ext.viewcode', 44 | ] 45 | 46 | # Add any paths that contain templates here, relative to this directory. 47 | templates_path = ['.templates'] 48 | 49 | # The suffix(es) of source filenames. 50 | # You can specify multiple suffix as a list of string: 51 | # source_suffix = ['.rst', '.md'] 52 | source_suffix = '.rst' 53 | 54 | # The encoding of source files. 55 | #source_encoding = 'utf-8-sig' 56 | 57 | # The master toctree document. 58 | master_doc = 'index' 59 | 60 | # General information about the project. 61 | project = u'openni2_launch' 62 | copyright = u'2017, Julius Kammerl, Michael Ferguson' 63 | author = u'Julius Kammerl, Michael Ferguson' 64 | 65 | # The version info for the project you're documenting, acts as replacement for 66 | # |version| and |release|, also used in various other places throughout the 67 | # built documents. 68 | # 69 | # The short X.Y version. 70 | version = catkin_package.version 71 | # The full version, including alpha/beta/rc tags. 72 | release = catkin_package.version 73 | 74 | # The language for content autogenerated by Sphinx. Refer to documentation 75 | # for a list of supported languages. 76 | # 77 | # This is also used if you do content translation via gettext catalogs. 78 | # Usually you set "language" from the command line for these cases. 79 | language = None 80 | 81 | # There are two options for replacing |today|: either, you set today to some 82 | # non-false value, then it is used: 83 | #today = '' 84 | # Else, today_fmt is used as the format for a strftime call. 85 | #today_fmt = '%B %d, %Y' 86 | 87 | # List of patterns, relative to source directory, that match files and 88 | # directories to ignore when looking for source files. 89 | exclude_patterns = ['.build'] 90 | 91 | # The reST default role (used for this markup: `text`) to use for all 92 | # documents. 93 | #default_role = None 94 | 95 | # If true, '()' will be appended to :func: etc. cross-reference text. 96 | #add_function_parentheses = True 97 | 98 | # If true, the current module name will be prepended to all description 99 | # unit titles (such as .. function::). 100 | #add_module_names = True 101 | 102 | # If true, sectionauthor and moduleauthor directives will be shown in the 103 | # output. They are ignored by default. 104 | #show_authors = False 105 | 106 | # The name of the Pygments (syntax highlighting) style to use. 107 | pygments_style = 'sphinx' 108 | 109 | # A list of ignored prefixes for module index sorting. 110 | #modindex_common_prefix = [] 111 | 112 | # If true, keep warnings as "system message" paragraphs in the built documents. 113 | #keep_warnings = False 114 | 115 | # If true, `todo` and `todoList` produce output, else they produce nothing. 116 | todo_include_todos = True 117 | 118 | 119 | # -- Options for HTML output ---------------------------------------------- 120 | 121 | # The theme to use for HTML and HTML Help pages. See the documentation for 122 | # a list of builtin themes. 123 | html_theme = 'alabaster' 124 | 125 | # Theme options are theme-specific and customize the look and feel of a theme 126 | # further. For a list of options available for each theme, see the 127 | # documentation. 128 | #html_theme_options = {} 129 | 130 | # Add any paths that contain custom themes here, relative to this directory. 131 | #html_theme_path = [] 132 | 133 | # The name for this set of Sphinx documents. If None, it defaults to 134 | # " v documentation". 135 | #html_title = None 136 | 137 | # A shorter title for the navigation bar. Default is the same as html_title. 138 | #html_short_title = None 139 | 140 | # The name of an image file (relative to this directory) to place at the top 141 | # of the sidebar. 142 | #html_logo = None 143 | 144 | # The name of an image file (relative to this directory) to use as a favicon of 145 | # the docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 146 | # pixels large. 147 | #html_favicon = None 148 | 149 | # Add any paths that contain custom static files (such as style sheets) here, 150 | # relative to this directory. They are copied after the builtin static files, 151 | # so a file named "default.css" will overwrite the builtin "default.css". 152 | html_static_path = ['.static'] 153 | 154 | # Add any extra paths that contain custom files (such as robots.txt or 155 | # .htaccess) here, relative to this directory. These files are copied 156 | # directly to the root of the documentation. 157 | #html_extra_path = [] 158 | 159 | # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, 160 | # using the given strftime format. 161 | #html_last_updated_fmt = '%b %d, %Y' 162 | 163 | # If true, SmartyPants will be used to convert quotes and dashes to 164 | # typographically correct entities. 165 | #html_use_smartypants = True 166 | 167 | # Custom sidebar templates, maps document names to template names. 168 | #html_sidebars = {} 169 | 170 | # Additional templates that should be rendered to pages, maps page names to 171 | # template names. 172 | #html_additional_pages = {} 173 | 174 | # If false, no module index is generated. 175 | #html_domain_indices = True 176 | 177 | # If false, no index is generated. 178 | #html_use_index = True 179 | 180 | # If true, the index is split into individual pages for each letter. 181 | #html_split_index = False 182 | 183 | # If true, links to the reST sources are added to the pages. 184 | #html_show_sourcelink = True 185 | 186 | # If true, "Created using Sphinx" is shown in the HTML footer. Default is True. 187 | #html_show_sphinx = True 188 | 189 | # If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. 190 | #html_show_copyright = True 191 | 192 | # If true, an OpenSearch description file will be output, and all pages will 193 | # contain a tag referring to it. The value of this option must be the 194 | # base URL from which the finished HTML is served. 195 | #html_use_opensearch = '' 196 | 197 | # This is the file name suffix for HTML files (e.g. ".xhtml"). 198 | #html_file_suffix = None 199 | 200 | # Language to be used for generating the HTML full-text search index. 201 | # Sphinx supports the following languages: 202 | # 'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja' 203 | # 'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr' 204 | #html_search_language = 'en' 205 | 206 | # A dictionary with options for the search language support, empty by default. 207 | # Now only 'ja' uses this config value 208 | #html_search_options = {'type': 'default'} 209 | 210 | # The name of a javascript file (relative to the configuration directory) that 211 | # implements a search results scorer. If empty, the default will be used. 212 | #html_search_scorer = 'scorer.js' 213 | 214 | # Output file base name for HTML help builder. 215 | htmlhelp_basename = 'openni2_launchdoc' 216 | 217 | # -- Options for LaTeX output --------------------------------------------- 218 | 219 | latex_elements = { 220 | # The paper size ('letterpaper' or 'a4paper'). 221 | #'papersize': 'letterpaper', 222 | 223 | # The font size ('10pt', '11pt' or '12pt'). 224 | #'pointsize': '10pt', 225 | 226 | # Additional stuff for the LaTeX preamble. 227 | #'preamble': '', 228 | 229 | # Latex figure (float) alignment 230 | #'figure_align': 'htbp', 231 | } 232 | 233 | # Grouping the document tree into LaTeX files. List of tuples 234 | # (source start file, target name, title, 235 | # author, documentclass [howto, manual, or own class]). 236 | latex_documents = [ 237 | (master_doc, 'openni2_launch.tex', u'openni2\\_launch Documentation', 238 | u'Julius Kammerl, Michael Ferguson', 'manual'), 239 | ] 240 | 241 | # The name of an image file (relative to this directory) to place at the top of 242 | # the title page. 243 | #latex_logo = None 244 | 245 | # For "manual" documents, if this is true, then toplevel headings are parts, 246 | # not chapters. 247 | #latex_use_parts = False 248 | 249 | # If true, show page references after internal links. 250 | #latex_show_pagerefs = False 251 | 252 | # If true, show URL addresses after external links. 253 | #latex_show_urls = False 254 | 255 | # Documents to append as an appendix to all manuals. 256 | #latex_appendices = [] 257 | 258 | # If false, no module index is generated. 259 | #latex_domain_indices = True 260 | 261 | 262 | # -- Options for manual page output --------------------------------------- 263 | 264 | # One entry per manual page. List of tuples 265 | # (source start file, name, description, authors, manual section). 266 | man_pages = [ 267 | (master_doc, 'openni2_launch', u'openni2_launch Documentation', 268 | [author], 1) 269 | ] 270 | 271 | # If true, show URL addresses after external links. 272 | #man_show_urls = False 273 | 274 | 275 | # -- Options for Texinfo output ------------------------------------------- 276 | 277 | # Grouping the document tree into Texinfo files. List of tuples 278 | # (source start file, target name, title, author, 279 | # dir menu entry, description, category) 280 | texinfo_documents = [ 281 | (master_doc, 'openni2_launch', u'openni2_launch Documentation', 282 | author, 'openni2_launch', 'One line description of project.', 283 | 'Miscellaneous'), 284 | ] 285 | 286 | # Documents to append as an appendix to all manuals. 287 | #texinfo_appendices = [] 288 | 289 | # If false, no module index is generated. 290 | #texinfo_domain_indices = True 291 | 292 | # How to display URL addresses: 'footnote', 'no', or 'inline'. 293 | #texinfo_show_urls = 'footnote' 294 | 295 | # If true, do not generate a @detailmenu in the "Top" node's menu. 296 | #texinfo_no_detailmenu = False 297 | 298 | 299 | # Example configuration for intersphinx: refer to the Python standard library. 300 | intersphinx_mapping = {'https://docs.python.org/': None} 301 | -------------------------------------------------------------------------------- /openni2_camera/src/openni2_device.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | */ 31 | 32 | #include "OpenNI.h" 33 | #include // For XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE property 34 | 35 | #include "openni2_camera/openni2_device.h" 36 | #include "openni2_camera/openni2_exception.h" 37 | #include "openni2_camera/openni2_convert.h" 38 | #include "openni2_camera/openni2_frame_listener.h" 39 | 40 | #include 41 | 42 | namespace openni2_wrapper 43 | { 44 | 45 | void replace_all(std::string &input, const std::string &search, const std::string &replace) 46 | { 47 | size_t pos; 48 | while ((pos = input.find(search)) != std::string::npos) 49 | input.replace(pos, 1, replace); 50 | } 51 | 52 | OpenNI2Device::OpenNI2Device(const std::string& device_URI, 53 | rclcpp::Node* node) : 54 | openni_device_(), 55 | ir_video_started_(false), 56 | color_video_started_(false), 57 | depth_video_started_(false), 58 | image_registration_activated_(false) 59 | { 60 | openni::Status rc = openni::OpenNI::initialize(); 61 | if (rc != openni::STATUS_OK) 62 | THROW_OPENNI_EXCEPTION("Initialize failed\n%s\n", openni::OpenNI::getExtendedError()); 63 | 64 | openni_device_ = std::make_shared(); 65 | 66 | if (device_URI.length() > 0) 67 | { 68 | rc = openni_device_->open(device_URI.c_str()); 69 | } 70 | else 71 | { 72 | rc = openni_device_->open(openni::ANY_DEVICE); 73 | } 74 | 75 | if (rc != openni::STATUS_OK) 76 | THROW_OPENNI_EXCEPTION("Device open failed\n%s\n", openni::OpenNI::getExtendedError()); 77 | 78 | device_info_ = std::make_shared(); 79 | *device_info_ = openni_device_->getDeviceInfo(); 80 | 81 | ir_frame_listener = std::make_shared(node); 82 | color_frame_listener = std::make_shared(node); 83 | depth_frame_listener = std::make_shared(node); 84 | 85 | } 86 | 87 | OpenNI2Device::~OpenNI2Device() 88 | { 89 | stopAllStreams(); 90 | 91 | shutdown(); 92 | 93 | openni_device_->close(); 94 | } 95 | 96 | const std::string OpenNI2Device::getUri() const 97 | { 98 | return std::string(device_info_->getUri()); 99 | } 100 | 101 | const std::string OpenNI2Device::getVendor() const 102 | { 103 | return std::string(device_info_->getVendor()); 104 | } 105 | 106 | const std::string OpenNI2Device::getName() const 107 | { 108 | return std::string(device_info_->getName()); 109 | } 110 | 111 | uint16_t OpenNI2Device::getUsbVendorId() const 112 | { 113 | return device_info_->getUsbVendorId(); 114 | } 115 | 116 | uint16_t OpenNI2Device::getUsbProductId() const 117 | { 118 | return device_info_->getUsbProductId(); 119 | } 120 | 121 | const std::string OpenNI2Device::getStringID() const 122 | { 123 | std::string ID_str = getName() + "_" + getVendor(); 124 | 125 | replace_all(ID_str, "/", ""); 126 | replace_all(ID_str, ".", ""); 127 | replace_all(ID_str, "@", ""); 128 | 129 | return ID_str; 130 | } 131 | 132 | bool OpenNI2Device::isValid() const 133 | { 134 | return (openni_device_.get() != 0) && openni_device_->isValid(); 135 | } 136 | 137 | float OpenNI2Device::getIRFocalLength(int output_y_resolution) const 138 | { 139 | float focal_length = 0.0f; 140 | std::shared_ptr stream = getIRVideoStream(); 141 | 142 | if (stream) 143 | { 144 | focal_length = (float)output_y_resolution / (2 * tan(stream->getVerticalFieldOfView() / 2)); 145 | } 146 | 147 | return focal_length; 148 | } 149 | 150 | float OpenNI2Device::getColorFocalLength(int output_y_resolution) const 151 | { 152 | float focal_length = 0.0f; 153 | std::shared_ptr stream = getColorVideoStream(); 154 | 155 | if (stream) 156 | { 157 | focal_length = (float)output_y_resolution / (2 * tan(stream->getVerticalFieldOfView() / 2)); 158 | } 159 | 160 | return focal_length; 161 | } 162 | 163 | float OpenNI2Device::getDepthFocalLength(int output_y_resolution) const 164 | { 165 | float focal_length = 0.0f; 166 | std::shared_ptr stream = getDepthVideoStream(); 167 | 168 | if (stream) 169 | { 170 | focal_length = (float)output_y_resolution / (2 * tan(stream->getVerticalFieldOfView() / 2)); 171 | } 172 | 173 | return focal_length; 174 | } 175 | 176 | float OpenNI2Device::getBaseline() const 177 | { 178 | float baseline = 0.075f; 179 | std::shared_ptr stream = getDepthVideoStream(); 180 | 181 | if (stream && stream->isPropertySupported(XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE)) 182 | { 183 | double baseline_meters; 184 | stream->getProperty(XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE, &baseline_meters); // Device specific -- from PS1080.h 185 | baseline = static_cast(baseline_meters * 0.01f); // baseline from cm -> meters 186 | } 187 | return baseline; 188 | } 189 | 190 | bool OpenNI2Device::isIRVideoModeSupported(const OpenNI2VideoMode& video_mode) const 191 | { 192 | getSupportedIRVideoModes(); 193 | 194 | bool supported = false; 195 | 196 | std::vector::const_iterator it = ir_video_modes_.begin(); 197 | std::vector::const_iterator it_end = ir_video_modes_.end(); 198 | 199 | while (it != it_end && !supported) 200 | { 201 | supported = (*it == video_mode); 202 | ++it; 203 | } 204 | 205 | return supported; 206 | } 207 | 208 | bool OpenNI2Device::isColorVideoModeSupported(const OpenNI2VideoMode& video_mode) const 209 | { 210 | getSupportedColorVideoModes(); 211 | 212 | bool supported = false; 213 | 214 | std::vector::const_iterator it = color_video_modes_.begin(); 215 | std::vector::const_iterator it_end = color_video_modes_.end(); 216 | 217 | while (it != it_end && !supported) 218 | { 219 | supported = (*it == video_mode); 220 | ++it; 221 | } 222 | 223 | return supported; 224 | } 225 | 226 | bool OpenNI2Device::isDepthVideoModeSupported(const OpenNI2VideoMode& video_mode) const 227 | { 228 | getSupportedDepthVideoModes(); 229 | 230 | bool supported = false; 231 | 232 | std::vector::const_iterator it = depth_video_modes_.begin(); 233 | std::vector::const_iterator it_end = depth_video_modes_.end(); 234 | 235 | while (it != it_end && !supported) 236 | { 237 | supported = (*it == video_mode); 238 | ++it; 239 | } 240 | 241 | return supported; 242 | 243 | } 244 | 245 | bool OpenNI2Device::hasIRSensor() const 246 | { 247 | return openni_device_->hasSensor(openni::SENSOR_IR); 248 | } 249 | 250 | bool OpenNI2Device::hasColorSensor() const 251 | { 252 | return openni_device_->hasSensor(openni::SENSOR_COLOR); 253 | } 254 | 255 | bool OpenNI2Device::hasDepthSensor() const 256 | { 257 | return openni_device_->hasSensor(openni::SENSOR_DEPTH); 258 | } 259 | 260 | void OpenNI2Device::startIRStream() 261 | { 262 | std::shared_ptr stream = getIRVideoStream(); 263 | 264 | if (stream) 265 | { 266 | stream->setMirroringEnabled(false); 267 | stream->start(); 268 | stream->addNewFrameListener(ir_frame_listener.get()); 269 | ir_video_started_ = true; 270 | } 271 | 272 | } 273 | 274 | void OpenNI2Device::startColorStream() 275 | { 276 | std::shared_ptr stream = getColorVideoStream(); 277 | 278 | if (stream) 279 | { 280 | stream->setMirroringEnabled(false); 281 | stream->start(); 282 | stream->addNewFrameListener(color_frame_listener.get()); 283 | color_video_started_ = true; 284 | } 285 | } 286 | void OpenNI2Device::startDepthStream() 287 | { 288 | std::shared_ptr stream = getDepthVideoStream(); 289 | 290 | if (stream) 291 | { 292 | stream->setMirroringEnabled(false); 293 | stream->start(); 294 | stream->addNewFrameListener(depth_frame_listener.get()); 295 | depth_video_started_ = true; 296 | } 297 | } 298 | 299 | void OpenNI2Device::stopAllStreams() 300 | { 301 | stopIRStream(); 302 | stopColorStream(); 303 | stopDepthStream(); 304 | } 305 | 306 | void OpenNI2Device::stopIRStream() 307 | { 308 | if (ir_video_stream_.get() != 0) 309 | { 310 | ir_video_started_ = false; 311 | 312 | ir_video_stream_->removeNewFrameListener(ir_frame_listener.get()); 313 | 314 | ir_video_stream_->stop(); 315 | } 316 | } 317 | void OpenNI2Device::stopColorStream() 318 | { 319 | if (color_video_stream_.get() != 0) 320 | { 321 | color_video_started_ = false; 322 | 323 | color_video_stream_->removeNewFrameListener(color_frame_listener.get()); 324 | 325 | color_video_stream_->stop(); 326 | } 327 | } 328 | void OpenNI2Device::stopDepthStream() 329 | { 330 | if (depth_video_stream_.get() != 0) 331 | { 332 | depth_video_started_ = false; 333 | 334 | depth_video_stream_->removeNewFrameListener(depth_frame_listener.get()); 335 | 336 | depth_video_stream_->stop(); 337 | } 338 | } 339 | 340 | void OpenNI2Device::shutdown() 341 | { 342 | if (ir_video_stream_.get() != 0) 343 | ir_video_stream_->destroy(); 344 | 345 | if (color_video_stream_.get() != 0) 346 | color_video_stream_->destroy(); 347 | 348 | if (depth_video_stream_.get() != 0) 349 | depth_video_stream_->destroy(); 350 | 351 | } 352 | 353 | bool OpenNI2Device::isIRStreamStarted() 354 | { 355 | return ir_video_started_; 356 | } 357 | bool OpenNI2Device::isColorStreamStarted() 358 | { 359 | return color_video_started_; 360 | } 361 | bool OpenNI2Device::isDepthStreamStarted() 362 | { 363 | return depth_video_started_; 364 | } 365 | 366 | const std::vector& OpenNI2Device::getSupportedIRVideoModes() const 367 | { 368 | std::shared_ptr stream = getIRVideoStream(); 369 | 370 | ir_video_modes_.clear(); 371 | 372 | if (stream) 373 | { 374 | const openni::SensorInfo& sensor_info = stream->getSensorInfo(); 375 | 376 | ir_video_modes_ = openni2_convert(sensor_info.getSupportedVideoModes()); 377 | } 378 | 379 | return ir_video_modes_; 380 | } 381 | 382 | const std::vector& OpenNI2Device::getSupportedColorVideoModes() const 383 | { 384 | std::shared_ptr stream = getColorVideoStream(); 385 | 386 | color_video_modes_.clear(); 387 | 388 | if (stream) 389 | { 390 | const openni::SensorInfo& sensor_info = stream->getSensorInfo(); 391 | 392 | color_video_modes_ = openni2_convert(sensor_info.getSupportedVideoModes()); 393 | } 394 | 395 | return color_video_modes_; 396 | } 397 | 398 | const std::vector& OpenNI2Device::getSupportedDepthVideoModes() const 399 | { 400 | std::shared_ptr stream = getDepthVideoStream(); 401 | 402 | depth_video_modes_.clear(); 403 | 404 | if (stream) 405 | { 406 | const openni::SensorInfo& sensor_info = stream->getSensorInfo(); 407 | 408 | depth_video_modes_ = openni2_convert(sensor_info.getSupportedVideoModes()); 409 | } 410 | 411 | return depth_video_modes_; 412 | } 413 | 414 | bool OpenNI2Device::isImageRegistrationModeSupported() const 415 | { 416 | return openni_device_->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR); 417 | } 418 | 419 | void OpenNI2Device::setImageRegistrationMode(bool enabled) 420 | { 421 | if (isImageRegistrationModeSupported()) 422 | { 423 | image_registration_activated_ = enabled; 424 | if (enabled) 425 | { 426 | openni::Status rc = openni_device_->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR); 427 | if (rc != openni::STATUS_OK) 428 | THROW_OPENNI_EXCEPTION("Enabling image registration mode failed: \n%s\n", openni::OpenNI::getExtendedError()); 429 | } 430 | else 431 | { 432 | openni::Status rc = openni_device_->setImageRegistrationMode(openni::IMAGE_REGISTRATION_OFF); 433 | if (rc != openni::STATUS_OK) 434 | THROW_OPENNI_EXCEPTION("Enabling image registration mode failed: \n%s\n", openni::OpenNI::getExtendedError()); 435 | } 436 | } 437 | } 438 | 439 | void OpenNI2Device::setDepthColorSync(bool enabled) 440 | { 441 | openni::Status rc = openni_device_->setDepthColorSyncEnabled(enabled); 442 | if (rc != openni::STATUS_OK) 443 | THROW_OPENNI_EXCEPTION("Enabling depth color synchronization failed: \n%s\n", openni::OpenNI::getExtendedError()); 444 | } 445 | 446 | const OpenNI2VideoMode OpenNI2Device::getIRVideoMode() 447 | { 448 | OpenNI2VideoMode ret; 449 | 450 | std::shared_ptr stream = getIRVideoStream(); 451 | 452 | if (stream) 453 | { 454 | openni::VideoMode video_mode = stream->getVideoMode(); 455 | 456 | ret = openni2_convert(video_mode); 457 | } 458 | else 459 | THROW_OPENNI_EXCEPTION("Could not create video stream."); 460 | 461 | return ret; 462 | } 463 | 464 | const OpenNI2VideoMode OpenNI2Device::getColorVideoMode() 465 | { 466 | OpenNI2VideoMode ret; 467 | 468 | std::shared_ptr stream = getColorVideoStream(); 469 | 470 | if (stream) 471 | { 472 | openni::VideoMode video_mode = stream->getVideoMode(); 473 | 474 | ret = openni2_convert(video_mode); 475 | } 476 | else 477 | THROW_OPENNI_EXCEPTION("Could not create video stream."); 478 | 479 | return ret; 480 | } 481 | 482 | const OpenNI2VideoMode OpenNI2Device::getDepthVideoMode() 483 | { 484 | OpenNI2VideoMode ret; 485 | 486 | std::shared_ptr stream = getDepthVideoStream(); 487 | 488 | if (stream) 489 | { 490 | openni::VideoMode video_mode = stream->getVideoMode(); 491 | 492 | ret = openni2_convert(video_mode); 493 | } 494 | else 495 | THROW_OPENNI_EXCEPTION("Could not create video stream."); 496 | 497 | return ret; 498 | } 499 | 500 | void OpenNI2Device::setIRVideoMode(const OpenNI2VideoMode& video_mode) 501 | { 502 | std::shared_ptr stream = getIRVideoStream(); 503 | 504 | if (stream) 505 | { 506 | const openni::VideoMode videoMode = openni2_convert(video_mode); 507 | const openni::Status rc = stream->setVideoMode(videoMode); 508 | if (rc != openni::STATUS_OK) 509 | THROW_OPENNI_EXCEPTION("Couldn't set IR video mode: \n%s\n", openni::OpenNI::getExtendedError()); 510 | } 511 | } 512 | 513 | void OpenNI2Device::setColorVideoMode(const OpenNI2VideoMode& video_mode) 514 | { 515 | std::shared_ptr stream = getColorVideoStream(); 516 | 517 | if (stream) 518 | { 519 | const openni::VideoMode videoMode = openni2_convert(video_mode); 520 | const openni::Status rc = stream->setVideoMode(videoMode); 521 | if (rc != openni::STATUS_OK) 522 | THROW_OPENNI_EXCEPTION("Couldn't set color video mode: \n%s\n", openni::OpenNI::getExtendedError()); 523 | } 524 | } 525 | 526 | void OpenNI2Device::setDepthVideoMode(const OpenNI2VideoMode& video_mode) 527 | { 528 | std::shared_ptr stream = getDepthVideoStream(); 529 | 530 | if (stream) 531 | { 532 | const openni::VideoMode videoMode = openni2_convert(video_mode); 533 | const openni::Status rc = stream->setVideoMode(videoMode); 534 | if (rc != openni::STATUS_OK) 535 | THROW_OPENNI_EXCEPTION("Couldn't set depth video mode: \n%s\n", openni::OpenNI::getExtendedError()); 536 | } 537 | } 538 | 539 | void OpenNI2Device::setAutoExposure(bool enable) 540 | { 541 | std::shared_ptr stream = getColorVideoStream(); 542 | 543 | if (stream) 544 | { 545 | openni::CameraSettings* camera_seeting = stream->getCameraSettings(); 546 | if (camera_seeting) 547 | { 548 | const openni::Status rc = camera_seeting->setAutoExposureEnabled(enable); 549 | if (rc != openni::STATUS_OK) 550 | THROW_OPENNI_EXCEPTION("Couldn't set auto exposure: \n%s\n", openni::OpenNI::getExtendedError()); 551 | } 552 | 553 | } 554 | } 555 | void OpenNI2Device::setAutoWhiteBalance(bool enable) 556 | { 557 | std::shared_ptr stream = getColorVideoStream(); 558 | 559 | if (stream) 560 | { 561 | openni::CameraSettings* camera_seeting = stream->getCameraSettings(); 562 | if (camera_seeting) 563 | { 564 | const openni::Status rc = camera_seeting->setAutoWhiteBalanceEnabled(enable); 565 | if (rc != openni::STATUS_OK) 566 | THROW_OPENNI_EXCEPTION("Couldn't set auto white balance: \n%s\n", openni::OpenNI::getExtendedError()); 567 | } 568 | 569 | } 570 | } 571 | 572 | void OpenNI2Device::setExposure(int exposure) 573 | { 574 | std::shared_ptr stream = getColorVideoStream(); 575 | 576 | if (stream) 577 | { 578 | openni::CameraSettings* camera_settings = stream->getCameraSettings(); 579 | if (camera_settings) 580 | { 581 | const openni::Status rc = camera_settings->setExposure(exposure); 582 | if (rc != openni::STATUS_OK) 583 | THROW_OPENNI_EXCEPTION("Couldn't set exposure: \n%s\n", openni::OpenNI::getExtendedError()); 584 | } 585 | } 586 | } 587 | 588 | bool OpenNI2Device::getAutoExposure() const 589 | { 590 | bool ret = false; 591 | 592 | std::shared_ptr stream = getColorVideoStream(); 593 | 594 | if (stream) 595 | { 596 | openni::CameraSettings* camera_seeting = stream->getCameraSettings(); 597 | if (camera_seeting) 598 | ret = camera_seeting->getAutoExposureEnabled(); 599 | } 600 | 601 | return ret; 602 | } 603 | 604 | bool OpenNI2Device::getAutoWhiteBalance() const 605 | { 606 | bool ret = false; 607 | 608 | std::shared_ptr stream = getColorVideoStream(); 609 | 610 | if (stream) 611 | { 612 | openni::CameraSettings* camera_seeting = stream->getCameraSettings(); 613 | if (camera_seeting) 614 | ret = camera_seeting->getAutoWhiteBalanceEnabled(); 615 | } 616 | 617 | return ret; 618 | } 619 | 620 | int OpenNI2Device::getExposure() const 621 | { 622 | int ret = 0; 623 | 624 | std::shared_ptr stream = getColorVideoStream(); 625 | 626 | if (stream) 627 | { 628 | openni::CameraSettings* camera_settings = stream->getCameraSettings(); 629 | if (camera_settings) 630 | ret = camera_settings->getExposure(); 631 | } 632 | 633 | return ret; 634 | } 635 | 636 | void OpenNI2Device::setUseDeviceTimer(bool enable) 637 | { 638 | if (ir_frame_listener) 639 | ir_frame_listener->setUseDeviceTimer(enable); 640 | 641 | if (color_frame_listener) 642 | color_frame_listener->setUseDeviceTimer(enable); 643 | 644 | if (depth_frame_listener) 645 | depth_frame_listener->setUseDeviceTimer(enable); 646 | } 647 | 648 | void OpenNI2Device::setIRFrameCallback(FrameCallbackFunction callback) 649 | { 650 | ir_frame_listener->setCallback(callback); 651 | } 652 | 653 | void OpenNI2Device::setColorFrameCallback(FrameCallbackFunction callback) 654 | { 655 | color_frame_listener->setCallback(callback); 656 | } 657 | 658 | void OpenNI2Device::setDepthFrameCallback(FrameCallbackFunction callback) 659 | { 660 | depth_frame_listener->setCallback(callback); 661 | } 662 | 663 | std::shared_ptr OpenNI2Device::getIRVideoStream() const 664 | { 665 | if (ir_video_stream_.get() == 0) 666 | { 667 | if (hasIRSensor()) 668 | { 669 | ir_video_stream_ = std::make_shared(); 670 | 671 | const openni::Status rc = ir_video_stream_->create(*openni_device_, openni::SENSOR_IR); 672 | if (rc != openni::STATUS_OK) 673 | THROW_OPENNI_EXCEPTION("Couldn't create IR video stream: \n%s\n", openni::OpenNI::getExtendedError()); 674 | } 675 | } 676 | return ir_video_stream_; 677 | } 678 | 679 | std::shared_ptr OpenNI2Device::getColorVideoStream() const 680 | { 681 | if (color_video_stream_.get() == 0) 682 | { 683 | if (hasColorSensor()) 684 | { 685 | color_video_stream_ = std::make_shared(); 686 | 687 | const openni::Status rc = color_video_stream_->create(*openni_device_, openni::SENSOR_COLOR); 688 | if (rc != openni::STATUS_OK) 689 | THROW_OPENNI_EXCEPTION("Couldn't create color video stream: \n%s\n", openni::OpenNI::getExtendedError()); 690 | } 691 | } 692 | return color_video_stream_; 693 | } 694 | 695 | std::shared_ptr OpenNI2Device::getDepthVideoStream() const 696 | { 697 | if (depth_video_stream_.get() == 0) 698 | { 699 | if (hasDepthSensor()) 700 | { 701 | depth_video_stream_ = std::make_shared(); 702 | 703 | const openni::Status rc = depth_video_stream_->create(*openni_device_, openni::SENSOR_DEPTH); 704 | if (rc != openni::STATUS_OK) 705 | THROW_OPENNI_EXCEPTION("Couldn't create depth video stream: \n%s\n", openni::OpenNI::getExtendedError()); 706 | } 707 | } 708 | return depth_video_stream_; 709 | } 710 | 711 | std::ostream& operator <<(std::ostream& stream, const OpenNI2Device& device) 712 | { 713 | 714 | stream << "Device info (" << device.getUri() << ")" << std::endl; 715 | stream << " Vendor: " << device.getVendor() << std::endl; 716 | stream << " Name: " << device.getName() << std::endl; 717 | stream << " USB Vendor ID: " << device.getUsbVendorId() << std::endl; 718 | stream << " USB Product ID: " << device.getUsbVendorId() << std::endl << std::endl; 719 | 720 | if (device.hasIRSensor()) 721 | { 722 | stream << "IR sensor video modes:" << std::endl; 723 | const std::vector& video_modes = device.getSupportedIRVideoModes(); 724 | 725 | std::vector::const_iterator it = video_modes.begin(); 726 | std::vector::const_iterator it_end = video_modes.end(); 727 | for (; it != it_end; ++it) 728 | stream << " - " << *it << std::endl; 729 | } 730 | else 731 | { 732 | stream << "No IR sensor available" << std::endl; 733 | } 734 | 735 | if (device.hasColorSensor()) 736 | { 737 | stream << "Color sensor video modes:" << std::endl; 738 | const std::vector& video_modes = device.getSupportedColorVideoModes(); 739 | 740 | std::vector::const_iterator it = video_modes.begin(); 741 | std::vector::const_iterator it_end = video_modes.end(); 742 | for (; it != it_end; ++it) 743 | stream << " - " << *it << std::endl; 744 | } 745 | else 746 | { 747 | stream << "No Color sensor available" << std::endl; 748 | } 749 | 750 | if (device.hasDepthSensor()) 751 | { 752 | stream << "Depth sensor video modes:" << std::endl; 753 | const std::vector& video_modes = device.getSupportedDepthVideoModes(); 754 | 755 | std::vector::const_iterator it = video_modes.begin(); 756 | std::vector::const_iterator it_end = video_modes.end(); 757 | for (; it != it_end; ++it) 758 | stream << " - " << *it << std::endl; 759 | } 760 | else 761 | { 762 | stream << "No Depth sensor available" << std::endl; 763 | } 764 | 765 | return stream; 766 | } 767 | 768 | } 769 | --------------------------------------------------------------------------------