tf tree for viso2

edit

I'm trying to set up a tf tree to use with viso2 to reconstruct point clouds, but I'm running into a lot of issues. Right now I'm just trying to set up a chain of dummy static transforms in order to get something working (i.e., I know the reconstruction will be bad because the transforms are arbitrary/incorrect, but I just want to see that I can get data displayed).

I've tried to set up a bunch of static transforms as follows, with the intent of creating the tf tree:

map -> odom -> base_link -> caodom -> camera_optical_frame

->base_link -> odom_combined -> base_footprint

(base_link is the parent of both caodom and odom_combined frames)

<launch>

  <arg name="simtime" value="true" />

  <!-- Get name of bagfile (passed in to roslaunch as arg) -->
  <!--  <arg name="bagfile" />-->

  <!-- Play bag file. -->
  <!-- not working right now <node pkg="rosbag" type="play" name="rosbag" args="$(find stereo_data)/$(arg bagfile).bag" />-->
  <!--  <node pkg="rosbag" type="play" name="rosbag" args="SCRIPPS.bag" />-->
  <node pkg="rosbag" type="play" name="rosbag" args="$(find stereo_data)/SCRIPPS.bag --clock" />


  <!-- Publish transforms -->

  <!-- Transform from camera odom frame to camera_optical_frame -->
  <node pkg="tf" type="static_transform_publisher" name="caodom_to_camera_optical_frame" args="0 0 0 0.1 0.1 0.1 /caodom /camera_optical_frame 100 --clock">
    <param name="queue_size" value="50" />
    <param name="/use_sim_time" value="simtime" />
  </node>

  <!-- Transform from base_link to caodom -->
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_caodom" args="0 0 0 0.1 0.1 0.1 /base_link /caodom 100 --clock">
    <param name="queue_size" value="50" />
    <param name="/use_sim_time" value="simtime" />
  </node>

  <!-- Transform from base_link to odom_combined -->
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_odom_combined" args="0 0 0 0.1 0.1 0.1 /base_link /odom_combined 100 --clock">
    <param name="queue_size" value="50" />
    <param name="/use_sim_time" value="simtime" />
  </node>

  <!-- Transform from odom_combined to base_footprint -->
  <node pkg="tf" type="static_transform_publisher" name="odom_combined_to_base_footprint" args="0 0 0 0.1 0.1 0.1 /odom_combined /base_footprint 100 --clock">
    <param name="queue_size" value="50" />
    <param name="/use_sim_time" value="simtime" />
  </node>



  <!-- Debayer and rectify images -->
  <group ns="stereo/left">
    <node pkg="image_proc" type="image_proc" name="image_proc_left">
      <param name="/use_sim_time" value="simtime" />
    </node>  
  </group>

  <group ns="stereo/right">
    <node pkg="image_proc" type="image_proc" name="image_proc_right">
      <param name="/use_sim_time" value="simtime" />
    </node>
  </group>

  <!-- Visual odometry node -->
  <node pkg="viso2_ros" type="stereo_odometer" name="vo2" 
      args="stereo:=stereo image:=image_rect _odom_frame_id:=/odom_combined _base_link_frame_id:=/base_footprint _publish_tf:=true">
    <param name="approximate_sync" value="true" />
    <param name="/use_sim_time" value="simtime" />
  </node>

  <!-- Compute disparities for images -->
  <node pkg="elas_ros" type="elas_ros" name="elas" args="stereo:=stereo image:=image_rect_color" respawn="true">
    <param name="approximate_sync" value="true" />
    <param name="queue_size" value="10" />
    <param name="/use_sim_type" value="simtime" />
  </node>

  <!-- Compute 3D point clouds from disparities -->
  <node pkg="elas_ros" type="pc_construction" name="elas_pc" 
    args="frame_data:=/elas/frame_data pose:=/vo2/pose base_frame_id:=/odom_combined pose_frame_id:=/base_footprint">
      <param name="approximate_sync" value="true" />
      <param name="queue_size" value="10" />
      <param name="/use_sim_time" value="simtime" />
  </node>

  <!-- View debayered/rectified images -->
  <node pkg="image_view" type="image_view" name="image_view_left" args="image:=stereo/left/image_rect_color" />
  <node pkg="image_view" type="image_view" name="image_view_right" args="image:=stereo/right ...