ROS2 Foxy T265 and D435i

[AndreaEirale]

Hi everyone! I'm having a problem running T265 and D435i on Ubuntu 20.04 with ROS2 Foxy. I can't see both with rs-enumerate-devices and with realsense-viewer, but they do not run in ROS2. Initially T265 cannot run on its own, if not adding these lines in rs_t265.cpp:

#include <unistd.h>                                 //ADD THIS

namespace realsense
{
RealSenseT265::RealSenseT265(rs2::context ctx, rs2::device dev, rclcpp::Node & node)
: RealSenseBase(ctx, dev, node)
{ 
  RCLCPP_INFO(node_.get_logger(),"Force Reinitialization for T265 Just workaround");    //AND THIS
  dev_.hardware_reset();                                //AND THIS
  sleep(1);                                     //AND THIS
  for (auto & stream : STREAMS) {
    setupStream(stream);
  }
 //  if (enable_[ACCEL] == true) {
  //   linear_accel_cov_ = node_.declare_parameter("accel0.linear_acceleration_covariance", DEFAULT_LINEAR_ACCEL_COV);
  // }
  // if (enable_[GYRO] == true) {
  //   angular_velocity_cov_ = node_.declare_parameter("gyro0.angular_velocity_covariance", DEFAULT_ANGULAR_VELOCITY_COV);
  // }
  linear_accel_cov_ = DEFAULT_LINEAR_ACCEL_COV;
  angular_velocity_cov_ = DEFAULT_ANGULAR_VELOCITY_COV;
  initialized_ = true;
}

Adding these lines, I can run T265 and D435i on their own, but not with rs_t265_and_d400.launch.py script. When I run this script both cameras return errors, so I have also added these lines to rs_base.cpp, which on Dashing were helpful:

void RealSenseBase::startPipeline()
{
  if (base_frame_id_ == "t265_link"){           //ADD THIS
    dev_.hardware_reset();
  }

But in Foxy do not change anything. As now, rs_t265_and_d400.launch.py, returns the following errors:

[INFO] [rviz2-1]: process started with pid [5210]
[INFO] [static_transform_publisher-2]: process started with pid [5212]
[INFO] [realsense_node-3]: process started with pid [5214]
[INFO] [realsense_node-4]: process started with pid [5216]
[static_transform_publisher-2] [INFO] [1620735074.721625322] [static_transform_publisher_5GerHvrePu1KDHP2]: Spinning until killed publishing transform from 't265_link' to 'd435_link'
[rviz2-1] QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-root'
[rviz2-1] [ERROR] [1620735074.798272653] [rviz2]: Invalid type 'string' for parameter 'use_sim_time', should be 'bool'
[realsense_node-3] terminate called after throwing an instance of 'rs2::error'
[realsense_node-3]   what():  failed to set power state
[ERROR] [realsense_node-3]: process has died [pid 5214, exit code -6, cmd '/root/ros2_ws/install/realsense_node/lib/realsense_node/realsense_node --ros-args -r __ns:=/t265 --params-file /tmp/launch_params_h7xzunuz -r /t265/camera/odom/sample:=/camera/odom/sample'].
[realsense_node-4] [INFO] [1620735075.077077626] [d435.camera]: Device with serial number 923322073270 was found.
[realsense_node-4] [INFO] [1620735075.077245418] [d435.camera]: Create a node for D435i Camera
[realsense_node-4] terminate called after throwing an instance of 'rclcpp::exceptions::InvalidParameterTypeException'
[realsense_node-4]   what():  parameter 'enable_pointcloud' has invalid type: expected [bool] got [string]
[ERROR] [realsense_node-4]: process has died [pid 5216, exit code -6, cmd '/root/ros2_ws/install/realsense_node/lib/realsense_node/realsense_node --ros-args -r __ns:=/d435 --params-file /tmp/launch_params_mw2njabq'].
[rviz2-1] [INFO] [1620735075.337526826] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-1] [INFO] [1620735075.337599652] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-1] [INFO] [1620735075.383102780] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-1] [ERROR] [1620735075.400013611] [transform_listener_impl_56509c9e0bd0]: Invalid type 'string' for parameter 'use_sim_time', should be 'bool'
[rviz2-1] [ERROR] [1620735075.463658226] [transform_listener_impl_56509ca5bcb0]: Invalid type 'string' for parameter 'use_sim_time', should be 'bool'

This is really tedious because already with Dashing it was necessary to add all these changes to scripts in order to make everything work, and these errors were never corrected. Now on Foxy the problem is even bigger since also these workaround are solving some error but not everything, and I can't run the two cameras together.

Can you please release a correction or something in order to make the two cameras work together on Foxy? Thank you very much!