Release 0.8.4

.travis.sh

@@ -0,0 +1,35 @@
+#!/bin/bash
+
+set -e
+
+apt-get update && apt-get install -y -q wget sudo lsb-release # for docker 
+
+#before_install:
+sh -c "echo \"deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main\" > /etc/apt/sources.list.d/ros-latest.list"
+sh -c "echo \"deb http://realsense-alm-public.s3.amazonaws.com/apt-repo `lsb_release -sc` main\" > /etc/apt/sources.list.d/realsense-latest.list"
+wget http://packages.ros.org/ros.key -O - | apt-key add -
+apt-key adv --keyserver keys.gnupg.net --recv-key D6FB2970
+apt-get update
+apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-kinetic-catkin librealsense-object-recognition-dev librealsense-persontracking-dev librealsense-slam-dev libopencv-dev
+source /opt/ros/$ROS_DISTRO/setup.bash
+rosdep init
+rosdep update
+
+#install:
+mkdir -p ~/catkin_ws/src
+cd ~/catkin_ws/src
+catkin_init_workspace
+cd ~/catkin_ws
+catkin_make
+source devel/setup.bash
+cd ~/catkin_ws/src
+ln -s $CI_SOURCE_PATH .
+
+#before_script:
+cd ~/catkin_ws
+rosdep install -q -y -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO
+
+#script:
+source /opt/ros/$ROS_DISTRO/setup.bash
+cd ~/catkin_ws
+catkin_make

.travis.yml

