T265 VSLAM and working ROS robot example

ShettyHarapanahalli commented 5 years ago

Hi,

Do we have an example project/configuration of T265 with move base ROS package.

Any example configuration of robot like : https://github.com/NVIDIA-AI-IOT/jetbot or https://github.com/autorope/donkeycar working with this, this will be a very good point to start over.

BR, SSH

mrbhjv commented 5 years ago

Hello,

I'd like to hop on this train and say, any complete example on adding wheel odometry and how to fill the calibration file, given a robot setup, would be awesome to have. I've been following the PRs and Issues, and the information is kind of sparse everywhere and not entirely clean. It doesn't necessarily have to come with the add-on of the d435i

Best Regards

joekeo commented 5 years ago

Anyone has come by an example on how to use the T265 with a wheeled robot?

I am having many issues setting up all the frame_ids to go along my robot, and to align the IMU for integration with cartographer.

Anyone has any pointers?

iDonghq commented 5 years ago

Anyone has come by an example on how to use the T265 with a wheeled robot?

I am having many issues setting up all the frame_ids to go along my robot, and to align the IMU for integration with cartographer.

Anyone has any pointers?

Hi joekeo: Have you integrated the T265 imu into cartographer，or example about t265 with a wheeled robot ? Maybe I met the same problem

joekeo commented 5 years ago

I have integrated the T265 IMU with cartographer, and have it running at the moment. this are the relevant files that I am using: This is the launch file for the T265

<launch>
<!-- change the serial number to the correct one for the camera that is being used -->
  <arg name="serial_no"           default="905312111492"/>  
  <!-- d435I  845112071659 t265 905312111492 -->
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="rs_t265"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>
  <arg name="initial_reset"       default="true"/>

  <arg name="fisheye_width"       default="848"/> 
  <arg name="fisheye_height"      default="800"/>
  <arg name="enable_fisheye1"     default="true"/>
  <arg name="enable_fisheye2"     default="true"/>

  <arg name="fisheye_fps"         default="30"/>

  <arg name="gyro_fps"            default="200"/>
  <arg name="accel_fps"           default="62"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>

  <arg name="enable_sync"           default="false"/>

  <arg name="linear_accel_cov"      default="0.01"/>
  <arg name="unite_imu_method"      default="linear_interpolation"/>
  <arg name="publish_odom_tf"       default="true"/>
  <arg name="odom_frame_id"         default="odom"/>
  <arg name="topic_odom_in"         default="/rr_robot/mobile_base_controller/odom"/>
  <arg name="calib_odom_file"       default="$(find dev_platform_base)/config/calibration_odometry.json" />"/>
  <arg name="base_frame_id"         default="$(arg tf_prefix)_link"/>
  <arg name="pose_frame_id"         default="$(arg tf_prefix)_pose_frame"/>

  <group ns="$(arg camera)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
      <arg name="serial_no"                value="$(arg serial_no)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>

      <arg name="enable_sync"              value="$(arg enable_sync)"/>

      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
      <arg name="enable_fisheye1"          value="$(arg enable_fisheye1)"/>
      <arg name="enable_fisheye2"          value="$(arg enable_fisheye2)"/>

      <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>

      <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="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
      <arg name="topic_odom_in"            value="$(arg topic_odom_in)"/>
      <arg name="calib_odom_file"          value="$(arg calib_odom_file)"/>
      <arg name="odom_frame_id"            value="$(arg odom_frame_id)"/>
      <arg name="base_frame_id"            value="$(arg base_frame_id)"/>
      <arg name="pose_frame_id"            value="$(arg pose_frame_id)"/>
    </include>
  </group>
  <!-- the realsense overrides the URDF transforms from the .xacro, so to over rule it back, we place the 
  base link to realsense link with a static tf -->
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_rs_t265_link" args="0.33 0 0.29 3.1457 3.1457 0 base_link $(arg tf_prefix)_link 100" />

</launch>

this is the config lua for carto, I am using an RP Lidar


include "map_builder.lua"
include "trajectory_builder.lua"

-- Cartographer_ros configuration reference:
-- https://google-cartographer-ros.readthedocs.io/en/latest/configuration.html

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "rs_t265_link",
  published_frame = "base_link",
  use_odometry = true,
  provide_odom_frame = true,
  odom_frame = "odom",
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = on,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}
--tunning guide
--https://google-cartographer-ros.readthedocs.io/en/latest/tuning.html

