diff --git a/smb_msf/CMakeLists.txt b/smb_msf/CMakeLists.txt
index 2679df9..dd45032 100644
--- 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
new file mode 100644
index 0000000..3e909f2
--- /dev/null
+++ b/smb_msf/launch/plotting.launch
@@ -0,0 +1,21 @@
+
+
+
+
+ _
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/smb_msf/launch/smb_msf.launch b/smb_msf/launch/smb_msf.launch
index 2896248..2eb1b8a 100644
--- a/smb_msf/launch/smb_msf.launch
+++ b/smb_msf/launch/smb_msf.launch
@@ -1,17 +1,35 @@
-
+
+
+
+
+
+
+
+
+ # be sure to run --clock
+
+
#launch-prefix="gdb -ex run --args"
-
+
-
+
-
+
+
-
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/smb_msf/package.xml b/smb_msf/package.xml
index 806045b..867f1ba 100644
--- a/smb_msf/package.xml
+++ b/smb_msf/package.xml
@@ -8,6 +8,7 @@
catkin
msf_updates
+ msf_core
diff --git a/smb_msf/param/smb_msf_params.yaml b/smb_msf/param/smb_msf_params.yaml
index 8c0782d..fbf6667 100644
--- 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
\ No newline at end of file
+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
\ No newline at end of file
diff --git a/smb_msf/rviz/smb.rviz b/smb_msf/rviz/smb.rviz
new file mode 100644
index 0000000..c8eefad
--- /dev/null
+++ b/smb_msf/rviz/smb.rviz
@@ -0,0 +1,314 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /TF1/Frames1
+ - /Odometry1/Shape1
+ - /Odometry2/Shape1
+ Splitter Ratio: 0.314031183719635
+ Tree Height: 836
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: PointCloud2
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 50
+ Reference Frame:
+ Value: true
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 90
+ Frames:
+ All Enabled: false
+ camera_accel_frame:
+ Value: true
+ camera_accel_optical_frame:
+ Value: true
+ camera_gyro_frame:
+ Value: true
+ camera_imu_optical_frame:
+ Value: true
+ camera_link:
+ Value: true
+ camera_pose_frame:
+ Value: true
+ imu_frame:
+ Value: true
+ map:
+ Value: true
+ map_o3d:
+ Value: true
+ odom_o3d:
+ Value: true
+ range_sensor_o3d:
+ Value: true
+ raw_odom_o3d:
+ Value: true
+ raw_rs_o3d:
+ Value: true
+ Marker Alpha: 1
+ Marker Scale: 2
+ Name: TF
+ Show Arrows: false
+ Show Axes: true
+ Show Names: true
+ Tree:
+ map:
+ map_o3d:
+ odom_o3d:
+ range_sensor_o3d:
+ imu_frame:
+ {}
+ raw_odom_o3d:
+ {}
+ raw_rs_o3d:
+ {}
+ Update Interval: 0
+ Value: true
+ - Angle Tolerance: 0.15000000596046448
+ Class: rviz/Odometry
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: false
+ Enabled: true
+ Keep: 100
+ Name: Odometry
+ Position Tolerance: 0.15000000596046448
+ Queue Size: 10
+ Shape:
+ Alpha: 0.699999988079071
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Color: 252; 233; 79
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Value: Arrow
+ Topic: /msf_core/odometry
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: PointCloud2
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 1
+ Size (m): 0.009999999776482582
+ Style: Points
+ Topic: /mapping_node/assembled_map
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Angle Tolerance: 0.10000000149011612
+ Class: rviz/Odometry
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Keep: 100
+ Name: Odometry
+ Position Tolerance: 0.10000000149011612
+ Queue Size: 10
+ Shape:
+ Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Color: 255; 25; 0
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Shaft Length: 0.5
+ Shaft Radius: 0.05000000074505806
+ Value: Arrow
+ Topic: /mapping_node/scan2map_odometry
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 25; 255; 240
+ Color Transformer: FlatColor
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: PointCloud2
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Points
+ Topic: /mapping_node/raw_cloud
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Class: jsk_rviz_plugin/TFTrajectory
+ Enabled: true
+ Name: TFTrajectory
+ Value: true
+ color: 252; 233; 79
+ duration: 1000
+ frame: msf_imu_state
+ line_width: 0.009999999776482582
+ - Class: jsk_rviz_plugin/TFTrajectory
+ Enabled: true
+ Name: TFTrajectory
+ Value: true
+ color: 255; 25; 0
+ duration: 1000
+ frame: range_sensor_o3d
+ line_width: 0.009999999776482582
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 11.839879989624023
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Field of View: 0.7853981852531433
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: false
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.8203981518745422
+ Target Frame:
+ Yaw: 3.250401496887207
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1128
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd0000000400000000000001c3000003cffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003cf000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003cffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003cf000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000080000000039fc0100000002fb0000000800540069006d0065010000000000000800000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000522000003cf00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 2048
+ X: 0
+ Y: 0
diff --git a/smb_msf/setup.py b/smb_msf/setup.py
new file mode 100644
index 0000000..c6263ca
--- /dev/null
+++ b/smb_msf/setup.py
@@ -0,0 +1,9 @@
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+setup_args = generate_distutils_setup(
+ packages = ['smb_msf'],
+ package_name='smb_msf'
+)
+
+setup(**setup_args)
\ No newline at end of file
diff --git a/smb_msf/smb_msf/imu_noisifier.py b/smb_msf/smb_msf/imu_noisifier.py
new file mode 100644
index 0000000..f13cc8a
--- /dev/null
+++ b/smb_msf/smb_msf/imu_noisifier.py
@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+
+# Global
+import rospy
+import numpy as np
+from sensor_msgs.msg import Imu
+
+class PoseNoisifier:
+
+ def __init__(self):
+
+ self.imu_topic_name = "/versavis/imu"
+
+ # ROS
+ rospy.init_node("PoseNoisifierNode", anonymous=True)
+
+ # Publisher
+ self.imu_publisher = rospy.Publisher("/versavis/imu_noisified", Imu, queue_size=10)
+
+ def subscriber_callback(self, imu):
+ print("Received imu message.")
+ x_bias = 0.0
+ y_bias = 0.0
+ z_bias = 0.8
+ imu.linear_acceleration.x += x_bias
+ imu.linear_acceleration.y += y_bias
+ imu.linear_acceleration.z += z_bias
+ self.imu_publisher.publish(imu)
+
+ def start_noisifying(self):
+ print("======================")
+ print("Starting imu noisifying...")
+ print("======================")
+ rospy.Subscriber(self.imu_topic_name, Imu, self.subscriber_callback)
+ rospy.spin()
+
+if __name__ == "__main__":
+
+ pose_noisifier = PoseNoisifier()
+ pose_noisifier.start_noisifying()
\ No newline at end of file
diff --git a/smb_msf/smb_msf/plotting_node.py b/smb_msf/smb_msf/plotting_node.py
new file mode 100644
index 0000000..f60273f
--- /dev/null
+++ b/smb_msf/smb_msf/plotting_node.py
@@ -0,0 +1,149 @@
+#!/usr/bin/env python3
+
+# Global
+import numpy as np
+import matplotlib.pyplot as plt
+import rospy
+import os
+import time
+
+# Custom
+from msf_core.msg import DoubleArrayStamped
+
+experiment_name = ""
+
+
+def plot_2d(x_coords: list, y_coords: list, x_axis_name: str, y_axis_name: str, plot_name: str):
+ # Check length
+ print("Plot: " + plot_name + ". Length of " + x_axis_name + ": " + str(len(x_coords)) + ". Length of " + y_axis_name + ": " + str(len(y_coords)))
+ assert np.abs(len(x_coords) - len(y_coords)) <= 1
+ if len(x_coords) - len(y_coords) == 1:
+ x_coords.pop() # remove last element
+ elif len(y_coords) - len(x_coords) == 1:
+ y_coords.pop()
+ # Home-dir
+ home_dir = os.path.expanduser('~')
+ # Plot
+ plt.rcParams['lines.markersize'] = 0.5
+ plt.rcParams["figure.figsize"] = (6,6)
+ plt.xticks(fontsize=24)
+ plt.yticks(fontsize=24)
+ fig, ax = plt.subplots(1,1)
+ ax.set(xlabel=x_axis_name, ylabel=y_axis_name, title=plot_name)
+ ax.grid()
+ plt.tight_layout()
+ ax.plot(x_coords, y_coords, c="green", linewidth=0.7, alpha=1.0)
+ fig.savefig(os.path.join(home_dir, "rss_plots", experiment_name + "_" + plot_name + ".png"))
+
+
+class MsfPlotter:
+
+ def __init__(self):
+
+ self.state_topic_name = "/msf_core/state_out"
+
+ # ROS
+ rospy.init_node('MSF Publisher Node', anonymous=True)
+
+ # Create dir
+ home_dir = os.path.expanduser('~')
+ if not os.path.exists(os.path.join(home_dir, "rss_plots")):
+ os.makedirs(os.path.join(home_dir, "rss_plots"))
+
+ # Stored messages
+ ## Time
+ self.initial_time = []
+ self.time = []
+ ## Position
+ self.x_pos = []
+ self.y_pos = []
+ self.z_pos = []
+ ## Velocity
+ self.x_vel = []
+ self.y_vel = []
+ ## Acc Bias
+ self.x_acc_bias = []
+ self.y_acc_bias = []
+ self.z_acc_bias = []
+
+ def plot(self):
+
+ # Plot diagrams when destructor is called
+ print("Saving plots...")
+ ## x-y-plot
+ plot_name = "position-x-y"
+ print(plot_name)
+ x_axis_name = "x[m]"
+ y_axis_name = "y[m]"
+ plot_2d(x_coords=self.x_pos, y_coords=self.y_pos, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
+ ## z-plot
+ plot_name = "position-z"
+ print(plot_name)
+ x_axis_name = "t[s]"
+ y_axis_name = "z[m]"
+ plot_2d(x_coords=self.time, y_coords=self.z_pos, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
+ ## x-velocity
+ plot_name = "velocity-x"
+ print(plot_name)
+ x_axis_name = "t[s]"
+ y_axis_name = "v[m/s]"
+ plot_2d(x_coords=self.time, y_coords=self.x_vel, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
+ ## y-velocity
+ plot_name = "velocity-y"
+ print(plot_name)
+ x_axis_name = "t[s]"
+ y_axis_name = "v[m/s]"
+ plot_2d(x_coords=self.time, y_coords=self.y_vel, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
+ ## x-Bias
+ plot_name = "acc-bias-x"
+ print(plot_name)
+ x_axis_name = "t[s]"
+ y_axis_name = "b[m/s^2]"
+ plot_2d(x_coords=self.time, y_coords=self.x_acc_bias, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
+ ## x-Bias
+ plot_name = "acc-bias-y"
+ print(plot_name)
+ x_axis_name = "t[s]"
+ y_axis_name = "b[m/s^2]"
+ plot_2d(x_coords=self.time, y_coords=self.y_acc_bias, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
+ ## x-Bias
+ plot_name = "acc-bias-z"
+ print(plot_name)
+ x_axis_name = "t[s]"
+ y_axis_name = "b[m/s^2]"
+ plot_2d(x_coords=self.time, y_coords=self.z_acc_bias, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
+
+
+ def subscriber_callback(self, state_estimate):
+ print("Received state message.")
+ # Time
+ if not self.initial_time:
+ self.initial_time = state_estimate.header.stamp.to_sec()
+ self.time = self.time + [state_estimate.header.stamp.to_sec() - self.initial_time]
+ # Position
+ self.x_pos = self.x_pos + [state_estimate.data[0]]
+ self.y_pos = self.y_pos + [state_estimate.data[1]]
+ self.z_pos = self.z_pos + [state_estimate.data[2]]
+ # Velocity
+ self.x_vel = self.x_vel + [state_estimate.data[3]]
+ self.y_vel = self.y_vel + [state_estimate.data[4]]
+ # Acc. Bias
+ self.x_acc_bias = self.x_acc_bias + [state_estimate.data[13]]
+ self.y_acc_bias = self.y_acc_bias + [state_estimate.data[14]]
+ self.z_acc_bias = self.z_acc_bias + [state_estimate.data[15]]
+
+ def start_plotting(self):
+ print("======================")
+ print("Starting plotting...")
+ print("======================")
+ rospy.Subscriber(self.state_topic_name, DoubleArrayStamped,
+ self.subscriber_callback)
+
+ rospy.spin()
+ self.plot()
+
+if __name__ == "__main__":
+ time.sleep(1.0)
+ experiment_name = input("Enter experiment name (used for file naming):")
+ msf_plotter = MsfPlotter()
+ msf_plotter.start_plotting()
\ No newline at end of file
diff --git a/smb_msf/smb_msf/pose_noisifier.py b/smb_msf/smb_msf/pose_noisifier.py
new file mode 100644
index 0000000..a224ff7
--- /dev/null
+++ b/smb_msf/smb_msf/pose_noisifier.py
@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+
+# Global
+import rospy
+import numpy as np
+from geometry_msgs.msg import TransformStamped
+
+class PoseNoisifier:
+
+ def __init__(self):
+
+ self.transform_topic_name = "/mapping_node/scan2map_transform"
+
+ # ROS
+ rospy.init_node("PoseNoisifierNode", anonymous=True)
+
+ # Publisher
+ self.transform_publisher = rospy.Publisher("/mapping_node/scan2map_transform_noisified", TransformStamped, queue_size=10)
+
+ def subscriber_callback(self, transform):
+ print("Received transform message.")
+ gaussian_noise = np.random.normal(0.0, 0.1, size=3)
+ transform.transform.translation.x += gaussian_noise[0]
+ transform.transform.translation.y += gaussian_noise[1]
+ transform.transform.translation.z += gaussian_noise[2]
+ self.transform_publisher.publish(transform)
+
+ def start_noisifying(self):
+ print("======================")
+ print("Starting pose noisifying...")
+ print("======================")
+ rospy.Subscriber(self.transform_topic_name, TransformStamped, self.subscriber_callback)
+ rospy.spin()
+
+if __name__ == "__main__":
+
+ pose_noisifier = PoseNoisifier()
+ pose_noisifier.start_noisifying()
\ No newline at end of file
diff --git a/smb_slam/config/localization/param_with_camera_odometry.yaml b/smb_slam/config/localization/param_with_camera_odometry.yaml
index 2119bbe..6480535 100644
--- a/smb_slam/config/localization/param_with_camera_odometry.yaml
+++ b/smb_slam/config/localization/param_with_camera_odometry.yaml
@@ -14,8 +14,8 @@ icp_localization:
is_provide_odom_frame: true
gravity_vector_filter_time_constant: 0.01
fixed_frame: "map"
- min_num_odom_msgs_before_ready: 300
-
+ min_num_odom_msgs_before_ready: 20
+ use_odometry_after_num_scans: 0 # wait for this amount of time until odometry is used --> sometimes external odometry requires time to converge
calibration:
imu_to_range_sensor:
diff --git a/smb_slam/config/localization/param_with_lidar_odometry.yaml b/smb_slam/config/localization/param_with_lidar_odometry.yaml
index 2f770fa..848611c 100644
--- a/smb_slam/config/localization/param_with_lidar_odometry.yaml
+++ b/smb_slam/config/localization/param_with_lidar_odometry.yaml
@@ -14,7 +14,8 @@ icp_localization:
is_provide_odom_frame: true
gravity_vector_filter_time_constant: 0.01
fixed_frame: "map"
- min_num_odom_msgs_before_ready: 300
+ min_num_odom_msgs_before_ready: 20
+ use_odometry_after_num_scans: 0 # wait for this amount of time until odometry is used --> sometimes external odometry requires time to converge
calibration:
diff --git a/smb_slam/config/open3d_slam/params_robosense_rs16.yaml b/smb_slam/config/open3d_slam/params_robosense_rs16.yaml
index f370a6b..06c6a9a 100644
--- a/smb_slam/config/open3d_slam/params_robosense_rs16.yaml
+++ b/smb_slam/config/open3d_slam/params_robosense_rs16.yaml
@@ -1,4 +1,5 @@
odometry:
+ is_publish_odometry_msgs: true
scan_matching:
icp_objective: PointToPlane
max_correspondence_dist: 1.0
diff --git a/smb_slam/launch/localization.launch b/smb_slam/launch/localization.launch
index f5b66a8..4c12d02 100644
--- a/smb_slam/launch/localization.launch
+++ b/smb_slam/launch/localization.launch
@@ -11,7 +11,7 @@
-
+
@@ -19,12 +19,6 @@
-
-
-
-
-
-