Merge pull request #23 from abhijitmajumdar/por_upstream_sync

Por upstream sync v2.2.3

README.md

@@ -11,7 +11,7 @@ The following instructions support ROS Indigo, on **Ubuntu 14.04**, and ROS Kine
 - #### 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.19.0) and follow the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md)
+- #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.19.2) and follow the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md)
 
 ### Step 2: Install the ROS distribution
 - #### Install [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu), on Ubuntu 16.04
@@ -83,6 +83,7 @@ The topics are of the form: ```/camera/aligned_depth_to_color/image_raw``` etc.
    - ```disparity``` - convert depth to disparity before applying other filters and back.
    - ```spatial``` - filter the depth image spatially.
    - ```temporal``` - filter the depth image temporally.
+   - ```hole_filling``` - apply hole-filling filter.
    - ```decimation``` - reduces depth scene complexity.
 - **enable_sync**: gathers closest frames of different sensors, infra red, color and depth, to be sent with the same timetag. This happens automatically when such filters as pointcloud are enabled.
 - ***<stream_type>*_width**, ***<stream_type>*_height**, ***<stream_type>*_fps**: <stream_type> can be any of *infra, color, fisheye, depth, gyro, accel, pose*. Sets the required format of the device. If the specified combination of parameters is not available by the device, the stream will not be published. Setting a value to 0, will choose the first format in the inner list. (i.e. consistent between runs but not defined). Note: for gyro accel and pose, only _fps option is meaningful.
@@ -98,6 +99,10 @@ Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default
 - **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.
