diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e6e148dd8..90d231b20 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -250,9 +250,13 @@ add_library(control SHARED ) target_link_libraries(control utils) -add_library(commands STATIC +add_library(commands SHARED commands/DriveToWaypointCommand.cpp) +add_library(autonomous SHARED + autonomous/AutonomousTask.cpp) +target_link_libraries(autonomous commands) + list(APPEND simulator_libs simulator_interface) diff --git a/src/Constants.cpp b/src/Constants.cpp index 9daec5449..e5dbf815e 100644 --- a/src/Constants.cpp +++ b/src/Constants.cpp @@ -147,5 +147,14 @@ const std::array IK_MOTORS = ([]() { })(); } // namespace arm +namespace autonomous { +const double THETA_KP = 2.0; +const double DRIVE_VEL = 1.5; +const double SLOW_DRIVE_THRESHOLD = 3.0; +const double DONE_THRESHOLD = 0.5; +// Duration long enough to confirm we are there, not so long that time is wasted +const util::dseconds CLOSE_TO_TARGET_DUR_VAL = std::chrono::milliseconds(750); +} // namespace autonomous + const double CONTROL_HZ = 10.0; } // namespace Constants diff --git a/src/Constants.h b/src/Constants.h index 8df086c66..da592f251 100644 --- a/src/Constants.h +++ b/src/Constants.h @@ -1,5 +1,6 @@ #pragma once +#include "utils/time.h" #include "world_interface/data.h" #include @@ -175,5 +176,15 @@ constexpr frozen::unordered_map + +using namespace std::chrono_literals; +using namespace Constants::autonomous; + +namespace autonomous { + +AutonomousTask::AutonomousTask(){}; + +AutonomousTask::~AutonomousTask() { + if (_autonomous_task_thread.joinable()) { + _autonomous_task_thread.join(); + } +} + +void AutonomousTask::start(const navtypes::point_t& waypointCoords) { + if (_autonomous_task_thread.joinable()) { + kill(); + } + + _waypoint_coords = waypointCoords; + _kill_called = false; + _autonomous_task_thread = std::thread(&autonomous::AutonomousTask::navigate, this); +} + +void AutonomousTask::navigate() { + commands::DriveToWaypointCommand cmd(_waypoint_coords, THETA_KP, DRIVE_VEL, + SLOW_DRIVE_THRESHOLD, DONE_THRESHOLD, + CLOSE_TO_TARGET_DUR_VAL); + + DiffDriveKinematics diffDriveKinematics(Constants::EFF_WHEEL_BASE); + + auto sleepUntil = std::chrono::steady_clock().now(); + while (!cmd.isDone()) { + auto latestGPS = robot::readGPS(); + auto latestHeading = robot::readIMUHeading(); + + if (latestGPS.isFresh(2000ms) && latestHeading.isFresh(2000ms)) { + LOG_SCOPE_F(INFO, "AutoNav"); + auto gpsData = latestGPS.getData(); + navtypes::pose_t latestPos(gpsData.x(), gpsData.y(), latestHeading.getData()); + cmd.setState(latestPos, robot::types::dataclock::now()); + commands::command_t output = cmd.getOutput(); + auto scaledVels = diffDriveKinematics.ensureWithinWheelSpeedLimit( + DiffDriveKinematics::PreferredVelPreservation::PreferThetaVel, output.xVel, + output.thetaVel, Constants::MAX_WHEEL_VEL); + navtypes::point_t relTarget = util::toTransform(latestPos) * _waypoint_coords; + LOG_F(INFO, "Relative Target: (%lf, %lf)", relTarget(0), relTarget(1)); + LOG_F(INFO, "thetaVel: %lf", scaledVels(2)); + LOG_F(INFO, "xVel: %lf", scaledVels(0)); + robot::setCmdVel(scaledVels(2), scaledVels(0)); + } + + std::unique_lock autonomousTaskLock(_autonomous_task_mutex); + sleepUntil += 500ms; + // Wait 500ms or return out of while loop if kill called + if (_autonomous_task_cv.wait_until(autonomousTaskLock, sleepUntil, + [&] { return _kill_called; })) { + return; + } + } +} + +void AutonomousTask::kill() { + { + std::lock_guard lock(_autonomous_task_mutex); + _kill_called = true; + } + + _autonomous_task_cv.notify_all(); + + if (_autonomous_task_thread.joinable()) { + _autonomous_task_thread.join(); + } +} + +} // namespace autonomous diff --git a/src/autonomous/AutonomousTask.h b/src/autonomous/AutonomousTask.h new file mode 100644 index 000000000..78c6a5c01 --- /dev/null +++ b/src/autonomous/AutonomousTask.h @@ -0,0 +1,53 @@ +#pragma once + +#include "../navtypes.h" + +#include +#include +#include +#include + +namespace autonomous { + +/* + * @brief This class facilitates autonomous navigation to a specified waypoint. + */ +class AutonomousTask { +public: + /* + * @brief Constructs a new AutonomousTask + */ + AutonomousTask(); + + /* + * @brief Destructs AutonomousTask object, joins _autonomous_task_thread + */ + ~AutonomousTask(); + + /* + * @brief Starts an autonomous task to the provided waypoint + * + * @param navtypes::pose_t The waypoint to navigate to + */ + void start(const navtypes::point_t&); + + /* + * @brief Halts autonomous navigation by exiting the navigate() function and joining the + * autonomous task thread + */ + void kill(); + +private: + /* + * @brief Handles navigation to waypoint, called by start() + */ + void navigate(); + + navtypes::point_t _waypoint_coords; + std::mutex _autonomous_task_mutex; + std::thread _autonomous_task_thread; + std::condition_variable _autonomous_task_cv; + bool _kill_called; +}; + +} // namespace autonomous diff --git a/src/commands/DriveToWaypointCommand.cpp b/src/commands/DriveToWaypointCommand.cpp index 1301e5621..47df855cd 100644 --- a/src/commands/DriveToWaypointCommand.cpp +++ b/src/commands/DriveToWaypointCommand.cpp @@ -9,12 +9,12 @@ using namespace std::chrono_literals; namespace commands { DriveToWaypointCommand::DriveToWaypointCommand(const point_t& target, double thetaKP, - double driveVel, double slowDriveVel, + double driveVel, double slowDriveThresh, double doneThresh, util::dseconds closeToTargetDur) : target(target), pose(pose_t::Zero()), thetaKP(thetaKP), driveVel(driveVel), - slowDriveVel(slowDriveVel), doneThresh(doneThresh), setStateCalledBeforeOutput(false), - closeToTargetDur(closeToTargetDur) {} + slowDriveThresh(slowDriveThresh), doneThresh(doneThresh), + setStateCalledBeforeOutput(false), closeToTargetDur(closeToTargetDur) {} void DriveToWaypointCommand::setState(const navtypes::pose_t& pose, robot::types::datatime_t time) { @@ -33,11 +33,13 @@ command_t DriveToWaypointCommand::getOutput() { double targetAngle = std::atan2(toTarget(1), toTarget(0)); double angleDelta = targetAngle - pose(2); double thetaErr = std::atan2(std::sin(angleDelta), std::cos(angleDelta)); - double thetaVel = thetaKP * thetaErr; double dist = (pose.topRows<2>() - target.topRows<2>()).norm(); - double xVel = dist <= 2 * doneThresh ? slowDriveVel : driveVel; + double xVel = driveVel; + if (dist <= slowDriveThresh) { + xVel *= dist / slowDriveThresh; + } return {.thetaVel = thetaVel, .xVel = xVel}; } diff --git a/src/commands/DriveToWaypointCommand.h b/src/commands/DriveToWaypointCommand.h index e6c50ac2d..c076b5154 100644 --- a/src/commands/DriveToWaypointCommand.h +++ b/src/commands/DriveToWaypointCommand.h @@ -20,16 +20,16 @@ class DriveToWaypointCommand : CommandBase { /** * Creates a new DriveToWaypointCommand with the specified targeting information. * - * @param target, the position of the target in world coordinates. - * @param thetaKP, the scaling factor to use for robot steering proportional control. - * @param driveVel, the speed to drive the robot at. - * @param slowDriveVel, the slower speed to drive the robot at when near the target. - * @param doneThresh, the threshold for minimum robot-target distance to complete command. - * @param closeToTargetDurVal, the duration within doneThresh after which robot navigation + * @param target the position of the target in world coordinates. + * @param thetaKP the scaling factor to use for robot steering proportional control. + * @param driveVel the speed to drive the robot at. + * @param slowDriveThresh the distance at which the robot will start slowing down. + * @param doneThresh the threshold for minimum robot-target distance to complete command. + * @param closeToTargetDurVal the duration within doneThresh after which robot navigation * is considered done, seconds. */ DriveToWaypointCommand(const navtypes::point_t& target, double thetaKP, double driveVel, - double slowDriveVel, double doneThresh, + double slowDriveThresh, double doneThresh, util::dseconds closeToTargetDur); /** @@ -62,7 +62,7 @@ class DriveToWaypointCommand : CommandBase { navtypes::pose_t pose; double thetaKP; double driveVel; - double slowDriveVel; + double slowDriveThresh; double doneThresh; bool setStateCalledBeforeOutput; util::dseconds closeToTargetDur; diff --git a/src/network/CMakeLists.txt b/src/network/CMakeLists.txt index 9a2918b08..02e49452f 100644 --- a/src/network/CMakeLists.txt +++ b/src/network/CMakeLists.txt @@ -9,6 +9,6 @@ target_link_libraries(websocket_utils add_library(mission_control_interface STATIC MissionControlProtocol.cpp MissionControlTasks.cpp) -target_link_libraries(mission_control_interface utils video) +target_link_libraries(mission_control_interface utils video autonomous) set_target_properties(mission_control_interface PROPERTIES POSITION_INDEPENDENT_CODE ON) diff --git a/src/network/MissionControlMessages.h b/src/network/MissionControlMessages.h index b44d6dcf8..39fca962a 100644 --- a/src/network/MissionControlMessages.h +++ b/src/network/MissionControlMessages.h @@ -14,6 +14,7 @@ constexpr const char* MOTOR_POSITION_REQ_TYPE = "motorPositionRequest"; constexpr const char* JOINT_POSITION_REQ_TYPE = "jointPositionRequest"; constexpr const char* CAMERA_STREAM_OPEN_REQ_TYPE = "cameraStreamOpenRequest"; constexpr const char* CAMERA_STREAM_CLOSE_REQ_TYPE = "cameraStreamCloseRequest"; +constexpr const char* WAYPOINT_NAV_REQ_TYPE = "waypointNavRequest"; // report keys constexpr const char* MOTOR_STATUS_REP_TYPE = "motorStatusReport"; diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp index fb058687a..d551eea41 100644 --- a/src/network/MissionControlProtocol.cpp +++ b/src/network/MissionControlProtocol.cpp @@ -2,6 +2,7 @@ #include "../Constants.h" #include "../Globals.h" +#include "../autonomous/AutonomousTask.h" #include "../utils/core.h" #include "../utils/json.h" #include "../world_interface/data.h" @@ -71,6 +72,7 @@ void MissionControlProtocol::handleOperationModeRequest(const json& j) { // if we have entered autonomous mode, we need to stop all the power repeater stuff. this->stopAndShutdownPowerRepeat(true); } else { + _autonomous_task.kill(); // if we have left autonomous mode, we need to start the power repeater again. _power_repeat_task.start(); } @@ -152,6 +154,32 @@ void MissionControlProtocol::handleJointPowerRequest(const json& j) { } } +static bool validateWaypointNavRequest(const json& j) { + bool lat_is_unsigned = util::validateKey(j, "latitude", val_t::number_unsigned); + bool lon_is_unsigned = util::validateKey(j, "longitude", val_t::number_unsigned); + return (lat_is_unsigned || util::validateKey(j, "latitude", val_t::number_float)) && + (lon_is_unsigned || util::validateKey(j, "longitude", val_t::number_float)) && + util::validateKey(j, "isApproximate", val_t::boolean) && + util::validateKey(j, "isGate", val_t::boolean); +} + +void MissionControlProtocol::handleWaypointNavRequest(const json& j) { + float latitude = j["latitude"]; + float longitude = j["longitude"]; + bool isApproximate = j["isApproximate"]; + bool isGate = j["isGate"]; + + if (Globals::AUTONOMOUS && !isApproximate && !isGate) { + navtypes::gpscoords_t coords = {latitude, longitude}; + auto target = robot::gpsToMeters(coords); + if (target) { + _autonomous_task.start(target.value()); + } else { + LOG_F(WARNING, "No GPS converter initialized!"); + } + } +} + static bool validateJointPositionRequest(const json& j) { return validateJoint(j) && util::validateKey(j, "position", val_t::number_integer); } @@ -233,7 +261,8 @@ void MissionControlProtocol::stopAndShutdownPowerRepeat(bool sendDisableIK) { MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server) : WebSocketProtocol(Constants::MC_PROTOCOL_NAME), _server(server), - _camera_stream_task(server), _telem_report_task(server), _arm_ik_task(server) { + _camera_stream_task(server), _telem_report_task(server), _arm_ik_task(server), + _autonomous_task() { // TODO: Add support for tank drive requests // TODO: add support for science station requests (lazy susan, lazy susan lid, drill, // syringe) @@ -271,6 +300,10 @@ MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server) CAMERA_STREAM_CLOSE_REQ_TYPE, std::bind(&MissionControlProtocol::handleCameraStreamCloseRequest, this, _1), validateCameraStreamCloseRequest); + this->addMessageHandler( + WAYPOINT_NAV_REQ_TYPE, + std::bind(&MissionControlProtocol::handleWaypointNavRequest, this, _1), + validateWaypointNavRequest); this->addConnectionHandler(std::bind(&MissionControlProtocol::handleConnection, this)); this->addDisconnectionHandler( std::bind(&MissionControlProtocol::stopAndShutdownPowerRepeat, this, false)); diff --git a/src/network/MissionControlProtocol.h b/src/network/MissionControlProtocol.h index da01c34eb..e79040f6e 100644 --- a/src/network/MissionControlProtocol.h +++ b/src/network/MissionControlProtocol.h @@ -1,5 +1,6 @@ #pragma once +#include "../autonomous/AutonomousTask.h" #include "../utils/scheduler.h" #include "../video/H264Encoder.h" #include "../world_interface/world_interface.h" @@ -38,12 +39,14 @@ class MissionControlProtocol : public WebSocketProtocol { // TODO: add documenta tasks::CameraStreamTask _camera_stream_task; tasks::TelemReportTask _telem_report_task; tasks::ArmIKTask _arm_ik_task; + autonomous::AutonomousTask _autonomous_task; void handleEmergencyStopRequest(const json& j); void handleOperationModeRequest(const json& j); void handleCameraStreamOpenRequest(const json& j); void handleCameraStreamCloseRequest(const json& j); void handleJointPowerRequest(const json& j); + void handleWaypointNavRequest(const json& j); void handleDriveRequest(const json& j); void handleRequestArmIKEnabled(const json& j); void sendArmIKEnabledReport(bool enabled); diff --git a/src/utils/transform.cpp b/src/utils/transform.cpp index 222815f75..1f9c717c3 100644 --- a/src/utils/transform.cpp +++ b/src/utils/transform.cpp @@ -22,9 +22,9 @@ double quatToHeading(const Eigen::Quaterniond& quat) { } Eigen::Quaterniond eulerAnglesToQuat(const navtypes::eulerangles_t& rpy) { - Eigen::Quaterniond quat(Eigen::AngleAxisd(rpy.roll, Eigen::Vector3d::UnitX()) * + Eigen::Quaterniond quat(Eigen::AngleAxisd(rpy.yaw, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(rpy.pitch, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy.yaw, Eigen::Vector3d::UnitZ())); + Eigen::AngleAxisd(rpy.roll, Eigen::Vector3d::UnitX())); return quat; } diff --git a/src/world_interface/gps_common_interface.cpp b/src/world_interface/gps_common_interface.cpp index 30fbcc06f..db4d9b5ed 100644 --- a/src/world_interface/gps_common_interface.cpp +++ b/src/world_interface/gps_common_interface.cpp @@ -34,7 +34,7 @@ DataPoint readGPS() { } std::optional gpsToMeters(const gpscoords_t& coord) { - if (converter) { + if (gpsHasFix()) { return converter->gpsToMeters(coord); } else { return {}; diff --git a/src/world_interface/real_world_constants.h b/src/world_interface/real_world_constants.h index f68d8caff..6bba6375d 100644 --- a/src/world_interface/real_world_constants.h +++ b/src/world_interface/real_world_constants.h @@ -118,7 +118,7 @@ constexpr auto positive_pwm_scales = {motorid_t::wrist, 0.1}, {motorid_t::frontLeftWheel, 0.7}, {motorid_t::frontRightWheel, 0.7}, - {motorid_t::rearLeftWheel, -0.7}, + {motorid_t::rearLeftWheel, 0.7}, {motorid_t::rearRightWheel, -0.7}, {motorid_t::hand, -0.75}, {motorid_t::activeSuspension, -0.5}}); @@ -134,7 +134,7 @@ constexpr auto negative_pwm_scales = {motorid_t::wrist, 0.1}, {motorid_t::frontLeftWheel, 0.7}, {motorid_t::frontRightWheel, 0.7}, - {motorid_t::rearLeftWheel, -0.7}, + {motorid_t::rearLeftWheel, 0.7}, {motorid_t::rearRightWheel, -0.7}, {motorid_t::hand, -0.75}, {motorid_t::activeSuspension, -0.5}}); diff --git a/tests/commands/DriveToWaypointCommandTest.cpp b/tests/commands/DriveToWaypointCommandTest.cpp index 566ff7dd6..33af99524 100644 --- a/tests/commands/DriveToWaypointCommandTest.cpp +++ b/tests/commands/DriveToWaypointCommandTest.cpp @@ -1,110 +1,111 @@ -#include +#include "../../src/commands/DriveToWaypointCommand.h" + +#include "../../src/commands/CommandBase.h" + #include #include -#include "../../src/commands/CommandBase.h" -#include "../../src/commands/DriveToWaypointCommand.h" +#include using namespace robot::types; using namespace std::chrono_literals; namespace { -const double ATAN_4_3 = 0.927295218002; // value of atan(4/3) in rad. +const double ATAN_4_3 = 0.927295218002; // value of atan(4/3) in rad. const double THETA_KP = 3.5; const double NORMAL_DRIVE_SPEED = 4.0; -const double SLOW_DRIVE_SPEED = 2.0; +const double SLOW_DRIVE_THRESH = 1.5; const double DONE_THRESHOLD = 0.25; const util::dseconds CLOSE_TO_TARGET_DUR_VAL = 1s; const util::dseconds CLOSE_TO_TARGET_DUR_ALTVAL = 2s; void assertApprox(double expectedThetaVel, double expectedXVel, - const commands::command_t& actual, double angleThresh = 1e-5, - double velThresh = 1e-5) { - REQUIRE_THAT(actual.thetaVel, - Catch::WithinAbs(expectedThetaVel, angleThresh)); - REQUIRE_THAT(actual.xVel, Catch::WithinAbs(expectedXVel, velThresh)); + const commands::command_t& actual, double angleThresh = 1e-5, + double velThresh = 1e-5) { + REQUIRE_THAT(actual.thetaVel, Catch::WithinAbs(expectedThetaVel, angleThresh)); + REQUIRE_THAT(actual.xVel, Catch::WithinAbs(expectedXVel, velThresh)); } -} // namespace +} // namespace TEST_CASE("DriveToWaypointCommand Test") { - // sample target is at position (3,4). - navtypes::point_t target(3.0, 4.0, 1.5); - - datatime_t startTime = dataclock::now(); - - // instantiate DriveToWaypointCommand with sample target. - commands::DriveToWaypointCommand cmd(target, THETA_KP, NORMAL_DRIVE_SPEED, - SLOW_DRIVE_SPEED, DONE_THRESHOLD, - CLOSE_TO_TARGET_DUR_VAL); - - // test robot at origin, target at (3,4) - navtypes::pose_t origin(0.0, 0.0, 0.0); - cmd.setState(origin, startTime); - assertApprox(THETA_KP * ATAN_4_3, NORMAL_DRIVE_SPEED, cmd.getOutput()); - REQUIRE_FALSE(cmd.isDone()); - - // test robot on midpoint along line segment between origin and target. - navtypes::pose_t similar(1.5, 2.0, 0.0); - cmd.setState(similar, startTime); - assertApprox(THETA_KP * ATAN_4_3, NORMAL_DRIVE_SPEED, cmd.getOutput()); - REQUIRE_FALSE(cmd.isDone()); - - // test robot and target parallel along X-axis, robot directly facing target. - navtypes::pose_t parallelX(0.0, 4.0, 0.0); - cmd.setState(parallelX, startTime); - assertApprox(0.0, NORMAL_DRIVE_SPEED, cmd.getOutput()); - REQUIRE_FALSE(cmd.isDone()); - - // test robot parallel to target along X-axis, robot facing opposite target. - navtypes::pose_t aparallelX(6.0, 4.0, 0.0); - cmd.setState(aparallelX, startTime); - assertApprox(THETA_KP * M_PI, NORMAL_DRIVE_SPEED, cmd.getOutput()); - REQUIRE_FALSE(cmd.isDone()); - - // test robot and target parallel along Y-axis, robot directly facing target - navtypes::pose_t parallelY(3.0, 0.0, 0.0); - cmd.setState(parallelY, startTime); - assertApprox(THETA_KP * M_PI / 2.0, NORMAL_DRIVE_SPEED, cmd.getOutput()); - REQUIRE_FALSE(cmd.isDone()); - - // test robot and target parallel along Y-axis, robot facing opposite target. - navtypes::pose_t aparallelY(3.0, 0.0, -M_PI / 2.0); - cmd.setState(aparallelY, startTime); - assertApprox(THETA_KP * M_PI, NORMAL_DRIVE_SPEED, cmd.getOutput()); - REQUIRE_FALSE(cmd.isDone()); - - // test robot uses slow drive speed if within double the done threshold - // (in this case, done threshold is 25 cm) - navtypes::pose_t within_2d(2.7, 3.6, 0.0); - cmd.setState(within_2d, startTime); - assertApprox(THETA_KP * ATAN_4_3, SLOW_DRIVE_SPEED, cmd.getOutput()); - REQUIRE_FALSE(cmd.isDone()); - - // Ensure cmd.isDone() behaves correctly. - // set robot position to within distance threshold. - navtypes::pose_t within_d(2.85, 3.81, 0.0); - cmd.setState(within_d, startTime); - REQUIRE_FALSE(cmd.isDone()); - cmd.setState(within_d, startTime + 1s); - REQUIRE(cmd.isDone()); + // sample target is at position (3,4). + navtypes::point_t target(3.0, 4.0, 1); + + datatime_t startTime = dataclock::now(); + + // instantiate DriveToWaypointCommand with sample target. + commands::DriveToWaypointCommand cmd(target, THETA_KP, NORMAL_DRIVE_SPEED, + SLOW_DRIVE_THRESH, DONE_THRESHOLD, + CLOSE_TO_TARGET_DUR_VAL); + + // test robot at origin, target at (3,4) + navtypes::pose_t origin(0.0, 0.0, 0.0); + cmd.setState(origin, startTime); + assertApprox(THETA_KP * ATAN_4_3, NORMAL_DRIVE_SPEED, cmd.getOutput()); + REQUIRE_FALSE(cmd.isDone()); + + // test robot on midpoint along line segment between origin and target. + navtypes::pose_t similar(1.5, 2.0, 0.0); + cmd.setState(similar, startTime); + assertApprox(THETA_KP * ATAN_4_3, NORMAL_DRIVE_SPEED, cmd.getOutput()); + REQUIRE_FALSE(cmd.isDone()); + + // test robot and target parallel along X-axis, robot directly facing target. + navtypes::pose_t parallelX(0.0, 4.0, 0.0); + cmd.setState(parallelX, startTime); + assertApprox(0.0, NORMAL_DRIVE_SPEED, cmd.getOutput()); + REQUIRE_FALSE(cmd.isDone()); + + // test robot parallel to target along X-axis, robot facing opposite target. + navtypes::pose_t aparallelX(6.0, 4.0, 0.0); + cmd.setState(aparallelX, startTime); + assertApprox(THETA_KP * M_PI, NORMAL_DRIVE_SPEED, cmd.getOutput()); + REQUIRE_FALSE(cmd.isDone()); + + // test robot and target parallel along Y-axis, robot directly facing target + navtypes::pose_t parallelY(3.0, 0.0, 0.0); + cmd.setState(parallelY, startTime); + assertApprox(THETA_KP * M_PI / 2.0, NORMAL_DRIVE_SPEED, cmd.getOutput()); + REQUIRE_FALSE(cmd.isDone()); + + // test robot and target parallel along Y-axis, robot facing opposite target. + navtypes::pose_t aparallelY(3.0, 0.0, -M_PI / 2.0); + cmd.setState(aparallelY, startTime); + assertApprox(THETA_KP * M_PI, NORMAL_DRIVE_SPEED, cmd.getOutput()); + REQUIRE_FALSE(cmd.isDone()); + + // test robot slows down as it approaches target. + // (in this case, done threshold is 25 cm) + navtypes::pose_t within_2d(2.7, 3.6, 0.0); + cmd.setState(within_2d, startTime); + assertApprox(THETA_KP * ATAN_4_3, 4.0 / 3.0, cmd.getOutput()); + REQUIRE_FALSE(cmd.isDone()); + + // Ensure cmd.isDone() behaves correctly. + // set robot position to within distance threshold. + navtypes::pose_t within_d(2.85, 3.81, 0.0); + cmd.setState(within_d, startTime); + REQUIRE_FALSE(cmd.isDone()); + cmd.setState(within_d, startTime + 1s); + REQUIRE(cmd.isDone()); } TEST_CASE("DriveToWaypointCommand Test Alternate closeToTargetDur") { - navtypes::point_t target(3.0, 4.0, 1.5); - - datatime_t startTime = dataclock::now(); - - // instantiate DriveToWaypointCommand with sample target. - commands::DriveToWaypointCommand cmd(target, THETA_KP, NORMAL_DRIVE_SPEED, - SLOW_DRIVE_SPEED, DONE_THRESHOLD, - CLOSE_TO_TARGET_DUR_ALTVAL); - - // Ensure cmd.isDone() behaves correctly. - // set robot position to within distance threshold. - navtypes::pose_t within_d(2.85, 3.81, 0.0); - cmd.setState(within_d, startTime); - REQUIRE_FALSE(cmd.isDone()); - cmd.setState(within_d, startTime + 2s); - REQUIRE(cmd.isDone()); + navtypes::point_t target(3.0, 4.0, 1.5); + + datatime_t startTime = dataclock::now(); + + // instantiate DriveToWaypointCommand with sample target. + commands::DriveToWaypointCommand cmd(target, THETA_KP, NORMAL_DRIVE_SPEED, + SLOW_DRIVE_THRESH, DONE_THRESHOLD, + CLOSE_TO_TARGET_DUR_ALTVAL); + + // Ensure cmd.isDone() behaves correctly. + // set robot position to within distance threshold. + navtypes::pose_t within_d(2.85, 3.81, 0.0); + cmd.setState(within_d, startTime); + REQUIRE_FALSE(cmd.isDone()); + cmd.setState(within_d, startTime + 2s); + REQUIRE(cmd.isDone()); }