Lots of changes, latest change was support for OpenStreetmaps

CMakeLists.txt

@@ -3,6 +3,7 @@ project(ros2ascii)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
+set(CMAKE_CXX_STANDARD 17)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -13,7 +14,10 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   geometry_msgs
   sensor_msgs
-  message_generation
+ # message_generation
+  tf2
+  tf2_ros
+  geodesy
 )
 
 ## System dependencies are found with CMake's conventions
@@ -72,10 +76,10 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-generate_messages(
-   DEPENDENCIES
-   std_msgs
- )
+#generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -107,7 +111,7 @@ generate_messages(
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#   INCLUDE_DIRS include  #uncomment
+  INCLUDE_DIRS include  #uncomment
 #  LIBRARIES can_speed_decoder
 #  CATKIN_DEPENDS roscpp rospy std_msgs
 CATKIN_DEPENDS message_runtime
@@ -121,7 +125,7 @@ CATKIN_DEPENDS message_runtime
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(
-#  include
+  include
   ${catkin_INCLUDE_DIRS}
 )
 
@@ -239,20 +243,9 @@ else()
 	message(" - curses-gfx NOT found")
 endif()
 
-#add_executable(gpsfix src/gps_node.cpp)
-#target_link_libraries(gpsfix ${catkin_LIBRARIES})
-
-if(DEFINED LIBPANDA_FOUND)
-#	add_executable(publish_libpanda src/publish_libpanda.cpp)
-#	target_link_libraries(publish_libpanda ${catkin_LIBRARIES} libpandac.so) #libpandac.so
-endif()
-
-#add_executable(cbf_smoother src/cbf_smoother.cpp)
-#target_link_libraries(cbf_smoother ${catkin_LIBRARIES})
 
 if(DEFINED CURSES_GFX_FOUND)
-	add_executable(radar2ascii src/radar2ascii.cpp)
+	add_executable(radar2ascii src/radar2ascii.cpp src/osm-loader.cpp)
 	target_link_libraries(radar2ascii  ${catkin_LIBRARIES} ncurses cursesgfx)
 endif()
 
-# message( " @@@@@@@@@@@@@@@@@@@@@@@ " ${PROJECT_SOURCE_DIR})

include/osm-loader.h

@@ -0,0 +1,32 @@
+/*
+ Author: Matt Bunting
+ Copyright (c) 2023 Vanderbilt
+ All rights reserved.
+
+ */
+
+/*
+ This Documentaion
+ */
+
+// https://wiki.openstreetmap.org/wiki/Slippy_map_tilenames
+
+#ifndef OSM_LOADER_H
+#define OSM_LOADER_H
+
+typedef struct _OsmTile {
+    int zoom;
+    int x;
+    int y;
+} OsmTile;
+
+OsmTile latLongToTile(int zoom, double longitude, double latitude);
+
+
+void tileToLatLong(const OsmTile& tile, double& longitude, double& latitude);
+double tileWidthInMeters(double latitude, int zoom);
+bool downloadTile(const OsmTile& tile, const char* cachedirectory, char* resultPath);
+
+void longLatToOffsetMeters(int zoom, double longitude, double latitude, double& xOffset, double& yOffset);
+
+#endif //OSM_LOADER_H

include/test.cpp

@@ -0,0 +1,36 @@
+/*
+ Author: Matt Bunting
+ Copyright (c) 2023 Vanderbilt
+ All rights reserved.
+
+ */
+
+/*
+ This Documentaion
+ */
+#include <iostream>
+#include <cstdio>
+
+#include "osm-loader.h"
+
+int main(void) {
+    OsmTile result = latLongToTile(19, -86.7931535, 36.14507466666666);
+
+    printf("https://tile.openstreetmap.org/%d/%d/%d.png\n", result.zoom, result.x, result.y);
+
+    char tileFile[100];
+    if(downloadTile(result, ".", tileFile)) {
+        printf("Failed to download file!\n");
+        return 1;
+    }
+    printf("File result: %s\n", tileFile);
+
+
+    double latitude, longitude;
+    tileToLatLong(result, longitude, latitude);
+    printf("Upper left tile Longitude/Latitude: %f,%f\n", longitude, latitude);
+
+
+    printf("Width of tile (meters): %f\n", tileWidthInMeters(latitude, result.zoom));
+    return 0;
+};

