From 0d2de51a9ff99d8f501a942895a6794143d4e85a Mon Sep 17 00:00:00 2001 From: mtribou Date: Thu, 10 Apr 2014 17:20:35 +0000 Subject: [PATCH] Adding intermediate tf node to the launch files for visualizing the cameras easily in rviz. Also adding a template rviz settings file in the launch folder. Finally, forcing a map dump to a file if MapMakerBase.cc throws the pClosestKF assertion. --- launch/mcptam.launch | 2 + launch/mcptam.rviz | 180 ++++++++++++++++++++++++++++++++++ launch/mcptam_client.launch | 2 + launch/mcptam_server.launch | 2 + launch/pose_calibrator.launch | 2 + src/MapMakerBase.cc | 7 ++ 6 files changed, 195 insertions(+) create mode 100644 launch/mcptam.rviz diff --git a/launch/mcptam.launch b/launch/mcptam.launch index 3c0795c..32e5e23 100644 --- a/launch/mcptam.launch +++ b/launch/mcptam.launch @@ -25,5 +25,7 @@ + + diff --git a/launch/mcptam.rviz b/launch/mcptam.rviz new file mode 100644 index 0000000..c360b61 --- /dev/null +++ b/launch/mcptam.rviz @@ -0,0 +1,180 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Tracker Pose1 + - /Tracker Pose1/Status1 + - /Tracker Pose1/Frames1 + Splitter Ratio: 0.580537 + Tree Height: 565 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.550186 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Map Points +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /mcptam/map_mkfs_array + Name: Map Frames + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + camera1: + Value: false + camera2: + Value: false + vision_pose: + Value: true + vision_world: + Value: true + world: + Value: false + Marker Scale: 1 + Name: Tracker Pose + Show Arrows: false + Show Axes: true + Show Names: true + Tree: + world: + vision_world: + camera1: + {} + camera2: + {} + vision_pose: + {} + Update Interval: 0 + 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: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Map Points + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /mcptam/map_points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + 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 + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + - Class: rviz_plugin_tutorials/PlantFlag + Flags: ~ + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.74663 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.705202 + Target Frame: vision_world + Value: Orbit (rviz) + Yaw: 3.7272 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000013c000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 45 + Y: 60 diff --git a/launch/mcptam_client.launch b/launch/mcptam_client.launch index 2cc6b60..18e3b82 100644 --- a/launch/mcptam_client.launch +++ b/launch/mcptam_client.launch @@ -31,5 +31,7 @@ + + diff --git a/launch/mcptam_server.launch b/launch/mcptam_server.launch index bcfef1a..d9465b7 100644 --- a/launch/mcptam_server.launch +++ b/launch/mcptam_server.launch @@ -18,5 +18,7 @@ + + diff --git a/launch/pose_calibrator.launch b/launch/pose_calibrator.launch index eb00270..99f645c 100644 --- a/launch/pose_calibrator.launch +++ b/launch/pose_calibrator.launch @@ -21,5 +21,7 @@ + + diff --git a/src/MapMakerBase.cc b/src/MapMakerBase.cc index 2527794..0a637fb 100644 --- a/src/MapMakerBase.cc +++ b/src/MapMakerBase.cc @@ -138,6 +138,13 @@ KeyFrame* MapMakerBase::ClosestKeyFrame(KeyFrame &kf, KeyFrameRegion region, boo } } } + + if( pClosestKF == NULL ) + { + // Dump the cameras and map data + DumpToFile("fail_map.dat"); + } + ROS_ASSERT(pClosestKF != NULL); return pClosestKF; }