-- Cartographer configuration options:
-- https://google-cartographer.readthedocs.io/en/latest/configuration.html

MAP_BUILDER.use_trajectory_builder_2d = true

--Local Slam
--there are more parameters to tune, but this ones are the ones I found more impactful

--this one tries to match two laser scans together to estimate the position,
--I think if not on it will rely more on wheel odometry
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true

-- tune this value to the amount of samples (i think revolutions) to average over
--before estimating te position of the walls and features in the environment
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1

--use or not use IMU, if used, the tracking_frame should be set to the one that the IMU is on
TRAJECTORY_BUILDER_2D.use_imu_data = true

--bandpass filter for lidar distance measurements
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.max_z = .1
TRAJECTORY_BUILDER_2D.min_z = -.1

--This is the scan matcher and the weights to different assumptions
--occupied_space gives more weight to the 'previous' features detected.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40.

--this will help continue making the map while the robot is static
--default time is 5 seconds
TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.1

--imu configuration parameters
TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 10.

--map output parameters

--Global Slam
--Setting POSE_GRAPH.optimize_every_n_nodes to 0 is a handy way
--to disable global SLAM and concentrate on the behavior of local SLAM.
--This is usually one of the first thing to do to tune Cartographer.
POSE_GRAPH.optimize_every_n_nodes = 90. --90 default
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 10
POSE_GRAPH.optimization_problem.odometry_translation_weight = 1.
POSE_GRAPH.optimization_problem.fixed_frame_pose_translation_weight = 1e-1
POSE_GRAPH.optimization_problem.fixed_frame_pose_rotation_weight = 1e-1

return options

and this is the carto launch file:

<launch>

  <arg name="config_dir" />
  <arg name="config_file" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(arg config_dir)
          -configuration_basename $(arg config_file)">
      <!-- remap the topics for cartographer to find them -->
    <remap from="imu"  to="/rs_t265/imu" />
    <remap from="odom" to="/rr_robot/mobile_base_controller/odom"/>
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
    type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

</launch>

in this version I am inputting the imu from my wheels into carto, but IIRC you can input the odom from T265 by changing <arg name="publish_odom_tf" default="true"/> to <arg name="publish_odom_tf" default="false"/> and then in the carto launch file <remap from="odom" to="/rr_robot/mobile_base_controller/odom"/> to <remap from="odom" to="rs_t265/sample/odom"/> on that last one, please double check the topic name, as I am writing it down from memory.

Hope this helps. Now I am working in making a 3D map (and a 2D for navigation) with carto, RPLidar, t265 and d435i, I am a little bit confused here.

iDonghq commented 5 years ago

I have integrated the T265 IMU with cartographer, and have it running at the moment. this are the relevant files that I am using: This is the launch file for the T265

<launch>
<!-- change the serial number to the correct one for the camera that is being used -->
  <arg name="serial_no"           default="905312111492"/>  
  <!-- d435I  845112071659 t265 905312111492 -->
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="rs_t265"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>
  <arg name="initial_reset"       default="true"/>

  <arg name="fisheye_width"       default="848"/> 
  <arg name="fisheye_height"      default="800"/>
  <arg name="enable_fisheye1"     default="true"/>
  <arg name="enable_fisheye2"     default="true"/>

  <arg name="fisheye_fps"         default="30"/>

  <arg name="gyro_fps"            default="200"/>
  <arg name="accel_fps"           default="62"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>

  <arg name="enable_sync"           default="false"/>

  <arg name="linear_accel_cov"      default="0.01"/>
  <arg name="unite_imu_method"      default="linear_interpolation"/>
  <arg name="publish_odom_tf"       default="true"/>
  <arg name="odom_frame_id"         default="odom"/>
  <arg name="topic_odom_in"         default="/rr_robot/mobile_base_controller/odom"/>
  <arg name="calib_odom_file"       default="$(find dev_platform_base)/config/calibration_odometry.json" />"/>
  <arg name="base_frame_id"         default="$(arg tf_prefix)_link"/>
  <arg name="pose_frame_id"         default="$(arg tf_prefix)_pose_frame"/>

  <group ns="$(arg camera)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
      <arg name="serial_no"                value="$(arg serial_no)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>

      <arg name="enable_sync"              value="$(arg enable_sync)"/>

      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
      <arg name="enable_fisheye1"          value="$(arg enable_fisheye1)"/>
      <arg name="enable_fisheye2"          value="$(arg enable_fisheye2)"/>

      <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>

      <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="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
      <arg name="topic_odom_in"            value="$(arg topic_odom_in)"/>
      <arg name="calib_odom_file"          value="$(arg calib_odom_file)"/>
      <arg name="odom_frame_id"            value="$(arg odom_frame_id)"/>
      <arg name="base_frame_id"            value="$(arg base_frame_id)"/>
      <arg name="pose_frame_id"            value="$(arg pose_frame_id)"/>
    </include>
  </group>
  <!-- the realsense overrides the URDF transforms from the .xacro, so to over rule it back, we place the 
  base link to realsense link with a static tf -->
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_rs_t265_link" args="0.33 0 0.29 3.1457 3.1457 0 base_link $(arg tf_prefix)_link 100" />