src/osm-loader.cpp

@@ -0,0 +1,83 @@
+/*
+ Author: Matt Bunting
+ Copyright (c) 2023 Vanderbilt
+ All rights reserved.
+
+ */
+
+/*
+ This Documentaion
+ */
+
+#include "osm-loader.h"
+
+#include <cstdlib>
+#include <cstdio>
+#include <math.h>
+
+// https://wiki.openstreetmap.org/wiki/Slippy_map_tilenames
+
+OsmTile latLongToTile(int zoom, double longitude, double latitude) {
+    OsmTile result;
+    int n = 1 << zoom;
+    result.x = n*((longitude + 180) / 360);
+    double lat_rad = latitude*3.1415926535897/180.0;
+    result.y = n*(1.0 - (log(tan(lat_rad) + 1.0/cos(lat_rad))/3.1415926535897))/2;
+    result.zoom = zoom;
+    return result;
+}
+
+void longLatToOffsetMeters(int zoom, double longitude, double latitude, double& xOffset, double& yOffset) {
+    OsmTile tile = latLongToTile( zoom, longitude, latitude);
+
+    double longTile, latTile;
+    tileToLatLong(tile, longTile, latTile);
+
+    const double EARTH_CIR_METERS = 40075016.686;
+    const double degreesPerMeter = 360 / EARTH_CIR_METERS;
+
+    double metersPerPixelEW = EARTH_CIR_METERS / (double)(1 << (zoom + 8));
+    double metersPerPixelNS = EARTH_CIR_METERS / (double)(1 << (zoom + 8)) * cos(latitude*3.1415926535897/180.0);
+
+//    const shiftMetersEW = width/2 * metersPerPixelEW;
+//      const shiftMetersNS = height/2 * metersPerPixelNS;
+
+    xOffset =  ( longTile - longitude)/ degreesPerMeter* cos(latitude*3.1415926535897/180.0);
+    yOffset =  ( latTile - latitude)/ degreesPerMeter;
+
+}
+
+void tileToLatLong(const OsmTile& tile, double& longitude, double& latitude) {
+    int n = 1 << tile.zoom;
+    longitude = ((double)tile.x/(double)n) * 360.0 - 180.0;
+    double lat_rad = atan(sinh(3.1415926535897*(1.0 - 2.0*((double)tile.y/(double)n))));
+    latitude = lat_rad*180.0/3.1415926535897;
+}
+
+double tileWidthInMeters(double latitude, int zoom) {
+    return 156543.03 * cos(latitude*3.1415926535897/180.0) / (double)(1 << zoom) * 256; // each image is 256 pixels
+}
+
+bool downloadTile(const OsmTile& tile, const char* cachedirectory, char* resultPath) {
+    char command[256];
+
+    snprintf(resultPath, 200, "%s/%d/%d/%d.png", cachedirectory, tile.zoom, tile.x, tile.y);
+
+    snprintf(command, 256, "[ -f %s ]", resultPath);
+//    printf("Executing command: %s\n", command);
+    if(system(command) == 0) {
+        return 0;
+    }
+    snprintf(command, 256, "mkdir -p %s/%d/%d", cachedirectory, tile.zoom, tile.x);
+//    printf("Executing command: %s\n", command);
+    system(command);
+//    snprintf(command, 256, "curl --remove-on-error --max-time 3 https://tile.openstreetmap.org/%d/%d/%d.png -o %s", tile.zoom, tile.x, tile.y, resultPath);
+    snprintf(command, 256, "curl --silent --max-time 2 https://tile.openstreetmap.org/%d/%d/%d.png -o %s", tile.zoom, tile.x, tile.y, resultPath);
+//    printf("Executing command: %s\n", command);
+
+    if(system(command) != 0) {
+        return 1;
+    }
+
+    return 0;
+}