From f0ce62e1134eeaff013b596a5f2f938d9e2beded Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Sat, 2 Dec 2023 21:46:17 +0100 Subject: [PATCH] Load multiple tif files F --- launch/config.rviz | 81 ++++++++++++++++++++++++++------- launch/load_multiple_tif.launch | 22 +++++++++ launch/load_tif.launch | 5 +- src/grid_map_geo.cpp | 10 ++-- src/test_tif_loader.cpp | 10 ++-- 5 files changed, 101 insertions(+), 27 deletions(-) create mode 100644 launch/load_multiple_tif.launch diff --git a/launch/config.rviz b/launch/config.rviz index 808a555..60bae56 100644 --- a/launch/config.rviz +++ b/launch/config.rviz @@ -7,8 +7,12 @@ Panels: - /Global Options1 - /Status1 - /GridMap1 + - /GridMap2 + - /GridMap2/Status1 + - /TF1 + - /TF1/Status1 Splitter Ratio: 0.5 - Tree Height: 848 + Tree Height: 839 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -24,7 +28,6 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: "" @@ -59,11 +62,14 @@ Visualization Manager: Color: 200; 200; 200 Color Layer: color Color Transformer: ColorLayer + ColorMap: default Enabled: true + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 Height Layer: elevation Height Transformer: GridMapLayer History Length: 1 - Invert Rainbow: false + Invert ColorMap: false Max Color: 255; 255; 255 Max Intensity: 10 Min Color: 0; 0; 0 @@ -72,13 +78,56 @@ Visualization Manager: Show Grid Lines: false Topic: /elevation_map Unreliable: false - Use Rainbow: true + Use ColorMap: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + ColorMap: default + Enabled: true + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert ColorMap: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: false + Topic: /elevation2 + Unreliable: false + Use ColorMap: true + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + map: + Value: true + world: + Value: true + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 Value: true Enabled: true Global Options: Background Color: 255; 255; 255 Default Light: true - Fixed Frame: map + Fixed Frame: sertig Frame Rate: 30 Name: root Tools: @@ -102,33 +151,33 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 2959.1279296875 + Distance: 16350.146484375 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: 11.737425804138184 - Y: -95.61576843261719 - Z: 43.985958099365234 + X: -428.4516906738281 + Y: -385.00115966796875 + Z: 529.86865234375 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.8253984451293945 + Pitch: 0.7153984904289246 Target Frame: - Value: Orbit (rviz) - Yaw: 0.6203981637954712 + Yaw: 0.48539578914642334 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1145 + Height: 1136 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000003dbfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003db000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003dbfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003db000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004cc000003db00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000283000003d2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003d2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000039a000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -137,6 +186,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1853 - X: 67 + Width: 1848 + X: 72 Y: 27 diff --git a/launch/load_multiple_tif.launch b/launch/load_multiple_tif.launch new file mode 100644 index 0000000..82e95aa --- /dev/null +++ b/launch/load_multiple_tif.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/load_tif.launch b/launch/load_tif.launch index 30277ad..81efa37 100644 --- a/launch/load_tif.launch +++ b/launch/load_tif.launch @@ -4,8 +4,9 @@ - - + + + diff --git a/src/grid_map_geo.cpp b/src/grid_map_geo.cpp index faa13a8..f0e261f 100644 --- a/src/grid_map_geo.cpp +++ b/src/grid_map_geo.cpp @@ -83,6 +83,8 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path) { const char *pszProjection = dataset->GetProjectionRef(); std::cout << std::endl << "Wkt ProjectionRef: " << pszProjection << std::endl; + const OGRSpatialReference* spatial_ref = dataset->GetSpatialRef(); + std::string name_coordinate = spatial_ref->GetAttrValue("geogcs"); // Get image metadata unsigned width = dataset->GetRasterXSize(); unsigned height = dataset->GetRasterYSize(); @@ -125,15 +127,15 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path) { geometry_msgs::TransformStamped static_transformStamped; static_transformStamped.header.stamp = ros::Time::now(); - static_transformStamped.header.frame_id = "UTM"; + static_transformStamped.header.frame_id = name_coordinate; static_transformStamped.child_frame_id = frame_id_; - static_transformStamped.transform.translation.x = 0.0; - static_transformStamped.transform.translation.y = 0.0; + static_transformStamped.transform.translation.x = mapcenter_e; + static_transformStamped.transform.translation.y = mapcenter_n; static_transformStamped.transform.translation.z = 0.0; static_transformStamped.transform.rotation.x = 0.0; static_transformStamped.transform.rotation.y = 0.0; static_transformStamped.transform.rotation.z = 0.0; - static_transformStamped.transform.rotation.w = 0.0; + static_transformStamped.transform.rotation.w = 1.0; static_broadcaster.sendTransform(static_transformStamped); return true; diff --git a/src/test_tif_loader.cpp b/src/test_tif_loader.cpp index 7068828..9f2fe6d 100644 --- a/src/test_tif_loader.cpp +++ b/src/test_tif_loader.cpp @@ -54,12 +54,13 @@ int main(int argc, char **argv) { ros::NodeHandle nh(""); ros::NodeHandle nh_private("~"); - ros::Publisher original_map_pub = nh.advertise("elevation_map", 1, true); - - std::string frame_id, file_path, color_path; + std::string frame_id, file_path, color_path, topic_name; nh_private.param("frame_id", frame_id, "map"); nh_private.param("tif_path", file_path, ""); nh_private.param("color_path", color_path, ""); + nh_private.param("topic_name", topic_name, "elevation_map"); + + ros::Publisher original_map_pub = nh.advertise(topic_name, 1, true); std::shared_ptr map = std::make_shared(frame_id); map->Load(file_path, color_path); @@ -67,8 +68,7 @@ int main(int argc, char **argv) { while (true) { /// TODO: Publish gridmap MapPublishOnce(original_map_pub, map->getGridMap()); - ros::Duration(10.0).sleep(); - ros::Duration(3.0).sleep(); + ros::Duration(1.0).sleep(); } ros::spin();