</launch>

this is the config lua for carto, I am using an RP Lidar


include "map_builder.lua"
include "trajectory_builder.lua"

-- Cartographer_ros configuration reference:
-- https://google-cartographer-ros.readthedocs.io/en/latest/configuration.html

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "rs_t265_link",
  published_frame = "base_link",
  use_odometry = true,
  provide_odom_frame = true,
  odom_frame = "odom",
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = on,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}
--tunning guide
--https://google-cartographer-ros.readthedocs.io/en/latest/tuning.html

-- Cartographer configuration options:
-- https://google-cartographer.readthedocs.io/en/latest/configuration.html

MAP_BUILDER.use_trajectory_builder_2d = true

--Local Slam
--there are more parameters to tune, but this ones are the ones I found more impactful

--this one tries to match two laser scans together to estimate the position,
--I think if not on it will rely more on wheel odometry
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true

-- tune this value to the amount of samples (i think revolutions) to average over
--before estimating te position of the walls and features in the environment
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1

--use or not use IMU, if used, the tracking_frame should be set to the one that the IMU is on
TRAJECTORY_BUILDER_2D.use_imu_data = true

--bandpass filter for lidar distance measurements
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.max_z = .1
TRAJECTORY_BUILDER_2D.min_z = -.1

--This is the scan matcher and the weights to different assumptions
--occupied_space gives more weight to the 'previous' features detected.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40.

--this will help continue making the map while the robot is static
--default time is 5 seconds
TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.1

--imu configuration parameters
TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 10.

--map output parameters

--Global Slam
--Setting POSE_GRAPH.optimize_every_n_nodes to 0 is a handy way
--to disable global SLAM and concentrate on the behavior of local SLAM.
--This is usually one of the first thing to do to tune Cartographer.
POSE_GRAPH.optimize_every_n_nodes = 90. --90 default
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 10
POSE_GRAPH.optimization_problem.odometry_translation_weight = 1.
POSE_GRAPH.optimization_problem.fixed_frame_pose_translation_weight = 1e-1
POSE_GRAPH.optimization_problem.fixed_frame_pose_rotation_weight = 1e-1

return options

and this is the carto launch file:

<launch>

  <arg name="config_dir" />
  <arg name="config_file" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(arg config_dir)
          -configuration_basename $(arg config_file)">
      <!-- remap the topics for cartographer to find them -->
    <remap from="imu"  to="/rs_t265/imu" />
    <remap from="odom" to="/rr_robot/mobile_base_controller/odom"/>
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
    type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

</launch>

in this version I am inputting the imu from my wheels into carto, but IIRC you can input the odom from T265 by changing <arg name="publish_odom_tf" default="true"/> to <arg name="publish_odom_tf" default="false"/> and then in the carto launch file <remap from="odom" to="/rr_robot/mobile_base_controller/odom"/> to <remap from="odom" to="rs_t265/sample/odom"/> on that last one, please double check the topic name, as I am writing it down from memory.

Hope this helps. Now I am working in making a 3D map (and a 2D for navigation) with carto, RPLidar, t265 and d435i, I am a little bit confused here.

I have integrated the T265 IMU with cartographer, and have it running at the moment. this are the relevant files that I am using: This is the launch file for the T265