+- **topic_odom_in**: For T265, add wheel odometry information through this topic. The code refers only to the *twist.linear* field in the message.
+- **calib_odom_file**: For the T265 to include odometry input, it must be given a [configuration file](https://github.com/IntelRealSense/librealsense/blob/master/unit-tests/resources/calibration_odometry.json). Explanations can be found [here](https://github.com/IntelRealSense/librealsense/pull/3462). The calibration is done in ROS coordinates system.
+- **publish_odom_tf**: If True (default) publish TF from odom_frame to pose_frame.
+
 
 ### RGBD Point Cloud
 Here is an example of how to start the camera node and make it publish the RGBD point cloud using aligned depth topic.
@@ -119,7 +124,7 @@ The following command allow to change camera control values using [http://wiki.r
 ```bash
 rosrun rqt_reconfigure rqt_reconfigure
 ```
-<p align="center"><img src="https://user-images.githubusercontent.com/40540281/52912434-30783180-32ba-11e9-8ab8-b08be5eba434.PNG" /></p>
+<p align="center"><img src="https://user-images.githubusercontent.com/40540281/55330573-065d8600-549a-11e9-996a-5d193cbd9a93.PNG" /></p>
 
 ### Work with multiple cameras
 Here is an example of how to start the camera node and streaming with two cameras using the [rs_multiple_devices.launch](./realsense2_camera/launch/rs_multiple_devices.launch).
@@ -148,6 +153,14 @@ roslaunch realsense2_camera rs_camera.launch camera:=cam_2 serial_no:=<serial nu
 ## Using T265 ##
 **Important Notice:** For wheeled robots, odometer input is a requirement for robust and accurate tracking. The relevant APIs will be added to librealsense and ROS/realsense in upcoming releases. Currently, the API is available in the [underlying device driver](https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/libtm/include/TrackingDevice.h#L508-L515).
 
+### Start the camera node
+To start the camera node in ROS:
+
+```bash
+roslaunch realsense2_camera rs_t265.launch
+```
+
+This will stream all camera sensors and publish on the appropriate ROS topics.
 
 The T265 sets its usb unique ID during initialization and without this parameter it wont be found.
 Once running it will publish, among others, the following topics:
@@ -157,6 +170,11 @@ Once running it will publish, among others, the following topics:
 - /camera/fisheye1/image_raw
 - /camera/fisheye2/image_raw
 
+To visualize the pose output and frames in RViz, start:
+```bash
+roslaunch realsense2_camera demo_t265.launch
+```
+
 ## Packages using RealSense ROS Camera
 | Title | Links |
 | ----- | ----- |

ddynamic_reconfigure/CMakeLists.txt

@@ -16,7 +16,7 @@ generate_messages(DEPENDENCIES std_msgs)
 ## Catkin ##
 ############
 
-catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS message_runtime std_msgs)
+catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS dynamic_reconfigure message_runtime std_msgs)
 
 #############
 ## Library ##

realsense2_camera/CMakeLists.txt

@@ -7,6 +7,7 @@ option(SET_USER_BREAK_AT_STARTUP "Set user wait point in startup (for debug)" OF
 
 find_package(catkin REQUIRED COMPONENTS
     message_generation
+    nav_msgs
     roscpp
     sensor_msgs
     std_msgs
@@ -73,6 +74,7 @@ catkin_package(
     cv_bridge
     image_transport
     ddynamic_reconfigure
+    nav_msgs
     )
 
 add_library(${PROJECT_NAME}

realsense2_camera/include/base_realsense_node.h

@@ -134,6 +134,7 @@ namespace realsense2_camera
         std::map<stream_index_pair, std::string> _frame_id;
         std::map<stream_index_pair, std::string> _optical_frame_id;
         std::map<stream_index_pair, std::string> _depth_aligned_frame_id;
+        ros::NodeHandle& _node_handle, _pnh;
         bool _align_depth;
 
         virtual void calcAndPublishStaticTransform(const stream_index_pair& stream, const rs2::stream_profile& base_profile);
@@ -190,7 +191,8 @@ namespace realsense2_camera
         void setupFilters();
         void setupStreams();
         void setBaseTime(double frame_time, bool warn_no_metadata);
-        void clip_depth(rs2::depth_frame& depth_frame, float depth_scale, float clipping_dist);
+        void fix_depth_scale(rs2::depth_frame depth_frame);
+        void clip_depth(rs2::depth_frame depth_frame, float clipping_dist);
         void updateStreamCalibData(const rs2::video_stream_profile& video_profile);
         void publishStaticTransforms();
         void publishPointCloud(rs2::points f, const ros::Time& t, const rs2::frameset& frameset);
@@ -223,7 +225,6 @@ namespace realsense2_camera
         rs2_stream rs2_string_to_stream(std::string str);
 
         rs2::device _dev;
-        ros::NodeHandle& _node_handle, _pnh;
         std::map<stream_index_pair, rs2::sensor> _sensors;
         std::map<std::string, std::function<void(rs2::frame)>> _sensors_callback;
         std::vector<std::shared_ptr<ddynamic_reconfigure::DDynamicReconfigure>> _ddynrec;
@@ -264,6 +265,7 @@ namespace realsense2_camera
         ros::Time _ros_time_base;
         bool _sync_frames;
         bool _pointcloud;
+        bool _publish_odom_tf;
         imu_sync_method _imu_sync_method;
         std::string _filters_str;
         stream_index_pair _pointcloud_texture;

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    1
+#define REALSENSE_ROS_PATCH_VERSION    3
 
 #define STRINGIFY(arg) #arg
 #define VAR_ARG_STRING(arg) STRINGIFY(arg)
@@ -53,6 +53,7 @@ namespace realsense2_camera
     const bool ENABLE_FISHEYE = true;
     const bool ENABLE_IMU     = true;
     const bool HOLD_BACK_IMU_FOR_FRAMES = false;
+    const bool PUBLISH_ODOM_TF = true;
 
 
     const std::string DEFAULT_BASE_FRAME_ID            = "camera_link";
@@ -80,6 +81,8 @@ namespace realsense2_camera
 
     const std::string DEFAULT_UNITE_IMU_METHOD         = "";
     const std::string DEFAULT_FILTERS                  = "";
+    const std::string DEFAULT_TOPIC_ODOM_IN            = "";
 
+    const float ROS_DEPTH_SCALE = 0.001;
     using stream_index_pair = std::pair<rs2_stream, int>;
 }  // namespace realsense2_camera

realsense2_camera/include/t265_realsense_node.h

@@ -11,8 +11,18 @@ namespace realsense2_camera
                           ros::NodeHandle& privateNodeHandle,
                           rs2::device dev,
                           const std::string& serial_no);
+            void publishTopics();
 
         protected:
             void calcAndPublishStaticTransform(const stream_index_pair& stream, const rs2::stream_profile& base_profile) override;
+
+        private:
+            void initializeOdometryInput();
+            void setupSubscribers();
+            void odom_in_callback(const nav_msgs::Odometry::ConstPtr& msg);
+
+            ros::Subscriber _odom_subscriber;
+            rs2::wheel_odometer _wo_snr;
+            bool _use_odom_in;
     };
 }

realsense2_camera/launch/includes/nodelet.launch.xml

@@ -71,7 +71,9 @@
   <arg name="aligned_depth_to_fisheye2_frame_id" default="$(arg tf_prefix)_aligned_depth_to_fisheye2_frame"/>
 
   <arg name="odom_frame_id"            default="$(arg tf_prefix)_odom_frame"/>
-
+  <arg name="topic_odom_in"            default="$(arg tf_prefix)/odom_in"/>
+  <arg name="calib_odom_file"          default=""/>
+  <arg name="publish_odom_tf"          default="true"/>
   <arg name="filters"                  default=""/>
   <arg name="clip_distance"            default="-1"/>
   <arg name="linear_accel_cov"         default="0.01"/>
