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 @@ - - - - - -