@@ -0,0 +1,15 @@
+sudo: required
+dist: trusty
+language: generic
+
+env:
+  - ROS_DISTRO=kinetic
+
+before_install:
+  - export CI_SOURCE_PATH=$(pwd)
+  - export REPOSITORY_NAME=${PWD##*/}
+
+script:
+  - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME"
+  - docker run --rm -i -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -e "ROS_DISTRO=$ROS_DISTRO" -t ubuntu:xenial sh -c "cd $CI_SOURCE_PATH; ./.travis.sh"
+

realsense_ros_camera/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package realsense_ros_camera
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.8.4 (2017-04-28)
+------------------
+* Fixed broken dependencies in packages, causing rosdep to fail when building on clean machine
+* ROS timestamp fixes
+* Contributors: Matt Curfman
+
 0.8.3 (2017-04-21)
 ------------------
 * Added URDF models for R200 and ZR300 cameras

realsense_ros_camera/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>realsense_ros_camera</name>
-  <version>0.8.3</version>
+  <version>0.8.4</version>
   <description>The realsense_ros_camera package</description>
   <maintainer email="realsense-sw@intel.com">Intel RealSense</maintainer>
   <license>Apache 2.0</license>
@@ -15,6 +15,7 @@
   <build_depend>cv_bridge</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>tf</build_depend>
+  <build_depend>camera_info_manager</build_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>cv_bridge</run_depend>
   <run_depend>nodelet</run_depend>  
@@ -24,6 +25,7 @@
   <run_depend>std_msgs</run_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>tf</run_depend>
+  <run_depend>camera_info_manager</run_depend>
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml" />
   </export>

realsense_ros_camera/src/camera_node.cpp

@@ -35,7 +35,8 @@ class NodeletCamera: public nodelet::Nodelet
 {
 public:
   NodeletCamera() :
-    cameraStarted_(false)
+    cameraStarted_(false),
+    intialize_time_base_(false)
   {
     // Types for depth stream
     format_[rs::stream::depth] = rs::format::z16;   // libRS type
@@ -209,11 +210,6 @@ class NodeletCamera: public nodelet::Nodelet
       info_publisher_[rs::stream::fisheye] = 
         node_handle.advertise< sensor_msgs::CameraInfo >("camera/fisheye/camera_info", 1);
 
-      // HW timestamp version of image publishers, for realsense_ros_slam package
-      image_publishers_hw_timestamp_[rs::stream::color] = node_handle.advertise<sensor_msgs::Image>("/camera/color/image_raw_hw_timestamp", 1);
-      image_publishers_hw_timestamp_[rs::stream::depth] = node_handle.advertise<sensor_msgs::Image>("/camera/depth/image_raw_hw_timestamp", 1);
-      image_publishers_hw_timestamp_[rs::stream::fisheye] = node_handle.advertise<sensor_msgs::Image>("/camera/fisheye/image_raw_hw_timestamp", 1);
-
       imu_publishers_[RS_EVENT_IMU_GYRO] = node_handle.advertise< sensor_msgs::Imu >("camera/gyro/sample", 100); 
       imu_publishers_[RS_EVENT_IMU_ACCEL] = node_handle.advertise< sensor_msgs::Imu >("camera/accel/sample", 100);
 
@@ -240,14 +236,33 @@ class NodeletCamera: public nodelet::Nodelet
       stream_callback_per_stream[stream] = [this,stream](rs::frame frame)
       {
         image_[stream].data = (unsigned char *) frame.get_data();
-        ros::Time t = ros::Time::now();
+
+        if ((true == isZR300_) && (rs::timestamp_domain::microcontroller != frame.get_frame_timestamp_domain()))
+        {
+          ROS_ERROR_STREAM("error: Junk time stamp in stream:" << (int)(stream) <<
+            "\twith frame counter:" << frame.get_frame_number());
+          return;
+        }
+
+        // We compute a ROS timestamp which is based on an initial ROS time at point of first frame,
+        // and the incremental timestamp from the camera.
+        if(false == intialize_time_base_)
+        {
+          intialize_time_base_ = true;
+          ros_time_base_ = ros::Time::now();   
+          camera_time_base_ = frame.get_timestamp();
+        }
+        double elapsed_camera_ms = (/*ms*/ frame.get_timestamp() - /*ms*/ camera_time_base_) / /*ms to seconds*/ 1000;
+        ros::Time t(ros_time_base_.toSec() + elapsed_camera_ms);
 
+        // If this stream is associated with depth and we have at least one point cloud subscriber,
+        // service it here.
         if((stream == rs::stream::depth) && (0 != pointcloud_publisher_.getNumSubscribers()))
           publishPCTopic(t);
 
-        seq_[stream] += 1;                   
-        if((0 != image_publishers_[stream].getNumSubscribers()) || 
-           (0 != image_publishers_hw_timestamp_[stream].getNumSubscribers()))
+        seq_[stream] += 1;        
+        if((0 != info_publisher_[stream].getNumSubscribers()) ||
+           (0 != image_publishers_[stream].getNumSubscribers())) 
         {
           sensor_msgs::ImagePtr img;          
           img = cv_bridge::CvImage(std_msgs::Header(), encoding_[stream], image_[stream]).toImageMsg();
@@ -259,28 +274,8 @@ class NodeletCamera: public nodelet::Nodelet
           img->header.stamp = t;
           img->header.seq = seq_[stream];
 
-          // ROS Timestamp
-          if(0 != image_publishers_[stream].getNumSubscribers())
-            image_publishers_[stream].publish(img);
-
-          // Camera HW Timestamp for realsense_ros_slam
-          if(0 != image_publishers_hw_timestamp_[stream].getNumSubscribers())
-          {
-            if (rs::timestamp_domain::microcontroller != frame.get_frame_timestamp_domain())
-            {
-              ROS_ERROR_STREAM("error: Junk time stamp in stream:" << (int)(stream) <<
-                               "\twith frame counter:" << frame.get_frame_number());
-            }
-            else
-            {
-              img->header.stamp = ros::Time(frame.get_timestamp());
-              image_publishers_hw_timestamp_[stream].publish(img);
-            }
-          }
-        }
-
-        if(0 != image_publishers_[stream].getNumSubscribers())
-        {
+          image_publishers_[stream].publish(img);
+
           camera_info_[stream].header.stamp = t;
           camera_info_[stream].header.seq = seq_[stream];
           info_publisher_[stream].publish(camera_info_[stream]);
@@ -307,6 +302,7 @@ class NodeletCamera: public nodelet::Nodelet
     {
       // Needed to align image timestamps to common clock-domain with the motion events
       device_->set_option(rs::option::fisheye_strobe, 1); 
+
       // This option causes the fisheye image to be aquired in-sync with the depth image.
       device_->set_option(rs::option::fisheye_external_trigger, 1); 
       device_->set_option(rs::option::fisheye_color_auto_exposure, 1);
@@ -328,8 +324,12 @@ class NodeletCamera: public nodelet::Nodelet
         if( 0 == imu_publishers_[motionType].getNumSubscribers())
           return;
 
+        if(false == intialize_time_base_)
+          return;
+        double elapsed_camera_ms = (/*ms*/ entry.timestamp_data.timestamp - /*ms*/ camera_time_base_) / /*ms to seconds*/ 1000;
+        ros::Time t(ros_time_base_.toSec() + elapsed_camera_ms);
+
         sensor_msgs::Imu imu_msg = sensor_msgs::Imu();
-        imu_msg.header.stamp = ros::Time::now();
         imu_msg.header.frame_id = optical_imu_id_[motionType];
         imu_msg.orientation.x = 0.0;
         imu_msg.orientation.y = 0.0;
@@ -350,7 +350,7 @@ class NodeletCamera: public nodelet::Nodelet
         }
         seq_motion[motionType] += 1;
         imu_msg.header.seq = seq_motion[motionType];
-        imu_msg.header.stamp = ros::Time(entry.timestamp_data.timestamp);
+        imu_msg.header.stamp = t;
         imu_publishers_[motionType].publish(imu_msg);
       };
 
@@ -703,7 +703,6 @@ class NodeletCamera: public nodelet::Nodelet
 
   // R200 and ZR300 types
   std::map<rs::stream, image_transport::Publisher> image_publishers_;
-  std::map<rs::stream, ros::Publisher> image_publishers_hw_timestamp_;
   std::map<rs::stream, int> image_format_;
   std::map<rs::stream, rs::format> format_;
   std::map<rs::stream, ros::Publisher> info_publisher_;
@@ -718,6 +717,9 @@ class NodeletCamera: public nodelet::Nodelet
   std::map<rs::stream, std::function<void(rs::frame)>> stream_callback_per_stream;
   std::map<rs::stream, sensor_msgs::CameraInfo> camera_info_;
   ros::Publisher pointcloud_publisher_;
+  bool intialize_time_base_;
+  double camera_time_base_;
+  ros::Time ros_time_base_;
 
   // ZR300 specific types
   bool isZR300_ = false;

realsense_ros_object/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package realsense_ros_object
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.8.4 (2017-04-28)
+------------------
+
 0.8.3 (2017-04-21)
 ------------------
 

realsense_ros_object/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>realsense_ros_object</name> 
-  <version>0.8.3</version>
+  <version>0.8.4</version>
   <description>The realsense_ros_object package</description>
   <maintainer email="realsense-sw@intel.com">Intel RealSense</maintainer>
   <license>Apache 2.0</license>

realsense_ros_person/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package realsense_ros_person
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.8.4 (2017-04-28)
+------------------
+* Fixed broken dependencies in packages, causing rosdep to fail when building on clean machine
+* Contributors: Matt Curfman
+
 0.8.3 (2017-04-21)
 ------------------
 * Added instructions for record/playback

realsense_ros_person/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>realsense_ros_person</name>
-  <version>0.8.3</version>
+  <version>0.8.4</version>
   <description>The realsense_ros_person package</description>
   <maintainer email="realsense-sw@intel.com">Intel RealSense</maintainer>
   <license>Apache 2.0</license>
@@ -17,8 +17,6 @@
   <build_depend>roscpp_serialization</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>sensor_msgs</build_depend>
-  <build_depend>realsense_msgs</build_depend>
-  <build_depend>realsense_srvs</build_depend>
   <run_depend>cv_bridge</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>message_filters</run_depend>
@@ -27,7 +25,7 @@
   <run_depend>roscpp</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>rostime</run_depend>
-  <run_depend>realsense_camera</run_depend>
+  <run_depend>realsense_ros_camera</run_depend>
   <run_depend>roscpp_serialization</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>sensor_msgs</run_depend>

realsense_ros_slam/CHANGELOG.rst

@@ -2,6 +2,13 @@
 Changelog for package realsense_ros_slam
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.8.4 (2017-04-28)
+------------------
+* Fixed broken dependencies in packages, causing rosdep to fail when building on clean machine
+* ROS timestamp fixes
+* Added launch arguments for loading of occupancy map and relocalization map.
+* Contributors: Ben Hirashima, Matt Curfman
+
 0.8.3 (2017-04-21)
 ------------------
 * Added a service to save slam output to files.

realsense_ros_slam/launch/demo_slam.launch

@@ -1,5 +1,8 @@
 <launch>
   <arg name="manager" default="realsense_ros_camera_manager"/>
+  <arg name="load_relocalization_map" default=""/>
+  <arg name="load_occupancy_map"      default=""/>
+
   <include file="$(find realsense_ros_camera)/launch/camera.launch">
     <arg name="manager" value="$(arg manager)"/>
     <arg name="fisheye_width" value="640"/>
@@ -12,5 +15,7 @@
   </include>
   <include file="$(find realsense_ros_slam)/launch/setup_demo.launch">
     <arg name="manager" value="$(arg manager)"/>
+    <arg name="load_relocalization_map" value="$(arg load_relocalization_map)"/>
+    <arg name="load_occupancy_map" value="$(arg load_occupancy_map)"/>
   </include>
 </launch>

realsense_ros_slam/launch/record_bag_slam.launch

@@ -6,7 +6,7 @@ roslaunch realsense_ros_camera record_bag_slam.launch bag_path:=my_value
 -->
 <launch>
   <arg name="bag_path" default="$(env HOME)/test_slam_320x240d_640x480f_imu.bag" />
-  <arg name="bag_topics" default="/camera/depth/image_raw_hw_timestamp /camera/depth/camera_info /camera/fisheye/camera_info /camera/fisheye/image_raw_hw_timestamp /camera/accel/sample /camera/gyro/sample /camera/accel/imu_info /camera/gyro/imu_info /camera/extrinsics/fisheye2imu /camera/extrinsics/fisheye2depth"/>
+  <arg name="bag_topics" default="/camera/depth/image_raw /camera/depth/camera_info /camera/fisheye/camera_info /camera/fisheye/image_raw /camera/accel/sample /camera/gyro/sample /camera/accel/imu_info /camera/gyro/imu_info /camera/extrinsics/fisheye2imu /camera/extrinsics/fisheye2depth"/>
 
   <node pkg="rosbag" type="record" name="rosbag" args="-o $(arg bag_path) $(arg bag_topics)"/>
 

realsense_ros_slam/launch/setup_demo.launch

@@ -1,5 +1,7 @@
 <launch>
     <arg name="manager" default="realsense_ros_camera_manager"/>
+    <arg name="load_relocalization_map" default=""/>
+    <arg name="load_occupancy_map"      default=""/>
     <!-- setup some static transforms to allow the camera pose to be shown in rviz -->
     <node pkg="tf2_ros" type="static_transform_publisher"   name="map_to_odom"              args="0 0 0 0 0 0 1 map odom" />
     <node pkg="tf2_ros" type="static_transform_publisher"   name="base_link_to_camera_link" args="0 0 0 0 0 0 1 base_link camera_link" />
@@ -8,5 +10,7 @@
     <include file="$(find realsense_ros_slam)/launch/slam_nodelet.launch">  
         <arg name="manager" value="$(arg manager)"/>
         <arg name="publish_odometry" value="true" />   
+        <arg name="load_relocalization_map" value="$(arg load_relocalization_map)"/>
+        <arg name="load_occupancy_map" value="$(arg load_occupancy_map)"/>
     </include>
 </launch>

realsense_ros_slam/launch/slam_nodelet.launch

@@ -12,6 +12,8 @@
     <arg name="hoi_max"                 default="0.1" />
     <arg name="doi_min"                 default="0.3" />
     <arg name="doi_max"                 default="3.0" />
+    <arg name="load_relocalization_map" default=""/>
+    <arg name="load_occupancy_map"      default=""/>
 
     <node pkg="nodelet" type="nodelet" name="realsense_ros_slam" args="load realsense_ros_slam/SNodeletSlam $(arg manager)" output="screen">
         <rosparam file="$(find realsense_ros_slam)/launch/params.yaml" command="load"/>            
@@ -27,6 +29,8 @@
         <param name="hoi_max"   type="double"  value="$(arg hoi_max)" />
         <param name="doi_min"   type="double"  value="$(arg doi_min)" />
         <param name="doi_max"   type="double"  value="$(arg doi_max)" />
+        <param name="load_relocalization_map" value="$(arg load_relocalization_map)"/>
+        <param name="load_occupancy_map" value="$(arg load_occupancy_map)"/>
     </node>
      <!--launch-prefix="gdb -ex run --><!--args"/-->
 

realsense_ros_slam/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>realsense_ros_slam</name>
-  <version>0.8.3</version>
+  <version>0.8.4</version>
   <description>The realsense_ros_slam package</description>
   <maintainer email="realsense-sw@intel.com">Intel RealSense</maintainer>
   <license>Apache 2.0</license>
@@ -17,6 +17,7 @@
   <build_depend>message_filters</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>message_generation</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>message_filters</run_depend>
   <run_depend>image_transport</run_depend>
@@ -28,6 +29,7 @@
   <run_depend>rospy</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>std_msgs</run_depend>
+  <run_depend>tf2_geometry_msgs</run_depend>
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml"/>
   </export>