<launch>
<!-- change the serial number to the correct one for the camera that is being used -->
  <arg name="serial_no"           default="905312111492"/>  
  <!-- d435I  845112071659 t265 905312111492 -->
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="rs_t265"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>
  <arg name="initial_reset"       default="true"/>

  <arg name="fisheye_width"       default="848"/> 
  <arg name="fisheye_height"      default="800"/>
  <arg name="enable_fisheye1"     default="true"/>
  <arg name="enable_fisheye2"     default="true"/>

  <arg name="fisheye_fps"         default="30"/>

  <arg name="gyro_fps"            default="200"/>
  <arg name="accel_fps"           default="62"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>

  <arg name="enable_sync"           default="false"/>

  <arg name="linear_accel_cov"      default="0.01"/>
  <arg name="unite_imu_method"      default="linear_interpolation"/>
  <arg name="publish_odom_tf"       default="true"/>
  <arg name="odom_frame_id"         default="odom"/>
  <arg name="topic_odom_in"         default="/rr_robot/mobile_base_controller/odom"/>
  <arg name="calib_odom_file"       default="$(find dev_platform_base)/config/calibration_odometry.json" />"/>
  <arg name="base_frame_id"         default="$(arg tf_prefix)_link"/>
  <arg name="pose_frame_id"         default="$(arg tf_prefix)_pose_frame"/>

  <group ns="$(arg camera)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
      <arg name="serial_no"                value="$(arg serial_no)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>

      <arg name="enable_sync"              value="$(arg enable_sync)"/>

      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
      <arg name="enable_fisheye1"          value="$(arg enable_fisheye1)"/>
      <arg name="enable_fisheye2"          value="$(arg enable_fisheye2)"/>

      <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>

      <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="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
      <arg name="topic_odom_in"            value="$(arg topic_odom_in)"/>
      <arg name="calib_odom_file"          value="$(arg calib_odom_file)"/>
      <arg name="odom_frame_id"            value="$(arg odom_frame_id)"/>
      <arg name="base_frame_id"            value="$(arg base_frame_id)"/>
      <arg name="pose_frame_id"            value="$(arg pose_frame_id)"/>
    </include>
  </group>
  <!-- the realsense overrides the URDF transforms from the .xacro, so to over rule it back, we place the 
  base link to realsense link with a static tf -->
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_rs_t265_link" args="0.33 0 0.29 3.1457 3.1457 0 base_link $(arg tf_prefix)_link 100" />

</launch>

this is the config lua for carto, I am using an RP Lidar


include "map_builder.lua"
include "trajectory_builder.lua"

-- Cartographer_ros configuration reference:
-- https://google-cartographer-ros.readthedocs.io/en/latest/configuration.html

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "rs_t265_link",
  published_frame = "base_link",
  use_odometry = true,
  provide_odom_frame = true,
  odom_frame = "odom",
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = on,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}
--tunning guide
--https://google-cartographer-ros.readthedocs.io/en/latest/tuning.html

-- Cartographer configuration options:
-- https://google-cartographer.readthedocs.io/en/latest/configuration.html

MAP_BUILDER.use_trajectory_builder_2d = true

--Local Slam
--there are more parameters to tune, but this ones are the ones I found more impactful

--this one tries to match two laser scans together to estimate the position,
--I think if not on it will rely more on wheel odometry
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true

-- tune this value to the amount of samples (i think revolutions) to average over
--before estimating te position of the walls and features in the environment
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1

--use or not use IMU, if used, the tracking_frame should be set to the one that the IMU is on
TRAJECTORY_BUILDER_2D.use_imu_data = true

--bandpass filter for lidar distance measurements
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.max_z = .1
TRAJECTORY_BUILDER_2D.min_z = -.1

--This is the scan matcher and the weights to different assumptions
--occupied_space gives more weight to the 'previous' features detected.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40.

--this will help continue making the map while the robot is static
--default time is 5 seconds
TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.1

--imu configuration parameters
TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 10.

--map output parameters

