wheel_odometry (#691)

* use wheel_Odometry

Add parameters to launch files: 
* topic_odom_in - The topic on which wheel odometry arrives.
* calib_odom_file - path to calibration.json file, of the librealsense format. i.e.: https://github.com/IntelRealSense/librealsense/blob/master/unit-tests/resources/calibration_odometry.json

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);
@@ -224,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;

realsense2_camera/include/constants.h

@@ -80,6 +80,7 @@ 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>;

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,6 +71,8 @@
   <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="filters"                  default=""/>
   <arg name="clip_distance"            default="-1"/>
@@ -150,6 +152,8 @@
     <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="filters"                  type="str"    value="$(arg filters)"/>
     <param name="clip_distance"            type="double" value="$(arg clip_distance)"/>

realsense2_camera/launch/rs_camera.launch

@@ -42,6 +42,8 @@
   <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=""/>
 
   <group ns="$(arg camera)">
     <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
@@ -86,6 +88,8 @@
       <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)"/>
     </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/src/base_realsense_node.cpp

@@ -81,8 +81,8 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle,
                                      ros::NodeHandle& privateNodeHandle,
                                      rs2::device dev,
                                      const std::string& serial_no) :
-    _base_frame_id(""), _dev(dev),  _node_handle(nodeHandle),
-    _pnh(privateNodeHandle), _json_file_path(""),
+    _base_frame_id(""),  _node_handle(nodeHandle),
+    _pnh(privateNodeHandle), _dev(dev), _json_file_path(""),
     _serial_no(serial_no),
     _is_initialized_time_base(false),
     _namespace(getNamespaceStr())
@@ -1290,19 +1290,19 @@ void BaseRealSenseNode::pose_callback(rs2::frame frame)
         v_msg.vector.x = -pose.velocity.z;
         v_msg.vector.y = -pose.velocity.x;
         v_msg.vector.z = pose.velocity.y;
-	tf::Vector3 tfv;
-	tf::vector3MsgToTF(v_msg.vector,tfv);
-	tf::Quaternion q(-msg.transform.rotation.x,-msg.transform.rotation.y,-msg.transform.rotation.z,msg.transform.rotation.w);
-	tfv=tf::quatRotate(q,tfv);
-	tf::vector3TFToMsg(tfv,v_msg.vector);
+        tf::Vector3 tfv;
+        tf::vector3MsgToTF(v_msg.vector,tfv);
+        tf::Quaternion q(-msg.transform.rotation.x,-msg.transform.rotation.y,-msg.transform.rotation.z,msg.transform.rotation.w);
+        tfv=tf::quatRotate(q,tfv);
+        tf::vector3TFToMsg(tfv,v_msg.vector);
 
         geometry_msgs::Vector3Stamped om_msg;
         om_msg.vector.x = -pose.angular_velocity.z;
         om_msg.vector.y = -pose.angular_velocity.x;
         om_msg.vector.z = pose.angular_velocity.y;
-	tf::vector3MsgToTF(om_msg.vector,tfv);
-	tfv=tf::quatRotate(q,tfv);
-	tf::vector3TFToMsg(tfv,om_msg.vector);
+        tf::vector3MsgToTF(om_msg.vector,tfv);
+        tfv=tf::quatRotate(q,tfv);
+        tf::vector3TFToMsg(tfv,om_msg.vector);
 
 
         nav_msgs::Odometry odom_msg;

realsense2_camera/src/t265_realsense_node.cpp

@@ -6,7 +6,67 @@ T265RealsenseNode::T265RealsenseNode(ros::NodeHandle& nodeHandle,
                                      ros::NodeHandle& privateNodeHandle,
                                      rs2::device dev,
                                      const std::string& serial_no) : 
-                                     BaseRealSenseNode(nodeHandle, privateNodeHandle, dev, serial_no) {}
+                                     BaseRealSenseNode(nodeHandle, privateNodeHandle, dev, serial_no),
+                                     _wo_snr(dev.first<rs2::wheel_odometer>()),
+                                     _use_odom_in(false) 
+                                     {
+                                         initializeOdometryInput();
+                                     }
+
+void T265RealsenseNode::initializeOdometryInput()
+{
+    std::string calib_odom_file;
+    _pnh.param("calib_odom_file", calib_odom_file, std::string(""));
+    if (calib_odom_file.empty())
+    {
+        ROS_INFO("No calib_odom_file. No input odometry accepted.");
+        return;
+    }
+    std::ifstream calibrationFile(calib_odom_file);
+    if (not calibrationFile)
+    {
+        ROS_FATAL_STREAM("calibration_odometry file not found. calib_odom_file = " << calib_odom_file);
+        throw std::runtime_error("calibration_odometry file not found" );
+    }
+    const std::string json_str((std::istreambuf_iterator<char>(calibrationFile)),
+        std::istreambuf_iterator<char>());
+    const std::vector<uint8_t> wo_calib(json_str.begin(), json_str.end());
+
+    if (!_wo_snr.load_wheel_odometery_config(wo_calib))
+    {
+        ROS_FATAL_STREAM("Format error in calibration_odometry file: " << calib_odom_file);
+        throw std::runtime_error("Format error in calibration_odometry file" );
+    }
+    _use_odom_in = true;
+}
+
+void T265RealsenseNode::publishTopics()
+{
+    BaseRealSenseNode::publishTopics();
+    setupSubscribers();
+}
+
+void T265RealsenseNode::setupSubscribers()
+{
+    if (not _use_odom_in) return;
+
+    std::string topic_odom_in;
+    _pnh.param("topic_odom_in", topic_odom_in, DEFAULT_TOPIC_ODOM_IN);
+    ROS_INFO_STREAM("Subscribing to in_odom topic: " << topic_odom_in);
+
+    _odom_subscriber = _node_handle.subscribe(topic_odom_in, 1, &T265RealsenseNode::odom_in_callback, this);
+}
+
+void T265RealsenseNode::odom_in_callback(const nav_msgs::Odometry::ConstPtr& msg)
+{
+    ROS_INFO("Got in_odom message");
+    rs2_vector velocity {-(float)(msg->twist.twist.linear.y),
+                          (float)(msg->twist.twist.linear.z),
+                         -(float)(msg->twist.twist.linear.x)};
+
+    ROS_INFO_STREAM("Add odom: " << velocity.x << ", " << velocity.y << ", " << velocity.z);
+    _wo_snr.send_wheel_odometry(0, 0, velocity);
+}
 
 void T265RealsenseNode::calcAndPublishStaticTransform(const stream_index_pair& stream, const rs2::stream_profile& base_profile)
 {