@@ -150,7 +152,9 @@
     <param name="aligned_depth_to_fisheye2_frame_id" type="str"  value="$(arg aligned_depth_to_fisheye2_frame_id)"/>
 
     <param name="odom_frame_id"            type="str"  value="$(arg odom_frame_id)"/>
-
+    <param name="topic_odom_in"            type="str"  value="$(arg topic_odom_in)"/>
+    <param name="calib_odom_file"          type="str"    value="$(arg calib_odom_file)"/>
+    <param name="publish_odom_tf"          type="bool" value="$(arg publish_odom_tf)"/>
     <param name="filters"                  type="str"    value="$(arg filters)"/>
     <param name="clip_distance"            type="double" value="$(arg clip_distance)"/>
     <param name="linear_accel_cov"         type="double" value="$(arg linear_accel_cov)"/>

realsense2_camera/launch/rs_camera.launch

@@ -42,6 +42,10 @@
   <arg name="linear_accel_cov"      default="0.01"/>
   <arg name="initial_reset"         default="false"/>
   <arg name="unite_imu_method"      default=""/>
+  <arg name="topic_odom_in"         default="odom_in"/>
+  <arg name="calib_odom_file"       default=""/>
+  <arg name="publish_odom_tf"       default="true"/>
+
 
   <group ns="$(arg camera)">
     <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
@@ -86,6 +90,9 @@
       <arg name="linear_accel_cov"         value="$(arg linear_accel_cov)"/>
       <arg name="initial_reset"            value="$(arg initial_reset)"/>
       <arg name="unite_imu_method"         value="$(arg unite_imu_method)"/>
+      <arg name="topic_odom_in"            value="$(arg topic_odom_in)"/>
+      <arg name="calib_odom_file"          value="$(arg calib_odom_file)"/>
+      <arg name="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
     </include>
   </group>
 </launch>

realsense2_camera/launch/rs_d400_and_t265.launch

@@ -12,6 +12,8 @@
   <arg name="depth_width"               default="640"/>
   <arg name="depth_height"              default="480"/>
   <arg name="clip_distance"             default="-2"/>
+  <arg name="topic_odom_in"             default="odom_in"/>
+  <arg name="calib_odom_file"           default=""/>
 
   <group ns="$(arg camera1)">
     <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
@@ -20,6 +22,8 @@
       <arg name="initial_reset"         value="$(arg initial_reset)"/>
       <arg name="enable_fisheye1"       value="$(arg enable_fisheye)"/>
       <arg name="enable_fisheye2"       value="$(arg enable_fisheye)"/>
+      <arg name="topic_odom_in"         value="$(arg topic_odom_in)"/>
+      <arg name="calib_odom_file"       value="$(arg calib_odom_file)"/>
     </include>
   </group>
 

realsense2_camera/package.xml

@@ -6,7 +6,7 @@
   <maintainer email="sergey.dorodnicov@intel.com">Sergey Dorodnicov</maintainer>
   <maintainer email="doron.hirshberg@intel.com">Doron Hirshberg</maintainer>
   <license>Apache 2.0</license>
-  
+
   <url type="website">http://www.ros.org/wiki/RealSense</url>
   <url type="bugtracker">https://github.com/intel-ros/realsense/issues</url>
 
@@ -15,7 +15,8 @@
   <buildtool_depend>catkin</buildtool_depend>
   <depend>image_transport</depend>
   <depend>cv_bridge</depend>
-  <depend>nodelet</depend>  
+  <depend>nav_msgs</depend>
+  <depend>nodelet</depend>
   <depend>genmsg</depend>
   <depend>roscpp</depend>
   <depend>sensor_msgs</depend>

realsense2_camera/scripts/show_center_depth.py

@@ -0,0 +1,31 @@
+import rospy
+from sensor_msgs.msg import Image as msg_Image
+from cv_bridge import CvBridge, CvBridgeError
+import sys
+import os
+
+class ImageListener:
+    def __init__(self, topic):
+        self.topic = topic
+        self.bridge = CvBridge()
+        self.sub = rospy.Subscriber(topic, msg_Image, self.imageDepthCallback)
+
+    def imageDepthCallback(self, data):
+        try:
+            cv_image = self.bridge.imgmsg_to_cv2(data, data.encoding)
+            pix = (data.width/2, data.height/2)
+            sys.stdout.write('%s: Depth at center(%d, %d): %f(mm)\r' % (self.topic, pix[0], pix[1], cv_image[pix[1], pix[0]]))
+            sys.stdout.flush()
+        except CvBridgeError as e:
+            print(e)
+            return
+
+def main():
+    topic = '/camera/depth/image_rect_raw'
+    listener = ImageListener(topic)
+    rospy.spin()
+
+if __name__ == '__main__':
+    node_name = os.path.basename(sys.argv[0]).split('.')[0]
+    rospy.init_node(node_name)
+    main()