diff --git a/CMakeLists.txt b/CMakeLists.txt index f132dd0..87f9fb9 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/docs/.DS_Store b/docs/.DS_Store index 8648256..378bbcb 100644 Binary files a/docs/.DS_Store and b/docs/.DS_Store differ diff --git a/include/.DS_Store b/include/.DS_Store new file mode 100644 index 0000000..edd4cd8 Binary files /dev/null and b/include/.DS_Store differ diff --git a/include/19/135742/205619.png b/include/19/135742/205619.png new file mode 100644 index 0000000..1b49032 Binary files /dev/null and b/include/19/135742/205619.png differ diff --git a/include/osm-loader.h b/include/osm-loader.h new file mode 100644 index 0000000..b3aaf6e --- /dev/null +++ b/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 diff --git a/include/test.cpp b/include/test.cpp new file mode 100644 index 0000000..e45b058 --- /dev/null +++ b/include/test.cpp @@ -0,0 +1,36 @@ +/* + Author: Matt Bunting + Copyright (c) 2023 Vanderbilt + All rights reserved. + + */ + +/* + This Documentaion + */ +#include +#include + +#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; +}; diff --git a/src/osm-loader.cpp b/src/osm-loader.cpp new file mode 100644 index 0000000..eb2cc97 --- /dev/null +++ b/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 +#include +#include + +// 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; +} diff --git a/src/radar2ascii.cpp b/src/radar2ascii.cpp index 0aec74d..5082b40 100644 --- a/src/radar2ascii.cpp +++ b/src/radar2ascii.cpp @@ -22,6 +22,8 @@ #include #include +#include +#include // ROS headers: #include "ros/ros.h" @@ -29,12 +31,50 @@ #include "std_msgs/Float64.h" #include "geometry_msgs/PointStamped.h" #include "sensor_msgs/NavSatFix.h" +#include "nav_msgs/Odometry.h" +#include "tf/tf.h" // Just for Tf functions +#include "tf2_ros/transform_listener.h" +#include "geometry_msgs/TransformStamped.h" +#include "std_msgs/Float64MultiArray.h" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/MagneticField.h" +#include "geodesy/utm.h" +#include "osm-loader.h" -#define METERS_PER_DEGREE (111000.0) +//#define METERS_PER_DEGREE (111000.0) + +typedef struct _UniformInfo { + Mat4D modelView; + Mat4D modelViewProjection; +} UniformInfo; + +template void myVertexShader(U* uniformInfo, T& output, T& input) { + output.vertex = matrixVectorMultiply(uniformInfo->modelViewProjection, input.vertex); +// output.location = matrixVectorMultiply(uniformInfo->modelView, input.vertex); +// output.normal = matrixVectorMultiply(uniformInfo->modelView, input.normal); +} + +typedef struct _MapVertexInfo { + Coordinates4D vertex; + Coordinates2Df textureCoord; +} MapVertexInfo; + +REGISTER_VERTEX_LAYOUT(MapVertexInfo) + MEMBER(textureCoord) +END_VERTEX_LAYOUT(MapVertexInfo) + +void mapShader(const FragmentInfo& fInfo) { + MapVertexInfo* vertexInfo = (MapVertexInfo*)fInfo.interpolated; + + Texture* mapTexture = (Texture*) fInfo.data; + *fInfo.colorOutput = mapTexture->sample(vertexInfo->textureCoord.x, vertexInfo->textureCoord.y); + fInfo.colorOutput->a = 0; +} + /* Catch ctrl-c for cleaner exits */ @@ -71,6 +111,14 @@ void setupTerminal() init_pair(6, COLOR_MAGENTA, COLOR_BLACK); init_pair(7, COLOR_WHITE, COLOR_BLACK); +// init_pair(1, COLOR_RED, COLOR_WHITE); +// init_pair(2, COLOR_YELLOW, COLOR_WHITE); +// init_pair(3, COLOR_GREEN, COLOR_WHITE); +// init_pair(4, COLOR_CYAN, COLOR_WHITE); +// init_pair(5, COLOR_BLUE, COLOR_WHITE); +// init_pair(6, COLOR_MAGENTA, COLOR_WHITE); +// init_pair(7, COLOR_BLACK, COLOR_WHITE); + // init_pair(1, COLOR_RED, -1); // init_pair(2, COLOR_YELLOW, -1); // init_pair(3, COLOR_GREEN, -1); @@ -90,66 +138,391 @@ void setupTerminal() // atexit(destroy); } +class ImuListener { +private: + ros::Subscriber subscriber; + + ros::Subscriber subscriberMagnetic; + +public: + + Mat4D orientation; + Coordinates3D acceleration; + Coordinates3D rotationRate; + double q[4]; + + sensor_msgs::MagneticField magnetic; + + void callback(const sensor_msgs::Imu::ConstPtr& msg) { +// tf::Quaternion myQuaternion(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w); +// tf::Matrix3x3 mat(myQuaternion); +// tf::Matrix3x3 mat(msg->orientation); +// mat.getEulerYPR(yaw, pitch, roll); + q[0] = msg->orientation.w; + q[1] = msg->orientation.x; + q[2] = msg->orientation.y; + q[3] = msg->orientation.z; + + + double q00 = msg->orientation.w*msg->orientation.w; + double q11 = msg->orientation.x*msg->orientation.x; + double q22 = msg->orientation.y*msg->orientation.y; + double q33 = msg->orientation.z*msg->orientation.z; + + double q01 = msg->orientation.w*msg->orientation.x; + double q02 = msg->orientation.w*msg->orientation.y; + double q03 = msg->orientation.w*msg->orientation.z; + + double q12 = msg->orientation.x*msg->orientation.y; + double q13 = msg->orientation.x*msg->orientation.z; + + double q23 = msg->orientation.y*msg->orientation.z; + + orientation.d[0][0] = 2*(q00 + q11) - 1; + orientation.d[0][1] = 2*(q12 - q03); + orientation.d[0][2] = 2*(q13 + q02); + + orientation.d[1][0] = 2*(q12 + q03); + orientation.d[1][1] = 2*(q00 + q22) - 1; + orientation.d[1][2] = 2*(q23 - q01); + + orientation.d[2][0] = 2*(q13 - q02); + orientation.d[2][1] = 2*(q23 + q01); + orientation.d[2][2] = 2*(q00 + q33) - 1; + + + rotationRate.x = msg->angular_velocity.x; + rotationRate.y = msg->angular_velocity.y; + rotationRate.z = msg->angular_velocity.z; + + acceleration.x = msg->linear_acceleration.x; + acceleration.y = msg->linear_acceleration.y; + acceleration.z = msg->linear_acceleration.z; + + } + void callbackMagnetic(const sensor_msgs::MagneticField::ConstPtr& msg) { + magnetic = *msg; + + + } + void printMagnetic(int x, int y) { + int i = 0; + mvprintw(y+i++, x, "IMU: Mag Gyro Accel"); + mvprintw(y+i++, x, " x: % 0.02f % 0.02f % 0.02f", magnetic.magnetic_field.x/0.00005, // constant to convert Tesla to "EFU" + rotationRate.x, + acceleration.x); + mvprintw(y+i++, x, " y: % 0.02f % 0.02f % 0.02f", magnetic.magnetic_field.y/0.00005, + rotationRate.y, + acceleration.y); + mvprintw(y+i++, x, " z: % 0.02f % 0.02f % 0.02f", magnetic.magnetic_field.z/0.00005, + rotationRate.z, + acceleration.z); + + mvprintw(y+i++, x, " L2: % 0.02f % 0.02f % 0.02f", sqrt(magnetic.magnetic_field.x*magnetic.magnetic_field.x + magnetic.magnetic_field.y*magnetic.magnetic_field.y + magnetic.magnetic_field.z*magnetic.magnetic_field.z)/0.00005, // constant to convert Tesla to "EFU" + sqrt(rotationRate.x*rotationRate.x + rotationRate.y*rotationRate.y + rotationRate.z*rotationRate.z), + sqrt(acceleration.x*acceleration.x + acceleration.y*acceleration.y + acceleration.z*acceleration.z)); + } + ImuListener(ros::NodeHandle* nh) { + q[0] = 1; + q[1] = 0; + q[2] = 0; + q[3] = 0; + subscriber = nh->subscribe("/microstrain/imu", 1000, &ImuListener::callback, this); + subscriberMagnetic = nh->subscribe("/microstrain/mag", 1000, &ImuListener::callbackMagnetic, this); + acceleration.z = 1; + rotationRate.z = 1; + orientation = translationMatrix(0, 0, 0); + } +}; + class GpsListener { private: ros::NodeHandle* nodeHandle; - ros::Subscriber subscriber; + ros::Subscriber subscriberGps; + ros::Subscriber subscriberOdom; + ros::Subscriber subscriberOdomFilteredLocal; + ros::Subscriber subscriberOdomFilteredGlobal; double priorLong; double priorLat; + // tf stuff + tf2_ros::Buffer tfBuffer; + tf2_ros::TransformListener *tfListener; public: + double longitudeRaw; + double latitudeRaw; + double longitude; double latitude; double diffLong, diffLat; + double headingFromImu; double heading; // calculated form lat/long bool first; + double carX, carY, carTheta; + double odomFilteredGlobalX, odomFilteredGlobalY, odomFilteredGlobalTheta; + double odomFilteredLocalX, odomFilteredLocalY, odomFilteredLocalTheta; + + double x, y, theta; // final output + double covX, covY; + + typedef enum { + LOCALIZATION_GPS, + LOCALIZATION_CAR_ODOM, + LOCALIZATION_FILTERED_LOCAL, + LOCALIZATION_FILTERED_GLOBAL, + LOCALIZATION_TF_MAP_TO_BASE_LINK, + LOCALIZATION_TF_ODOM_TO_BASE_LINK, + LOCALIZATION_TF_UTM_TO_BASE_LINK, + LOCALIZATION_COUNT + } LocalizationMethod; + + LocalizationMethod localizationMethodToShow; + GpsListener(ros::NodeHandle* nh) { + longitude = latitude = 0; + diffLong = diffLat = 0; + longitudeRaw = latitudeRaw = 0; + + headingFromImu = 0; + + heading = 0; // calculated form lat/long first = true; + + carX = carY = carTheta = 0; + odomFilteredGlobalX = odomFilteredGlobalY = odomFilteredGlobalTheta = 0; + odomFilteredLocalX = odomFilteredLocalY = odomFilteredLocalTheta = 0; + + x = y = theta = 0; // final output + covX = covY = 0; + + localizationMethodToShow = LOCALIZATION_FILTERED_GLOBAL; + nodeHandle = nh; - subscriber = nodeHandle->subscribe("/gps_fix", 1000, &GpsListener::callbackGps, this); + subscriberGps = nodeHandle->subscribe("/car/gps/gps_fix_patched", 1000, &GpsListener::callbackGps, this); + subscriberOdom = nodeHandle->subscribe("/car/odometry/differential_drive", 1000, &GpsListener::callbackCarOdom, this); + subscriberOdomFilteredLocal = nodeHandle->subscribe("/odometry/filtered/local", 1000, &GpsListener::callbackOdomFilteredLocal, this); + subscriberOdomFilteredGlobal = nodeHandle->subscribe("/odometry/filtered/global", 1000, &GpsListener::callbackOdomFilteredGlobal, this); + + tfListener = new tf2_ros::TransformListener(tfBuffer); + } + + const char* localizationMethodToString( LocalizationMethod method ) { + switch (method) { + case LOCALIZATION_GPS: return "GPS"; + case LOCALIZATION_CAR_ODOM: return "Car Odometry"; + case LOCALIZATION_FILTERED_LOCAL: return "Kalman Odometry"; + case LOCALIZATION_FILTERED_GLOBAL: return "Kalman Global"; + case LOCALIZATION_TF_ODOM_TO_BASE_LINK: return "tf odom->base_link"; + case LOCALIZATION_TF_MAP_TO_BASE_LINK: return "tf map->base_link"; + case LOCALIZATION_TF_UTM_TO_BASE_LINK: return "tf utm->base_link"; + default: + break; + } + return ""; + } + + void setLocalizationMethod(LocalizationMethod method) { + localizationMethodToShow = method; + } + + void updateTf() { + geometry_msgs::TransformStamped transformStamped; + try { + if(localizationMethodToShow == LOCALIZATION_TF_MAP_TO_BASE_LINK) { + transformStamped = tfBuffer.lookupTransform("map", "base_link", ros::Time(0)); + } else if(localizationMethodToShow == LOCALIZATION_TF_UTM_TO_BASE_LINK) { + transformStamped = tfBuffer.lookupTransform("utm", "base_link", ros::Time(0)); + } else if(localizationMethodToShow == LOCALIZATION_TF_ODOM_TO_BASE_LINK) { + transformStamped = tfBuffer.lookupTransform("odom", "base_link", ros::Time(0)); + } + } catch (tf2::TransformException &ex) { + return; + } + + if((localizationMethodToShow == LOCALIZATION_TF_MAP_TO_BASE_LINK) || + (localizationMethodToShow == LOCALIZATION_TF_UTM_TO_BASE_LINK) || + (localizationMethodToShow == LOCALIZATION_TF_ODOM_TO_BASE_LINK)) { + x = transformStamped.transform.translation.x; + y = transformStamped.transform.translation.y; + + tfScalar yaw, pitch, roll; + tf::Quaternion myQuaternion(transformStamped.transform.rotation.x, transformStamped.transform.rotation.y, transformStamped.transform.rotation.z, transformStamped.transform.rotation.w); + tf::Matrix3x3 mat(myQuaternion); + mat.getEulerYPR(yaw, pitch, roll); + + theta = yaw; + } } void callbackGps(const sensor_msgs::NavSatFix::ConstPtr& msg) { + // helpful on how to use conversions: https://github.com/ros-geographic-info/geographic_info/blob/master/geodesy/tests/test_utm.cpp + geographic_msgs::GeoPoint fromLL; + fromLL.latitude = msg->latitude; + fromLL.longitude = msg->longitude; + fromLL.altitude = msg->altitude; + geodesy::UTMPoint gpsAsUtm(fromLL); + latitudeRaw = msg->latitude; + longitudeRaw = msg->longitude; if(first) { first = false; - longitude = msg->longitude; - latitude = msg->latitude; + latitude = gpsAsUtm.northing; + longitude = gpsAsUtm.easting; priorLong = longitude; priorLat = latitude; heading = 0; return; } - longitude = 0.85*longitude + 0.15*msg->longitude; - latitude = 0.85*latitude + 0.15*msg->latitude; - diffLong = (longitude - priorLong) * METERS_PER_DEGREE; - diffLat = (latitude - priorLat) * METERS_PER_DEGREE; - if( (diffLong*diffLong + diffLat*diffLat) > 0.25) { - heading = atan2(diffLong, diffLat); + + +// longitude = 0.85*longitude + 0.15*gpsAsUtm.northing; +// latitude = 0.85*latitude + 0.15*gpsAsUtm.easting; + longitude = gpsAsUtm.easting; + latitude = gpsAsUtm.northing; + + diffLong = (longitude - priorLong);// * METERS_PER_DEGREE; + diffLat = (latitude - priorLat);// * METERS_PER_DEGREE; + if( (diffLong*diffLong + diffLat*diffLat) > 0.75) { +// heading = -atan2(diffLong, diffLat); + + heading = atan2(diffLat,diffLong);// + 3.1415926535897/180.0/2.0; priorLong = longitude; priorLat = latitude; diffLong = 0; diffLat = 0; } + + + covX = msg->position_covariance[0]; + covY = msg->position_covariance[4]; + if(localizationMethodToShow == LOCALIZATION_GPS) { + + +// x = latitude * METERS_PER_DEGREE; +// y = longitude * METERS_PER_DEGREE; + x = gpsAsUtm.easting; + y = gpsAsUtm.northing; +// theta = -heading + 3.1415926535897/180.0/2.0; + theta = heading; + theta = headingFromImu; // HACK + } + + } + + void callbackCarOdom(const nav_msgs::Odometry::ConstPtr& msg) { + + carX = msg->pose.pose.position.x; + carY = msg->pose.pose.position.y; + + tfScalar yaw, pitch, roll; + tf::Quaternion myQuaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w); + tf::Matrix3x3 mat(myQuaternion); + mat.getEulerYPR(yaw, pitch, roll); + + carTheta = yaw; + + if(localizationMethodToShow == LOCALIZATION_CAR_ODOM) { + x = carX; + y = carY; + theta = carTheta; + } + } + + void callbackOdomFilteredGlobal(const nav_msgs::Odometry::ConstPtr& msg) { + odomFilteredGlobalX = msg->pose.pose.position.x; + odomFilteredGlobalY = msg->pose.pose.position.y; + + tfScalar yaw, pitch, roll; + tf::Quaternion myQuaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w); + tf::Matrix3x3 mat(myQuaternion); + mat.getEulerYPR(yaw, pitch, roll); + + odomFilteredGlobalTheta = yaw; + if(localizationMethodToShow == LOCALIZATION_FILTERED_GLOBAL) { + x = odomFilteredGlobalX; + y = odomFilteredGlobalY; + theta = odomFilteredGlobalTheta; + } + } + void callbackOdomFilteredLocal(const nav_msgs::Odometry::ConstPtr& msg) { + odomFilteredLocalX = msg->pose.pose.position.x; + odomFilteredLocalY = msg->pose.pose.position.y; + + tfScalar yaw, pitch, roll; + tf::Quaternion myQuaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w); + tf::Matrix3x3 mat(myQuaternion); + mat.getEulerYPR(yaw, pitch, roll); + + odomFilteredLocalTheta = yaw; + if(localizationMethodToShow == LOCALIZATION_FILTERED_LOCAL) { + x = odomFilteredLocalX; + y = odomFilteredLocalY; + theta = odomFilteredLocalTheta; + } } void draw(int x, int y) { - mvprintw(y, x, "Longitude %.06f", longitude); - mvprintw(y+1, x, "Latitude %.06f", latitude); - mvprintw(y+2, x, "Heading %.02f", heading * 180/M_PI); - mvprintw(y+3, x, "diffMag %.02f", (diffLong*diffLong + diffLat*diffLat)); + int i = 0; + mvprintw(y+i++, x, "Current Method %s", localizationMethodToString(localizationMethodToShow)); + mvprintw(y+i++, x, "Longitude (x) %.06f", this->x ); + mvprintw(y+i++, x, "Latitude (y) %.06f", this->y); + mvprintw(y+i++, x, "Heading %.02f", theta* 180/M_PI); + mvprintw(y+i++, x, "GPS diff Long %.06f", longitude - this->x); + mvprintw(y+i++, x, "GPS diff Lat %.06f", latitude - this->y ); + mvprintw(y+i++, x, "GPS diff head %.02f", (heading-theta) * 180/M_PI); + + //mvprintw(y+i++, x, "diffMag %.02f", (diffLong*diffLong + diffLat*diffLat)); } }; + +void drawGuage(double* angles, int angleCount, Coordinates2D center, double width, double height, const char* labels[]) { + Coordinates2D point1 = center; + Coordinates2D point2 = {.x = 25, .y=0}; + + + + for (double i = 1; i <= 60; i+=1) { + attron(COLOR_PAIR(4)); + double x = center.x + ((double)width)*sin(i *M_PI*2.0/60.0 + angles[0]) + 0.5; + double y = center.y - ((double)height)*cos(i *M_PI*2.0/60.0 + angles[0]) + 0.5; + drawDotFloat(x,y); +// mvaddch(y, x, '*'); + attroff(COLOR_PAIR(4)); + } + + + point1 = center; + + attron(A_BOLD); + + for (int a = 0; a < angleCount; a++) { + attron(COLOR_PAIR(1+a)); + point2.x = point1.x + width*cos(angles[a]) + 0.5; + point2.y = point1.y - height*sin(angles[a]) + 0.5; + ln2(point1, point2); + attroff(COLOR_PAIR(1+a)); + } + attroff(A_BOLD); + + int offset = 0; + for (int a = 0; a < angleCount; a++) { + attron(COLOR_PAIR(1+a)); + mvprintw(center.y - (height + 1), center.x - 20/2 + offset, labels[a]); + offset += 1 + strlen(labels[a]); + attroff(COLOR_PAIR(1+a)); + } +} + class SteeringListener { private: ros::NodeHandle* nodeHandle; @@ -160,11 +533,12 @@ class SteeringListener { SteeringListener(ros::NodeHandle* nh) { nodeHandle = nh; - subscriber = nodeHandle->subscribe("/steering_angle", 1000, &SteeringListener::callbackSteerAngle, this); + subscriber = nodeHandle->subscribe("/car/state/steering", 1000, &SteeringListener::callbackSteering, this); } - void callbackSteerAngle(const std_msgs::Float64::ConstPtr& msg) { - steeringAngle = -msg->data; // For whatever reason the steering angle reported in ROS is reversed? + void callbackSteering(const std_msgs::Float64MultiArray::ConstPtr& msg) { +// steeringAngle = -msg->data; // For whatever reason the steering angle reported in ROS is reversed? + steeringAngle = -msg->data[0]; // For whatever reason the steering angle reported in ROS is reversed? } void draw(Coordinates2D center, double width, double height) { @@ -206,22 +580,22 @@ class RadarListener { RadarListener(ros::NodeHandle* nh) { nodeHandle = nh; - subscriberATracks[ 0] = nodeHandle->subscribe("/track_a0", 1000, &RadarListener::callbackRadar0, this); - subscriberATracks[ 1] = nodeHandle->subscribe("/track_a1", 1000, &RadarListener::callbackRadar1, this); - subscriberATracks[ 2] = nodeHandle->subscribe("/track_a2", 1000, &RadarListener::callbackRadar2, this); - subscriberATracks[ 3] = nodeHandle->subscribe("/track_a3", 1000, &RadarListener::callbackRadar3, this); - subscriberATracks[ 4] = nodeHandle->subscribe("/track_a4", 1000, &RadarListener::callbackRadar4, this); - subscriberATracks[ 5] = nodeHandle->subscribe("/track_a5", 1000, &RadarListener::callbackRadar5, this); - subscriberATracks[ 6] = nodeHandle->subscribe("/track_a6", 1000, &RadarListener::callbackRadar6, this); - subscriberATracks[ 7] = nodeHandle->subscribe("/track_a7", 1000, &RadarListener::callbackRadar7, this); - subscriberATracks[ 8] = nodeHandle->subscribe("/track_a8", 1000, &RadarListener::callbackRadar8, this); - subscriberATracks[ 9] = nodeHandle->subscribe("/track_a9", 1000, &RadarListener::callbackRadar9, this); - subscriberATracks[10] = nodeHandle->subscribe("/track_a10", 1000, &RadarListener::callbackRadar10, this); - subscriberATracks[11] = nodeHandle->subscribe("/track_a11", 1000, &RadarListener::callbackRadar11, this); - subscriberATracks[12] = nodeHandle->subscribe("/track_a12", 1000, &RadarListener::callbackRadar12, this); - subscriberATracks[13] = nodeHandle->subscribe("/track_a13", 1000, &RadarListener::callbackRadar13, this); - subscriberATracks[15] = nodeHandle->subscribe("/track_a14", 1000, &RadarListener::callbackRadar14, this); - subscriberATracks[15] = nodeHandle->subscribe("/track_a15", 1000, &RadarListener::callbackRadar15, this); + subscriberATracks[ 0] = nodeHandle->subscribe("/car/radar/track_a0", 1000, &RadarListener::callbackRadar0, this); + subscriberATracks[ 1] = nodeHandle->subscribe("/car/radar/track_a1", 1000, &RadarListener::callbackRadar1, this); + subscriberATracks[ 2] = nodeHandle->subscribe("/car/radar/track_a2", 1000, &RadarListener::callbackRadar2, this); + subscriberATracks[ 3] = nodeHandle->subscribe("/car/radar/track_a3", 1000, &RadarListener::callbackRadar3, this); + subscriberATracks[ 4] = nodeHandle->subscribe("/car/radar/track_a4", 1000, &RadarListener::callbackRadar4, this); + subscriberATracks[ 5] = nodeHandle->subscribe("/car/radar/track_a5", 1000, &RadarListener::callbackRadar5, this); + subscriberATracks[ 6] = nodeHandle->subscribe("/car/radar/track_a6", 1000, &RadarListener::callbackRadar6, this); + subscriberATracks[ 7] = nodeHandle->subscribe("/car/radar/track_a7", 1000, &RadarListener::callbackRadar7, this); + subscriberATracks[ 8] = nodeHandle->subscribe("/car/radar/track_a8", 1000, &RadarListener::callbackRadar8, this); + subscriberATracks[ 9] = nodeHandle->subscribe("/car/radar/track_a9", 1000, &RadarListener::callbackRadar9, this); + subscriberATracks[10] = nodeHandle->subscribe("/car/radar/track_a10", 1000, &RadarListener::callbackRadar10, this); + subscriberATracks[11] = nodeHandle->subscribe("/car/radar/track_a11", 1000, &RadarListener::callbackRadar11, this); + subscriberATracks[12] = nodeHandle->subscribe("/car/radar/track_a12", 1000, &RadarListener::callbackRadar12, this); + subscriberATracks[13] = nodeHandle->subscribe("/car/radar/track_a13", 1000, &RadarListener::callbackRadar13, this); + subscriberATracks[15] = nodeHandle->subscribe("/car/radar/track_a14", 1000, &RadarListener::callbackRadar14, this); + subscriberATracks[15] = nodeHandle->subscribe("/car/radar/track_a15", 1000, &RadarListener::callbackRadar15, this); } @@ -245,15 +619,112 @@ class RadarListener { void lightModelFs(const FragmentInfo& fInfo) { Coordinates3D* colorRGB = (Coordinates3D*)fInfo.data; - setRGB(fInfo.pixel, *colorRGB); +// setRGB(fInfo.pixel, *colorRGB); -// Coordinates3D clippedRGB = clipRGB(*colorRGB); -// fInfo.colorOutput->r = clippedRGB.x*255; -// fInfo.colorOutput->g = clippedRGB.y*255; -// fInfo.colorOutput->b = clippedRGB.z*255; -// fInfo.colorOutput->a = 0; + Coordinates3D clippedRGB = clipRGB(*colorRGB); + fInfo.colorOutput->r = clippedRGB.x*255; + fInfo.colorOutput->g = clippedRGB.y*255; + fInfo.colorOutput->b = clippedRGB.z*255; + fInfo.colorOutput->a = 0; } +void generateCirclePolygon(Polygon4D* polygon) { + polygon->numVertices = 10; + for(int i = 0; i < polygon->numVertices; i++) { + polygon->vertices[i].x = cos(((double)i)/(double)polygon->numVertices*2.0*M_PI); + polygon->vertices[i].y = sin(((double)i)/(double)polygon->numVertices*2.0*M_PI); + polygon->vertices[i].z = 0; + polygon->vertices[i].w = 1; + + polygon->normals[i].x = 0; + polygon->normals[i].y = 0; + polygon->normals[i].z = 1; + } +} + +class MapHandler { +private: + double longitude; + double latitude; + + OsmTile currentTile; + double currentTileWidth; + double currentTileTopLeftLong; + double currentTileTopLeftLat; + +public: + int zoom; + + Texture map[4]; + bool mapGood[4]; + + Mat4D mapModel; + + MapHandler() { +// map = new Texture[4]("load failure"); +// mapGood = new bool[4]; + longitude = 0; + latitude = 0; + zoom = 18; + for (int i = 0; i < 4; i++) { + mapGood[i] = false; + } + + currentTileWidth = 0; + currentTileTopLeftLong = 0; + currentTileTopLeftLat = 0; + mapModel = translationMatrix(0,0,0); + } + + bool insideMap( double longitude, double latitude) { +// OsmTile tile = {zoom, longitude, latitude}; +// tileToLatLong( tile, longitude, latitude); + OsmTile mOsmTile = latLongToTile(zoom, longitude, latitude); + return + (currentTile.x == mOsmTile.x) && + (currentTile.y == mOsmTile.y) && + (currentTile.zoom == mOsmTile.zoom) + ; + + } + + void updateModelMatrix() { + double xOffset, yOffset; + longLatToOffsetMeters(zoom, longitude, latitude, xOffset, yOffset); + + Mat4D translation = translationMatrix(xOffset,yOffset,0); + Mat4D scale = scaleMatrix(currentTileWidth,currentTileWidth,0); + mapModel = matrixMultiply(translation , scale); + } + + void setLongLat( double longitude, double latitude) { + if(insideMap(longitude, latitude)){ + this->longitude = longitude; + this->latitude = latitude; + updateModelMatrix(); + return; + } + this->longitude = longitude; + this->latitude = latitude; + + currentTile = latLongToTile(zoom, longitude, latitude); + tileToLatLong( currentTile, currentTileTopLeftLong, currentTileTopLeftLat); + currentTileWidth = tileWidthInMeters( latitude, zoom); +// mapModel = scaleMatrix(currentTileWidth,currentTileWidth,0); + updateModelMatrix(); + char tileFile[200]; + if(downloadTile(currentTile, "/home/matt", tileFile)) { +// printf("Failed to download file!\n"); + map[0].resize(1,1); + } +// printf("File result: %s\n", tileFile); +// Texture mapTexture(tileFile); + map[0].loadPng(tileFile); + map[0].offsetAvergageToCenter(0.75); + map[0].normalize(1.); + map[0].invert(); + } +}; int main(int argc, char **argv) { ros::init(argc, argv, "radar2ascii", ros::init_options::AnonymousName); @@ -261,11 +732,28 @@ int main(int argc, char **argv) { ros::NodeHandle nh; + Polygon4D unitCircle; + generateCirclePolygon(&unitCircle); + + UniformInfo mUniformInfo; + setupTerminal(); + + + + GpsListener::LocalizationMethod mLocalizationMethod = GpsListener::LOCALIZATION_FILTERED_GLOBAL; RadarListener mRadarListener(&nh); SteeringListener mSteeringListener(&nh); GpsListener mGpsListener(&nh); + ImuListener mImuListener(&nh); + + tf2_ros::Buffer tfBuffer; + tf2_ros::TransformListener *tfListener; + + tfListener = new tf2_ros::TransformListener(tfBuffer); + + Coordinates4D cube[] = { {-1, -1, -1, 1}, @@ -302,8 +790,8 @@ int main(int argc, char **argv) { {0, 4, 5, 1} // back }; -#define NUM_GRID_LINES (11) -#define GRID_LINE_UNIT (4) +#define NUM_GRID_LINES (15) +#define GRID_LINE_UNIT (10) Coordinates4D grid[NUM_GRID_LINES*2*2]; // 5x5 grid int gridEdgeIndices[NUM_GRID_LINES*2][2]; for (int i = 0; i < NUM_GRID_LINES*2*2; i++) { @@ -342,55 +830,90 @@ int main(int argc, char **argv) { int screenSizeX, screenSizeY; getmaxyx(stdscr, screenSizeY, screenSizeX); -#ifdef FB_SUPPORT // HACK - characterAspect = 1.0; // RGB panel -#define PANEL_X_RES (64) -#define PANEL_Y_RES (64) - screenSizeY = PANEL_Y_RES; - screenSizeX = PANEL_X_RES; -#endif + + RenderPipeline mRenderPipeline; + mRenderPipeline.resize(screenSizeX, screenSizeY); // Depth buffer (old method) - DepthBuffer depthBuffer; - depthBuffer.setSize(screenSizeX, screenSizeY); +// DepthBuffer depthBuffer; +// depthBuffer.setSize(screenSizeX, screenSizeY); double screenAspect = (double)screenSizeX/(double)screenSizeY / characterAspect; + MapVertexInfo squareVi[4]; + int squareViIndices[2][3]; + squareVi[0].vertex = {0, -1, 0, 1}; // "top left corner" is the OSM standard + squareVi[1].vertex = { 1, -1, 0, 1}; + squareVi[2].vertex = { 1, 0, 0, 1}; + squareVi[3].vertex = {0, 0, 0, 1}; + + squareVi[0].textureCoord = {0, 0}; + squareVi[1].textureCoord = {0, 1}; + squareVi[2].textureCoord = {1, 1}; + squareVi[3].textureCoord = {1, 0}; + + squareViIndices[0][0] = 0; // right handed + squareViIndices[0][1] = 1; + squareViIndices[0][2] = 2; + squareViIndices[1][0] = 0; // left handed + squareViIndices[1][1] = 2; + squareViIndices[1][2] = 3; + + +// OsmTile mOsmTile = latLongToTile(17, -86.7931535, 36.14507466666666); +// char tileFile[200]; +// if(downloadTile(mOsmTile, "/home/matt", tileFile)) { +// printf("Failed to download file!\n"); +// } +// printf("File result: %s\n", tileFile); +// Texture mapTexture(tileFile); +// mapTexture.offsetAvergageToCenter(0.75); +// mapTexture.normalize(1.); +// mapTexture.invert(); + + MapHandler mMapHandler; + double testIncrementor = 0; + double testLong = -86.7931535; + double testLat = 36.14507466666666; + mMapHandler.setLongLat(testLong, testLat); // Model Mat4D scaleMat = scaleMatrix(1, 1, 1); // View - Mat4D cameraTranslation = translationMatrix(0, 0, -5); + Mat4D cameraTranslation = translationMatrix(0, 0, -25); Coordinates3D cameraAxis = {0, 1, 0}; cameraAxis = normalizeVector(cameraAxis); Mat4D cameraOrientation = rotationFromAngleAndUnitAxis(-M_PI_4, cameraAxis); Mat4D viewMatrix = matrixMultiply( cameraOrientation, cameraTranslation ); // Projection - double zFar = 100; - double zNear = 0.1; + double zFar = 1000; + double zNear = 1; double viewAngle = M_PI*0.5; + double orthoScale = 5*5; Mat4D projection = projectionMatrixPerspective(viewAngle, screenAspect, zFar, zNear); // Viewport // Mat4D windowScale = scaleMatrix((double)screenSizeX/2, (double)screenSizeY/2, 1); // Mat4D translationScreen = translationMatrix((double)screenSizeX/2 -0.5, (double)screenSizeY/2 -0.5, 0); // Mat4D windowFull = matrixMultiply(translationScreen, windowScale); - Mat4D windowFull = makeWindowTransform(screenSizeX, screenSizeY, characterAspect); - windowFull.d[0][0] *= -1; // HACK the car is aligned with GPS, heading, and radar but mirrores over x. This mirrors the viewport +// Mat4D windowFull = makeWindowTransform(screenSizeX, screenSizeY, characterAspect); + mRenderPipeline.viewport = makeWindowTransform(screenSizeX, screenSizeY, characterAspect); +// windowFull.d[0][0] *= -1; // HACK the car is aligned with GPS, heading, and radar but mirrores over x. This mirrors the viewport int numEdges; int debugLine = 0; double lightAngle = 0; - double angle = 0; + double angle = -M_PI/2; // default points double cube2angle = 0; double tilt = M_PI/4; bool usePerspective = true; bool showGrid = true; bool autoRotate = false; bool showDepth = false; + bool showUnitVectors = false; ros::Rate rate(60); @@ -399,12 +922,14 @@ int main(int argc, char **argv) { rate.sleep(); debugLine = 0; - depthBuffer.reset(); - erase(); +// depthBuffer.reset(); +// erase(); + mRenderPipeline.reset(); mvprintw(debugLine++, 0, "Hello darkness..."); mvprintw(debugLine++, 0, "Press ESC to quit"); + mGpsListener.updateTf(); /* Build the view/camera matrix: @@ -415,8 +940,14 @@ int main(int argc, char **argv) { Mat4D camPan = rotationFromAngleAndUnitAxis(angle, cameraAxis); camPan = transpose(camPan); - -// viewMatrix = matrixMultiply( cameraOrientation, cameraTranslation); + // HACK HACK HACK + tfScalar yaw = 0, pitch = 0, roll = 0; + tf::Quaternion myQuaternion(mImuListener.q[1], mImuListener.q[2], mImuListener.q[3], mImuListener.q[0]); + tf::Matrix3x3 mat(myQuaternion); + mat.getEulerYPR(yaw, pitch, roll); +// mGpsListener.heading = yaw -4.0*M_PI/180.0; // magnetic declination Nashville from https://www.magnetic-declination.com/USA/Nashville/2807841.html# + // Also https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml + mGpsListener.headingFromImu = yaw - 4.04*M_PI/180.0; cameraAxis.x = 1; cameraAxis.y = 0; @@ -427,44 +958,131 @@ int main(int argc, char **argv) { cameraOrientation = matrixMultiply(camTilt, camPan); if(autoRotate){ - double distance = 2*sin(angle/2); - cameraTranslation = translationMatrix(-(5+distance)*sin(angle), (5+distance) * cos(angle), -(2*cos(angle/5)+distance)-4); + double distance = 10*sin(angle/2); + cameraTranslation = translationMatrix(-(25+distance)*sin(angle), (25+distance) * cos(angle), -(10*cos(angle/5)+distance)-20); } viewMatrix = matrixMultiply( cameraOrientation, cameraTranslation); + double kalmanYaw = 0; + if(showUnitVectors) { +#define NUM_FRAMES (5) + Mat4D frame[NUM_FRAMES]; + + // 0, 0, 0 + frame[0] = rotationFromAngleAndUnitAxis(0, cameraAxis); + + // TF base_link -> odom + geometry_msgs::TransformStamped transformStamped; + // base_link -> odom frame + tfScalar yaw = 0, pitch = 0, roll = 0; + try { + transformStamped = tfBuffer.lookupTransform("base_link", "odom", ros::Time(0)); + tf::Quaternion myQuaternion(transformStamped.transform.rotation.x, transformStamped.transform.rotation.y, transformStamped.transform.rotation.z, transformStamped.transform.rotation.w); + tf::Matrix3x3 mat(myQuaternion); + mat.getEulerYPR(yaw, pitch, roll); + } catch (tf2::TransformException &ex) { + yaw = pitch = roll = 0; + } + cameraAxis.x = 0; + cameraAxis.y = 0; + cameraAxis.z = 1; + Mat4D frameRotation = rotationFromAngleAndUnitAxis(yaw, cameraAxis); + Mat4D frameTranslation = translationMatrix(transformStamped.transform.translation.x, transformStamped.transform.translation.y, transformStamped.transform.translation.z); + frame[1] = matrixMultiply(frameTranslation, frameRotation); + + // Again, but get utm -> base_link heading (not a unit vector) + try { + transformStamped = tfBuffer.lookupTransform("utm", "base_link", ros::Time(0)); + tf::Quaternion myQuaternion(transformStamped.transform.rotation.x, transformStamped.transform.rotation.y, transformStamped.transform.rotation.z, transformStamped.transform.rotation.w); + tf::Matrix3x3 mat(myQuaternion); + mat.getEulerYPR(kalmanYaw, pitch, roll); + } catch (tf2::TransformException &ex) { + kalmanYaw = pitch = roll = 0; + } + + + frame[2] = mImuListener.orientation; + + frameTranslation = translationMatrix(20, 20, 0); + frame[3] = scaleMatrix( mImuListener.acceleration.x/9.81+1, mImuListener.acceleration.y/9.81+1, mImuListener.acceleration.z/9.81+1); + frame[3] = matrixMultiply(frameTranslation, frame[3]); + + frameTranslation = translationMatrix(-20, 20, 0); + frame[4] = scaleMatrix( mImuListener.rotationRate.x+1, mImuListener.rotationRate.y+1, mImuListener.rotationRate.z+1); + frame[4] = matrixMultiply(frameTranslation, frame[4]); + + for(int i = 0; i < NUM_FRAMES; i++) { + numEdges = sizeof(cubeQuadIndices)/sizeof(cubeQuadIndices[0]); + + Mat4D scale = scaleMatrix(10, 1, 1); + Mat4D translation = translationMatrix(1, 0, 0); + Mat4D model = matrixMultiply(scale, translation); + model = matrixMultiply(frame[i], model); + Mat4D modelView = matrixMultiply(viewMatrix, model); + Coordinates3D color = {1, 0, 0}; +// rasterizeQuadsShader(cube, cubeQuadIndices, numEdges, modelView, projection, windowFull, (void*)&color, &depthBuffer, lightModelFs, debugLine); + mRenderPipeline.setFragmentShader(lightModelFs); + mRenderPipeline.rasterizeQuadsShader(cube, cubeQuadIndices, numEdges, modelView, projection, mRenderPipeline.viewport, (void*)&color, debugLine); + + scale= scaleMatrix(1, 10, 1); + translation = translationMatrix(0, 1, 0); + model = matrixMultiply(scale, translation); + model = matrixMultiply(frame[i], model); + modelView = matrixMultiply(viewMatrix, model); + color = {0, 1, 0}; +// rasterizeQuadsShader(cube, cubeQuadIndices, numEdges, modelView, projection, windowFull, (void*)&color, &depthBuffer, lightModelFs, debugLine); + mRenderPipeline.setFragmentShader(lightModelFs); + mRenderPipeline.rasterizeQuadsShader(cube, cubeQuadIndices, numEdges, modelView, projection, mRenderPipeline.viewport, (void*)&color, debugLine); + + scale = scaleMatrix(1, 1, 10); + translation = translationMatrix(0, 0, 1); + model = matrixMultiply(scale, translation); + model = matrixMultiply(frame[i], model); + modelView = matrixMultiply(viewMatrix, model); + color = {0, 0, 1}; +// rasterizeQuadsShader(cube, cubeQuadIndices, numEdges, modelView, projection, windowFull, (void*)&color, &depthBuffer, lightModelFs, debugLine); + mRenderPipeline.setFragmentShader(lightModelFs); + mRenderPipeline.rasterizeQuadsShader(cube, cubeQuadIndices, numEdges, modelView, projection, mRenderPipeline.viewport, (void*)&color, debugLine); + } + } int numObjects = 16; double carTranslationLongitude = -0; // -8 - double carScale = 0.2; + double carScale = 1;//0.2; for( int i = 0; i < numObjects; i++) { - Mat4D scale = scaleMatrix(0.15, 0.15, 0.15); + Mat4D scale = scaleMatrix(0.15*5, 0.15*5, 0.15*5); // Mat4D translation = translationMatrix(30 + 40*sin(lightAngle*(i+1)/(double)numObjects), i-numObjects/2, 1); // Notice I swapped x and y here: - Mat4D translation = translationMatrix(-mRadarListener.points[i].x*carScale , -mRadarListener.points[i].y*carScale+ carTranslationLongitude, 1.7272*carScale/2); + Mat4D translation = translationMatrix(mRadarListener.points[i].x*carScale+ carTranslationLongitude, -mRadarListener.points[i].y*carScale, 1.7272*carScale/2); Mat4D model = matrixMultiply(translation, scale); Mat4D objectModelView = matrixMultiply(viewMatrix, model); numEdges = sizeof(cubeQuadIndices)/sizeof(cubeQuadIndices[0]); Coordinates3D color = {0,0,1}; - rasterizeQuadsShader(cube, cubeQuadIndices, numEdges, objectModelView, projection, windowFull, (void*)&color, &depthBuffer, lightModelFs, debugLine); +// rasterizeQuadsShader(cube, cubeQuadIndices, numEdges, objectModelView, projection, windowFull, (void*)&color, &depthBuffer, lightModelFs, debugLine); + mRenderPipeline.setFragmentShader(lightModelFs); + mRenderPipeline.rasterizeQuadsShader(cube, cubeQuadIndices, numEdges, objectModelView, projection, mRenderPipeline.viewport, (void*)&color, debugLine); } // Grid: if (showGrid) { - double latitudeInMeters = mGpsListener.longitude * METERS_PER_DEGREE; - double longitudeInMeters = mGpsListener.latitude * METERS_PER_DEGREE; +// double latitudeInMeters = mGpsListener.longitude * METERS_PER_DEGREE; +// double longitudeInMeters = mGpsListener.latitude * METERS_PER_DEGREE; + double latitudeInMeters = -mGpsListener.y; + double longitudeInMeters = -mGpsListener.x; - double offestLat = fmod(longitudeInMeters *carScale, GRID_LINE_UNIT ); - double offestLong = fmod(latitudeInMeters*carScale, GRID_LINE_UNIT ); + double offestLat = fmod(latitudeInMeters *carScale, GRID_LINE_UNIT ); + double offestLong = fmod(longitudeInMeters*carScale, GRID_LINE_UNIT ); - Mat4D gridOffset = translationMatrix(offestLat, offestLong, 0 ); + Mat4D gridOffset = translationMatrix(offestLong, offestLat, 0 ); cameraAxis.x = 0; cameraAxis.y = 0; cameraAxis.z = 1; - Mat4D gridRotation = rotationFromAngleAndUnitAxis(-mGpsListener.heading - M_PI/2.0, cameraAxis); +// Mat4D gridRotation = rotationFromAngleAndUnitAxis(-mGpsListener.heading - M_PI/2.0, cameraAxis); + Mat4D gridRotation = rotationFromAngleAndUnitAxis(-mGpsListener.theta, cameraAxis); Coordinates4D grid2[sizeof(grid)/sizeof(grid[0])]; for (int i = 0; i < sizeof(grid2)/sizeof(grid2[0]); i++) { @@ -481,24 +1099,77 @@ int main(int argc, char **argv) { } numEdges = sizeof(gridEdgeIndices)/sizeof(gridEdgeIndices[0]); - rasterize(grid2, gridEdgeIndices, numEdges, windowFull, &depthBuffer); +// rasterize(grid2, gridEdgeIndices, numEdges, windowFull, &depthBuffer); + + + // Square floor: + + testIncrementor += 0.03; + testLong += 0.000010 * cos(testIncrementor*0.1); + testLat += 0.00001 * sin(testIncrementor*0.1); +// mMapHandler.setLongLat(testLong, testLat); + + mMapHandler.setLongLat(mGpsListener.longitudeRaw, mGpsListener.latitudeRaw); + + // mRenderPipeline.trianglesFill(squareVi, squareViIndices, 2); +// Mat4D modelTranslation = translationMatrix(0, 0, 0 ); +// Mat4D modelScale = mMapHandler.mapModel;// scaleMatrix(30, 30, 30); +// Mat4D modelSquare = matrixMultiply(modelTranslation, modelScale); + Mat4D modelViewMap = matrixMultiply(viewMatrix, mMapHandler.mapModel); + mUniformInfo.modelView = modelViewMap; + // mUniformInfo.modelView = modelSquare; + mUniformInfo.modelViewProjection = matrixMultiply(projection, mUniformInfo.modelView); + + // Map + + mRenderPipeline.setFragmentShader(mapShader); + mRenderPipeline.rasterizeShader(squareVi, &mUniformInfo, squareViIndices, 2, (void*)&mMapHandler.map[0], myVertexShader); + } // Car: Coordinates3D carColor = {0,1,1}; - Mat4D carScaleMatrix = scaleMatrix(1.8542*carScale/2, 4.6228*carScale/2, 1.7272*carScale/2); + Mat4D carScaleMatrix = scaleMatrix(4.6228*carScale/2, 1.8542*carScale/2, 1.7272*carScale/2); Mat4D carTranslation = translationMatrix(0, carTranslationLongitude, 1.7272*carScale/2 ); Mat4D carModel = matrixMultiply(carTranslation, carScaleMatrix); Mat4D carModelView = matrixMultiply(viewMatrix, carModel); numEdges = sizeof(cubeQuadIndices)/sizeof(cubeQuadIndices[0]); - rasterizeQuadsShader(cube, cubeQuadIndices, numEdges, carModelView, projection, windowFull, (void*)&carColor, &depthBuffer, lightModelFs, debugLine); - +// rasterizeQuadsShader(cube, cubeQuadIndices, numEdges, carModelView, projection, windowFull, (void*)&carColor, &depthBuffer, lightModelFs, debugLine); + mRenderPipeline.setFragmentShader(lightModelFs); + mRenderPipeline.rasterizeQuadsShader(cube, cubeQuadIndices, numEdges, carModelView, projection, mRenderPipeline.viewport, (void*)&carColor, debugLine); -// for (int i = 15; i >= 0; i--) { -// mvaddch(mRadarListener.points[0].x + 10 + i, mRadarListener.points[0].y + 10, '0' + (char)i); -// } + // Covariance circle: + if(mGpsListener.covX == mGpsListener.covX && + mGpsListener.covY == mGpsListener.covY && + mGpsListener.covX != 0 && + mGpsListener.covY != 0) { + cameraAxis.x = 0; + cameraAxis.y = 0; + cameraAxis.z = 1; + Mat4D covScale = scaleMatrix(mGpsListener.covX, mGpsListener.covY, 1); + Mat4D covRotation = rotationFromAngleAndUnitAxis(-mGpsListener.theta, cameraAxis); +// Mat4D covScale = scaleMatrix(10, 10, 1); + Mat4D covTranslation = translationMatrix(0, 0, 0.5 ); + Mat4D covModel = matrixMultiply(covRotation, covScale); + covModel = matrixMultiply(covTranslation, covModel); + Mat4D covModelView = matrixMultiply(viewMatrix, covModel); + Coordinates3D covColor = {0.125, 0, 0.125}; + + +// rasterizePolygonsShader(&unitCircle, 1, covModelView, projection, windowFull, (void*)&covColor, &depthBuffer, lightModelFs, debugLine); + mRenderPipeline.setFragmentShader(lightModelFs); + mRenderPipeline.rasterizePolygonsShader(&unitCircle, 1, covModelView, projection, mRenderPipeline.viewport, (void*)&covColor, debugLine); + + } + /* + End of 3D rendering + */ + mRenderPipeline.renderBufferToTerminal(); + /* + Start of HUD rendering + */ Coordinates2D steeringCenter; // double steeringWidth = 15; @@ -519,55 +1190,69 @@ int main(int argc, char **argv) { mGpsListener.draw(0,3); + steeringCenter.x -= steeringWidth*2 + 1; + double angles[2] ; + angles[0] = yaw -4.0*M_PI/180.0; + angles[1] = kalmanYaw; + const char* labels[2]; + labels[0] = "Heading"; + labels[1] = "Kalman"; + drawGuage(angles, 2, steeringCenter, steeringWidth, steeringHeight, labels); +// drawGuage(yaw -4.0*M_PI/180.0, steeringCenter, steeringWidth, steeringHeight, "Heading"); +// +// steeringCenter.x -= steeringWidth*2 + 1; +// drawGuage(kalmanYaw, steeringCenter, steeringWidth, steeringHeight, "Kalman Heading"); + + mImuListener.printMagnetic(30, 0); + if (autoRotate) { angle -= 0.01; } + + int ch; if ((ch = getch()) == 0x1B) { // Escape keepRunning = false; } else if (ch == KEY_RESIZE) { -#ifdef FB_SUPPORT // HACK - //screenSizeY = 64; - //screenSizeX = 64; -#else + getmaxyx(stdscr, screenSizeY, screenSizeX); -#endif + -// mRenderPipeline.resize(screenSizeX, screenSizeY); + mRenderPipeline.resize(screenSizeX, screenSizeY); screenAspect = (double)screenSizeX/(double)screenSizeY / characterAspect; - windowFull = makeWindowTransform(screenSizeX, screenSizeY, characterAspect); - windowFull.d[0][0] *= -1; // HACK the car is aligned with GPS, heading, and radar but mirrores over x. This mirrors the viewport +// windowFull = makeWindowTransform(screenSizeX, screenSizeY, characterAspect); + mRenderPipeline.viewport = makeWindowTransform(screenSizeX, screenSizeY, characterAspect); if (usePerspective) { projection = projectionMatrixPerspective(viewAngle, screenAspect, zFar, zNear); } else { - projection = projectionMatrixOrtho(5*screenAspect, 5, zFar, zNear); + projection = projectionMatrixOrtho(orthoScale*screenAspect, orthoScale, zFar, zNear); } // depthBuffer.setSize(screenSizeX, screenSizeY); - depthBuffer.setSize(screenSizeX, screenSizeY); +// depthBuffer.setSize(screenSizeX, screenSizeY); } else if (ch == 'o' || ch == 'O') { usePerspective = !usePerspective; if (usePerspective) { projection = projectionMatrixPerspective(viewAngle, screenAspect, zFar, zNear); } else { - projection = projectionMatrixOrtho(5*screenAspect, 5, zFar, zNear); + projection = projectionMatrixOrtho(orthoScale*screenAspect, orthoScale, zFar, zNear); } } else if ( ch == 'g' || ch == 'G') { showGrid = !showGrid; } else if ( ch == KEY_LEFT) { - angle -= 0.05; - } else if ( ch == KEY_RIGHT) { angle += 0.05; + } else if ( ch == KEY_RIGHT) { + angle -= 0.05; } else if ( ch == KEY_UP) { tilt += 0.05; } else if ( ch == KEY_DOWN) { tilt -= 0.05; } else if ( ch == 'a' || ch == 'A') { // Left - Mat4D motion = translationMatrix(-0.25, 0, 0); + Mat4D motion = translationMatrix(+1.00, 0, 0); Mat4D camTrans = transpose(cameraOrientation); Mat4D offset = matrixMultiply(camTrans, motion); offset.d[0][0] = 1; offset.d[0][1] = 0; offset.d[0][2] = 0; @@ -576,7 +1261,7 @@ int main(int argc, char **argv) { cameraTranslation = matrixMultiply(offset, cameraTranslation); } else if ( ch == 'd' || ch == 'D') { // Right - Mat4D motion = translationMatrix(+0.25, 0, 0); + Mat4D motion = translationMatrix(-1.00, 0, 0); Mat4D camTrans = transpose(cameraOrientation); Mat4D offset = matrixMultiply(camTrans, motion); offset.d[0][0] = 1; offset.d[0][1] = 0; offset.d[0][2] = 0; @@ -586,7 +1271,7 @@ int main(int argc, char **argv) { } else if ( ch == 'w' || ch == 'W') { // Forward - Mat4D motion = translationMatrix(0, 0, 0.25); + Mat4D motion = translationMatrix(0, 0, 1.00); Mat4D camTrans = transpose(cameraOrientation); Mat4D offset = matrixMultiply(camTrans, motion); offset.d[0][0] = 1; offset.d[0][1] = 0; offset.d[0][2] = 0; @@ -595,7 +1280,7 @@ int main(int argc, char **argv) { cameraTranslation = matrixMultiply(offset, cameraTranslation); } else if ( ch == 's' || ch == 'S') { // Back - Mat4D motion = translationMatrix(0, 0, -0.25); + Mat4D motion = translationMatrix(0, 0, -1.00); Mat4D camTrans = transpose(cameraOrientation); Mat4D offset = matrixMultiply(camTrans, motion); offset.d[0][0] = 1; offset.d[0][1] = 0; offset.d[0][2] = 0; @@ -606,18 +1291,38 @@ int main(int argc, char **argv) { if (usePerspective) { viewAngle += M_PI * 0.0125; projection = projectionMatrixPerspective(viewAngle, screenAspect, zFar, zNear); + } else { + orthoScale += 0.5; + projection = projectionMatrixOrtho(orthoScale*screenAspect, orthoScale, zFar, zNear); } } else if ( ch == 't' || ch == 'T') { if (usePerspective) { viewAngle -= M_PI * 0.0125; projection = projectionMatrixPerspective(viewAngle, screenAspect, zFar, zNear); + } else { + orthoScale -= 0.5; + projection = projectionMatrixOrtho(orthoScale*screenAspect, orthoScale, zFar, zNear); } } else if ( ch == ' ' ) { autoRotate = !autoRotate; + } else if ( ch == '-' || ch == '_' ) { + mMapHandler.zoom--; + } else if ( ch == '=' || ch == '+' ) { + mMapHandler.zoom++; } else if ( ch == 'd' || ch == 'D' ) { showDepth = !showDepth; + } else if ( ch == 'u' || ch == 'U' ) { + showUnitVectors = !showUnitVectors; + } else if ( ch == 'l' || ch == 'L') { + mLocalizationMethod = (GpsListener::LocalizationMethod)(((int)mLocalizationMethod) + 1); + if(mLocalizationMethod == GpsListener::LOCALIZATION_COUNT) { + mLocalizationMethod = (GpsListener::LocalizationMethod)0; + } + + mGpsListener.setLocalizationMethod(mLocalizationMethod); } + ros::spinOnce(); }