Merge pull request #29 from ETHZ-RobotX/julian/dev

Julian/dev
ETHZ-RobotX · Jun 30, 2022 · 5607604 · 5607604
2 parents 4d951b3 + a9ee694
commit 5607604
Show file tree

Hide file tree

Showing 14 changed files with 630 additions and 23 deletions.
diff --git a/smb_msf/CMakeLists.txt b/smb_msf/CMakeLists.txt
@@ -6,11 +6,31 @@ find_package(catkin REQUIRED)
 catkin_package(
   CATKIN_DEPENDS
     msf_updates
-
+    msf_core
 )
 
+catkin_python_setup()
+
+#############
+## Install ##
+#############
+
 install(DIRECTORY
   param
   launch
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
+
+install(
+  TARGETS
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+catkin_install_python(PROGRAMS
+  smb_msf/plotting_node.py
+  smb_msf/pose_noisifier.py
+  smb_msf/imu_noisifier.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
diff --git a/smb_msf/launch/plotting.launch b/smb_msf/launch/plotting.launch
@@ -0,0 +1,21 @@
+<launch>
+
+  <!-- Arguments -->
+  <arg name="noisify_pose" default="false" />
+  <arg name="noisify_imu" default="false" />_
+
+  <!-- Launch Pose Noisifier -->
+  <group if="$(arg noisify_pose)">
+    <node name="pose_noisifier_node" pkg="smb_msf" type="pose_noisifier.py" output="screen" />
+  </group>
+
+  <!-- Launch Pose Noisifier -->
+  <group if="$(arg noisify_imu)">
+    <node name="imu_noisifier_node" pkg="smb_msf" type="imu_noisifier.py" output="screen" />
+  </group>
+
+  <!-- Launch Visualizer Node -->
+  <node name="plotting_node" pkg="smb_msf" type="plotting_node.py" output="screen" />
+
+
+</launch>
diff --git a/smb_msf/launch/smb_msf.launch b/smb_msf/launch/smb_msf.launch
@@ -1,17 +1,35 @@
 <launch>
 
-    <arg name="launch_state_to_base_link_publisher"  default="true"/>
+    <!-- Arguments -->
+    <arg name="use_sim_time" default="false" />
+    <arg name="launch_lidar_to_imu_publisher"  default="true" />
+    <arg name="launch_rviz" default="true" />
+    <arg name="imu_topic" default="imu_topic" />
+    <arg name="pose_topic" default="pose_topic" />
+
+    <!-- Sim -->
+    <param name="use_sim_time" value="$(arg use_sim_time)" /> # be sure to run --clock
+
+    <!-- MSF -->
     <node name="msf_smb" pkg="msf_updates" type="pose_sensor" clear_params="true" output="screen"> #launch-prefix="gdb -ex run --args" 
         <!-- IMU -->
-        <remap from="msf_core/imu_state_input" to="/versavis/imu" />
+        <remap from="msf_core/imu_state_input" to="$(arg imu_topic)" />
         <!-- 6DOF Pose -->
-        <remap from="msf_updates/transform_input" to="/transform_aft_mapped_to_init_CORRECTED" />
+        <remap from="msf_updates/transform_input" to="$(arg pose_topic)" />
         <!-- Params -->
         <rosparam file="$(find smb_msf)/param/smb_msf_params.yaml" command="load"/>
     </node>
-    <!-- Initialize service -->
+
+    <!-- Initialize service After MSF Has Started -->
     <node pkg="rosservice" type="rosservice" name="initializer" args="call --wait /msf_smb/pose_sensor/initialize_msf_scale 1.0"/>
 
-      <node pkg="tf" type="static_transform_publisher" name="imu_to_base" respawn="true" args="-0.255 0.024 -0.179 0 0 0 state base_link 100" if="$(arg launch_state_to_base_link_publisher)"/>
+    <!-- Publish extrinsic Calibration if Desired -->
+    <node pkg="tf" type="static_transform_publisher" name="lidar_to_imu" respawn="true" args="-0.024 -0.252 -0.255 0.0 0.0 -0.707 0.707 range_sensor_o3d imu_frame 100" if="$(arg launch_lidar_to_imu_publisher)"/>
+
+    <!-- Publish extrinsic Calibration between map and map_o3d -->
+    <node pkg="tf" type="static_transform_publisher" name="map_to_map_o3d" respawn="true" args="0 0 0 0 0 0 map map_o3d 100" />
+
+    <!-- RVIZ -->
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find smb_msf)/rviz/smb.rviz" required="true" if="$(arg launch_rviz)"/>
 
 </launch>
diff --git a/smb_msf/package.xml b/smb_msf/package.xml
@@ -8,6 +8,7 @@
 
 	<buildtool_depend>catkin</buildtool_depend>
 	<exec_depend>msf_updates</exec_depend>
+	<exec_depend>msf_core</exec_depend>
 
 
 </package>
diff --git a/smb_msf/param/smb_msf_params.yaml b/smb_msf/param/smb_msf_params.yaml
@@ -1,5 +1,6 @@
 core/data_playback: true             # Set to true for playback, set to false on the real system.
-core/msf_output_frame: odom           # Set MSF output frame
+core/msf_output_frame: map           # Set MSF output frame
+core/msf_state_frame: msf_imu_state
 ##############################
 #########IMU PARAMETERS#######
 ##############################
@@ -26,7 +27,7 @@ core/msf_output_frame: odom           # Set MSF output frame
 core/core_noise_acc: 0.0022563    # [m/s^2/sqrt(Hz)] 
 core/core_noise_gyr: 0.0004       # [rad/s/sqrt(Hz)] 
 
-core/core_fixed_bias: false
+core/core_fixed_bias: true
 core/core_noise_accbias: 0.002     # [m/s^3/sqrt(Hz)]
 core/core_noise_gyrbias: 0.0009    # [rad/s^2/sqrt(Hz)]
 
@@ -65,10 +66,10 @@ pose_sensor/pose_noise_p_ic: 0.0
 pose_sensor/pose_fixed_q_ic: true
 pose_sensor/pose_noise_q_ic: 0.0
 
-pose_sensor/init/q_ic/w: 0.707
+pose_sensor/init/q_ic/w: 1
 pose_sensor/init/q_ic/x: 0.0
 pose_sensor/init/q_ic/y: 0.0
-pose_sensor/init/q_ic/z: 0.707
-pose_sensor/init/p_ic/x: -0.251   
-pose_sensor/init/p_ic/y: 0.024
-pose_sensor/init/p_ic/z: 0.255
+pose_sensor/init/q_ic/z: 0.0
+pose_sensor/init/p_ic/x: 0.0   
+pose_sensor/init/p_ic/y: 0.0
+pose_sensor/init/p_ic/z: 0.0