Use in Gazebo

[jolags313]

I am trying to use Kimera in Gazebo with a simulated D435i camera mounted on a robot, but I have been unable to actually produce any trajectory or mesh, and all of the values output in the console are 0's as shown below.

Below is the launch file I am using to launch the simulation and camera.

<launch>
    <param name="office_geom" command="$(find xacro)/xacro --inorder '$(find cpr_office_gazebo)/urdf/office_construction_geometry.urdf.xacro'" />

    <arg name="platform" default="$(optenv CPR_GAZEBO_PLATFORM husky)" />

    <arg name="robot_x" default="0.0"/>
    <arg name="robot_y" default="0.0"/>
    <arg name="robot_z" default="0.2"/>
    <arg name="robot_yaw" default="0.0" />

    <arg name="world_x" default="0.0"/>
    <arg name="world_y" default="0.0"/>
    <arg name="world_z" default="0.0"/>
    <arg name="world_yaw" default="0.0"/>

    <arg name="use_sim_time" default="true" />
    <arg name="gui" default="true" />
    <arg name="headless" default="false" />
    <arg name="world_name" default="$(find cpr_office_gazebo)/worlds/actually_empty_world.world" />
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
      <arg name="debug" value="0" />
      <arg name="gui" value="$(arg gui)" />
      <arg name="use_sim_time" value="$(arg use_sim_time)" />
      <arg name="headless" value="$(arg headless)" />
      <arg name="world_name" value="$(arg world_name)" />
    </include>

    <node name="office_world_spawner" pkg="gazebo_ros" type="spawn_model"
      args="-urdf -model office_construction_geometry -param office_geom -x $(arg world_x) -y $(arg world_y) -z $(arg world_z) -Y $(arg world_yaw)" />

    <!-- static_transform_publisher from world frame to odom, extra 1 added for quaternion (?)-->
    <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 0 0 0 1 world odom 100" />

<!-- From VIO launch file kimera_vio_ros_realsense_IR.launch -->

    <!-- Launch static TF node from base_link to velo_link -->
    <node pkg="tf" type="static_transform_publisher" name="velo_link_broadcaster"
      args="0 0 0 0 0 0 1 base_link velo_link 100"/>

<!-- From VIO launch file kimera_vio_ros_realsense_IR.launch -->

    <!-- static_transform_publisher from odom to base_link_dvio for other links; or maybe do from base_link to base_link_dvio? velo link? -->
    <node pkg="tf" type="static_transform_publisher" name="link2_broadcaster" args="0 0 0 0 0 0 1 velo_link base_link_dvio 100" />

<!-- From semantic launch file kimera_metric_realsense.launch -->

    <!-- Tracking IMU to Tracking Camera tf -->
    <node pkg="tf" type="static_transform_publisher" name="imu_to_left_cam"
      args="-0.0107491 0.00339225 0.02843741 0.0052734748 0.0001508149  0.0022978430 0.9999834436 base_link_dvio left_cam 100"/>

    <!-- Tracking to Mapping Camera tf -->
    <node pkg="tf" type="static_transform_publisher" name="inter_cam_tf"
      args="0.075 0 -0.065 0 0.7071 0 0.7071  left_cam depth_cam 100"/>

    <node pkg="tf" type="static_transform_publisher" name="inter_cam_tf_2"
      args="0.075 0 -0.065 0 0.7071 0 0.7071 left_cam depth_camera_color_optical_frame 100"/>

<!-- From semantic launch file kimera_metric_realsense.launch -->

    <include file="$(find cpr_office_gazebo)/launch/spawn_$(arg platform).launch">
      <arg name="x" value="$(arg robot_x)"/>
      <arg name="y" value="$(arg robot_y)"/>
      <arg name="z" value="$(arg robot_z)"/>
      <arg name="yaw" value="$(arg robot_yaw)"/>
    </include>

</launch>

And below is the launch file I am using for Kimera VIO (kimera_vio_ros_realsense_IR.launch)

<launch>
  <arg name="dataset_name" value="RealSenseIR"/>
  <arg name="online" default="True" />

  <arg name="log_output" default="false"/>
  <arg name="log_output_path"
       default="$(find kimera_vio_ros)/output_logs/"
       if="$(arg log_output)"/>
  <arg name="log_gt_data" default="false"/>
  <arg name="gt_topic" default=""/>
  <!-- Set use_sim_time to true if you use rosbag with clock argument -->
  <arg name="should_use_sim_time" default="false"/>

  <!-- Only used when parsing a rosbag -->
  <arg name="rosbag_path" default="default.bag"
       unless="$(arg online)"/>

  <!-- Frame IDs. These DO NOT match frame id's on the video streams, as the 
	RealSense and Kimera publish conflicting Tf's -->
  <arg name="world_frame_id"     value="world"/>
  <arg name="base_link_frame_id" value="base_link"/>
  <arg name="map_frame_id"       value="map"/>
  <arg name="left_cam_frame_id"  value="VIO_left_cam"/>
  <arg name="right_cam_frame_id" value="right_cam"/>

  <!-- Subscribed Topics -->
  <arg name="left_cam_topic"  value="camera/infra1/image_rect_raw"/>
  <arg name="right_cam_topic" value="camera/infra2/image_rect_raw"/>
  <arg name="imu_topic"       value="camera/imu"/>

  <!-- Launch actual pipeline -->
  <include file="$(find kimera_vio_ros)/launch/kimera_vio_ros.launch"
           pass_all_args="true"/>

</launch>

Does anyone have any experience with this or know how I can address this issue?