diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index e69de29bb..000000000 diff --git a/rover-config/.motion/camera0.conf b/rover-config/.motion/camera0.conf deleted file mode 100644 index 0a6d98a78..000000000 --- a/rover-config/.motion/camera0.conf +++ /dev/null @@ -1,2 +0,0 @@ -videodevice /dev/video20 -stream_port 8080 diff --git a/rover-config/.motion/camera1.conf b/rover-config/.motion/camera1.conf deleted file mode 100644 index 44d8ee8ce..000000000 --- a/rover-config/.motion/camera1.conf +++ /dev/null @@ -1,2 +0,0 @@ -videodevice /dev/video30 -stream_port 8081 diff --git a/rover-config/.motion/camera2.conf b/rover-config/.motion/camera2.conf deleted file mode 100644 index 7c61b9658..000000000 --- a/rover-config/.motion/camera2.conf +++ /dev/null @@ -1,2 +0,0 @@ -videodevice /dev/video40 -stream_port 8082 diff --git a/rover-config/.motion/motion.conf b/rover-config/.motion/motion.conf deleted file mode 100644 index 0a0cb8c6c..000000000 --- a/rover-config/.motion/motion.conf +++ /dev/null @@ -1,30 +0,0 @@ -# Setup: -# $ sudo apt install motion -# See also: https://gist.github.com/endolith/2052778 -# -# This file should be saved in ~/.motion/motion.conf -# -# Start motion from a terminal by simply running: -# $ motion - -stream_localhost off -stream_motion on -#stream_maxrate 15 -#stream_quality 70 -stream_maxrate 30 -stream_quality 99 -# -#minimum_frame_time 1 # handy for collecting camera calibration data, maybe - -# This was called `output_pictures` in motion 4.0 -# Current Ubuntu packaged version is 4.0, but we use 4.2 by installing from source -# check https://motion-project.github.io/motion_config.html -# and https://motion-project.github.io/motion_build.html -picture_output off - -# This is only available in motion 4.2 -#stream_grey on - -camera /home/husky/.motion/camera0.conf -camera /home/husky/.motion/camera1.conf -#camera /home/husky/.motion/camera2.conf diff --git a/rover-config/README.md b/rover-config/README.md index 188a2cf32..b51dfb97c 100644 --- a/rover-config/README.md +++ b/rover-config/README.md @@ -8,7 +8,6 @@ filesystem. | **File/Directory** | **Location on Rover** | **Description** | | ----------------- | -------------------- | -------------- | -| `.motion/` | `/home/$USER/` | Configuration files for Motion (currently handles our video streaming.) | | `50-usb-hokuyo-lidar.rules` | `/etc/udev/rules.d/` | `udev` rules for making sure we have permissions to access the lidar over USB. | | `50-rover-cameras.rules` | `/etc/udev/rules.d/` | `udev` rules for making sure cameras have stable IDs. | diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index cc76f9fa5..5cfbf036a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -273,13 +273,8 @@ list(APPEND simulator_libs add_executable(Rover Rover.cpp) add_library(rover_common SHARED Globals.cpp - rospub.cpp network/websocket/WebSocketServer.cpp network/websocket/WebSocketProtocol.cpp - worldmap/GlobalMap.cpp - worldmap/TrICP.cpp - worldmap/QuadTree.cpp - planning/plan.cpp ) target_link_libraries(rover_common utils @@ -318,7 +313,6 @@ target_link_libraries(TunePID ${vision_libs}) if(WITH_TESTS) add_executable(tests Tests.cpp - rospub_test.cpp # AR Detection tests ar/DetectorTests.cpp ar/MarkerSetTests.cpp @@ -341,9 +335,6 @@ if(WITH_TESTS) ../tests/filters/MultiSensorEKFTest.cpp ../tests/filters/StateSpaceUtilsTest.cpp ../tests/filters/FullPoseEstimatorTest.cpp - ../tests/worldmap/TrICPTest.cpp - ../tests/worldmap/GlobalMapTest.cpp - ../tests/worldmap/QuadTreeTest.cpp # Command tests ../tests/commands/DriveToWaypointCommandTest.cpp # Util tests diff --git a/src/Rover.cpp b/src/Rover.cpp index a278056be..05bcd42c1 100644 --- a/src/Rover.cpp +++ b/src/Rover.cpp @@ -2,7 +2,6 @@ #include "Globals.h" #include "navtypes.h" #include "network/MissionControlProtocol.h" -#include "rospub.h" #include "world_interface/world_interface.h" #include @@ -30,7 +29,6 @@ using namespace robot::types; void closeRover(int signum) { robot::emergencyStop(); - rospub::shutdown(); Globals::websocketServer.stop(); raise(SIGTERM); } @@ -188,7 +186,6 @@ int main(int argc, char** argv) { robot::world_interface_init(); auto mcProto = std::make_unique(Globals::websocketServer); Globals::websocketServer.addProtocol(std::move(mcProto)); - rospub::init(); // Ctrl+C doesn't stop the simulation without this line signal(SIGINT, closeRover); diff --git a/src/WorldData.h b/src/WorldData.h deleted file mode 100644 index 64bea46e6..000000000 --- a/src/WorldData.h +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -#include "lidar/PointCloudProcessing.h" - -class WorldData { -public: - virtual PointXY getGPS() = 0; - virtual float getHeading() = 0; - virtual bool lidarSees() = 0; - virtual float targetDistance() = 0; -}; diff --git a/src/gps/example_gps_legs.txt b/src/gps/example_gps_legs.txt deleted file mode 100644 index a75d61891..000000000 --- a/src/gps/example_gps_legs.txt +++ /dev/null @@ -1,12 +0,0 @@ -Example simulator legs close to UW GPS coordinates -This file is properly formatted and will be parsed correctly - -Leg 1: -0 -1 47.653116 -122.305619 - -Leg 2: -1 2 47.653125 -122.305486 - -3 4 47.653325 -122.325486 (Leg 3) - -5 -1 47.653525 -123.305486 diff --git a/src/gps/simulator_legs.txt b/src/gps/simulator_legs.txt deleted file mode 100644 index db4e4ca7b..000000000 --- a/src/gps/simulator_legs.txt +++ /dev/null @@ -1,17 +0,0 @@ -Coordinates for the default configuration of simulator legs -Note these are in meters, not GPS degrees. -Also note: (lat, long) corresponds to (y,x) not (x,y) - -0 -1 0 50 - -1 -1 50 25 - -2 -1 50 -25 - -3 4 0 -50 - -5 6 -50 -25 - -7 8 -50 25 - -9 10 5 -2 diff --git a/src/gps/zero_legs.txt b/src/gps/zero_legs.txt deleted file mode 100644 index 3c0bfaa8b..000000000 --- a/src/gps/zero_legs.txt +++ /dev/null @@ -1,16 +0,0 @@ -Autonomy leg configuration where we have no prior information -(just assume all landmarks are at 0,0). - -0 -1 0 0 - -1 -1 0 0 - -2 -1 0 0 - -3 4 0 0 - -5 6 0 0 - -7 8 0 0 - -9 10 0 0 diff --git a/src/math/PointXY.h b/src/math/PointXY.h deleted file mode 100644 index 231e1fce5..000000000 --- a/src/math/PointXY.h +++ /dev/null @@ -1,9 +0,0 @@ -#pragma once - -/** - @deprecated Will eventually be phased out in favor of point_t. - */ -struct PointXY { - float x; - float y; -}; diff --git a/src/network/tests.cpp b/src/network/tests.cpp deleted file mode 100644 index 9871fba6d..000000000 --- a/src/network/tests.cpp +++ /dev/null @@ -1,374 +0,0 @@ -#include "../Globals.h" -#include "../world_interface/world_interface.h" -#include "IK.h" -#include "NetworkingStubs.h" -#include "ParseBaseStation.h" -#include "ParseCAN.h" -#include "TestPackets.h" -#include "motor_interface.h" -extern "C" { -#include "../HindsightCAN/CANMotorUnit.h" -#include "../HindsightCAN/Port.h" -} -#include - -#include - -using nlohmann::json; -using namespace robot; - -void setupEncoders() { - int32_t start_angle = 1337; - Globals::status_data["arm_base"]["angular_position"] = start_angle; - Globals::status_data["shoulder"]["angular_position"] = start_angle; - Globals::status_data["elbow"]["angular_position"] = start_angle; -} - -TEST_CASE("Basic CAN ID construction", "[CAN]") { - uint16_t CAN_ID = ConstructCANID(PACKET_PRIORITY_NORMAL, DEVICE_GROUP_MOTOR_CONTROL, 0x05); - uint16_t EXPECTED = 0x0505; - REQUIRE(CAN_ID == EXPECTED); -} - -TEST_CASE("Basic packet creation", "[CAN]") { - uint16_t CAN_ID = ConstructCANID(PACKET_PRIORITY_NORMAL, DEVICE_GROUP_MOTOR_CONTROL, 0x05); - uint8_t testDataPacket[2]; - testDataPacket[0] = 0x60; - testDataPacket[1] = 0x43; - CANPacket packet = ConstructCANPacket(CAN_ID, 0x02, testDataPacket); - - REQUIRE(packet.id == 0x0505); - REQUIRE(packet.dlc == 0x02); - REQUIRE(packet.data[0] == 0x60); - REQUIRE(packet.data[1] == 0x43); -} - -TEST_CASE("ParseCAN can handle telemetry", "[CAN]") { - ParseCANPacket(motorTelemetry()); - std::cout << Globals::status_data << std::endl; - REQUIRE(Globals::status_data.dump() == "{\"front_right_wheel\":{\"current\":256}}"); -} - -TEST_CASE("Can handle malformed packets", "[BaseStation]") { - char const* msg = "{\"type\": \"moto"; - REQUIRE(ParseBaseStationPacket(msg) == false); - json m = json::parse(popBaseStationPacket()); - REQUIRE(m["msg"] == "Parse error"); -} - -TEST_CASE("Ignores invalid motor names", "[BaseStation]") { - char const* msg = "{\"type\": \"motor\", \"motor\": \"xyzzy\", \"mode\": \"PID\"}"; - REQUIRE(ParseBaseStationPacket(msg) == false); - json m = json::parse(popBaseStationPacket()); - REQUIRE(m["msg"] == "Unrecognized motor xyzzy"); -} - -TEST_CASE("Does not allow incremental PID without encoder data", "[BaseStation]") { - clearTestGlobals(); - char const* msg = - "{\"type\": \"motor\", \"motor\": \"arm_base\", \"incremental PID speed\":1.0}"; - REQUIRE(ParseBaseStationPacket(msg) == false); - json m = json::parse(popBaseStationPacket()); - REQUIRE(m["msg"] == "Cannot use PID for motor arm_base: No encoder data yet"); - REQUIRE(numCANPackets() == 0); -} - -TEST_CASE("Allows IK axis as 'motor' when using IK", "[BaseStation]") { - clearTestGlobals(); - setupEncoders(); - char const* msg = - "{\"type\": \"motor\", \"motor\": \"IK_X\", \"incremental IK speed\":1.0}"; - REQUIRE(ParseBaseStationPacket(msg) == false); - REQUIRE(Globals::status_data["arm_base"]["operation_mode"] == "incremental IK speed"); - REQUIRE(Globals::status_data["shoulder"]["operation_mode"] == "incremental IK speed"); - REQUIRE(Globals::status_data["elbow"]["operation_mode"] == "incremental IK speed"); - json m = json::parse(popBaseStationPacket()); - REQUIRE(m["msg"] == "IK is not yet supported"); - // TODO once implemented, make sure IK incremental speed values are set properly - // REQUIRE(Globals::status_data["IK_X"]["???"] == "???"); -} - -TEST_CASE("Does not allow IK axis as 'motor' for non-IK modes", "[BaseStation]") { - clearTestGlobals(); - char const* msg = - "{\"type\": \"motor\", \"motor\": \"IK_X\", \"incremental PID speed\":1.0}"; - REQUIRE(ParseBaseStationPacket(msg) == false); - json m = json::parse(popBaseStationPacket()); - REQUIRE(m["msg"] == "Invalid operation mode 'incremental PID speed' for motor IK_X"); -} - -TEST_CASE("Incremental PID", "[BaseStation]") { - clearTestGlobals(); - int32_t start_angle = 1337; - Globals::status_data["arm_base"]["angular_position"] = start_angle; - CANPacket p; - - char const* msg = - "{\"type\": \"motor\", \"motor\": \"arm_base\", \"incremental PID speed\":1.0}"; - REQUIRE(ParseBaseStationPacket(msg) == true); - REQUIRE(numCANPackets() == 4); // mode set + 3 PID coefficients - p = popCANPacket(); - REQUIRE(GetDeviceSerialNumber(&p) == 1); - REQUIRE(PacketIsOfID(&p, ID_MOTOR_UNIT_MODE_SEL)); - REQUIRE(p.data[1] == MOTOR_UNIT_MODE_PID); - p = popCANPacket(); - REQUIRE(PacketIsOfID(&p, ID_MOTOR_UNIT_PID_P_SET)); - p = popCANPacket(); - REQUIRE(PacketIsOfID(&p, ID_MOTOR_UNIT_PID_I_SET)); - p = popCANPacket(); - REQUIRE(PacketIsOfID(&p, ID_MOTOR_UNIT_PID_D_SET)); - - REQUIRE(Globals::status_data["arm_base"]["operation_mode"] == "incremental PID speed"); - REQUIRE(Globals::status_data["arm_base"]["target_angle"] == start_angle); - int expected_increment = 360 * 1000 * ((M_PI / 8) / (2 * M_PI)) / 10; - REQUIRE(Globals::status_data["arm_base"]["millideg_per_control_loop"] == - expected_increment); - REQUIRE(Globals::status_data["arm_base"]["most_recent_command"] > 0); - - msg = "{\"type\": \"motor\", \"motor\": \"arm_base\", \"incremental PID speed\":-0.5}"; - REQUIRE(ParseBaseStationPacket(msg) == true); - REQUIRE(numCANPackets() == 0); // Shouldn't change the mode a second time - expected_increment *= -0.5; - REQUIRE(Globals::status_data["arm_base"]["millideg_per_control_loop"] == - expected_increment); - - incrementArm(); - REQUIRE(numCANPackets() == 1); - p = popCANPacket(); - REQUIRE(GetDeviceSerialNumber(&p) == 1); - REQUIRE(PacketIsOfID(&p, ID_MOTOR_UNIT_PID_POS_TGT_SET)); - REQUIRE(GetPIDTargetFromPacket(&p) == start_angle + expected_increment); - - incrementArm(); - REQUIRE(numCANPackets() == 1); - p = popCANPacket(); - REQUIRE(GetDeviceSerialNumber(&p) == 1); - REQUIRE(PacketIsOfID(&p, ID_MOTOR_UNIT_PID_POS_TGT_SET)); - REQUIRE(GetPIDTargetFromPacket(&p) == start_angle + 2 * expected_increment); - - // Artificially trigger the timeout - Globals::status_data["arm_base"]["most_recent_command"] = 0; - incrementArm(); - REQUIRE(numCANPackets() == 0); - REQUIRE(Globals::status_data["arm_base"]["millideg_per_control_loop"] == 0); - - // Should clear incremental PID state if we switch back to PWM - msg = "{\"type\": \"motor\", \"motor\": \"arm_base\", \"PWM target\":-0.5}"; - REQUIRE(ParseBaseStationPacket(msg) == true); - REQUIRE(Globals::status_data["arm_base"]["millideg_per_control_loop"].is_null()); - REQUIRE(Globals::status_data["arm_base"]["target_angle"].is_null()); -} - -TEST_CASE("Does not allow incremental PID for non-PID motors", "[BaseStation]") { - clearTestGlobals(); - setupEncoders(); - char const* msg = - "{\"type\": \"motor\", \"motor\": \"hand\", \"incremental PID speed\":1.0}"; - REQUIRE(ParseBaseStationPacket(msg) == false); - json m = json::parse(popBaseStationPacket()); - REQUIRE(m["msg"] == "Invalid operation mode 'incremental PID speed' for motor hand"); -} - -TEST_CASE("Can handle drive packets", "[BaseStation]") { - char const* msg = "{\"type\":\"drive\",\"forward_backward\":0.3,\"left_right\":-0.4}"; - REQUIRE(ParseBaseStationPacket(msg) == true); - REQUIRE(numCANPackets() == 4); - for (uint8_t i = 8; i < 12; i++) { - CANPacket p = popCANPacket(); - // We expect the packets to be sent in ascending order, starting with - // the front-left wheel. - REQUIRE(GetDeviceSerialNumber(&p) == i); - REQUIRE(PacketIsOfID(&p, ID_MOTOR_UNIT_PWM_DIR_SET)); - } -} - -TEST_CASE("Can handle malformed drive packets", "[BaseStation]") { - char const* msg = "{\"type\":\"drive\",\"forward_backward\":2.9}"; - REQUIRE(ParseBaseStationPacket(msg) == false); -} - -CANPacket popPIDPkt() { - CANPacket p; - while (!PacketIsOfID(&p, ID_MOTOR_UNIT_PID_POS_TGT_SET)) - p = popCANPacket(); - return p; -} - -void assert_IK_equals(double base, double shoulder, double elbow) { - CANPacket p; - p = popPIDPkt(); - int base_val = DecodeBytesToIntMSBFirst(p.data, 1, 5); - REQUIRE(intToRad(base_val, 1) == Approx(base).margin(0.01)); - - p = popPIDPkt(); - int shoulder_val = DecodeBytesToIntMSBFirst(p.data, 1, 5); - REQUIRE(intToRad(shoulder_val, 2) == Approx(shoulder).margin(0.01)); - - p = popPIDPkt(); - int elbow_val = DecodeBytesToIntMSBFirst(p.data, 1, 5); - REQUIRE(intToRad(elbow_val, 3) == Approx(elbow).margin(0.01)); -} - -void set_radian_arm_angles(double arm_base, double shoulder, double elbow) { - Globals::status_data["arm_base"]["angular_position"] = radToInt(arm_base, 1); - Globals::status_data["shoulder"]["angular_position"] = radToInt(shoulder, 2); - Globals::status_data["elbow"]["angular_position"] = radToInt(elbow, 3); -} - -void assert_FK_equals(double x, double y, double z) { - std::array xyz = forward_kinematics(); - REQUIRE(x == Approx(xyz[0]).margin(0.01)); - REQUIRE(y == Approx(xyz[1]).margin(0.01)); - REQUIRE(z == Approx(xyz[2]).margin(0.01)); -} - -TEST_CASE("radToInt and intToRad", "[BaseStation]") { - for (int i = -360 * 1000; i < 360 * 1000; i += 56565) { - for (int serial = 1; serial < 4; serial++) { - // printf("%d (serial %d) %f %d\n", i, serial, intToRad(i, serial), - // radToInt(intToRad(i, serial), serial)); - REQUIRE(abs(radToInt(intToRad(i, serial), serial) - i) < 5); - } - } -} - -TEST_CASE("Forward kinematics in stowed position", "[BaseStation]") { - Globals::status_data["arm_base"]["angular_position"] = 0; - Globals::status_data["shoulder"]["angular_position"] = 0; - Globals::status_data["elbow"]["angular_position"] = 0; - assert_FK_equals(0.119867, 0., 0.0152843); -} - -TEST_CASE("Forward kinematics at full extension", "[BaseStation]") { - double full_extension = Constants::SHOULDER_LENGTH + Constants::ELBOW_LENGTH; - - set_radian_arm_angles(0., 0., 0.); - assert_FK_equals(full_extension, 0., 0.); - - set_radian_arm_angles(M_PI / 2, 0., 0.); - assert_FK_equals(0., full_extension, 0.); - - set_radian_arm_angles(0., M_PI / 2, 0.); - assert_FK_equals(0., 0., full_extension); -} - -TEST_CASE("Forward kinematics near the ground", "[BaseStation]") { - set_radian_arm_angles(0.9, 1.1, 2.3); - assert_FK_equals(0.326845, 0.411873, -0.117700); -} - -TEST_CASE("Can handle IK packets", "[BaseStation]") { - clearTestGlobals(); - setupEncoders(); - char const* msg = "{\"type\":\"ik\",\"wrist_base_target\":[1.2, 0.0, 0.0]}"; - REQUIRE(ParseBaseStationPacket(msg) == true); - assert_IK_equals(0., 0.428172, 0.792013); -} - -TEST_CASE("Can reach targets below the ground plane", "[BaseStation]") { - clearTestGlobals(); - setupEncoders(); - char const* msg = "{\"type\":\"ik\",\"wrist_base_target\":[0.6, 0.8, -0.2]}"; - REQUIRE(ParseBaseStationPacket(msg) == true); - assert_IK_equals(0.927291, 0.534880, 1.342617); -} - -// Not sure if we actually need the rover to do this during the competition, -// but it doesn't hurt to be prepared. -// This also helps make sure the robot doesn't go too crazy near the vertical pole? -TEST_CASE("Can reach some targets behind the vertical", "[BaseStation]") { - clearTestGlobals(); - setupEncoders(); - char const* msg = "{\"type\":\"ik\",\"wrist_base_target\":[-0.1, 0.0, 1.26]}"; - REQUIRE(ParseBaseStationPacket(msg) == true); - assert_IK_equals(0., 1.905204, 0.473425); -} - -TEST_CASE("Can handle degenerate IK packets", "[BaseStation]") { - setupEncoders(); - char const* msg = "{\"type\":\"ik\"}"; - REQUIRE(ParseBaseStationPacket(msg) == true); - msg = "{\"type\":\"ik\",\"wrist_base_target\":\"asdf\"}"; - REQUIRE(ParseBaseStationPacket(msg) == false); -} - -TEST_CASE("Reports error if IK target is infeasible", "[BaseStation]") { - clearTestGlobals(); - setupEncoders(); - char const* msg = "{\"type\":\"ik\",\"wrist_base_target\":[100.0, 0.0, 0.0]}"; - REQUIRE(ParseBaseStationPacket(msg) == false); - json m = json::parse(popBaseStationPacket()); - REQUIRE(m["msg"] == "Infeasible IK target"); -} - -TEST_CASE("Respects joint limits", "[BaseStation]") { - clearTestGlobals(); - setupEncoders(); - char const* msg = "{\"type\":\"ik\",\"wrist_base_target\":[-1.0, 0.0, 0.0]}"; - REQUIRE(ParseBaseStationPacket(msg) == false); - json m = json::parse(popBaseStationPacket()); - REQUIRE(m["msg"] == "IK solution outside joint limits for shoulder"); -} - -TEST_CASE("Forward kinematics is the inverse of inverse kinematics", "[BaseStation]") { - clearTestGlobals(); - setupEncoders(); - char const* msg = "{\"type\":\"ik\",\"wrist_base_target\":[0.6, -0.3, -0.1]}"; - REQUIRE(ParseBaseStationPacket(msg) == true); - assert_IK_equals(-0.463647, 1.005327, 2.053642); - set_radian_arm_angles(-0.463647, 1.005327, 2.053642); - assert_FK_equals(0.6, -0.3, -0.1); -} - -TEST_CASE("Deactivates wheel motors if e-stopped", "e-stop") { - clearTestGlobals(); - char const* estop_msg = "{\"type\":\"estop\",\"release\":false}"; - char const* drive_msg = - "{\"type\":\"drive\",\"forward_backward\":0.3,\"left_right\":-0.4}"; - json m; - CANPacket p; - REQUIRE(ParseBaseStationPacket(drive_msg) == true); - p = popCANPacket(); - REQUIRE(PacketIsOfID(&p, ID_MOTOR_UNIT_PWM_DIR_SET)); - REQUIRE(GetPWMFromPacket(&p) != 0); - - clearTestGlobals(); - REQUIRE(ParseBaseStationPacket(estop_msg) == true); - m = json::parse(popBaseStationPacket()); - REQUIRE(m["status"] == "ok"); - // Should send broadcast e-stop - p = popCANPacket(); - REQUIRE(PacketIsOfID(&p, ID_ESTOP)); - REQUIRE(GetDeviceGroupCode(&p) == DEVICE_GROUP_MOTOR_CONTROL); - REQUIRE(GetDeviceSerialNumber(&p) == 0x0); // broadcast - // Should also send individual motor e-stops - for (uint8_t serial = 1; serial < 12; serial++) { - p = popCANPacket(); - REQUIRE(PacketIsOfID(&p, ID_ESTOP)); - REQUIRE(GetDeviceGroupCode(&p) == DEVICE_GROUP_MOTOR_CONTROL); - REQUIRE(GetDeviceSerialNumber(&p) == serial); - } - // Should also set wheel motor PWM to zero (mostly for use in simulation) - p = popCANPacket(); - REQUIRE(PacketIsOfID(&p, ID_MOTOR_UNIT_PWM_DIR_SET)); - REQUIRE(GetPWMFromPacket(&p) == 0); - - clearTestGlobals(); - REQUIRE(ParseBaseStationPacket(drive_msg) == false); - m = json::parse(popBaseStationPacket()); - REQUIRE(m["msg"] == "Emergency stop is activated"); - REQUIRE(numCANPackets() == 0); -} - -TEST_CASE("Does not allow setCmdVel with nonzero values if e-stopped", "e-stop") { - clearTestGlobals(); - char const* estop_msg = "{\"type\":\"estop\",\"release\":false}"; - REQUIRE(ParseBaseStationPacket(estop_msg) == true); - clearTestGlobals(); - REQUIRE(setCmdVel(0.0, 0.1) == false); - REQUIRE(numCANPackets() == 0); - REQUIRE(setCmdVel(-0.1, 0.0) == false); - REQUIRE(numCANPackets() == 0); - REQUIRE(setCmdVel(0.0, 0.0) == true); - REQUIRE(numCANPackets() == 4); -} diff --git a/src/planning/plan.cpp b/src/planning/plan.cpp deleted file mode 100644 index 71e0a03da..000000000 --- a/src/planning/plan.cpp +++ /dev/null @@ -1,185 +0,0 @@ - -#include "plan.h" - -#include "../Constants.h" -#include "../utils/ScopedTimer.h" -#include "../utils/transform.h" - -#include -#include -#include -#include - -#include - -constexpr double EPSILON = 1.2; // heuristic weight for weighted A* - -using namespace Constants::Nav; -using util::toTransform; -using namespace navtypes; - -// TODO implement goal orientations? -double heuristic(int x, int y, const point_t& goal) { - double dx = goal(0) - x * PLAN_RESOLUTION; - double dy = goal(1) - y * PLAN_RESOLUTION; - return sqrt(dx * dx + dy * dy); -} - -struct Node { - int x; - int y; - int theta; // allows values 0..7; gives angle in increments of pi/4 - double acc_cost; // accumulated cost - double heuristic_to_goal; - int acc_steps; // for knowing how long (in discrete steps) the plan will be - struct Node* parent; - // action: the action taken to reach this node from the parent node - // Tracking this makes it easier to construct the plan later. - action_t action; - - // Note: In the robot frame, starting pose is always (0,0,0). - Node(Node* p, action_t& a, const point_t& goal) - : x(0), y(0), theta(0), acc_cost(0.), heuristic_to_goal(0.), acc_steps(0), parent(p), - action(a) { - if (p != nullptr) { - acc_steps = p->acc_steps + 1; - bool moving_forward = action(1) > 0.0; - double step_cost = moving_forward ? action(1) : RADIAN_COST * fabs(action(0)); - acc_cost = p->acc_cost + step_cost; - if (moving_forward) { - theta = p->theta; - int dx(0), dy(0); - if (theta >= 1 && theta <= 3) - dy = 1; - if (theta >= 3 && theta <= 5) - dx = -1; - if (theta >= 5 && theta <= 7) - dy = -1; - if (theta == 7 || theta <= 1) - dx = 1; - x = p->x + dx; - y = p->y + dy; - } else { - x = p->x; - y = p->y; - theta = (p->theta + ((action(0) > 0.0) ? 1 : 7)) % 8; - } - } - heuristic_to_goal = heuristic(x, y, goal); - } -}; - -std::ostream& operator<<(std::ostream& out, const Node& n) { - out << "(" << n.x << " " << n.y << " " << n.theta << ") "; - return out; -} - -class NodeEqualityCompare { -public: - bool operator()(const Node* lhs, const Node* rhs) const { - bool res = lhs->x < rhs->x || (lhs->x == rhs->x && lhs->y < rhs->y) || - (lhs->x == rhs->x && lhs->y == rhs->y && lhs->theta < rhs->theta); - return res; - } -}; - -class NodeCompare { -public: - bool operator()(const Node* lhs, const Node* rhs) { - return (lhs->acc_cost + EPSILON * (lhs->heuristic_to_goal)) > - (rhs->acc_cost + EPSILON * (rhs->heuristic_to_goal)); - } -}; - -using pqueue_t = std::priority_queue, NodeCompare>; -using set_t = std::set; -using collides_predicate_t = std::function; - -bool is_valid(const Node* n, const collides_predicate_t& collides) { - // To get away from obstacles, turning is always allowed. - if (n->action(1) == 0.0) - return true; - double x = n->x * PLAN_RESOLUTION; - double y = n->y * PLAN_RESOLUTION; - return !collides(x, y, SAFE_RADIUS); -} - -plan_t getPlan(const collides_predicate_t& collides, const point_t& goal, double goal_radius) { - util::ScopedTimer timer; - LOG_F(2, "Planning... "); - action_t action = action_t::Zero(); - std::vector allocated_nodes; - Node* start = new Node(nullptr, action, goal); - allocated_nodes.push_back(start); - pqueue_t fringe; // If you haven't guessed, we'll be using A* - set_t visited_set; - fringe.push(start); - visited_set.insert(start); - Node* n = start; - plan_t valid_actions(3, 2); - valid_actions << 0., 0., M_PI / 4, 0., -M_PI / 4, 0.; - int counter = 0; - bool success = false; - while (fringe.size() > 0) { - if (counter++ > MAX_ITERS) { - break; - } - n = fringe.top(); - fringe.pop(); - if (n->heuristic_to_goal < goal_radius) { - success = true; - break; - } else { - double forward_dist = - (n->theta % 2 == 1) ? PLAN_RESOLUTION * 1.414 : PLAN_RESOLUTION; - valid_actions(0, 1) = forward_dist; - for (int i = 0; i < valid_actions.rows(); i++) { - action = valid_actions.row(i); - Node* next = new Node(n, action, goal); - allocated_nodes.push_back(next); - if (is_valid(next, collides) && visited_set.count(next) == 0) { - visited_set.insert(next); - fringe.push(next); - } - } - } - } - if (success == false) - n = start; - - plan_t plan(n->acc_steps, 2); - for (int i = n->acc_steps - 1; i >= 0; i--) { - plan.row(i) = n->action; - n = n->parent; - } - - std::chrono::milliseconds endTime = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - - LOG_F(2, "finished in %d iterations (%ld visited nodes), Time: %ldms", counter, - allocated_nodes.size(), timer.elapsedTime().count() / 1000); - for (Node* p : allocated_nodes) { - free(p); - } - - return plan; -} - -// Goal given in robot frame -plan_t getPlan(const points_t& lidar_hits, const point_t& goal, double goal_radius) { - collides_predicate_t collidesPredicate = [&](double x, double y, double radius) { - pose_t p = {x, y, 0}; - transform_t trf = toTransform(p); - return util::collides(trf, lidar_hits, SAFE_RADIUS); - }; - return getPlan(collidesPredicate, goal, goal_radius); -} - -double planCostFromIndex(plan_t& plan, int idx) { - double cost = 0; - for (int i = idx; i < plan.rows(); i++) { - action_t action = plan.row(i); - cost += fabs(action(1)) + RADIAN_COST * fabs(action(0)); - } - return cost; -} diff --git a/src/planning/plan.h b/src/planning/plan.h deleted file mode 100644 index 1562824a4..000000000 --- a/src/planning/plan.h +++ /dev/null @@ -1,42 +0,0 @@ - -#ifndef _PLAN_H_ -#define _PLAN_H_ - -#include "../navtypes.h" - -#include - -// Sequence of controls. Each control is a (x distance, theta distance) pair. -using plan_t = Eigen::Matrix; -using action_t = Eigen::Vector2d; - -/** - * Finds a path to the given goal point, stopping when at most goal_radius distance away. - * - * @param collides A collision predicate, used to determine if the given x and y coords are - * a at most a given distance away from any point. Users can implement this callback using - * whatever algorithm they want. Note that the x and y coords will be in the robot's local - * space, at the time of invoking this method. - * @param goal The goal point to pathfind towards. This is in robot local space. - * @param goal_radius The maximum allowable distance to the goal. - * @return A plan that takes the robot from its current position to the goal point. - */ -plan_t getPlan(const std::function& collides, - const navtypes::point_t& goal, double goal_radius); - -/** - * Finds a path to the given goal point, stopping when at most goal_radius distance away. - * This is functionally equivalent to the other getPlan() method, but uses a linear scan - * through the lidar points to check for collisions, instead of something more efficient. - * For large amounts of points, you should prefer the other getPlan() method. - * - * @param lidar_scan A vector of points given by the lidar scan. These should all be in robot - * local space. - * @param goal The goal point to pathfind towards. This is in robot local space. - * @param goal_radius The maximum allowable distance to the goal. - * @return A plan that takes the robot from its current position to the goal point. - */ -plan_t getPlan(const navtypes::points_t& lidar_scan, const navtypes::point_t& goal, double goal_radius); -double planCostFromIndex(plan_t& plan, int idx); - -#endif diff --git a/src/rospub.cpp b/src/rospub.cpp deleted file mode 100644 index 5443b8ae5..000000000 --- a/src/rospub.cpp +++ /dev/null @@ -1,139 +0,0 @@ -#include "rospub.h" - -#include "Constants.h" -#include "navtypes.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -using json = nlohmann::json; -using namespace navtypes; - -namespace rospub { -namespace ws = websocketpp; - -// TODO: Completely remove this in favor of using the new plan reports -/** - @deprecated - */ -class PlanVizServer { -public: - PlanVizServer(); - ~PlanVizServer(); - void run(uint16_t port); - void stop(); - bool running(); - void publish(const pose_t& pose, PointPub topic); - void publish(const points_t& points, ArrayPub topic); - -private: - typedef ws::server server; - typedef std::set> conn_set; - - void onOpen(ws::connection_hdl handle); - void onClose(ws::connection_hdl handle); - void publish(const json& msg); - - bool _running; - server _endpoint; - conn_set _connections; - std::thread _server_thread; -}; - -static PlanVizServer server; - -void init() { - if (!server.running()) { - server.run(Constants::PLANVIZ_SERVER_PORT); - } -} - -void shutdown() { - server.stop(); -} - -void publish(const pose_t& pose, PointPub topic) { - server.publish(pose, topic); -} - -void PlanVizServer::publish(const pose_t& pose, PointPub topic) { - json message; - message["topic"] = topic; - message["x"] = pose(0); - message["y"] = pose(1); - message["theta"] = pose(2); - this->publish(message); -} - -void publish_array(const points_t& points, ArrayPub topic) { - server.publish(points, topic); -} - -void PlanVizServer::publish(const points_t& points, ArrayPub topic) { - json message; - message["topic"] = topic; - message["points"] = json::array(); - for (point_t current : points) { - auto obj = json::object(); - obj["x"] = current(0); - obj["y"] = current(1); - obj["theta"] = current(2); - message["points"].push_back(obj); - } - this->publish(message); -} - -void PlanVizServer::publish(const json& message) { - std::string msg_str = message.dump(); - for (ws::connection_hdl handle : _connections) { - _endpoint.send(handle, msg_str, ws::frame::opcode::text); - } -} - -PlanVizServer::PlanVizServer() { - using ws::lib::placeholders::_1; - _endpoint.init_asio(); - _endpoint.set_open_handler(ws::lib::bind(&PlanVizServer::onOpen, this, _1)); - _endpoint.set_close_handler(ws::lib::bind(&PlanVizServer::onClose, this, _1)); -} - -PlanVizServer::~PlanVizServer() { - this->stop(); -} - -void PlanVizServer::run(uint16_t port) { - if (!_running) { - _endpoint.listen(port); - _endpoint.start_accept(); - _server_thread = std::thread([this] { this->_endpoint.run(); }); - _running = true; - } -} - -void PlanVizServer::onOpen(ws::connection_hdl handle) { - this->_connections.insert(handle); -} - -void PlanVizServer::onClose(ws::connection_hdl handle) { - this->_connections.erase(handle); -} - -void PlanVizServer::stop() { - if (_running) { - _endpoint.stop(); - _server_thread.join(); - _running = false; - } -} - -bool PlanVizServer::running() { - return _running; -} - -} // namespace rospub diff --git a/src/rospub.h b/src/rospub.h deleted file mode 100644 index 79c8dbf74..000000000 --- a/src/rospub.h +++ /dev/null @@ -1,30 +0,0 @@ - -#pragma once - -#include "navtypes.h" - -#include - -namespace rospub { - -enum PointPub { PP_INVALID, PLAN_VIZ, CURRENT_POSE, DRIVE_TARGET, PLAN_TARGET, POSE_GRAPH }; - -enum ArrayPub { AP_INVALID, LIDAR_SCAN, LANDMARKS }; - -NLOHMANN_JSON_SERIALIZE_ENUM(PointPub, {{PP_INVALID, nullptr}, - {PLAN_VIZ, "plan_viz"}, - {CURRENT_POSE, "current_pose"}, - {DRIVE_TARGET, "drive_target"}, - {PLAN_TARGET, "plan_target"}, - {POSE_GRAPH, "pose_graph"}}) - -NLOHMANN_JSON_SERIALIZE_ENUM(ArrayPub, {{AP_INVALID, nullptr}, - {LIDAR_SCAN, "lidar_scan"}, - {LANDMARKS, "landmarks"}}) - -void init(); -void shutdown(); -void publish(const navtypes::pose_t& pose, PointPub topic); // Also can be used for point_t -void publish_array(const navtypes::points_t& points, ArrayPub topic); - -} // namespace rospub diff --git a/src/rospub_test.cpp b/src/rospub_test.cpp deleted file mode 100644 index 53222118d..000000000 --- a/src/rospub_test.cpp +++ /dev/null @@ -1,83 +0,0 @@ -#include "rospub.h" - -#include - -#include - -using json = nlohmann::json; - -namespace rospub { - -constexpr std::array PointPub_values = {PLAN_VIZ, CURRENT_POSE, DRIVE_TARGET, - PLAN_TARGET, POSE_GRAPH}; -constexpr std::array ArrayPub_values = {LIDAR_SCAN, LANDMARKS}; - -TEST_CASE("PointPub enum serialized correctly") { - // go through all possible PointPub values - for (PointPub value : PointPub_values) { - // add the value to a JSON object and serialize it. - json j; - j["value"] = value; - std::string serialized; - REQUIRE_NOTHROW(serialized = j.dump()); - - // deserialize the json message, get the value, and check it. - json deserialized; - REQUIRE_NOTHROW(deserialized = json::parse(serialized)); - PointPub check = deserialized.at("value").get(); - REQUIRE(value == check); - } -} - -TEST_CASE("ArrayPub enum serialized correctly") { - // go through all possible ArrayPub values - for (ArrayPub value : ArrayPub_values) { - // add the value to a JSON object and serialize it. - json j; - j["value"] = value; - std::string serialized; - REQUIRE_NOTHROW(serialized = j.dump()); - - // deserialize the JSON message, get the value, and check it. - json deserialized; - REQUIRE_NOTHROW(deserialized = json::parse(serialized)); - ArrayPub check = deserialized.at("value").get(); - REQUIRE(value == check); - } -} - -TEST_CASE("ArrayPub cannot be deserialized to PointPub") { - // go through all possible ArrayPub values - for (ArrayPub value : ArrayPub_values) { - // add the value to a JSON object and serialize it. - json j; - j["value"] = value; - std::string serialized; - REQUIRE_NOTHROW(serialized = j.dump()); - - // deserialize the JSON message, get the value, and check it. - json deserialized; - REQUIRE_NOTHROW(deserialized = json::parse(serialized)); - PointPub check = deserialized.at("value").get(); - REQUIRE(check == PP_INVALID); - } -} - -TEST_CASE("PointPub cannot be deserialized to ArrayPub") { - // go through all possible PointPub values - for (PointPub value : PointPub_values) { - // add the value to a JSON object and serialize it. - json j; - j["value"] = value; - std::string serialized; - REQUIRE_NOTHROW(serialized = j.dump()); - - // deserialize the JSON message, get the value, and check it. - json deserialized; - REQUIRE_NOTHROW(deserialized = json::parse(serialized)); - ArrayPub check = deserialized.at("value").get(); - REQUIRE(check == AP_INVALID); - } -} - -} // namespace rospub diff --git a/src/worldmap/GlobalMap.cpp b/src/worldmap/GlobalMap.cpp deleted file mode 100644 index 2e00bc505..000000000 --- a/src/worldmap/GlobalMap.cpp +++ /dev/null @@ -1,76 +0,0 @@ -#include "GlobalMap.h" - -#include "../navtypes.h" - -#include - -#include - -using namespace navtypes; - -GlobalMap::GlobalMap(double areaSize, int scanStride, int maxIter, double relErrChangeThresh) - : tree(areaSize), icp(maxIter, relErrChangeThresh, - std::bind(&GlobalMap::getClosest, this, std::placeholders::_1)), - scanStride(scanStride) {} - -points_t GlobalMap::getPoints() const { - return tree.getAllPoints(); -} - -size_t GlobalMap::size() const { - return tree.getSize(); -} - -void GlobalMap::addPoints(const transform_t& robotTrf, const points_t& toAdd, double overlap) { - if (toAdd.empty()) { - return; - } - transform_t trfInv = robotTrf.inverse(); - points_t transformed; - for (size_t i = 0; i < toAdd.size(); i += scanStride) { - const point_t& point = toAdd[i]; - if (point(2) != 0) { - transformed.push_back(trfInv * point); - } - } - - if (!tree.empty()) { - if (overlap > 0) { - // find the transformation from the sample to the map - transform_t adj = icp.correct(transformed, overlap); - for (point_t& p : transformed) { - p = adj * p; - } - } - } - - // add to global map - for (const point_t& p : transformed) { - CHECK_F(tree.add(p)); - } -} - -point_t GlobalMap::getClosest(const point_t& point) const { - if (tree.empty()) { - return {0, 0, 0}; - } - point_t closest = tree.getClosest(point); - return closest; -} - -points_t GlobalMap::getPointsWithin(const point_t& point, double dist) const { - points_t points; - if (!tree.empty()) { - points_t within = tree.getPointsWithin(point, dist * 2); - for (const point_t& p1 : within) { - if ((p1 - point).topRows<2>().norm() <= dist) { - points.push_back(p1); - } - } - } - return points; -} - -bool GlobalMap::hasPointWithin(const point_t& point, double dist) const { - return tree.hasPointWithin(point, dist); -} diff --git a/src/worldmap/GlobalMap.h b/src/worldmap/GlobalMap.h deleted file mode 100644 index ae3979f84..000000000 --- a/src/worldmap/GlobalMap.h +++ /dev/null @@ -1,92 +0,0 @@ -#pragma once - -#include "../navtypes.h" -#include "QuadTree.h" -#include "TrICP.h" - -#include - -/** - * Represents a 2D global point cloud map. As the robot moves around, it will store - * information about its environment for mapping purposes. - */ -class GlobalMap { -public: - /** - * Creates a new GlobalMap object, for use with building global lidar point clouds. - * - * @param areaSize The side length of the square represented by this global map. The units - * don't matter, as long as they're consistent with the data. - * @param scanStride Indicates the amount of points per scan to ignore. The number of - * points kept from each scan is 1/scanStride, rounded down. - * @param maxIter Maximum iterations allowed for fitting new samples to the map - * @param relErrChangeThresh Minimum relative error change threshold for fitting new - * samples to the map. Iteration will terminate early if the relative change in error - * drops below this amount. - */ - explicit GlobalMap(double areaSize, int scanStride = 1, int maxIter = 25, - double relErrChangeThresh = 0.005); - - /** - * Register the given points into the global map. - * - * @param robotTrf The transform of the robot pose. - * @param points The lidar sample point cloud, in the reference frame of the given robot - * transform. - * @param overlap The estimated overlap, in the range [0,1] between this sample and the - * global map. It's better to underestimate than overestimate. However, if it's too low - * then no meaningful features can be matched. Set to 0 to add points directly - * with no feature matching. - */ - void addPoints(const navtypes::transform_t& robotTrf, const navtypes::points_t& points, - double overlap); - - /** - * Get the points in the global map. This vector is a copy, so modifying it will not - * harm this class. - * - * @return A vector of points in the 2D point map. - */ - navtypes::points_t getPoints() const; - - /** - * Gets the number of points stored in this map. - * - * @return The size of this map. - */ - size_t size() const; - - /** - * Get the point in the point cloud closest to the given point. - * - * @param point The point for which the closest point will be found. - * @return The closest point, or [0,0,0] if no points are in this map. - */ - navtypes::point_t getClosest(const navtypes::point_t& point) const; - - /** - * Gets all points in the map within the given distance (inclusive) from the given point. - * - * @param point The point to find the nearby points for. - * @param dist The distance within which to search. - * @return A vector of points that are within the given distance from the given point. - */ - navtypes::points_t getPointsWithin(const navtypes::point_t& point, double dist) const; - - /** - * Checks if the given point has at least one point within the given distance from it. - * Although it's functionally equivalent to checking the size of the vector returned by - * getPointsWithin(), it is much cheaper to call this method. - * - * @param point The point to check if it has a nearby point. - * @param dist The distance within which to search. - * @return True iff there exists a point in the map, which may be the given point, within - * the given distance from the given point. False otherwise. - */ - bool hasPointWithin(const navtypes::point_t& point, double dist) const; - -private: - QuadTree tree; - TrICP icp; - int scanStride; -}; diff --git a/src/worldmap/QuadTree.cpp b/src/worldmap/QuadTree.cpp deleted file mode 100644 index fa4f67a26..000000000 --- a/src/worldmap/QuadTree.cpp +++ /dev/null @@ -1,263 +0,0 @@ -#include "QuadTree.h" - -#include -#include -#include - -using namespace navtypes; - -// checks if the given point is contained in the specified square axis-aligned bounding box -bool inBounds(const point_t& center, double size, const point_t& point) { - point_t diff = (point - center).array().abs(); - double halfSize = size / 2; - return diff(0) <= halfSize && diff(1) <= halfSize; -} - -// checks if two square axis-aligned bounding boxes intersect -bool boundsIntersect(const point_t& center1, double size1, const point_t& center2, - double size2) { - point_t absDiff = (center1 - center2).array().abs(); - return (absDiff.x() * 2 <= (size1 + size2)) && (absDiff.y() * 2 <= (size1 + size2)); -} - -QuadTree::QuadTree(point_t center, double width, int nodeCapacity) - : center(std::move(center)), width(width), points({}), nodeCapacity(nodeCapacity), - size(0) {} - -QuadTree::QuadTree(double width, int nodeCapacity) - : QuadTree({0, 0, 1}, width, nodeCapacity) {} - -size_t QuadTree::getSize() const { - return size; -} - -bool QuadTree::empty() const { - return size == 0; -} - -points_t QuadTree::getAllPoints() const { - points_t allPoints; - allPoints.reserve(size); - getAllPoints(*this, allPoints); - return allPoints; -} - -void QuadTree::getAllPoints(const QuadTree& tree, points_t& allPoints) const { - allPoints.insert(allPoints.end(), tree.points.begin(), tree.points.end()); - if (tree.hasChildren()) { - for (const auto& ptr : tree.children) { - getAllPoints(*ptr, allPoints); - } - } -} - -point_t QuadTree::getArbitraryPoint() const { - point_t zero = point_t(0, 0, 0); - if (!points.empty()) { - return points[0]; - } else if (hasChildren()) { - for (const auto& child : children) { - point_t p = child->getArbitraryPoint(); - if (p != zero) { - return p; - } - } - } - - return zero; -} - -bool QuadTree::add(const point_t& point) { - if (!inBounds(center, width, point)) { - return false; - } - - // if we have room for this point in this node, add it - if (points.size() < nodeCapacity && !hasChildren()) { - points.push_back(point); - size++; - return true; - } - - // create children if this node doesn't have any - if (!hasChildren()) { - subdivide(); - } - - // add this node to the child that contains it - for (const std::shared_ptr& childPtr : children) { - if (childPtr->add(point)) { - size++; - return true; - } - } - - // this should never happen - return false; -} - -bool QuadTree::remove(const point_t& point) { - if (!inBounds(center, width, point)) { - return false; - } - - // if the point is in this node, erase it and return - auto itr = std::find(points.begin(), points.end(), point); - if (itr != points.end()) { - points.erase(itr); - size--; - return true; - } - - // search through children for this point - if (hasChildren()) { - for (const std::shared_ptr& childPtr : children) { - if (childPtr->remove(point)) { - size--; - return true; - } - } - } - - // the point is not stored in this tree - return false; -} - -// gets the index of the child node of the node centered at `center` that contains `point` -int getChildIdx(const point_t& center, const point_t& point) { - // 0=SW,1=SE,2=NW,3=NE, so bit 1 is north-south and bit 0 is east-west - bool isTop = point.y() >= center.y(); - bool isRight = point.x() >= center.x(); - return (isTop ? 0b10 : 0) | (isRight ? 1 : 0); -} - -point_t QuadTree::getClosest(const point_t& point) const { - // if there are no children just search through this tree's points - if (!hasChildren()) { - point_t closest = {0, 0, 0}; - double minDist = std::numeric_limits::infinity(); - for (const point_t& p : points) { - double dist = (p - point).topRows<2>().norm(); - if (dist < minDist) { - closest = p; - minDist = dist; - } - } - return closest; - } - const QuadTree* tree = this; - std::stack stack; - stack.push(tree); - - // traverse down the tree until we find a square w/o children containing the search point - while (tree->hasChildren()) { - QuadTree* child = tree->children[getChildIdx(tree->center, point)].get(); - stack.push(child); - tree = child; - } - - // traverse back up until we find a nonempty square (usually the first one) - while (!stack.empty() && tree->empty()) { - tree = stack.top(); - stack.pop(); - } - - // if there are no points in this tree, return {0,0,0} - if (tree->empty()) { - CHECK_EQ_F(size, 0); // this should only happen if the tree is empty - return {0, 0, 0}; - } else { - CHECK_F(!tree->empty()); // should be guaranteed - // choose an arbitrary point - point_t p = tree->getArbitraryPoint(); - // Search area is square, so using the distance guarantees that the closest is found - // multiply by two because this is half side-length - double areaSize = 2 * (p - point).topRows<2>().norm(); - // get the closest of all points within this area - return getClosestWithin(point, areaSize); - } -} - -point_t QuadTree::getClosestWithin(const point_t& point, double areaSize) const { - points_t pointsInRange = getPointsWithin(point, areaSize); - if (pointsInRange.empty()) { - // no points in range - return {0, 0, 0}; - } else { - // compute the nearest neighbor in linear time (on a much smaller subset of points) - point_t closest = pointsInRange[0]; - double minDist = (closest - point).topRows<2>().norm(); - for (size_t i = 1; i < pointsInRange.size(); i++) { - point_t p = pointsInRange[i]; - double dist = (p - point).norm(); - if (dist < minDist) { - minDist = dist; - closest = p; - } - } - return closest; - } -} - -std::vector QuadTree::getPointsWithin(const point_t& point, double areaSize) const { - points_t ret; - // if the given bounding box doesn't intersect this node's area, return empty vector - if (!boundsIntersect(center, width, point, areaSize)) { - return ret; - } - - // add any points from this node in the search area to the vector - for (const point_t& p : points) { - if (inBounds(point, areaSize, p)) { - ret.push_back(p); - } - } - - // search through children, if they exist - if (hasChildren()) { - for (const auto& child : children) { - points_t pointsInChild = child->getPointsWithin(point, areaSize); - ret.insert(ret.end(), pointsInChild.begin(), pointsInChild.end()); - } - } - - return ret; -} - -bool QuadTree::hasPointWithin(const point_t& point, double dist) const { - // if the given bounding box doesn't intersect this node's area, return false - if (empty() || !boundsIntersect(center, width, point, dist * 2)) { - return false; - } - - // check any points from this node - for (const point_t& p : points) { - if ((p - point).topRows<2>().norm() <= dist) - return true; - } - - // search through children, if they exist - if (hasChildren()) { - for (const auto& child : children) { - if (child->hasPointWithin(point, dist)) - return true; - } - } - - return false; -} - -void QuadTree::subdivide() { - double d = width / 4; - for (int i = 0; i < 4; i++) { - // 0=SW,1=SE,2=NW,3=NE, so bit 1 is north-south and bit 0 is east-west - double dx = (i & 1) == 0 ? -1 : 1; - double dy = (i & 0b10) == 0 ? -1 : 1; - point_t newCenter = center + d * point_t(dx, dy, 0); - children[i] = std::make_shared(newCenter, width / 2, nodeCapacity); - } -} - -bool QuadTree::hasChildren() const { - return static_cast(children[0]); // if one child is initialized, all are -} diff --git a/src/worldmap/QuadTree.h b/src/worldmap/QuadTree.h deleted file mode 100644 index d519ed9f5..000000000 --- a/src/worldmap/QuadTree.h +++ /dev/null @@ -1,136 +0,0 @@ -#pragma once - -#include "../navtypes.h" - -#include - -/** - * Implements a quadtree for fast point lookups in 2D space. All points are expected - * to be in {x,y,1} format. - * - * The quadtree represents a square area of a given size and supports roughly - * logarithmic lookup times within that area. - * - * @see Quadtree - */ -class QuadTree { -public: - /** - * Creates a new quadtree with the given area size centered at the origin. - * @param width The size (height and width) of the square area covered by this quadtree. - * @param nodeCapacity The number of points stored in each node. - * Too high means slower lookup times, too low means higher memory overhead. - */ - explicit QuadTree(double width, int nodeCapacity = 4); - - /** - * Creates a new quadtree with the given area size centered at the given point. - * @param center The center of the area spanned by this quadtree. - * @param width The size (height and width) of the square area covered by this quadtree. - * @param nodeCapacity The number of points stored in each node. - * Too high means slower lookup times, too low means higher memory overhead. - */ - QuadTree(navtypes::point_t center, double width, int nodeCapacity = 4); - - /** - * Gets the total number of points stored in this quadtree. - * @return The number of points in this quadtree. - */ - size_t getSize() const; - - /** - * Gets if this quadtree is empty. - * @return True iff there are no points in this quadtree. - */ - bool empty() const; - - /** - * Gets a vector of all points in this quadtree. This is a linear-time operation, so - * don't use too often if there are a large number of points. - * Modifying this vector does not modify the tree. - * @return A vector consisting of all points in this quadtree. - */ - navtypes::points_t getAllPoints() const; - - /** - * Add the given point to this quadtree. - * @param point The point to add. - * @return True if the point was successfully added. The only way this will return false - * is if the given point is outside of the area spanned by the quadtree. - */ - bool add(const navtypes::point_t& point); - - /** - * Remove the first occurrence of the given point from this quadtree. - * @param point The point to remove. - * @return True if the point was successfully removed. False if the point was not in - * the quadtree to begin with. - */ - bool remove(const navtypes::point_t& point); - - /** - * Gets the closest point to the given point in this quadtree. - * - * @param point The point for which to find the nearest neighbor. - * @return The closest point to this point in the quadtree, or {0,0,0} if this quadtree is - * empty. - */ - navtypes::point_t getClosest(const navtypes::point_t& point) const; - - /** - * Get the point in this quadtree that is closest to the given point. - * @param point The point for which to find the nearest neighbor. - * @param areaSize The size of the axis-aligned square bounding box centered at the given - * point in which to perform the search. Larger values will slow down the search. - * @return The nearest neighbor to the given point, or {0,0,0} if none found. - */ - navtypes::point_t getClosestWithin(const navtypes::point_t& point, double areaSize) const; - - /** - * Checks if the given point has at least one point within the given distance from it. - * Although it's functionally equivalent to checking the size of the vector returned by - * getPointsWithin(), it is much cheaper to call this method. - * - * @param point The point to check if it has a nearby point. - * @param dist The distance within which to search. - * @return True iff there exists a point in the tree, which may be the given point, within - * the given distance from the given point. False otherwise. - */ - bool hasPointWithin(const navtypes::point_t& point, double areaSize) const; - - /** - * Gets all points in the quadtree that lie in the axis-aligned square bounding box - * centered at the given point with the given size. - * @param point The center of the bounding box. - * @param areaSize The size of the bounding box. - * @return All points that lie within that bounding box. This vector may be empty. - */ - navtypes::points_t getPointsWithin(const navtypes::point_t& point, double areaSize) const; - - /** - * Gets an arbitrary point stored in this QuadTree. It is undefined which specific - * point this is. - * - * @return An arbitrary point in this tree, or {0,0,0} if this is empty. - */ - navtypes::point_t getArbitraryPoint() const; - - // TODO: add method to remove random points, to help keep the tree size down - -private: - // 0=SW,1=SE,2=NW,3=NE, so bit 1 is north-south and bit 0 is east-west - // if one is initialized then all are initialized - std::shared_ptr children[4]; - navtypes::point_t center; // center of bounding box, in word coords - double width; // size of square area - navtypes::points_t points; // the points in this node, 0 <= points.size() <= nodeCapacity - size_t nodeCapacity; // number of points stored in each node - size_t size; // number of nodes stored in this or its descendants - - // create children nodes (doesn't check for already existing) - void subdivide(); - // check if children exist - bool hasChildren() const; - // private method paired with QuadTree::getAllPoints() - void getAllPoints(const QuadTree& tree, navtypes::points_t& allPoints) const; -}; diff --git a/src/worldmap/TrICP.cpp b/src/worldmap/TrICP.cpp deleted file mode 100644 index 04d046fef..000000000 --- a/src/worldmap/TrICP.cpp +++ /dev/null @@ -1,155 +0,0 @@ -#include "TrICP.h" - -#include -#include - -using namespace navtypes; - -struct PointPair // NOLINT(cppcoreguidelines-pro-type-member-init) -{ - point_t mapPoint; - point_t samplePoint; - double dist; -}; - -void heapify(PointPair arr[], int len, int i) { - int smallest = i; - int l = 2 * i + 1; - int r = 2 * i + 2; - if (l < len && arr[l].dist < arr[smallest].dist) { - smallest = l; - } - if (r < len && arr[r].dist < arr[smallest].dist) { - smallest = r; - } - - if (smallest != i) { - std::swap(arr[i], arr[smallest]); - heapify(arr, len, smallest); - } -} - -// get some of the point pairs with the least distance using heapsort -std::vector getMinN(PointPair* arr, int len, int minNum) { - for (int i = len / 2 - 1; i >= 0; i--) { - heapify(arr, len, i); - } - - std::vector ret; - for (int i = len - 1; i > len - 1 - minNum; i--) { - ret.push_back(arr[0]); - std::swap(arr[0], arr[i]); - heapify(arr, i, 0); - } - - return ret; -} - -transform_t computeTransformation(const std::vector& pairs) { - /* - * We need to find a rigid transformation that maps points in the sample to points in - * the map. This transformation is not a regular affine, as only rotations and translations - * are permitted. - * - * We can do this with the Kabsch algorithm. - * https://en.wikipedia.org/wiki/Kabsch_algorithm - */ - Eigen::Vector2d mapCentroid = Eigen::Vector2d::Zero(); - Eigen::Vector2d sampleCentroid = Eigen::Vector2d::Zero(); - - for (const PointPair& pair : pairs) { - mapCentroid += pair.mapPoint.topRows<2>(); - sampleCentroid += pair.samplePoint.topRows<2>(); - } - - mapCentroid /= pairs.size(); - sampleCentroid /= pairs.size(); - - int size = pairs.size(); - Eigen::Matrix P(size, 2); - Eigen::Matrix Q(size, 2); - for (int i = 0; i < size; i++) { - P.row(i) = pairs[i].samplePoint.topRows<2>() - sampleCentroid; - Q.row(i) = pairs[i].mapPoint.topRows<2>() - mapCentroid; - } - - Eigen::Matrix2d H = P.transpose() * Q; - - // computing SVD for square matrices, so no QR preconditioner needed - Eigen::JacobiSVD svd; - svd.compute(H, Eigen::ComputeFullU | Eigen::ComputeFullV); - - Eigen::Matrix2d U = svd.matrixU(); - Eigen::Matrix2d V = svd.matrixV(); - double d = (V * U.transpose()).determinant() > 0 ? 1 : -1; - Eigen::Matrix2d D = Eigen::Matrix2d::Identity(); - D.bottomRightCorner<1, 1>()(0, 0) = d; - Eigen::Matrix2d R = V * D * U.transpose(); - - Eigen::Vector2d translation = mapCentroid - R * sampleCentroid; - - transform_t trf = transform_t::Identity(); - trf.topLeftCorner<2, 2>() = R; - trf.topRightCorner<2, 1>() = translation; - return trf; -} - -TrICP::TrICP(int maxIter, double relErrChangeThresh, - std::function getClosest) - : maxIter(maxIter), relErrChangeThresh(relErrChangeThresh), - getClosest(std::move(getClosest)) { -} - -transform_t TrICP::correct(const points_t& sample, double overlap) { - if (sample.empty() || overlap == 0) { - return transform_t::Identity(); - } - int i = 0; - double mse = 1e9; - double oldMSE; - points_t points = sample; - transform_t trf = transform_t::Identity(); - int N = static_cast(overlap * sample.size()); - do { - i++; - oldMSE = mse; - transform_t t = iterate(points, N, mse); - trf = t * trf; - } while (!isDone(i, mse, oldMSE)); - - return trf; -} - -bool TrICP::isDone(int numIter, double mse, double oldMSE) const { - if (mse <= 1e-9) { - return true; - } - double relErrChange = fabs(mse - oldMSE) / mse; - return numIter >= maxIter || relErrChange <= relErrChangeThresh; -} - -transform_t TrICP::iterate(points_t& sample, int N, double& mse) const { - PointPair pairs[sample.size()]; - for (size_t i = 0; i < sample.size(); i++) { - const point_t& point = sample[i]; - point_t closestPoint = getClosest(point); - double dist = (point - closestPoint).norm(); - PointPair pair{closestPoint, point, dist}; - pairs[i] = pair; - } - - std::vector closestPairs = getMinN(pairs, sample.size(), N); - - double newS = 0; - for (const PointPair& pair : closestPairs) { - newS += pair.dist * pair.dist; - } - mse = newS / N; - - transform_t trf = computeTransformation(closestPairs); - - for (point_t& point : sample) { - point = trf * point; - } - return trf; -} diff --git a/src/worldmap/TrICP.h b/src/worldmap/TrICP.h deleted file mode 100644 index a297054e4..000000000 --- a/src/worldmap/TrICP.h +++ /dev/null @@ -1,52 +0,0 @@ -#pragma once - -#include "../navtypes.h" - -/** - * Implements the Trimmed Iterative Closest Point algorithm for 2D point cloud registration. - * - * @see Research - * paper - */ -class TrICP { -public: - /** - * Creates a new TrICP object. - * - * @param maxIter Maximum number of iterations for point cloud registration. - * @param relErrChangeThresh Minimum relative error change threshold for registration. - * Iteration will terminate early if the relative change in error drops below this amount. - * @param getClosest A function that finds the closest point to a given point in the - * collection of points that the new point cloud sample is being registered to. - */ - explicit TrICP(int maxIter, double relErrChangeThresh, - std::function getClosest); - /** - * Finds a rigid transformation that transforms the given sample to match the map. - * - * @param sample The sample to register. - * @param overlap The estimated overlap, in the range [0,1] between this sample and the - * global map. It's better to underestimate than overestimate. However, if it's too low - * then no meaningful features can be matched. Set to 0 to add points directly - * with no feature matching. - * @return A rigid transformation that transforms the given sample to match the map, - * or the identity matrix if the sample is empty or overlap is 0. - */ - navtypes::transform_t correct(const navtypes::points_t& sample, double overlap); - -private: - // maximum number of iterations to use - int maxIter; - // if the relative error change drops below this threshold, stop iterating - double relErrChangeThresh; - // find the closest point in the reference to the given point - std::function getClosest; - - // performs a single iteration of point cloud registration, returning the computed - // transform - navtypes::transform_t iterate(navtypes::points_t& sample, int N, double& mse) const; - - // returns true iff any of the stopping conditions are met - bool isDone(int numIter, double S, double oldMSE) const; -}; diff --git a/tests/Pathfinding/ObstacleMapTest.cpp b/tests/Pathfinding/ObstacleMapTest.cpp deleted file mode 100644 index 18aa3a681..000000000 --- a/tests/Pathfinding/ObstacleMapTest.cpp +++ /dev/null @@ -1,186 +0,0 @@ -#include -#include -#include - -#include "../../src/Pathfinding/ObstacleMap.h" -#include "../../src/math/PointXY.h" - -#define CATCH_CONFIG_MAIN - -namespace Pathfinding -{ - //constant constraints for dummy double array sol - const int radius = 10; - const int size = 2 * radius + 1; - - //prints visualization of bool double array where 1 is True and 0 is false - void print(bool sol[21][21]) - { - for (int i = 0; i < 21; i++) - { - for (int j = 0; j < 21; j++) - { - if(sol[i][j]) - { - std::cout << 1 << " "; - } else { - std::cout << 0 << " "; - } - } - std::cout << std::endl; - } - } - - //sets all valus of sol to false - inline void fillFalse(bool sol[][size]) - { - for (int i = 0; i < size; i++) - { - for (int j = 0; j < size; j++) - { - sol[i][j] = false; - } - } - } - - //checks if coord is within bounds of the dummy double array dimensions - bool withinBounds(int coord) - { - return coord < size && coord >= 0; - } - - //makes dummy obstacle map resultant array out of sol - //similar to ObstacleMap.update(, x. y) - void getMapObjSol(bool sol[][size], std::vector& vectorOfPoints, float shiftX=0.0f, float shiftY=0.0f) - { - fillFalse(sol); - for (PointXY p : vectorOfPoints) - { - int x = static_cast(p.x - shiftX) + radius; - int y = static_cast(p.y - shiftY) + radius; - if(withinBounds(x) && withinBounds(y)) { - if (y + 1 < size && x + 1 < size) { - sol[y + 1][x + 1] = true; - } - if (y + 1 < size && x >= 0) { - sol[y + 1][x] = true; - } - if (y >= 0 && x + 1 < size) { - sol[y][x + 1] = true; - } - if (y >= 0 && x >= 0) { - sol[y][x] = true; - } - } - } - } - - //checks equivalancy of size and values of obstacleMap object and sol - bool boolMapsEquals(bool obstacleMap[][size], bool sol[][size]) - { - for (int i = 0; i < size; i++) - { - for (int j = 0; j < size; j++) - { - bool obsMapVal = obstacleMap[i][j]; - bool solVal = sol[i][j]; - if (obsMapVal != solVal) - { - return false; - } - } - } - return true; - } - -} - -TEST_CASE("ObstacleMap") -{ - //this test tests basic creation of ObstacleMap - //also tests updating ObstacleMap as input updates - //assumes center at 0, 0 - - std::vector vectorOfPoints = { - PointXY{2.0f, 2.0f}, - PointXY{-2.0f, -2.0f}, - PointXY{-3.0f, 3.0f}, - PointXY{3.0f, -3.0f}, - PointXY{5.0f, -5.0f}, - PointXY{1.0f, 1.0f} - }; - - ObstacleMap obsMap; - bool sol[Pathfinding::size][Pathfinding::size]; - - // Create an ObstacleMap out of given points and dummy double array (sol) - // check that they have equal resulting structure by value - obsMap.update(vectorOfPoints, 0.0f, 0.0f); - Pathfinding::getMapObjSol(sol, vectorOfPoints); - REQUIRE(Pathfinding::boolMapsEquals(obsMap.obstacle_map, sol)); - - // Adds point to the vector of points and update both obsMap and sol - // check that they have equal resulting structure by value - vectorOfPoints.push_back(PointXY{-7.0, 3.0}); - obsMap.update(vectorOfPoints, 0.0f, 0.0f); - Pathfinding::getMapObjSol(sol, vectorOfPoints); - REQUIRE(Pathfinding::boolMapsEquals(obsMap.obstacle_map, sol)); - -} - -TEST_CASE("ObstacleMap OUT OF BOUNDS") -{ - //this test tests with points outside of the bounds of ObstacleMap - //and points with non-rounded float values - //assumes center at 0, 0 - - std::vector vectorOfPoints = { - PointXY{20.0f, 2.0f}, //Out OF BOUNDS - PointXY{-2.0f, -2.0f}, - PointXY{-30.0f, 3.0f}, //Out OF BOUNDS - PointXY{3.0f, -3.00008f}, - PointXY{5.0f, -5.770f}, - PointXY{1.50f, 1.60f} - }; - - ObstacleMap obsMap; - bool sol[Pathfinding::size][Pathfinding::size]; - - // Create an ObstacleMap out of given points and dummy double array (sol) - // check that they have equal resulting structure by value - obsMap.update(vectorOfPoints, 0.0f, 0.0f); - Pathfinding::getMapObjSol(sol, vectorOfPoints); - REQUIRE(Pathfinding::boolMapsEquals(obsMap.obstacle_map, sol)); -} - -TEST_CASE("ObstacleMap CHANGE CENTER") -{ - std::vector vectorOfPoints = { - PointXY{10.0f, 10.0f}, - PointXY{10.0f, -10.0f}, - PointXY{-10.0f, 10.0f}, - PointXY{-10.0f, -10.0f} - }; - - ObstacleMap obsMap; - // std::cout << "rx: " << obsMap.robotX << ", ry: " << obsMap.robotY << std::endl; - bool sol[Pathfinding::size][Pathfinding::size]; - obsMap.update(vectorOfPoints, 0.0f, 0.0f); - // std::cout << "rx: " << obsMap.robotX << ", ry: " << obsMap.robotY << std::endl; - Pathfinding::getMapObjSol(sol, vectorOfPoints); - REQUIRE(Pathfinding::boolMapsEquals(obsMap.obstacle_map, sol)); - - float newRobotX = 5.0f; - float newRobotY = 5.0f; - // std::cout << "rx: " << obsMap.robotX << ", ry: " << obsMap.robotY << std::endl; - obsMap.update(vectorOfPoints, newRobotX, newRobotY); - REQUIRE(!Pathfinding::boolMapsEquals(obsMap.obstacle_map, sol)); - - - Pathfinding::getMapObjSol(sol, vectorOfPoints, newRobotX, newRobotY); - // std::cout << "obsMap" << std::endl; - // obsMap.print(); - // std::cout << "sol" << std::endl; - // Pathfinding::print(sol); - REQUIRE(Pathfinding::boolMapsEquals(obsMap.obstacle_map, sol)); -} \ No newline at end of file diff --git a/tests/Pathfinding/PatherTest.cpp b/tests/Pathfinding/PatherTest.cpp deleted file mode 100644 index e292801b4..000000000 --- a/tests/Pathfinding/PatherTest.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include -#include -#include -#include "../../src/math/PointXY.h" -#include "../../src/Pathfinding/Pather2.h" -#include "../../src/Pathfinding/ObstacleMap.h" - -#define CATCH_CONFIG_MAIN - -namespace Pathfinding -{ - bool nextPointEqual(PointXY given, PointXY expect) { - return given.x == expect.x && given.y == expect.y; - } -} - -TEST_CASE("Pathfinding") -{ - std::vector obstaclePosition= { - PointXY{1.0f, 5.0f}, - PointXY{2.0f, 5.0f}, - PointXY{3.0f, 6.0f}, - PointXY{4.0f, 5.0f}, - PointXY{5.0f, 4.0f}, - PointXY{6.0f, 4.0f}, - PointXY{7.0f, 4.0f}, - PointXY{6.0f, 3.0f} - }; - //Construct an obstacle map - ObstacleMap obj; - obj.update(obstaclePosition, 0.0f, 0.0f); - - //Construct a searching path object - Pather2 searchingPath; - PointXY nextPoint = searchingPath.mainBFS(obj.obstacle_map, PointXY{10.0f, 10.0f}); - - PointXY expectPoint = {1.0f, 0.0}; - //Check if returned next point is as expected. - REQUIRE(Pathfinding::nextPointEqual(nextPoint, expectPoint)); -} \ No newline at end of file diff --git a/tests/mapping/Map2DTest.cpp b/tests/mapping/Map2DTest.cpp deleted file mode 100644 index 097d29910..000000000 --- a/tests/mapping/Map2DTest.cpp +++ /dev/null @@ -1,104 +0,0 @@ -#define CATCH_CONFIG_MAIN - -#include "mapping/Map2D.h" - -#include -#include -#include -#include -#include - -constexpr int test_iterations = 100; -constexpr int vertex_count = 1000; -constexpr int sparsity_factor = 2; - -#include - -namespace Mapping -{ - -// creates a map with given number of vertices -// when connecting neighbors, the probability that any node will be connected to any other node -// is 1 / sparse_factor -// thus if sparse_factor is 1, each node is connected to each other node -std::shared_ptr CreateRandMap(int vertices, int sparse_factor) -{ - Map2D map; - for (int i = 0; i < vertices; i++) - { - Point2D p; - p.id = i; - p.x = (float)(rand() % vertices); - p.y = (float)(rand() % vertices); - std::shared_ptr p_ptr = std::make_shared(p); - for (int j = 0; j < map.vertices.size(); j++) - { - if (rand() % sparse_factor == 0) - { - Connect(p_ptr, map.vertices.at(j)); - } - } - map.vertices.push_back(p_ptr); - } - return std::make_shared(map); -} - -// randomly picks a start and end vertex, making sure they are different nodes -// returns average time of shortest path computation, in seconds -double TimeShortestPath(std::shared_ptr map, int num_iterations) -{ - double avg = 0; - for (int i = 0; i < num_iterations; i++) - { - int start_ind = rand() % map->vertices.size(); - int target_ind = rand() % map->vertices.size(); - while (target_ind == start_ind) - { - target_ind = rand() % map->vertices.size(); - } - - auto start = std::chrono::system_clock::now(); - ComputePath(map, map->vertices[start_ind], map->vertices[target_ind]); - auto end = std::chrono::system_clock::now(); - std::chrono::duration elapsed_seconds = end - start; - avg += elapsed_seconds.count() / num_iterations; - } - return avg; -} - -bool verifyCorrectPath() -{ - Map2D map; - float x_pts[10] = {0, -1, 2, -3, 4, -5, 6, -7, 8, -9}; - float y_pts[10] = {0, -1, 2, -3, 4, -5, 6, -7, 8, -9}; - for (int i = 0; i < 10; i++) - { - Point2D p; - p.id = i; - p.x = x_pts[i]; - p.y = y_pts[i]; - map.vertices.push_back(std::make_shared(p)); - if (i > 0) - { - Connect(map.vertices[i], map.vertices[i - 1]); - } - } - Connect(map.vertices[9], map.vertices[2]); - std::vector> actual = ComputePath(std::make_shared(map), - map.vertices[9], map.vertices[0]); - int expected[3] = {0, 1, 2}; - for (int i = 0; i < actual.size(); i++) - { - if (expected[i] != actual[i]->id) - { - return false; - } - } - return true; -} -} // namespace Mapping - -TEST_CASE("computes correct path") -{ - REQUIRE(Mapping::verifyCorrectPath()); -} diff --git a/tests/runtests.sh b/tests/runtests.sh deleted file mode 100644 index 800bee611..000000000 --- a/tests/runtests.sh +++ /dev/null @@ -1,11 +0,0 @@ -#!/bin/bash - -# assumes that source code files are already compiled -# uses the .o files in the build directory - -for ARG in "$@"; do - echo "running test $ARG" - g++ $ARG -L ../build/*.a -I ../src/ - ./a.out - rm a.out -done diff --git a/tests/worldmap/GlobalMapTest.cpp b/tests/worldmap/GlobalMapTest.cpp deleted file mode 100644 index 5c4296367..000000000 --- a/tests/worldmap/GlobalMapTest.cpp +++ /dev/null @@ -1,177 +0,0 @@ -#include "../../src/worldmap/GlobalMap.h" - -#include "../../src/navtypes.h" -#include "../../src/utils/transform.h" - -#include -#include -#include - -#include - -using namespace navtypes; -using namespace util; - -namespace { -double rand(double low, double high) { - return low + (::rand() / (RAND_MAX / (high - low))); // NOLINT(cert-msc50-cpp) -} - -transform_t randTransform(double centerX, double centerY) { - // move back to the origin, rotate around the origin, then move back. - transform_t toOrigin = toTransformRotateFirst(centerX, centerY, 0); - transform_t fromOrigin = toTransformRotateFirst(-centerX, -centerY, 0); - transform_t trf = - toTransformRotateFirst(rand(-0.1, 0.1), rand(-0.1, 0.1), rand(-M_PI / 32, M_PI / 32)); - return fromOrigin * trf * toOrigin; -} - -double calculateMSE(const points_t& p1, const points_t& p2) { - double mse = 0; - REQUIRE(p2.size() >= p1.size()); - for (size_t i = 0; i < p1.size(); i++) { - point_t truth = p1[i]; - point_t p = p2[i]; - mse += pow((p - truth).norm(), 2); - } - mse /= p1.size(); - - return mse; -} -} // namespace - -TEST_CASE("Global Map - ScanStride", "[GlobalMap]") { - srand(time(nullptr)); // NOLINT(cert-msc51-cpp) - - for (int i = 1; i <= 3; i++) { - GlobalMap map(1000, i); - points_t points; - for (int j = 0; j < i * 1000; j++) { - point_t point = {rand(-100, 100), rand(-100, 100), 1}; - points.push_back(point); - } - map.addPoints(transform_t::Identity(), points, 0); - - REQUIRE(map.size() == 1000); - REQUIRE(map.getPoints().size() == 1000); - } -} - -TEST_CASE("Global Map - GetClosest", "[GlobalMap]") { - srand(time(nullptr)); // NOLINT(cert-msc51-cpp) - - GlobalMap map(1000); - points_t points; - - constexpr int areaMin = -5; - constexpr int areaMax = 5; - - for (int i = 0; i < 100; i++) { - double x = rand(areaMin, areaMax); - double y = rand(areaMin, areaMax); - point_t p = {x, y, 1}; - points.push_back(p); - } - map.addPoints(transform_t::Identity(), points, 0); - - for (int i = 0; i < 500; i++) { - double x = rand(areaMin, areaMax); - double y = rand(areaMin, areaMax); - point_t point = {x, y, 1}; - - // manually compute the nearest neighbor to check against - point_t closest = {0, 0, 0}; - double minDist = std::numeric_limits::infinity(); - for (const point_t& p : points) { - double dist = (p - point).topRows<2>().norm(); - if (dist < minDist) { - minDist = dist; - closest = p; - } - } - - point_t closestMap = map.getClosest(point); - REQUIRE(closest == closestMap); - REQUIRE(map.hasPointWithin(point, minDist)); - - REQUIRE(map.hasPointWithin(point, std::nextafter(minDist, DBL_MAX))); - REQUIRE_FALSE(map.hasPointWithin(point, std::nextafter(minDist, 0))); - - REQUIRE(map.hasPointWithin(point, minDist * 2)); - REQUIRE_FALSE(map.hasPointWithin(point, minDist / 2.0)); - } -} - -TEST_CASE("Global Map", "[GlobalMap]") { - srand(time(nullptr)); // NOLINT(cert-msc51-cpp) - - GlobalMap map(1000); - points_t allPoints; - // create 5 groups of points for aligning with each other, all in same area - for (int i = 0; i < 5; i++) { - points_t sample; - // first sample should be in axes reference frame, so we can check performance easily - transform_t trf = i > 0 ? randTransform(0, 0) : transform_t::Identity(); - for (int j = 0; j < 150; j++) { - double x1 = rand(-3, 3); - double y1 = pow(x1, 3); - point_t p = {x1, y1, 1}; - allPoints.push_back(p); - sample.push_back(trf * p); - } - map.addPoints(transform_t::Identity(), sample, 1); - } - - points_t mapPoints = map.getPoints(); - - // we need to sort because iteration order is not consistent - auto comp = [](const point_t& a, const point_t& b) { return a(0) < b(0); }; - std::sort(allPoints.begin(), allPoints.end(), comp); - std::sort(mapPoints.begin(), mapPoints.end(), comp); - - double mse = calculateMSE(allPoints, mapPoints); - - std::cout << "Global Map MSE Full Overlap: " << mse << std::endl; - REQUIRE(mse == Approx(0).margin(0.5)); -} - -TEST_CASE("Global Map 0.5 Overlap", "[GlobalMap]") { - srand(time(nullptr)); // NOLINT(cert-msc51-cpp) - // we'll be getting points along the sin function - std::function func = [](double x) { return sin(x); }; - - GlobalMap map(1000); - points_t truths; - // create 5 groups of points for aligning, each partially overlapping with the last - for (int i = -2; i <= 2; i++) { - // the area is 3 wide, and the center moves right by 1.5 each time - double center = i * 1.5; - double min = center - 1.5; - double max = center + 1.5; - points_t sample; - // last sample should be in axes reference frame, so we can check performance easily - transform_t trf = - i != 2 ? randTransform(center, func(center)) : transform_t::Identity(); - for (int j = 0; j < 100; j++) { - double x = rand(min, max); - double y = func(x); - point_t p = {x, y, 1}; - truths.push_back(p); - sample.push_back(trf * p); - } - - map.addPoints(transform_t::Identity(), sample, 0.4); - } - - points_t mapPoints = map.getPoints(); - - // we need to sort because iteration order is not consistent - auto comp = [](const point_t& a, const point_t& b) { return a(0) < b(0); }; - std::sort(truths.begin(), truths.end(), comp); - std::sort(mapPoints.begin(), mapPoints.end(), comp); - - double mse = calculateMSE(truths, mapPoints); - - std::cout << "Global Map MSE Partial Overlap: " << mse << std::endl; - REQUIRE(mse == Approx(0).margin(0.4)); -} diff --git a/tests/worldmap/QuadTreeTest.cpp b/tests/worldmap/QuadTreeTest.cpp deleted file mode 100644 index 9964dfa0c..000000000 --- a/tests/worldmap/QuadTreeTest.cpp +++ /dev/null @@ -1,251 +0,0 @@ -#include "../../src/worldmap/QuadTree.h" - -#include -#include - -#include - -using namespace navtypes; - -namespace { -double randNum(double low, double high) { - return low + (rand() / (RAND_MAX / (high - low))); // NOLINT(cert-msc50-cpp) -} - -point_t randPoint(double size) { - return {randNum(-size / 2, size / 2), randNum(-size / 2, size / 2), 1}; -} -} // namespace - -TEST_CASE("QuadTree - AddPoints", "[QuadTree]") { - srand(time(nullptr)); // NOLINT(cert-msc51-cpp) - QuadTree tree(100); - - REQUIRE(tree.empty()); - - for (size_t i = 0; i < 50; i++) { - const point_t& point = randPoint(100); - REQUIRE(tree.getSize() == i); - REQUIRE(tree.add(point)); - REQUIRE(tree.getSize() == i + 1); - } - - REQUIRE_FALSE(tree.empty()); -} - -TEST_CASE("QuadTree - RemovePoints", "[QuadTree]") { - srand(time(nullptr)); // NOLINT(cert-msc51-cpp) - QuadTree tree(100); - - points_t points; - - for (size_t i = 0; i < 50; i++) { - const point_t& point = randPoint(100); - points.push_back(point); - REQUIRE(tree.getSize() == i); - REQUIRE(tree.add(point)); - REQUIRE(tree.getSize() == i + 1); - } - - size_t size = tree.getSize(); - for (const point_t& point : points) { - REQUIRE(tree.getSize() == size); - REQUIRE(tree.remove(point)); - REQUIRE(tree.getSize() == --size); - } - REQUIRE(tree.getSize() == 0); - REQUIRE(tree.empty()); -} - -TEST_CASE("QuadTree - RemoveNonexistentPoints", "[QuadTree]") { - srand(time(nullptr)); // NOLINT(cert-msc51-cpp) - QuadTree tree(100, 4); - - for (int i = 0; i < 50; i++) { - // make the size smaller, so we can guarantee that some points are not in the tree - tree.add(randPoint(5)); - } - - auto size = tree.getSize(); - REQUIRE_FALSE(tree.remove({53, 1, 1})); - REQUIRE_FALSE(tree.remove({0, 0, 0})); - REQUIRE_FALSE(tree.remove({-32, 63, 1})); - REQUIRE_FALSE(tree.remove({-32, -84, 1})); - REQUIRE_FALSE(tree.remove({99, -13, 1})); - REQUIRE(size == tree.getSize()); -} - -TEST_CASE("QuadTree - GetAllPoints", "[QuadTree]") { - srand(time(nullptr)); // NOLINT(cert-msc51-cpp) - QuadTree tree(100, 4); - - points_t points; - - for (int i = 0; i < 50; i++) { - const point_t& point = randPoint(100); - points.push_back(point); - tree.add(point); - } - - REQUIRE(points.size() == tree.getSize()); - - // assert that all points are in this list - points_t treePoints = tree.getAllPoints(); - REQUIRE(points.size() == treePoints.size()); - for (const point_t& point : points) { - auto itr = std::find(treePoints.begin(), treePoints.end(), point); - REQUIRE(itr != treePoints.end()); - REQUIRE(point == *itr); - } - - // assert that modifying this list doesn't modify the tree - auto prevSize = treePoints.size(); - point_t erased = *treePoints.begin(); - treePoints.erase(treePoints.begin()); - // not a catch2 assert since this is just a sanity check - CHECK_F(prevSize == treePoints.size() + 1); - - REQUIRE(tree.getAllPoints().size() == prevSize); - REQUIRE(tree.getClosestWithin(erased, 1) == erased); - - // verify that mutating the objects in the list doesn't modify the tree - treePoints = tree.getAllPoints(); - REQUIRE(tree.getClosestWithin(treePoints[0], 0.5) == treePoints[0]); - treePoints[0](0) = treePoints[0](0) + 0.01; - REQUIRE(tree.getClosestWithin(treePoints[0], 0.5) != treePoints[0]); -} - -TEST_CASE("QuadTree - GetPointsWithin", "[QuadTree]") { - srand(time(nullptr)); // NOLINT(cert-msc51-cpp) - QuadTree tree(100, 4); - - // create two bunches: bottom-left and top-right - points_t left, right; - - for (int i = 0; i < 50; i++) { - point_t p = randPoint(10) - point_t(20, 20, 0); - left.push_back(p); - tree.add(p); - } - - for (int i = 0; i < 50; i++) { - point_t p = randPoint(10) + point_t(20, 20, 0); - right.push_back(p); - tree.add(p); - } - - REQUIRE(tree.getSize() == (left.size() + right.size())); - - points_t closestToLeft = tree.getPointsWithin(point_t(-20, -20, 1), 25); - points_t closestToRight = tree.getPointsWithin(point_t(20, 20, 1), 25); - - // verify that we retrieved all the points in the bottom-left bunch - REQUIRE(closestToLeft.size() == left.size()); - for (const point_t& point : left) { - auto itr = std::find(closestToLeft.begin(), closestToLeft.end(), point); - REQUIRE(itr != closestToLeft.end()); - REQUIRE(point == *itr); - } - - // verify that we retrieved all the points in the top-right bunch - REQUIRE(closestToRight.size() == right.size()); - for (const point_t& point : right) { - auto itr = std::find(closestToRight.begin(), closestToRight.end(), point); - REQUIRE(itr != closestToRight.end()); - REQUIRE(point == *itr); - } -} - -TEST_CASE("QuadTree - GetClosestPointRandom", "[QuadTree]") { - srand(time(nullptr)); // NOLINT(cert-msc51-cpp) - QuadTree tree(100); - - for (int i = 0; i < 100; i++) { - point_t p = randPoint(5); - tree.add(p); - } - - points_t points = tree.getAllPoints(); - - for (int i = 0; i < 200; i++) { - point_t point = randPoint(100); - point_t closestTree = tree.getClosest(point); - // manually compute the nearest neighbor to check against - point_t closest = {0, 0, 0}; - double minDist = std::numeric_limits::infinity(); - for (const point_t& p : points) { - double dist = (p - point).norm(); - if (dist < minDist) { - minDist = dist; - closest = p; - } - } - REQUIRE(closest == closestTree); - } -} - -TEST_CASE("QuadTree - GetClosestPoint", "[QuadTree]") { - srand(time(nullptr)); // NOLINT(cert-msc51-cpp) - QuadTree tree(100); - - for (int i = 0; i < 100; i++) { - tree.add(randPoint(100)); - } - - points_t points = tree.getAllPoints(); - - for (const point_t& point : points) { - point_t closestTree = tree.getClosest(point); - // manually compute the nearest neighbor to check against - point_t closest = {0, 0, 0}; - double minDist = std::numeric_limits::infinity(); - for (const point_t& p : points) { - double dist = (p - point).norm(); - if (dist < minDist) { - minDist = dist; - closest = p; - } - } - REQUIRE(closest == closestTree); - } -} - -TEST_CASE("QuadTree - GetClosestDiffQuadrants", "[QuadTree]") { - QuadTree tree(100, 1); // store only one point per node - point_t p1 = {5, 5, 1}; // stored in root - tree.add(p1); - point_t p2 = {1, 1, 1}; // stored in child - tree.add(p2); - tree.remove(p1); // now the root is empty - - REQUIRE(tree.getClosest({-1, -1, 0}) == p2); -} - -TEST_CASE("QuadTree - TestBoundary", "[QuadTree]") { - QuadTree tree(100); - - // test that adding things in the boundary work - REQUIRE(tree.add({50, 50, 1})); - REQUIRE_FALSE(tree.add({100, 100, 1})); - - REQUIRE(tree.add({-50, -50, 1})); - REQUIRE_FALSE(tree.add({-100, -100, 1})); - - REQUIRE(tree.getClosest({50, 50, 1}) == point_t(50, 50, 1)); - REQUIRE(tree.getClosest({-50, -50, 1}) == point_t(-50, -50, 1)); - - REQUIRE(tree.getClosest({1, 1, 1}) == point_t(50, 50, 1)); - REQUIRE(tree.getClosest({-1, -1, 1}) == point_t(-50, -50, 1)); -} - -TEST_CASE("QuadTree - EmptyTree", "[QuadTree]") { - // test behavior of empty graph - QuadTree tree(100); - - REQUIRE(tree.empty()); - REQUIRE(tree.getSize() == 0); - REQUIRE(tree.getClosest({0, 0, 1}) == point_t(0, 0, 0)); - REQUIRE(tree.getClosestWithin({0, 0, 1}, 15) == point_t(0, 0, 0)); - points_t points = tree.getPointsWithin(point_t(0, 0, 1), 15); - REQUIRE(points.empty()); -} diff --git a/tests/worldmap/TrICPTest.cpp b/tests/worldmap/TrICPTest.cpp deleted file mode 100644 index 14722b1e7..000000000 --- a/tests/worldmap/TrICPTest.cpp +++ /dev/null @@ -1,50 +0,0 @@ -#include "../../src/worldmap/TrICP.h" - -#include "../../src/utils/transform.h" -#include "../../src/worldmap/GlobalMap.h" - -#include - -#include -#include - -using namespace navtypes; -using namespace util; - -namespace { -double rand(double low, double high) { - return low + (::rand() / (RAND_MAX / (high - low))); // NOLINT(cert-msc50-cpp) -} -} // namespace - -TEST_CASE("Trimmed ICP") { - points_t map; - points_t sample; - points_t truths; - transform_t trf = toTransformRotateFirst(0.1, -0.25, M_PI / 24); - srand(time(nullptr)); // NOLINT(cert-msc51-cpp) - for (int i = 0; i < 150; i++) { - double x1 = rand(-6, 2); - double y1 = pow(x1, 3); - map.push_back({x1, y1, 1}); - - double x2 = rand(-2, 6); - double y2 = pow(x2, 3); - point_t p = {x2, y2, 1}; - truths.push_back(p); - p = trf * p; - sample.push_back(p); - } - - GlobalMap globalMap(1000); - globalMap.addPoints(transform_t::Identity(), map, 1); - - TrICP icp(25, 0.005, std::bind(&GlobalMap::getClosest, &globalMap, std::placeholders::_1)); - // approximate the inverse of the transform used to create the sample - transform_t trfApprox = icp.correct(sample, 0.3); - transform_t trfInv = trf.inverse(); - - double mse = (trfInv - trfApprox).array().square().mean(); - std::cout << "TrICP MSE: " << mse << std::endl; - CHECK(mse == Approx(0).margin(0.01)); -}