wheel_odometry

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

@@ -2,6 +2,7 @@
 #include "assert.h"
 #include <boost/algorithm/string.hpp>
 #include <algorithm>
+#include <cctype>
 #include <mutex>
 #include <tf/transform_broadcaster.h>
 
@@ -80,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())
@@ -208,23 +209,53 @@ std::map<std::string, int> get_enum_method(rs2::options sensor, rs2_option optio
     if (is_enum_option(sensor, option))
     {
         rs2::option_range op_range = sensor.get_option_range(option);
-        for (auto val = op_range.min; val <= op_range.max; val += op_range.step)
+        const auto op_range_min = int(op_range.min);
+        const auto op_range_max = int(op_range.max);
+        const auto op_range_step = int(op_range.step);
+        for (auto val = op_range_min; val <= op_range_max; val += op_range_step)
         {
-            dict[sensor.get_option_value_description(option, val)] = int(val);
+            dict[sensor.get_option_value_description(option, val)] = val;
         }
     }
     return dict;
 }
 
+namespace realsense2_camera
+{
+
+template <typename K, typename V>
+std::ostream& operator<<(std::ostream& os, const std::map<K, V>& m)
+{
+    os << '{';
+    for (const auto& kv : m)
+    {
+        os << " {" << kv.first << ": " << kv.second << '}';
+    }
+    os << " }";
+    return os;
+}
+
+}
+
+/**
+ * Same as ros::names::isValidCharInName, but re-implemented here because it's not exposed.
+ */
+bool isValidCharInName(char c)
+{
+    return std::isalnum(c) || c == '/' || c == '_';
+}
+
 /**
  * ROS Graph Resource names don't allow spaces and hyphens (see http://wiki.ros.org/Names),
  * so we replace them here with underscores.
  */
 std::string create_graph_resource_name(const std::string &original_name)
 {
   std::string fixed_name = original_name;
-  std::replace(fixed_name.begin(), fixed_name.end(), '-', '_');
-  std::replace(fixed_name.begin(), fixed_name.end(), ' ', '_');
+  std::transform(fixed_name.begin(), fixed_name.end(), fixed_name.begin(),
+                 [](unsigned char c) { return std::tolower(c); });
+  std::replace_if(fixed_name.begin(), fixed_name.end(), [](const char c) { return !isValidCharInName(c); },
+                  '_');
   return fixed_name;
 }
 
@@ -235,23 +266,44 @@ void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options
     for (auto i = 0; i < RS2_OPTION_COUNT; i++)
     {
         rs2_option option = static_cast<rs2_option>(i);
-        std::string option_name = create_graph_resource_name(rs2_option_to_string(option));
+        const std::string option_name(create_graph_resource_name(rs2_option_to_string(option)));
         if (!sensor.supports(option) || sensor.is_option_read_only(option))
         {
             continue;
         }
         if (is_checkbox(sensor, option))
         {
-            ddynrec->add(new DDBool(option_name, i, sensor.get_option_description(option), bool(sensor.get_option(option))));
+            auto option_value = bool(sensor.get_option(option));
+            if (nh1.param(option_name, option_value, option_value))
+            {
+                sensor.set_option(option, option_value);
+            }
+            ddynrec->add(new DDBool(option_name, i, sensor.get_option_description(option), option_value));
             continue;
         }
-        std::map<std::string, int> enum_dict = get_enum_method(sensor, option);
+        const auto enum_dict = get_enum_method(sensor, option);
         if (enum_dict.empty())
         {
             rs2::option_range op_range = sensor.get_option_range(option);
+            const auto sensor_option_value = sensor.get_option(option);
+            auto option_value = sensor_option_value;
+            if (nh1.param(option_name, option_value, option_value))
+            {
+                if (option_value < op_range.min || op_range.max < option_value)
+                {
+                    ROS_WARN_STREAM("Param '" << nh1.resolveName(option_name) << "' has value " << option
+                            << " outside the range [" << op_range.min << ", " << op_range.max
+                            << "]. Using current sensor value " << sensor_option_value << " instead.");
+                    option_value = sensor_option_value;
+                }
+                else
+                {
+                    sensor.set_option(option, option_value);
+                }
+            }
             if (is_int_option(sensor, option))
             {
-                ddynrec->add(new DDInt(option_name, i, sensor.get_option_description(option), sensor.get_option(option), op_range.min, op_range.max));
+                ddynrec->add(new DDInt(option_name, i, sensor.get_option_description(option), option_value, op_range.min, op_range.max));
             }
             else
             {
@@ -268,24 +320,32 @@ void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options
                 }
                 else
                 {
-                    ddynrec->add(new DDDouble(option_name, i, sensor.get_option_description(option), sensor.get_option(option), op_range.min, op_range.max));
+                    ddynrec->add(new DDDouble(option_name, i, sensor.get_option_description(option), option_value, op_range.min, op_range.max));
                 }
             }
         }
         else
         {
-            if (int(sensor.get_option(option)) > (int)enum_dict.size())
+            const auto sensor_option_value = sensor.get_option(option);
+            auto option_value = int(sensor_option_value);
+            if (nh1.param(option_name, option_value, option_value))
             {
-                ROS_WARN_STREAM("Option " << option_name << 
-                                " has a value: " << int(sensor.get_option(option)) << 
-                                " which is beyond it's scope: " << enum_dict.size());
-            ROS_DEBUG_STREAM("Add enum: " << rs2_option_to_string(option) << ". value=" << int(sensor.get_option(option)));
-                for (auto item: enum_dict)
+                if (std::find_if(enum_dict.cbegin(), enum_dict.cend(),
+                                 [&option_value](const std::pair<std::string, int>& kv) {
+                                     return kv.second == option_value;
+                                 }) == enum_dict.cend())
                 {
-                    ROS_INFO_STREAM("Add item: " << item.first << ":" << item.second); // << ":" << sensor.get_option_description(static_cast<rs2_option>(item.second)));
+                    ROS_WARN_STREAM("Param '" << nh1.resolveName(option_name) << "' has value " << option_value
+                                              << " that is not in the enum " << enum_dict
+                                              << ". Using current sensor value " << sensor_option_value << " instead.");
+                    option_value = sensor_option_value;
+                }
+                else
+                {
+                    sensor.set_option(option, option_value);
                 }
             }
-            ddynrec->add(new DDEnum(option_name, i, sensor.get_option_description(option), int(sensor.get_option(option)), enum_dict));
+            ddynrec->add(new DDEnum(option_name, i, sensor.get_option_description(option), option_value, enum_dict));
         }
     }
     ddynrec->start(boost::bind(callback, _1, _2, sensor));
@@ -902,14 +962,14 @@ void BaseRealSenseNode::fix_depth_scale(rs2::depth_frame depth_frame)
 void BaseRealSenseNode::clip_depth(rs2::depth_frame depth_frame, float clipping_dist)
 {
     uint16_t* p_depth_frame = reinterpret_cast<uint16_t*>(const_cast<void*>(depth_frame.get_data()));
+    uint16_t clipping_value = static_cast<uint16_t>(clipping_dist / _depth_scale_meters);
 
     int width = depth_frame.get_width();
     int height = depth_frame.get_height();
 
     #ifdef _OPENMP
     #pragma omp parallel for schedule(dynamic) //Using OpenMP to try to parallelise the loop
     #endif
-    uint16_t clipping_value = static_cast<uint16_t>(clipping_dist / _depth_scale_meters);
     for (int y = 0; y < height; y++)
     {
         auto depth_pixel_index = y * width;
@@ -1230,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)
 {