diff --git a/src/Rover.cpp b/src/Rover.cpp index 05bcd42c..f349e2dc 100644 --- a/src/Rover.cpp +++ b/src/Rover.cpp @@ -9,7 +9,6 @@ #include #include #include -#include #include #include #include @@ -33,39 +32,6 @@ void closeRover(int signum) { raise(SIGTERM); } -std::vector parseGPSLegs(std::string filepath) { - std::vector urc_legs; - std::ifstream gps_legs(filepath); - - int left_post_id, right_post_id; - double lat, lon; - std::string line; - - // A properly formatted file has each leg on a separate line, with each line of the form - // left_post_id right_post_id lat lon - // Improperly formatted lines (empty lines, comments) are ignored, and text - // after the above data on properly formatted lines is ignored - // An example can be found at example_gps_legs.txt - while (getline(gps_legs, line)) { - std::istringstream line_stream(line); - if (line_stream >> left_post_id >> right_post_id >> lat >> lon) { - // we assume that gps already has a fix - gpscoords_t gps = {lat, lon}; - URCLegGPS leg = {left_post_id, right_post_id, gps}; - LOG_F(INFO, "Got urc leg at lat=%f lon=%f", lat, lon); - urc_legs.push_back(leg); - } - } - LOG_F(INFO, "Got %ld urc legs\n", urc_legs.size()); - - if (urc_legs.size() == 0) { - LOG_F(ERROR, "could not get URC legs"); - std::exit(EXIT_FAILURE); - } - - return urc_legs; -} - void parseCommandLine(int argc, char** argv) { argparse::ArgumentParser program("Rover", "N/A"); diff --git a/src/navtypes.h b/src/navtypes.h index 4969109c..5ac7670f 100644 --- a/src/navtypes.h +++ b/src/navtypes.h @@ -25,11 +25,6 @@ struct eulerangles_t { double yaw; }; -/* Obstacles are specifed as a list of 2D vertices. Must be convex. - * Do not repeat the starting vertex at the end. */ -using obstacle_t = Eigen::ArrayX2d; -using obstacles_t = std::vector; - using pose_t = Eigen::Vector3d; // Robot pose: x, y, theta using transform_t = Eigen::Matrix3d; // a pose in matrix form; see toPose below using trajectory_t = std::vector; @@ -44,18 +39,5 @@ using point_t = Eigen::Vector3d; // x, y, 1 (or 0, 0, 0 to represent "no data") // The 1 at the end makes calculating affine transforms easier. using points_t = std::vector; -using traj_points_t = std::vector; // points_t for each time along a trajectory - -struct URCLeg { - int left_post_id; - int right_post_id; // This will be -1 for legs that are just single posts - point_t approx_GPS; -}; - -struct URCLegGPS { - int left_post_id; - int right_post_id; - gpscoords_t gps; -}; } // namespace navtypes diff --git a/src/world_interface/noop_world.cpp b/src/world_interface/noop_world.cpp index 400a97a4..22743c80 100644 --- a/src/world_interface/noop_world.cpp +++ b/src/world_interface/noop_world.cpp @@ -47,14 +47,6 @@ DataPoint getTruePose() { return {}; } -DataPoint readVisualOdomVel() { - return DataPoint{}; -} - -URCLeg getLeg(int /*id*/) { - return URCLeg{-1, -1, {0., 0., 0.}}; -} - void setMotorPower(motorid_t motor, double normalizedPWM) {} void setMotorPos(motorid_t motor, int32_t targetPos) {} diff --git a/src/world_interface/real_world_interface.cpp b/src/world_interface/real_world_interface.cpp index 48470201..5f2f7d57 100644 --- a/src/world_interface/real_world_interface.cpp +++ b/src/world_interface/real_world_interface.cpp @@ -234,14 +234,6 @@ landmarks_t readLandmarks() { return {}; } -DataPoint readVisualOdomVel() { - return DataPoint{}; -} - -URCLeg getLeg(int index) { - return URCLeg{0, -1, {0., 0., 0.}}; -} - template int getIndex(const std::vector& vec, const T& val) { auto itr = std::find(vec.begin(), vec.end(), val); diff --git a/src/world_interface/simulator_interface.cpp b/src/world_interface/simulator_interface.cpp index 0312fe42..995dce5a 100644 --- a/src/world_interface/simulator_interface.cpp +++ b/src/world_interface/simulator_interface.cpp @@ -338,10 +338,6 @@ landmarks_t readLandmarks() { return {}; } -DataPoint readVisualOdomVel() { - return DataPoint{}; -} - DataPoint readIMU() { std::lock_guard guard(orientationMutex); return lastOrientation; @@ -352,10 +348,6 @@ DataPoint getTruePose() { return lastTruePose; } -URCLeg getLeg(int index) { - return URCLeg{0, -1, {0., 0., 0.}}; -} - void setMotorPower(motorid_t motor, double normalizedPWM) { std::shared_ptr motor_ptr = getMotor(motor); motor_ptr->setMotorPower(normalizedPWM); diff --git a/src/world_interface/world_interface.h b/src/world_interface/world_interface.h index f5fd4d43..b5bccfb6 100644 --- a/src/world_interface/world_interface.h +++ b/src/world_interface/world_interface.h @@ -171,25 +171,6 @@ types::DataPoint getTruePose(); */ std::optional gpsToMeters(const navtypes::gpscoords_t& coord); -/** - * @brief Calculate the current pose velocity (in the robot's reference frame) using visual - * odometry. - * - * @return types::DataPoint The current velocity in each axis of the pose - * vector, or an empty data point if no data is available. - */ -types::DataPoint readVisualOdomVel(); - -/** - * @brief Get the URC leg of the given index. - * - * @param index The index of the URC leg, in the range [0, 6] - * @return navtypes::URCLeg Information for the URC leg of the given index. - * - * @deprecated This function is probably deprecated, and will be replaced in the future. - */ -navtypes::URCLeg getLeg(int index); - /** * @brief Set the robot indicator to indicate the given signal. * @@ -291,8 +272,6 @@ getMotorPositionsRad(const std::array& motors) { return types::DataPoint>(timestamp.value(), motorPositions); } -// TODO: document -void zeroJoint(robot::types::jointid_t joint); } // namespace robot namespace gps {