add: tf2 options to gps_vision

aau-cns · Sep 7, 2022 · 2041a2c · 2041a2c
1 parent 16e10da
commit 2041a2c
Showing 1 changed file with 60 additions and 10 deletions.
diff --git a/launch/mars_gps_vision.launch b/launch/mars_gps_vision.launch
@@ -2,15 +2,15 @@
 
 <launch>
   <!-- Input -->
-  <arg name="imu_in_topic"    default="~imu_in" />
+  <arg name="imu_in_topic"      default="~imu_in" />
 
   <arg name="vision1_in_topic"  default="~vision1_in" />
-  <arg name="gps1_pos_in"     default="~gps1_pos_in" />
-  <arg name="gps1_vel_in"     default="~gps1_vel_in" />
-  <arg name="pressure1_in"    default="~baro1_in" />
-  <arg name="mag1_in"         default="~mag1_in" />
+  <arg name="gps1_pos_in"       default="~gps1_pos_in" />
+  <arg name="gps1_vel_in"       default="~gps1_vel_in" />
+  <arg name="pressure1_in"      default="~baro1_in" />
+  <arg name="mag1_in"           default="~mag1_in" />
 
-  <arg name="use_gps_vel"     default="False" />
+  <arg name="use_gps_vel"       default="False" />
 
   <!-- Output Core State -->
   <arg name="full_state_out_topic"      default="~full_state_out" />
@@ -88,11 +88,61 @@
     <remap from="~mag1_cal_state_out"       to="$(arg mag1_cal_state_out_topic)"/>
   </node>
 
-  <include file="$(find mars_ros)/launch/rviz/mars_rviz.launch" if="$(arg rviz)">
-    <arg name="topic_path_in"   value="$(arg path_states_out_topic)" />
-    <arg name="topic_pose_in"   value="$(arg pose_state_out_topic)" />
+  <!-- launch rviz -->
+  <group if="$(arg rviz)">
     <arg name="namespace_mars"  value="mars_gps_vision_node" unless="$(arg use_gps_vel)"/>
     <arg name="namespace_mars"  value="mars_gpsvel_vision_node" if="$(arg use_gps_vel)"/>
-  </include>
+
+    <include file="$(find mars_ros)/launch/rviz/mars_rviz.launch" if="$(arg rviz)">
+      <arg name="topic_path_in"   value="$(arg path_states_out_topic)" />
+      <arg name="topic_pose_in"   value="$(arg pose_state_out_topic)" />
+      <arg name="namespace_mars"  value="$(arg namespace_mars)" />
+    </include>
+
+    <arg name="pose_topic"  value="$(eval eval(compile('pose_state_out_topic.replace(\'~\',\'\')','string','eval')))" />
+    <node name="rviz_mars_pose_br" pkg="mars_ros" type="mars_tf2_converter" clear_params="true" output="screen" args="-t pose">
+      <param name="child_id"      value="imu" />
+
+      <remap from="~pose_in" to="$(arg pose_state_out_topic)" unless="$(eval pose_state_out_topic[0]=='~')"/>
+      <remap from="~pose_in" to="/$(arg namespace_mars)/$(arg pose_topic)" if="$(eval pose_state_out_topic[0]=='~')"/>
+    </node>
+
+    <arg name="vision1_cal_topic"  value="$(eval eval(compile('vision1_cal_state_out_topic.replace(\'~\',\'\')','string','eval')))" />
+    <node name="rviz_mars_vision1_cal_br" pkg="mars_ros" type="mars_tf2_converter" clear_params="true" output="screen" args="-t vision">
+      <param name="child_id"      value="vision1_sensor" />
+      <param name="parent_id"     value="imu" />
+
+      <remap from="~state_in" to="$(arg vision1_cal_state_out_topic)" unless="$(eval vision1_cal_state_out_topic[0]=='~')"/>
+      <remap from="~state_in" to="/$(arg namespace_mars)/$(arg vision1_cal_topic)" if="$(eval vision1_cal_state_out_topic[0]=='~')"/>
+    </node>
+
+    <arg name="gps1_cal_topic"  value="$(eval eval(compile('gps1_cal_state_out_topic.replace(\'~\',\'\')','string','eval')))" />
+    <node name="rviz_mars_gps1_cal_br" pkg="mars_ros" type="mars_tf2_converter" clear_params="true" output="screen" args="-t posecov">
+      <param name="child_id"      value="gps1_sensor" />
+      <param name="parent_id"     value="imu" />
+
+      <remap from="~pose_in" to="$(arg gps1_cal_state_out_topic)" unless="$(eval gps1_cal_state_out_topic[0]=='~')"/>
+      <remap from="~pose_in" to="/$(arg namespace_mars)/$(arg gps1_cal_topic)" if="$(eval gps1_cal_state_out_topic[0]=='~')"/>
+    </node>
+
+    <arg name="mag1_cal_topic"  value="$(eval eval(compile('mag1_cal_state_out_topic.replace(\'~\',\'\')','string','eval')))" />
+    <node name="rviz_mars_mag1_cal_br" pkg="mars_ros" type="mars_tf2_converter" clear_params="true" output="screen" args="-t posecov">
+      <param name="child_id"      value="mag1_sensor" />
+      <param name="parent_id"     value="imu" />
+
+      <remap from="~pose_in" to="$(arg mag1_cal_state_out_topic)" unless="$(eval mag1_cal_state_out_topic[0]=='~')"/>
+      <remap from="~pose_in" to="/$(arg namespace_mars)/$(arg mag1_cal_topic)" if="$(eval mag1_cal_state_out_topic[0]=='~')"/>
+    </node>
+
+    <arg name="pressure1_cal_topic"  value="$(eval eval(compile('pressure1_cal_state_out_topic.replace(\'~\',\'\')','string','eval')))" />
+    <node name="rviz_mars_pressure1_cal_br" pkg="mars_ros" type="mars_tf2_converter" clear_params="true" output="screen" args="-t pose">
+      <param name="child_id"      value="pressure1_sensor" />
+      <param name="parent_id"     value="imu" />
+
+      <remap from="~pose_in" to="$(arg pressure1_cal_state_out_topic)" unless="$(eval pressure1_cal_state_out_topic[0]=='~')"/>
+      <remap from="~pose_in" to="/$(arg namespace_mars)/$(arg pressure1_cal_topic)" if="$(eval pressure1_cal_state_out_topic[0]=='~')"/>
+    </node>
+
+  </group>
 
 </launch>