Synchronize fork with upstream v2.2.14

README.md

@@ -23,48 +23,75 @@ Instructions on how to update the firmware can be found on the [Intel homepage](
 # ROS Wrapper for Intel® RealSense™ Devices
 These are packages for using Intel RealSense cameras (D400 series SR300 camera and T265 Tracking Module) with ROS.
 
-LibRealSense supported version: v2.31.0 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases))
+LibRealSense supported version: v2.35.2 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases))
 
 ## Installation Instructions
 
-The following instructions support ROS Indigo, on **Ubuntu 14.04**, and ROS Kinetic, on **Ubuntu 16.04**.
+The following instructions are written for ROS Kinetic, on **Ubuntu 16.04** but apply to ROS Melodic on **Ubuntu 18.04** as well, by replacing kinetic with melodic wherever is needed.
 
-#### The simplest way to install on a clean machine is to follow the instructions on the [.travis.yml](https://github.com/intel-ros/realsense/blob/development/.travis.yml) file. It basically summerize the elaborate instructions in the following 3 steps:
+   ### Step 1: Install the ROS distribution
+   - #### Install [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu), on Ubuntu 16.04 or [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) on Ubuntu 18.04.
 
-### Step 1: Install the latest Intel® RealSense™ SDK 2.0
-- #### Install from [Debian Package](https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages) - In that case treat yourself as a developer. Make sure you follow the instructions to also install librealsense2-dev and librealsense-dkms packages.
 
-#### OR
-- #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.31.0) and follow the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md)
+### There are 2 sources to install realsense2_camera from:
 
-### Step 2: Install the ROS distribution
-- #### Install [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu), on Ubuntu 16.04
+* ### Method 1: The ROS distribution:
+    realsense2_camera is available as a debian package of ROS distribution. It can be installed by typing:
+
+    ```
+    export ROS_VER=kinetic 
+    ```
+    or
+    ```
+    export ROS_VER=melodic 
+    ```
+    sudo apt-get install ros-$ROS_VER-realsense2-camera`
 
-### Step 3: Install Intel® RealSense™ ROS from Sources
-- Create a [catkin](http://wiki.ros.org/catkin#Installing_catkin) workspace
-```bash
-mkdir -p ~/catkin_ws/src
-cd ~/catkin_ws/src/
-```
-- Clone the latest Intel® RealSense™ ROS from [here](https://github.com/intel-ros/realsense/releases) into 'catkin_ws/src/'
-```bashrc
-git clone https://github.com/IntelRealSense/realsense-ros.git
-cd realsense-ros/
-git checkout `git tag | sort -V | grep -P "^\d+\.\d+\.\d+" | tail -1`
-cd ..
-```
-- Make sure all dependent packages are installed. You can check .travis.yml file for reference.
-- Specifically, make sure that the ros package *ddynamic_reconfigure* is installed. If *ddynamic_reconfigure* cannot be installed using APT, you may clone it into your workspace 'catkin_ws/src/' from [here](https://github.com/pal-robotics/ddynamic_reconfigure/tree/kinetic-devel) (Version 0.2.0)
+    This will install both realsense2_camera and its dependents, including librealsense2 library.
 
-```bash
-catkin_init_workspace
-cd ..
-catkin_make clean
-catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
-catkin_make install
-echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
-source ~/.bashrc
-```
+    Notice:
+    * The version of librealsense2 is almost always behind the one availeable in RealSense™ official repository.
+    * librealsense2 is not built to use native v4l2 driver but the less stable RS-USB protocol. That is because the last is more general and operational on a larger variety of platforms.
+    * any_realsense2_description is available as a separate debian package of ROS distribution. It includes the 3D-models of the devices and is necessary for running launch files that include these models (i.e. rs_d435_camera_with_model.launch). It can be installed by typing:
+    `sudo apt-get install ros-$ROS_VER-realsense2-description`
+
+
+* ### Method 2: The RealSense™ distribution:
+
+   #### This option is demonstrated in the [.travis.yml](https://github.com/intel-ros/realsense/blob/development/.travis.yml) file. It basically summerize the elaborate instructions in the following 2 steps:
+
+   ### Step 1: Install the latest Intel® RealSense™ SDK 2.0
+   - #### Install from [Debian Package](https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages) - In that case treat yourself as a developer. Make sure you follow the instructions to also install librealsense2-dev and librealsense-dkms packages.
+
+   #### OR
+   - #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.35.2) and follow the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md)
+
+
+   ### Step 2: Install Intel® RealSense™ ROS from Sources
+   - Create a [catkin](http://wiki.ros.org/catkin#Installing_catkin) workspace
+   ```bash
+   mkdir -p ~/catkin_ws/src
+   cd ~/catkin_ws/src/
+   ```
+   - Clone the latest Intel® RealSense™ ROS from [here](https://github.com/intel-ros/realsense/releases) into 'catkin_ws/src/'
+   ```bashrc
+   git clone https://github.com/IntelRealSense/realsense-ros.git
+   cd realsense-ros/
+   git checkout `git tag | sort -V | grep -P "^\d+\.\d+\.\d+" | tail -1`
+   cd ..
+   ```
+   - Make sure all dependent packages are installed. You can check .travis.yml file for reference.
+   - Specifically, make sure that the ros package *ddynamic_reconfigure* is installed. If *ddynamic_reconfigure* cannot be installed using APT, you may clone it into your workspace 'catkin_ws/src/' from [here](https://github.com/pal-robotics/ddynamic_reconfigure/tree/kinetic-devel) (Version 0.2.2)
+
+   ```bash
+  catkin_init_workspace
+  cd ..
+  catkin_make clean
+  catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
+  catkin_make install
+  echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
+  source ~/.bashrc
+  ```
 
 ## Usage Instructions
 
@@ -131,9 +158,9 @@ The topics are of the form: ```/camera/aligned_depth_to_color/image_raw``` etc.
 - **odom_frame_id**: defines the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). pose topic defines the pose relative to that system.
 - **All the rest of the frame_ids can be found in the template launch file: [nodelet.launch.xml](./realsense2_camera/launch/includes/nodelet.launch.xml)**
 - **unite_imu_method**: The D435i and T265 cameras have built in IMU components which produce 2 unrelated streams: *gyro* - which shows angular velocity and *accel* which shows linear acceleration. Each with it's own frequency. By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out.
-Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. Under the new topic, all the fields in the Imu message are filled out.
- - **linear_interpolation**: Each message contains the last original value of item A interpolated with the previous value of item A, combined with the last original value of item B on last item B's timestamp. Items A and B are accel and gyro interchangeably, according to which type recently arrived from the sensor. The idea is to give the most recent information, united and without repetitions.
- - **copy**: For each new message, accel or gyro, the relevant fields and timestamp are filled out while the others maintain the previous data.
+Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. The *imu* topic is published at the rate of the gyro. All the fields of the Imu message under the *imu* topic are filled out.
+ - **linear_interpolation**: Every gyro message is attached by the an accel message interpolated to the gyro's timestamp.
+ - **copy**: Every gyro message is attached by the last accel message.
 - **clip_distance**: remove from the depth image all values above a given value (meters). Disable by giving negative value (default)
 - **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. For the T265, these values are being modified by the inner confidence value.
 - **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin.
@@ -224,10 +251,10 @@ The wrapper publishes static transformations(TFs). The Frame Ids are divided int
 - base_link: For example: camera_link. A reference frame for the device. In D400 series and SR300 it is the depth frame. In T265, the pose frame.
 
 
-### realsense2_description package:
+### any_realsense2_description package:
 For viewing included models, a separate package is included. For example:
 ```bash