--Global Slam
--Setting POSE_GRAPH.optimize_every_n_nodes to 0 is a handy way
--to disable global SLAM and concentrate on the behavior of local SLAM.
--This is usually one of the first thing to do to tune Cartographer.
POSE_GRAPH.optimize_every_n_nodes = 90. --90 default
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 10
POSE_GRAPH.optimization_problem.odometry_translation_weight = 1.
POSE_GRAPH.optimization_problem.fixed_frame_pose_translation_weight = 1e-1
POSE_GRAPH.optimization_problem.fixed_frame_pose_rotation_weight = 1e-1

return options

and this is the carto launch file:

<launch>

  <arg name="config_dir" />
  <arg name="config_file" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(arg config_dir)
          -configuration_basename $(arg config_file)">
      <!-- remap the topics for cartographer to find them -->
    <remap from="imu"  to="/rs_t265/imu" />
    <remap from="odom" to="/rr_robot/mobile_base_controller/odom"/>
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
    type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

</launch>

in this version I am inputting the imu from my wheels into carto, but IIRC you can input the odom from T265 by changing <arg name="publish_odom_tf" default="true"/> to <arg name="publish_odom_tf" default="false"/> and then in the carto launch file <remap from="odom" to="/rr_robot/mobile_base_controller/odom"/> to <remap from="odom" to="rs_t265/sample/odom"/> on that last one, please double check the topic name, as I am writing it down from memory.

Hope this helps. Now I am working in making a 3D map (and a 2D for navigation) with carto, RPLidar, t265 and d435i, I am a little bit confused here.

thank you veryyyyyyy much!!!

joekeo commented 5 years ago

Remember to change or remove the <arg name="serial_no" default="905312111492"/> bit to match your sensor

iDonghq commented 5 years ago

Remember to change or remove the <arg name="serial_no" default="905312111492"/> bit to match your sensor

one more question, Mr joekeo, it seems that you just use the imu from t265, right?

joekeo commented 5 years ago

on those launch files yes, but if you do the modification that is on the end of the comment, you will be also feeding the odometry output of the t265 to cartographer. I am still tunning this at the moment tot make it more reliable. but as it is, with the RP Lidar works fine at low angular speeds. this is the bit that I am refering to, in the carto launch file: remap from="odom" to="/rr_robot/mobile_base_controller/odom"/> to <remap from="odom" to="rs_t265/odom/sample"/> I added the right toipoic name this time

iDonghq commented 5 years ago

on those launch files yes, but if you do the modification that is on the end of the comment, you will be also feeding the odometry output of the t265 to cartographer. I am still tunning this at the moment tot make it more reliable. but as it is, with the RP Lidar works fine at low angular speeds. this is the bit that I am refering to, in the carto launch file: remap from="odom" to="/rr_robot/mobile_base_controller/odom"/> to <remap from="odom" to="rs_t265/odom/sample"/> I added the right toipoic name this time

got it !　thank you very much again!

jungladicitta commented 5 years ago

@joekeo <arg name="calib_odom_file" default="$(find dev_platform_base)/config/calibration_odometry.json" />"/> what does mean "dev_platform_base"?

joekeo commented 5 years ago

that is the name of the directory for my software architecture.

On Thu, 25 Jul 2019 at 09:47, Erdem Erdyniev notifications@github.com wrote:

"/> what does mean "dev_platform_base"?

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/IntelRealSense/realsense-ros/issues/711?email_source=notifications&email_token=ABDLJIDUEMULGBUTZGVSZP3QBFSDRA5CNFSM4HC6IJEKYY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOD2YZ3HA#issuecomment-514956700, or mute the thread https://github.com/notifications/unsubscribe-auth/ABDLJIDCTUSDAIZW2DX2XB3QBFSDRANCNFSM4HC6IJEA .

jungladicitta commented 5 years ago

that is the name of the directory for my software architecture. … On Thu, 25 Jul 2019 at 09:47, Erdem Erdyniev @.***> wrote: "/> what does mean "dev_platform_base"? — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#711?email_source=notifications&email_token=ABDLJIDUEMULGBUTZGVSZP3QBFSDRA5CNFSM4HC6IJEKYY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOD2YZ3HA#issuecomment-514956700>, or mute the thread https://github.com/notifications/unsubscribe-auth/ABDLJIDCTUSDAIZW2DX2XB3QBFSDRANCNFSM4HC6IJEA .

ok, thanks for quick reply

Joar93 commented 5 years ago

@joekeo Nice work!

Did you ever get d435 working in conjunction with t265 in cartographer?

