diff --git a/.gitignore b/.gitignore
deleted file mode 100644
index 85c6b68e9eaccb40a1f8ba91b3a77074f1734fe8..0000000000000000000000000000000000000000
--- a/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-bag/*
diff --git a/bag/test_no_map.bag b/bag/demo_flat_interior.bag
similarity index 100%
rename from bag/test_no_map.bag
rename to bag/demo_flat_interior.bag
diff --git a/launch/demo_flat_interior.launch b/launch/demo_flat_interior.launch
new file mode 100644
index 0000000000000000000000000000000000000000..47115193abb4e4cd00dfefac7ce62a1452f4c7d9
--- /dev/null
+++ b/launch/demo_flat_interior.launch
@@ -0,0 +1,47 @@
+<!-- -->
+<launch>
+ <param name="use_sim_time" value="true" />
+ <arg name="speed" default="1" />
+ <arg name="sec" default="0" />
+ <arg name="profiling" default="false" />
+ <arg name="gdb" default="false" />
+ <arg name="launch_pref" default="" unless="$(eval profiling or gdb)"/>
+ <arg name="launch_pref" value="valgrind --tool=callgrind --callgrind-out-file='callgrind.wolf.%p'" if="$(arg profiling)" />
+ <arg name="launch_pref" value="gdb -ex run --args" if="$(arg gdb)" />
+ <!-- using "clock" option to use the simulated time. Rosbag launch as the first node -->
+
+ <node pkg="rostopic" type="rostopic" name="" args='pub /ana/sensors/bno055_imu/imu/topic_enable std_msgs/Bool "data: True"'/>
+
+ <node pkg="tf"
+ type="static_transform_publisher"
+ name="static_tf"
+ args="0 0 0 0 0 0 /ana/base_footprint /ana/base_link 100"/>
+
+ <node pkg="tf"
+ type="static_transform_publisher"
+ name="static_tf2"
+ args="0 0 0 3.14159265 0 0 /ana/base_link /ana/velodyne 100"/>
+
+ <node pkg="tf"
+ type="static_transform_publisher"
+ name="static_tf3"
+ args="0 0 0 0 0 0 /ana/base_link /ana/imu_bno055 100"/>
+
+ <node pkg="rosbag"
+ type="play"
+ name="player"
+ args="-s $(arg sec) -r $(arg speed) -k --clock $(find wolf_demo_imu2d)/bag/demo_flat_interior.bag"/>
+
+ <node type="rviz"
+ name="rviz"
+ pkg="rviz"
+ args="-d $(find wolf_demo_imu2d)/rviz/demo_flat_interior.rviz"/>
+ <node type="wolf_ros_node"
+ name="wolf_ros_node"
+ pkg="wolf_ros_node"
+ required="true"
+ output="screen"
+ launch-prefix="$(arg launch_pref)">
+ <param name="~yaml_file_path" value="$(find wolf_demo_imu2d)/yaml/Demo/demo_flat_interior.yaml" />
+ </node>
+</launch>
diff --git a/rviz/demo_flat_interior.rviz b/rviz/demo_flat_interior.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..478dd15e4cf33d6ad6c8d1d5aed17b12739ebef6
--- /dev/null
+++ b/rviz/demo_flat_interior.rviz
@@ -0,0 +1,377 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /TF1
+ - /TF1/Frames1
+ - /TF1/Tree1
+ - /Odometry1/Shape1
+ - /LaserScan1
+ - /Landmarks1
+ - /Factors1
+ - /Factors1/Namespaces1
+ - /Trajectory1
+ - /Axes1
+ - /PointCloud21
+ - /Imu1
+ Splitter Ratio: 0.5285326242446899
+ Tree Height: 728
+ - 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: 5
+ 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: 100
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: false
+ ana/base_footprint:
+ Value: true
+ ana/base_footprint_robot:
+ Value: false
+ ana/base_link:
+ Value: true
+ ana/front_left_axle:
+ Value: false
+ ana/front_right_axle:
+ Value: false
+ ana/imu_bno055:
+ Value: true
+ ana/odom:
+ Value: true
+ ana/rear_left_axle:
+ Value: false
+ ana/rear_right_axle:
+ Value: false
+ ana/velodyne:
+ Value: false
+ map:
+ Value: false
+ Marker Scale: 2
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ map:
+ ana/odom:
+ ana/base_footprint:
+ ana/base_link:
+ ana/front_left_axle:
+ {}
+ ana/front_right_axle:
+ {}
+ ana/imu_bno055:
+ {}
+ ana/rear_left_axle:
+ {}
+ ana/rear_right_axle:
+ {}
+ ana/velodyne:
+ {}
+ ana/base_footprint_robot:
+ {}
+ Update Interval: 0
+ 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: false
+ Enabled: false
+ Keep: 300
+ Name: Odometry
+ Position Tolerance: 0.10000000149011612
+ Shape:
+ Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Color: 255; 25; 0
+ Head Length: 0.10000000149011612
+ Head Radius: 0.10000000149011612
+ Shaft Length: 0.30000001192092896
+ Shaft Radius: 0.05000000074505806
+ Value: Arrow
+ Topic: /ana/odom
+ Unreliable: false
+ Value: false
+ - Alpha: 0.5
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensities
+ Class: rviz/LaserScan
+ Color: 250; 84; 255
+ Color Transformer: FlatColor
+ Decay Time: 100
+ Enabled: false
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: LaserScan
+ Position Transformer: XYZ
+ Queue Size: 1
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.05999999865889549
+ Style: Flat Squares
+ Topic: /ana/sensors/scan
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: false
+ - Class: rviz/MarkerArray
+ Enabled: false
+ Marker Topic: /wolf_ros_node/graph_landmarks
+ Name: Landmarks
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: false
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /wolf_ros_node/graph_factors
+ Name: Factors
+ Namespaces:
+ factors_processorimu2dbno: true
+ factors_processorodom2d: false
+ factors_processorodomicp: false
+ factors_text_processorimu2dbno: true
+ factors_text_processorodom2d: false
+ factors_text_processorodomicp: false
+ factors_text_unnamed_processor: false
+ factors_unnamed_processor: false
+ Queue Size: 100
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /wolf_ros_node/graph_trajectory
+ Name: Trajectory
+ Namespaces:
+ frames: false
+ frames_text: false
+ Queue Size: 100
+ Value: true
+ - Class: rviz/Axes
+ Enabled: true
+ Length: 1
+ Name: Axes
+ Radius: 0.10000000149011612
+ Reference Frame: ana/base_link
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 0
+ Min Value: 0
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 0; 170; 0
+ Color Transformer: RGB8
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 173; 14
+ Name: PointCloud2
+ Position Transformer: XYZ
+ Queue Size: 1
+ Selectable: false
+ Size (Pixels): 3
+ Size (m): 0.03999999910593033
+ Style: Squares
+ Topic: /wolf_ros_node/map_pointcloud
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 0.699999988079071
+ Class: rviz/Map
+ Color Scheme: map
+ Draw Behind: false
+ Enabled: false
+ Name: Map
+ Topic: /wolf_ros_node/map_occgrid
+ Unreliable: false
+ Use Timestamp: false
+ Value: false
+ - Alpha: 0.5
+ Class: rviz_plugin_tutorials/Imu
+ Color: 52; 101; 164
+ Enabled: true
+ History Length: 1
+ Name: Imu
+ Topic: /ana/sensors/bno055_imu/imu_remapped
+ Unreliable: true
+ Value: true
+ 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: 17.816146850585938
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0.5597602128982544
+ Y: -0.7654535174369812
+ Z: 0.7935692667961121
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.7197968363761902
+ Target Frame: map
+ Value: Orbit (rviz)
+ Yaw: 3.878161668777466
+ Saved:
+ - Angle: 0
+ Class: rviz/TopDownOrtho
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Invert Z Axis: false
+ Name: TopDownOrtho
+ Near Clip Distance: 0.009999999776482582
+ Scale: 64.65961456298828
+ Target Frame: map
+ Value: TopDownOrtho (rviz)
+ X: 8.825014114379883
+ Y: -0.17566636204719543
+ - Angle: 0
+ Class: rviz/TopDownOrtho
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Invert Z Axis: false
+ Name: TopDownOrtho
+ Near Clip Distance: 0.009999999776482582
+ Scale: 50.07240295410156
+ Target Frame: map
+ Value: TopDownOrtho (rviz)
+ X: 1.947830080986023
+ Y: 3.9687037467956543
+ - Angle: -1.5700000524520874
+ Class: rviz/TopDownOrtho
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Invert Z Axis: false
+ Name: TopDownOrtho
+ Near Clip Distance: 0.009999999776482582
+ Scale: 42.187442779541016
+ Target Frame: map
+ Value: TopDownOrtho (rviz)
+ X: 1.9746785163879395
+ Y: 4.48727560043335
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1025
+ Hide Left Dock: false
+ Hide Right Dock: true
+ QMainWindow State: 000000ff00000000fd0000000400000000000002e200000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004550000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1853
+ X: 67
+ Y: 27
diff --git a/yaml/Demo/csm_inactive.yaml b/yaml/Demo/csm_inactive.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b5336a90b5dad444b2bfd9d946582ab8d987cfc1
--- /dev/null
+++ b/yaml/Demo/csm_inactive.yaml
@@ -0,0 +1,47 @@
+verbose: false # prints debug messages
+
+# ALGORITHM OPTIONS
+use_point_to_line_distance: true # use PlICP (true) or use vanilla ICP (false).
+max_angular_correction_deg: 180 # Maximum angular displacement between scans (deg)
+max_linear_correction: 10 # Maximum translation between scans (m)
+
+max_correspondence_dist: 0.5 # Maximum distance for a correspondence to be valid
+use_corr_tricks: true # Use smart tricks for finding correspondences. Only influences speed; not convergence.
+debug_verify_tricks: false # Checks that find_correspondences_tricks give the right answer
+
+# STOP CRITERIA
+max_iterations: 50 # maximum iterations
+epsilon_xy: 1e-4 # distance change
+epsilon_theta: 1e-3 # angle change
+
+# RESTART
+restart: false # Restart algorithm
+restart_threshold_mean_error: 0 # Threshold for restarting
+restart_dt: 0 # Displacement for restarting
+restart_dtheta: 0 # Displacement for restarting
+
+# DISCARDING POINTS/CORRESPONDENCES
+min_reading: 0.023 # discard rays outside of this interval
+max_reading: 60 # discard rays outside of this interval
+outliers_maxPerc: 0.9 # Percentage of correspondences to consider: if 0.9, always discard the top 10% of correspondences with more error
+outliers_adaptive_order: 0.7 # Parameters describing a simple adaptive algorithm for discarding
+outliers_adaptive_mult: 1.5 # Parameters describing a simple adaptive algorithm for discarding
+outliers_remove_doubles: false # Do not allow two different correspondences to share a point
+do_visibility_test: false # If initial guess, visibility test can discard points that are not visible
+do_alpha_test: false # Discard correspondences based on the angles
+do_alpha_test_thresholdDeg: 10 #
+
+# POINT ORIENTATION
+clustering_threshold: 0.5 # Max-distance clustering for point orientation
+orientation_neighbourhood: 4 # Number of neighbour rays used to estimate the orientation
+
+# WEIGHTS
+use_ml_weights: false # weight the impact of each correspondence. This works fabolously if the first scan has no noise.
+use_sigma_weights: false # If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2
+sigma: 0.2 # Noise of the scan
+
+# COVARIANCE
+do_compute_covariance: true
+cov_factor: 5 # Factor multiplying the cov output of csm
+
+cov_factor: 9999999999999999999999
diff --git a/yaml/Demo/demo_flat_interior.yaml b/yaml/Demo/demo_flat_interior.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d355a7226671a6eb8e3eefca53a66a727a6717e5
--- /dev/null
+++ b/yaml/Demo/demo_flat_interior.yaml
@@ -0,0 +1,114 @@
+config:
+ debug:
+ profiling: true
+ profiling_file: "~/wolf_demo_flat_interior.txt"
+ print_problem: true
+ print_period: 2
+ print_depth: 4
+ print_constr_by: false
+ print_metric: true
+ print_state_blocks: true
+ problem:
+ tree_manager:
+ type: "none"
+ frame_structure: "POV"
+ dimension: 2
+ prior:
+ mode: "fix"
+ $state:
+ P: [0,0]
+ O: [0]
+ V: [0,0]
+ time_tolerance: 0.1
+ node_rate: 100
+ map:
+ type: "MapBase"
+ plugin: "core"
+
+ solver:
+ follow: "../solver.yaml"
+
+ sensors:
+ -
+ type: "SensorOdom2d"
+ name: "odom2d"
+ plugin: "core"
+ follow: "../test_odom2d_params.yaml"
+ -
+ type: "SensorLaser2d"
+ name: "scanner_front_left"
+ plugin: "laser"
+ follow: "demo_params_sensor_laser.yaml"
+ -
+ type: "SensorImu2d"
+ name: "bno"
+ plugin: "imu"
+ follow: "demo_params_sensor_imu.yaml"
+
+ processors:
+ -
+ type: "ProcessorOdom2d"
+ name: "processorodom2d"
+ sensor_name: "odom2d"
+ plugin: "core"
+ follow: "../test_odom2d_processor.yaml"
+ -
+ type: "ProcessorOdomIcp"
+ name: "processorodomicp"
+ sensor_name: "scanner_front_left"
+ plugin: "laser"
+ follow: "demo_params_processor_laser.yaml"
+ -
+ type: "ProcessorImu2d"
+ name: "processorimu2dbno"
+ sensor_name: "bno"
+ plugin: "imu"
+ follow: "demo_params_processor_imu.yaml"
+
+ ROS subscriber:
+ -
+ package: "wolf_ros_node"
+ type: "SubscriberOdom2d"
+ topic: "/ana/odom"
+ sensor_name: "odom2d"
+ -
+ package: "wolf_ros_laser"
+ type: "SubscriberLaser2d"
+ topic: "/ana/sensors/scan"
+ sensor_name: "scanner_front_left"
+ load_params_from_msg: true
+ -
+ package: "wolf_ros_imu"
+ type: "SubscriberImuEnableable"
+ topic: "/ana/sensors/bno055_imu/imu"
+ sensor_name: "bno"
+ follow: "demo_subscriber_imu.yaml"
+
+ ROS publisher:
+ -
+ package: "wolf_ros_node"
+ type: "PublisherTf"
+ topic: " "
+ period: 0.1
+ follow: "demo_publisher_ros_node.yaml"
+ -
+ package: "wolf_ros_node"
+ type: "PublisherGraph"
+ topic: "graph"
+ period: 1
+ viz_overlapped_factors: true
+ -
+ package: "wolf_ros_laser"
+ type: "PublisherLaserMap"
+ topic: "map"
+ period: 1
+ map_frame_id: "map"
+ update_dist_th: 0.05
+ update_angle_th: 0.05
+ max_n_cells: 1000000
+ grid_size: 0.1
+ p_free: 0.3
+ p_obst: 0.8
+ p_free_th: 0.2
+ p_obst_th: 0.9
+ discard_max_range: true
diff --git a/yaml/Demo/demo_params_processor_imu.yaml b/yaml/Demo/demo_params_processor_imu.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..79f6f5001b813bf2d55f53b424359930cb160594
--- /dev/null
+++ b/yaml/Demo/demo_params_processor_imu.yaml
@@ -0,0 +1,12 @@
+time_tolerance: 0.008 #Half the sample time, bno
+unmeasured_perturbation_std: 0.1
+keyframe_vote:
+ voting_active: false
+ max_time_span: 1
+ max_buff_length: 1000000
+ dist_traveled: 10
+ angle_turned: 3.14
+state_getter: true
+state_priority: 1
+apply_loss_function: false
+use_gravity_grid: false
diff --git a/yaml/Demo/demo_params_processor_laser.yaml b/yaml/Demo/demo_params_processor_laser.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5cd6782aad42cc8810a1f09914af033ab9325b29
--- /dev/null
+++ b/yaml/Demo/demo_params_processor_laser.yaml
@@ -0,0 +1,16 @@
+keyframe_vote:
+ voting_active: true
+ min_features_for_keyframe: 10
+ min_dist: 999
+ min_angle: 999
+ min_time: 2
+ min_error: 999
+ max_points: 0
+max_new_features: 15
+icp:
+ follow: "csm_inactive.yaml" #Inactive laser processor, only for visualization purposes
+time_tolerance: 0.05 #Half the sample time
+state_getter: true
+state_priority: 10
+apply_loss_function: false
+initial_guess: "zero"
diff --git a/yaml/Demo/demo_params_sensor_imu.yaml b/yaml/Demo/demo_params_sensor_imu.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0f58a9cdb59a555277e82db664e4bf659e082195
--- /dev/null
+++ b/yaml/Demo/demo_params_sensor_imu.yaml
@@ -0,0 +1,10 @@
+#bno
+extrinsic:
+ pose: [0,0,0]
+a_noise: 0.096 #best: 0.96
+w_noise: 0.02
+ab_initial_stdev: 5 # m/s2 - initial bias
+wb_initial_stdev: 1 # rad/sec - initial bias
+ab_rate_stdev: 0.0001 # m/s2/sqrt(s)
+wb_rate_stdev: 0.0001 # rad/s/sqrt(s)
+orthogonal_gravity: false
diff --git a/yaml/Demo/demo_params_sensor_laser.yaml b/yaml/Demo/demo_params_sensor_laser.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0fb41c67ad15463c536ae63b6744979b1ea5ef90
--- /dev/null
+++ b/yaml/Demo/demo_params_sensor_laser.yaml
@@ -0,0 +1,11 @@
+extrinsic:
+ pose: [0,0,3.1415926535] #Last value was pi
+LaserScanParams:
+ angle_min: -3.14159274101
+ angle_max: 3.14159274101
+ angle_step: 0.007 #0.00700000021607
+ scan_time: 0.0
+ range_min: 0.0
+ range_max: 200
+ range_std_dev: 0.2
+ angle_std_dev: 0.01
diff --git a/yaml/Demo/demo_publisher_ros_node.yaml b/yaml/Demo/demo_publisher_ros_node.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a3e37459266db719cbea053e906bc686d902fec3
--- /dev/null
+++ b/yaml/Demo/demo_publisher_ros_node.yaml
@@ -0,0 +1,4 @@
+map_frame_id: "map"
+odom_frame_id: "ana/odom"
+base_frame_id: "ana/base_footprint"
+publish_odom_tf: false
diff --git a/yaml/Demo/demo_subscriber_imu.yaml b/yaml/Demo/demo_subscriber_imu.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6ea8aed399a1915503eea69f5d39fc6a81d0d73c
--- /dev/null
+++ b/yaml/Demo/demo_subscriber_imu.yaml
@@ -0,0 +1,11 @@
+#bno:
+dt: 0.016
+imu_x_axis: 3
+imu_y_axis: -2
+imu_z_axis: 1
+cov_source: "sensor"
+in_degrees: true
+topic_enable: "/ana/sensors/bno055_imu/imu/topic_enable"
+static_init_duration: 8
+lowpass_filter: true
+lowpass_cutoff_freq: 5