-roslaunch realsense2_description view_d415_model.launch
+roslaunch any_realsense2_description view_d415_model.launch
 ```
 
 ### Unit tests:
@@ -250,7 +277,6 @@ python src/realsense/realsense2_camera/scripts/rs2_test.py --all
 ## Known Issues
 * This ROS node does not currently support [ROS Lunar Loggerhead](http://wiki.ros.org/lunar).
 * This ROS node does not currently work with [ROS 2](https://github.com/ros2/ros2/wiki).
-* This ROS node currently does not provide the unit-tests which ensure the proper operation of the camera.  Future versions of the node will provide ROS compatible unit-tests.
 * This ROS node currently does not support running multiple T265 cameras at once. This will be addressed in a future update. 
 
 ## License

realsense2_camera/CHANGELOG.rst

@@ -0,0 +1,21 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package realsense2_camera
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.2.14 (2020-06-18)
+-------------------
+* Fix compatibility with Librealsense2 Version 2.35.2.
+* Fix support for L515.
+* Fix urdf issues.
+* Add noetic support: change state_publisher into robot_state_publisher
+* fix distortion correction model for T265 (equidistant)
+* fix stability issues. Stop sensors at program termination.
+* Contributors: Brice, Helen Oleynikova, doronhi
+
+* upgrade version to 2.2.13
+* fix ctrl-C closing issues.
+* handle device creation exceptions.
+* support LiDAR camera L515.
+* optimize pointcloud. Contributors: Davide Faconti
+* fix usb port id parsing issues.
+* Add eigen dependency - missing for Melodic. Contributors: Antoine Hoarau

realsense2_camera/CMakeLists.txt

@@ -39,12 +39,17 @@ if(SET_USER_BREAK_AT_STARTUP)
 	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBPDEBUG")
 endif()
 
-# find_package(realsense2 2.29.0)
-# if(NOT realsense2_FOUND)
-#     message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n")
-# endif()
+find_package(realsense2 2.35.2)
+if(NOT realsense2_FOUND)
+    message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n")
+endif()
+
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif()
 
-if (CMAKE_BUILD_TYPE EQUAL "RELEASE")
+string(TOUPPER "${CMAKE_BUILD_TYPE}" uppercase_CMAKE_BUILD_TYPE)
+if (${uppercase_CMAKE_BUILD_TYPE} STREQUAL "RELEASE")
     message(STATUS "Create Release Build.")
     set(CMAKE_CXX_FLAGS "-O2 ${CMAKE_CXX_FLAGS}")
 else()
@@ -71,8 +76,8 @@ generate_messages(
 set(CMAKE_NO_SYSTEM_FROM_IMPORTED true)
 include_directories(
     include
-    SYSTEM
-        ${catkin_INCLUDE_DIRS}
+    ${realsense2_INCLUDE_DIR}
+    ${catkin_INCLUDE_DIRS}
         ${EIGEN3_INCLUDE_DIR}
     )
 

realsense2_camera/include/base_realsense_node.h

@@ -428,6 +428,9 @@ namespace realsense2_camera
         stream_index_pair _base_stream;
         const std::string _namespace;
 
+        sensor_msgs::PointCloud2 _msg_pointcloud;
+        std::vector< unsigned int > _valid_pc_indices;
+
     };//end class
 
 }

realsense2_camera/include/constants.h

@@ -7,7 +7,7 @@
 
 #define REALSENSE_ROS_MAJOR_VERSION    2
 #define REALSENSE_ROS_MINOR_VERSION    2
-#define REALSENSE_ROS_PATCH_VERSION    11
+#define REALSENSE_ROS_PATCH_VERSION    14
 
 #define STRINGIFY(arg) #arg
 #define VAR_ARG_STRING(arg) STRINGIFY(arg)
@@ -34,6 +34,8 @@ namespace realsense2_camera
     const uint16_t RS435i_RGB_PID   = 0x0B3A; // AWGC_MM
     const uint16_t RS405_PID        = 0x0b0c; // DS5U
     const uint16_t RS_T265_PID      = 0x0b37; // 
+    const uint16_t RS_L515_PID_PRE_PRQ = 0x0B3D; // 
+    const uint16_t RS_L515_PID      = 0x0B64; // 
 
 
     const bool ALIGN_DEPTH    = false;

realsense2_camera/include/realsense_node_factory.h

@@ -29,6 +29,7 @@ namespace realsense2_camera
 {
     const stream_index_pair COLOR{RS2_STREAM_COLOR, 0};
     const stream_index_pair DEPTH{RS2_STREAM_DEPTH, 0};
+    const stream_index_pair INFRA0{RS2_STREAM_INFRARED, 0};
     const stream_index_pair INFRA1{RS2_STREAM_INFRARED, 1};
     const stream_index_pair INFRA2{RS2_STREAM_INFRARED, 2};
     const stream_index_pair FISHEYE{RS2_STREAM_FISHEYE, 0};
@@ -39,7 +40,7 @@ namespace realsense2_camera
     const stream_index_pair POSE{RS2_STREAM_POSE, 0};
 
 
-    const std::vector<stream_index_pair> IMAGE_STREAMS = {DEPTH, INFRA1, INFRA2,
+    const std::vector<stream_index_pair> IMAGE_STREAMS = {DEPTH, INFRA0, INFRA1, INFRA2,
                                                           COLOR,
                                                           FISHEYE,
                                                           FISHEYE1, FISHEYE2};
@@ -67,6 +68,7 @@ namespace realsense2_camera
         void getDevice(rs2::device_list list);
         virtual void onInit() override;
         void tryGetLogSeverity(rs2_log_severity& severity) const;
+        static std::string parse_usb_port(std::string line);
 
         rs2::device _device;
         std::unique_ptr<InterfaceRealSenseNode> _realSenseNode;
@@ -77,5 +79,6 @@ namespace realsense2_camera
         bool _initial_reset;
         std::thread _query_thread;
 
+        bool _is_alive;
     };
 }//end namespace

realsense2_camera/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>any_realsense2_camera</name>
-  <version>2.2.11</version>
+  <version>2.2.14</version>
   <description>RealSense Camera package allowing access to Intel T265 Tracking module and SR300 and D400 3D cameras</description>
   <maintainer email="sergey.dorodnicov@intel.com">Sergey Dorodnicov</maintainer>
   <maintainer email="doron.hirshberg@intel.com">Doron Hirshberg</maintainer>
@@ -15,6 +15,7 @@
   <author email="sergey.dorodnicov@intel.com">Sergey Dorodnicov</author>
   <author email="doron.hirshberg@intel.com">Doron Hirshberg</author>
   <buildtool_depend>catkin</buildtool_depend>
+  <depend>eigen</depend>
   <depend>image_transport</depend>
   <depend>cv_bridge</depend>
   <depend>nav_msgs</depend>