Ping-Ju commented 5 years ago

Hi @joekeo ,

Can you please tell me what is the name (odom) of odom_frame and the name (base_link) of base_frame from /rr_robot/mobile_base_controller/odom or your robot? If those frames are odom and base_link as the frame name from your robot, how did you come out with ?

joekeo commented 5 years ago

Ping-Ju The names of my robots frames are odom and base_link. I dont really understand wht do you mean with: If those frames are odom and base_link as the frame name from your robot, how did you come out with ? But for me they worked perfectly

Ping-Ju commented 5 years ago

@joekeo So the "camera_odom_frame" rename to "odom" Therefore, the tf will be like "odom -> base_link -> camera_link. So do you know where is the camera_pose_frame?

joekeo commented 5 years ago

I did not rename any frame, my URDF has the base_link to the camera transform and the rest is done in the files that I provided here https://github.com/IntelRealSense/realsense-ros/issues/711#issuecomment-498133068

Therefore, the tf will be like "odom -> base_link -> camera_link.

I think this will output an error as you might be relocating something based on itself.

Ping-Ju commented 5 years ago

@joekeo So your robot odometry is: /rr_robot/mobile_base_controller/odom your robot frame is: odom -> base_link The camera odometry is: /camera/odom/sample camera frame is : camera_odom_frame -> camera_pose_frame -> camera_link

I did something like this as shown below!!! tf_tree

For your robot odometry, did you send transform (tf) to base_link?

Joar93 commented 5 years ago

Hi @Ping-Ju I solved it like this frames So i use one static_transform_publisher between the base and the t265 pose frame. I use another static_transform_publisher to "kidnap" the t265_imu_optical_frame (which is the IMUs frame-id) and connect it to my base. The reason i do this is becuse all the sensors in the t265 have different orientations (see picture below) and this confuses cartographer since i put tracking frame in cartographer as the imu (t265_imu_optical_frame) T265_sensor_extrinsics This works well with 3D cartographer but the same should work well for 2D. This is probably not the smartest solution but it works for me and it lets me feed cartographer with both the imu and odomotry from the t265. Maybe @joekeo can give some insights?

warm regards Joar

Ping-Ju commented 5 years ago

@Joar93 I was wondering that for your "base" Is that the "base" connected from robot odom -> base did you turn off the tf from odom -> base?

Because for me, I did turn off the transform from odom -> base, and connected from t265_pose_frame -> base. But I didn't get any value from /camera/odom/sample when I move the robot constant. and I have built in the wheel odometry from robot as well.

Kind regards, Ping-Ju

Joar93 commented 5 years ago

Hello @Ping-Ju!

I will take a look at your questions on monday and post here!

Ping-Ju commented 5 years ago

Hello @Joar93

calibration_odometry

what is the data for your calibration file (calibration_odometry.json)? For the extrinsic "T" array: Due to the fact the T265 has the following coordinate system

Positive X direction is towards right imager
Positive Y direction is upwards toward the top of the device
Positive Z direction is inwards toward the back of the device

For the "W" array:

Positive X direction is outwards toward the front of the robot
Positive Y direction is towards left of the robot
Positive Z direction is upwards toward the top of the robot

I want to put T265 camera in front of 0.5 meters from the center of the robot, and the 0.2 meters height from the robot.

So the T:[ 0, -0.2, -0.5 ]

Is that correct?

Joar93 commented 5 years ago

Hi @Ping-Ju! I havent changed the calibration file! Did it work like you expected?

Joar93 commented 5 years ago

@joekeo could you share your URDF model for your robot so we could see how you implemented the t265_camera into the robot_state_publisher?

Ping-Ju commented 5 years ago

@Joar93 I have changed the calibration file, but the result seems to be the same. Still dont know how to change it.

RealSenseCustomerSupport commented 5 years ago

We've recently updated some documentation regarding wheeled odometry on the T265. Please see the "Appendix" section in link below and let us know if this helps clarify things.

https://github.com/IntelRealSense/librealsense/blob/development/doc/t265.md

Thanks

RealSenseCustomerSupport commented 4 years ago

Will you be needing further help with this?

One of our engineers also suggested reducing the measurement noise covariance, probably by 1 or 2 orders to see an effect. Maybe try 1/100? (It’s quadratic.)

