diff --git a/config/params.yaml b/config/params.yaml
index 0f975fecc8b1e47ebb93822639ba92306748269f..ed3ddcc17203a1a300c359c5c733866c01cf7c87 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -1,5 +1,5 @@
rate: 240
-fixed_frame: imu_bno055
+fixed_frame: base_link
tf_timeout: 0.2
err_msg_rate: 0.5
collision_transition_counter_en: True
diff --git a/launch/collsions.launch b/launch/collisions.launch
similarity index 96%
rename from launch/collsions.launch
rename to launch/collisions.launch
index c4c2f6e778e3e7fc186c6b5f4c5433d37b7093c4..423ae2f56fa089299ce6cc06bd30334961834ff2 100644
--- a/launch/collsions.launch
+++ b/launch/collisions.launch
@@ -25,8 +25,8 @@
<!-- Collision manager parameters -->
<arg name="collision_config_file" default="$(find iri_collision_manager)/config/params.yaml"/>
- <arg name="front_laser_topic" default="/front_laser_scan"/>
- <arg name="rear_laser_topic" default="/rear_laser_scan"/>
+ <arg name="front_laser_topic" default="/front_laser/scan"/>
+ <arg name="rear_laser_topic" default="/rear_laser/scan"/>
<include file="$(find iri_bno055_imu_bringup)/launch/bno055_imu.launch"
if="$(arg imu_driver)">
diff --git a/rviz/dogs.rviz b/rviz/dogs.rviz
index e93389a4bdef6e7f6b81fc16a3dec81832f0627f..9823c0dfc1e596bc1bd0206a30c8a2096258bd23 100644
--- a/rviz/dogs.rviz
+++ b/rviz/dogs.rviz
@@ -6,7 +6,11 @@ Panels:
Expanded:
- /Global Options1
- /Status1
- Splitter Ratio: 0.5058823823928833
+ - /TF1
+ - /LandmarksRobotPose1/Covariance1
+ - /LandmarksRobotPose1/Covariance1/Position1
+ - /LandmarksRobotPose1/Covariance1/Orientation1
+ Splitter Ratio: 0.5
Tree Height: 728
- Class: rviz/Selection
Name: Selection
@@ -26,7 +30,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
- SyncSource: LaserScan
+ SyncSource: ""
Preferences:
PromptSaveOnExit: true
Toolbars:
@@ -34,6 +38,30 @@ Toolbars:
Visualization Manager:
Class: ""
Displays:
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ {}
+ Update Interval: 0
+ Value: true
+ - Alpha: 0.699999988079071
+ Class: rviz/Map
+ Color Scheme: map
+ Draw Behind: false
+ Enabled: true
+ Name: Map
+ Topic: /map
+ Unreliable: false
+ Use Timestamp: false
+ Value: true
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
@@ -52,134 +80,118 @@ Visualization Manager:
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- - Class: rviz/TF
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/LaserScan
+ Color: 239; 41; 41
+ Color Transformer: FlatColor
+ Decay Time: 0
Enabled: true
- Frame Timeout: 15
- Frames:
- All Enabled: false
- helena/base_footprint:
- Value: false
- helena/base_link:
- Value: true
- helena/camera_bottom_screw_frame:
- Value: false
- helena/camera_color_frame:
- Value: false
- helena/camera_color_optical_frame:
- Value: false
- helena/camera_depth_frame:
- Value: false
- helena/camera_depth_optical_frame:
- Value: false
- helena/camera_infra1_frame:
- Value: false
- helena/camera_infra1_optical_frame:
- Value: false
- helena/camera_infra2_frame:
- Value: false
- helena/camera_infra2_optical_frame:
- Value: false
- helena/camera_link:
- Value: false
- helena/front_left_axle:
- Value: false
- helena/front_left_hub:
- Value: false
- helena/front_left_wheel:
- Value: false
- helena/front_right_axle:
- Value: false
- helena/front_right_hub:
- Value: false
- helena/front_right_wheel:
- Value: false
- helena/front_sonar:
- Value: false
- helena/helena_box:
- Value: false
- helena/helena_realsense_support:
- Value: false
- helena/imu_bno055:
- Value: true
- helena/imu_bno055_base:
- Value: false
- helena/odom:
- Value: true
- helena/rear_left_axle:
- Value: false
- helena/rear_left_hub:
- Value: false
- helena/rear_left_wheel:
- Value: false
- helena/rear_right_axle:
- Value: false
- helena/rear_right_hub:
- Value: false
- helena/rear_right_wheel:
- Value: false
- helena/rear_sonar:
- Value: false
- helena/robosense:
- Value: false
- helena/robosense_base:
- Value: false
- helena/top_plate:
- Value: false
- Marker Scale: 1
- Name: TF
- Show Arrows: true
- Show Axes: true
- Show Names: true
- Tree:
- helena/odom:
- helena/base_footprint:
- helena/base_link:
- helena/front_left_axle:
- helena/front_left_hub:
- helena/front_left_wheel:
- {}
- helena/front_right_axle:
- helena/front_right_hub:
- helena/front_right_wheel:
- {}
- helena/front_sonar:
- {}
- helena/rear_left_axle:
- helena/rear_left_hub:
- helena/rear_left_wheel:
- {}
- helena/rear_right_axle:
- helena/rear_right_hub:
- helena/rear_right_wheel:
- {}
- helena/rear_sonar:
- {}
- helena/top_plate:
- helena/helena_box:
- {}
- helena/helena_realsense_support:
- helena/camera_bottom_screw_frame:
- helena/camera_link:
- helena/camera_color_frame:
- helena/camera_color_optical_frame:
- {}
- helena/camera_depth_frame:
- helena/camera_depth_optical_frame:
- {}
- helena/camera_infra1_frame:
- helena/camera_infra1_optical_frame:
- {}
- helena/camera_infra2_frame:
- helena/camera_infra2_optical_frame:
- {}
- helena/robosense_base:
- helena/robosense:
- {}
- helena/imu_bno055_base:
- helena/imu_bno055:
- {}
- Update Interval: 0
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: FrontLaser
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Points
+ Topic: /laser_front/scan
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
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/LaserScan
+ Color: 252; 233; 79
+ Color Transformer: FlatColor
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: RearLaser
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Points
+ Topic: /laser_rear/scan
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ 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/LaserScan
+ Color: 115; 210; 22
+ Color Transformer: FlatColor
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: GlobalScan
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 4
+ Size (m): 0.009999999776482582
+ Style: Points
+ Topic: /scan_global
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ 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: FlatColor
+ Decay Time: 0
+ Enabled: false
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: GlobalScanPCL
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Points
+ Topic: /gobal_cloud
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: false
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
@@ -197,10 +209,10 @@ Visualization Manager:
Scale: 1
Value: true
Value: true
- Enabled: true
+ Enabled: false
Keep: 100
Name: Odometry
- Position Tolerance: 0.10000000149011612
+ Position Tolerance: 0.5
Shape:
Alpha: 1
Axes Length: 1
@@ -211,51 +223,169 @@ Visualization Manager:
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Arrow
- Topic: /helena/odom
+ Topic: /odom
+ Unreliable: false
+ Value: false
+ - Alpha: 0.5
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Class: rviz/PoseWithCovariance
+ Color: 252; 233; 79
+ 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
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Name: amclPose
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Shape: Arrow
+ Topic: /amcl_pose
Unreliable: false
Value: true
- Alpha: 1
- Class: rviz_plugin_tutorials/Imu
- Color: 204; 51; 204
+ Arrow Length: 0.30000001192092896
+ Axes Length: 0.30000001192092896
+ Axes Radius: 0.009999999776482582
+ Class: rviz/PoseArray
+ Color: 255; 25; 0
Enabled: false
- History Length: 1
- Name: Imu
- Topic: /helena/sensors/bno055_imu/imu
+ Head Length: 0.07000000029802322
+ Head Radius: 0.029999999329447746
+ Name: PoseArray
+ Shaft Length: 0.23000000417232513
+ Shaft Radius: 0.009999999776482582
+ Shape: Arrow (Flat)
+ Topic: /particlecloud
Unreliable: false
Value: false
- - Alpha: 1
- Autocompute Intensity Bounds: true
- Autocompute Value Bounds:
- Max Value: 10
- Min Value: -10
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /rviz_markers
+ Name: MarkerArray
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /iri_landmarks_tracker/local_features
+ Name: Features
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /iri_landmarks_tracker/global_features
+ Name: landmarks
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /iri_landmarks_tracker/frame_data
+ Name: Frames
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Alpha: 0.5
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Class: rviz/PoseWithCovariance
+ Color: 255; 25; 0
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.5
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
Value: true
- Axis: Z
- Channel Name: intensity
- Class: rviz/LaserScan
- Color: 138; 226; 52
- Color Transformer: FlatColor
- Decay Time: 0
Enabled: true
- Invert Rainbow: false
- Max Color: 255; 255; 255
- Min Color: 0; 0; 0
- Name: LaserScan
- Position Transformer: XYZ
- Queue Size: 10
- Selectable: true
- Size (Pixels): 3
- Size (m): 0.009999999776482582
- Style: Points
- Topic: /helena/sensors/scan
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Name: LandmarksRobotPose
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Shape: Arrow
+ Topic: /iri_landmarks_tracker/landmarks_localization_pose
+ Unreliable: false
+ Value: true
+ - Alpha: 0.5
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Class: rviz/PoseWithCovariance
+ Color: 78; 154; 6
+ 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
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Name: FusedPose
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Shape: Arrow
+ Topic: /iri_localization_fusion/fused_pose
+ Unreliable: false
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /iri_localization_fusion/fusion_traj_vis
+ Name: Trajectories
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Alpha: 1
+ Class: rviz_plugin_tutorials/Imu
+ Color: 204; 51; 204
+ Enabled: true
+ History Length: 1
+ Name: Imu
+ Topic: /bno055_imu/imu
Unreliable: false
- Use Fixed Frame: true
- Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
- Fixed Frame: helena/odom
+ Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
@@ -278,34 +408,29 @@ Visualization Manager:
Value: true
Views:
Current:
- Class: rviz/Orbit
- Distance: 25.189504623413086
+ Angle: 0
+ Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
- Focal Point:
- X: 1.3301063776016235
- Y: -0.2307669073343277
- Z: -2.2700793743133545
- Focal Shape Fixed Size: true
- Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 1.5697963237762451
- Target Frame: <Fixed Frame>
- Value: Orbit (rviz)
- Yaw: 1.5953969955444336
+ Scale: 48.28645324707031
+ Target Frame: base_link
+ Value: TopDownOrtho (rviz)
+ X: 0.663458526134491
+ Y: -0.6147326827049255
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1025
Hide Left Dock: false
- Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004cc0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Hide Right Dock: true
+ QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005e10000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -313,7 +438,7 @@ Window Geometry:
Tool Properties:
collapsed: false
Views:
- collapsed: false
+ collapsed: true
Width: 1853
X: 67
Y: 27