Is it safe to close this issue?

Thanks

sasilva1998 commented 3 years ago

I have integrated the T265 IMU with cartographer, and have it running at the moment. this are the relevant files that I am using: This is the launch file for the T265

<launch>
<!-- change the serial number to the correct one for the camera that is being used -->
  <arg name="serial_no"           default="905312111492"/>  
  <!-- d435I  845112071659 t265 905312111492 -->
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="rs_t265"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>
  <arg name="initial_reset"       default="true"/>

  <arg name="fisheye_width"       default="848"/> 
  <arg name="fisheye_height"      default="800"/>
  <arg name="enable_fisheye1"     default="true"/>
  <arg name="enable_fisheye2"     default="true"/>

  <arg name="fisheye_fps"         default="30"/>

  <arg name="gyro_fps"            default="200"/>
  <arg name="accel_fps"           default="62"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>

  <arg name="enable_sync"           default="false"/>

  <arg name="linear_accel_cov"      default="0.01"/>
  <arg name="unite_imu_method"      default="linear_interpolation"/>
  <arg name="publish_odom_tf"       default="true"/>
  <arg name="odom_frame_id"         default="odom"/>
  <arg name="topic_odom_in"         default="/rr_robot/mobile_base_controller/odom"/>
  <arg name="calib_odom_file"       default="$(find dev_platform_base)/config/calibration_odometry.json" />"/>
  <arg name="base_frame_id"         default="$(arg tf_prefix)_link"/>
  <arg name="pose_frame_id"         default="$(arg tf_prefix)_pose_frame"/>

  <group ns="$(arg camera)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
      <arg name="serial_no"                value="$(arg serial_no)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>

      <arg name="enable_sync"              value="$(arg enable_sync)"/>

      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
      <arg name="enable_fisheye1"          value="$(arg enable_fisheye1)"/>
      <arg name="enable_fisheye2"          value="$(arg enable_fisheye2)"/>

      <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>

      <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="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
      <arg name="topic_odom_in"            value="$(arg topic_odom_in)"/>
      <arg name="calib_odom_file"          value="$(arg calib_odom_file)"/>
      <arg name="odom_frame_id"            value="$(arg odom_frame_id)"/>
      <arg name="base_frame_id"            value="$(arg base_frame_id)"/>
      <arg name="pose_frame_id"            value="$(arg pose_frame_id)"/>
    </include>
  </group>
  <!-- the realsense overrides the URDF transforms from the .xacro, so to over rule it back, we place the 
  base link to realsense link with a static tf -->
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_rs_t265_link" args="0.33 0 0.29 3.1457 3.1457 0 base_link $(arg tf_prefix)_link 100" />

</launch>

this is the config lua for carto, I am using an RP Lidar


include "map_builder.lua"
include "trajectory_builder.lua"

-- Cartographer_ros configuration reference:
-- https://google-cartographer-ros.readthedocs.io/en/latest/configuration.html

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "rs_t265_link",
  published_frame = "base_link",
  use_odometry = true,
  provide_odom_frame = true,
  odom_frame = "odom",
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = on,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}
--tunning guide
--https://google-cartographer-ros.readthedocs.io/en/latest/tuning.html

-- Cartographer configuration options:
-- https://google-cartographer.readthedocs.io/en/latest/configuration.html

MAP_BUILDER.use_trajectory_builder_2d = true

--Local Slam
--there are more parameters to tune, but this ones are the ones I found more impactful

--this one tries to match two laser scans together to estimate the position,
--I think if not on it will rely more on wheel odometry
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true

-- tune this value to the amount of samples (i think revolutions) to average over
--before estimating te position of the walls and features in the environment
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1

--use or not use IMU, if used, the tracking_frame should be set to the one that the IMU is on
TRAJECTORY_BUILDER_2D.use_imu_data = true

--bandpass filter for lidar distance measurements
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.max_z = .1
TRAJECTORY_BUILDER_2D.min_z = -.1

--This is the scan matcher and the weights to different assumptions
--occupied_space gives more weight to the 'previous' features detected.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40.

--this will help continue making the map while the robot is static
--default time is 5 seconds
TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.1

--imu configuration parameters
TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 10.

--map output parameters

--Global Slam
--Setting POSE_GRAPH.optimize_every_n_nodes to 0 is a handy way
--to disable global SLAM and concentrate on the behavior of local SLAM.
--This is usually one of the first thing to do to tune Cartographer.
POSE_GRAPH.optimize_every_n_nodes = 90. --90 default
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 10
POSE_GRAPH.optimization_problem.odometry_translation_weight = 1.
POSE_GRAPH.optimization_problem.fixed_frame_pose_translation_weight = 1e-1
POSE_GRAPH.optimization_problem.fixed_frame_pose_rotation_weight = 1e-1

return options

and this is the carto launch file:

<launch>

  <arg name="config_dir" />
  <arg name="config_file" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(arg config_dir)
          -configuration_basename $(arg config_file)">
      <!-- remap the topics for cartographer to find them -->
    <remap from="imu"  to="/rs_t265/imu" />
    <remap from="odom" to="/rr_robot/mobile_base_controller/odom"/>
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
    type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

</launch>

in this version I am inputting the imu from my wheels into carto, but IIRC you can input the odom from T265 by changing <arg name="publish_odom_tf" default="true"/> to <arg name="publish_odom_tf" default="false"/> and then in the carto launch file <remap from="odom" to="/rr_robot/mobile_base_controller/odom"/> to <remap from="odom" to="rs_t265/sample/odom"/> on that last one, please double check the topic name, as I am writing it down from memory.

Hope this helps. Now I am working in making a 3D map (and a 2D for navigation) with carto, RPLidar, t265 and d435i, I am a little bit confused here.

Hello @joekeo, I am following the example you gave and currently my t265 is listening to the odometry from the wheels of my robot, but I am not getting any publish of odometry from the t265. Do you know any reason why that could be happening? I am using ROS melodic with cartographer and t265 and d435i.

siddharthcb commented 3 years ago

hi @joekeo Thanks for the information. i just wanted to change odometry in cartographer to t265 odometry and disable any other odometry if cartographer is using. if I only change <arg name="publish_odom_tf" default="true"/> to <arg name="publish_odom_tf" default="false"/> and then in the carto launch file <remap from="odom" to="/rr_robot/mobile_base_controller/odom"/> to <remap from="odom" to="rs_t265/sample/odom"/> is sufficient? my cartographer.launch file looks like this:

<?xml version="1.0"?>
   <launch>
      <param name="/use_sim_time" value="false" />
      <node name="cartographer_node"
            pkg="cartographer_ros"
            type="cartographer_node"
            args="-configuration_directory $(find cartographer_ros)/configuration_files -configuration_basename cartographer.lua"
            output="screen">
        <remap from="odom" to="/camera/odom/sample" />
      </node>
      <node name="cartographer_occupancy_grid_node"
            pkg="cartographer_ros"
            type="cartographer_occupancy_grid_node" />
      <node name="robot_pose_publisher"
            pkg="robot_pose_publisher"
            type="robot_pose_publisher"
            respawn="false"
            output="screen" />
      <node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 base_link laser 100" />
   </launch>

.lua config file:

include "map_builder.lua"
include "trajectory_builder.lua"

options = {

    map_builder = MAP_BUILDER,
    trajectory_builder = TRAJECTORY_BUILDER,
    map_frame = "map",
    tracking_frame = "base_link",
    published_frame = "base_link",
    odom_frame = "odom",
    provide_odom_frame = true,
    use_odometry = false,
    use_nav_sat = false,
    use_landmarks = false,
    publish_frame_projected_to_2d = false,
    num_laser_scans = 1,
    num_multi_echo_laser_scans = 0,
    num_subdivisions_per_laser_scan = 1,
    rangefinder_sampling_ratio = 1,
    odometry_sampling_ratio = 1,
    fixed_frame_pose_sampling_ratio = 1,
    imu_sampling_ratio = 1,
    landmarks_sampling_ratio = 1,
    num_point_clouds = 0,
    lookup_transform_timeout_sec = 0.2,
    submap_publish_period_sec = 0.3,
    pose_publish_period_sec = 5e-3,
    trajectory_publish_period_sec = 30e-3,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.use_imu_data = false

TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true

POSE_GRAPH.optimization_problem.huber_scale = 1e2

return options

looking forward to hear from you. Thank you

IntelRealSense / realsense-ros

T265 VSLAM and working ROS robot example #711