diff --git a/README.md b/README.md index fd21544d0..b85cf6b52 100644 --- a/README.md +++ b/README.md @@ -238,12 +238,10 @@ The simulator does not have its own executable. Instead, you must configure the ```bash cd build -cmake ../src -DWITH_CAN=OFF -DGPS=NONE -DWITH_LIDAR=OFF -DWITH_TESTS=OFF -DWORLD_INTERFACE=SIMULATOR +cmake ../src -DWORLD_INTERFACE=SIMULATOR make -j Rover ``` -Note that (for now) unit tests cannot be run when configured to build the simulator rover code. - ### Launching the simulator Launch the appropriate simulator executable for your platform. Then, run the rover code, using the `p` flag to specify a peripheral: @@ -260,7 +258,6 @@ Since the `Rover` target now builds the simulator rover code instead of the real ```bash cd build -rm -r * -cmake ../src +cmake ../src -DWORLD_INTERFACE=REAL make -j Rover ``` diff --git a/src/Autonomous.cpp b/src/Autonomous.cpp deleted file mode 100644 index 15095118a..000000000 --- a/src/Autonomous.cpp +++ /dev/null @@ -1,750 +0,0 @@ -#include "Autonomous.h" - -#include "Constants.h" -#include "Globals.h" -#include "Util.h" -#include "log.h" -#include "rospub.h" -#include "world_interface/world_interface.h" - -#include -#include - -#include - -using namespace navtypes; -using namespace robot::types; -using util::toTransform; -using util::toPose; - -namespace pose_graph = filters::pose_graph; - -constexpr float PI = M_PI; -constexpr double KP_ANGLE = 2.0; -constexpr double FAST_DRIVE_SPEED = 1.5; -constexpr double CAREFUL_DRIVE_SPEED = 1.0; -const Eigen::Vector3d gpsStdDev = {2, 2, 3}; -constexpr int numSamples = 1; - -constexpr double FOLLOW_DIST = 2.0; -constexpr double PLANNING_GOAL_REGION_RADIUS = 1.0; -constexpr double PLAN_COLLISION_DIST = 1.3; -constexpr double PLAN_COLLISION_STOP_DIST = 4.0; -constexpr double INFINITE_COST = 1e10; -constexpr int REPLAN_PERIOD = 20; - -constexpr bool USE_MAP = false; -constexpr bool MOVING_LANDMARKS = false; - -constexpr bool LEFT = true; -constexpr bool RIGHT = false; - -constexpr auto ZERO_DURATION = std::chrono::microseconds(0); - -// twice the scan period, should be adjusted as necessary -constexpr std::chrono::milliseconds LIDAR_FRESH_PERIOD(200); -constexpr std::chrono::milliseconds GPS_FRESH_PERIOD(2000); // update to match gps specs - -const transform_t VIZ_BASE_TF = toTransform({0, 0, M_PI / 2}); // TODO: remove this - -/** - @brief Prints a list of landmarks using the logging mechanism. - - The message is formatted as "Landmarks: [i: {x, y, z}, ...]" where i is the index of the - landmark in the list. - - @param landmarks The list of landmarks to print. - @param log_level The optional log level to use; defaults to LOG_DEBUG so the message isn't - seen if on a higher logging level. - */ -static bool printLandmarks(const points_t& landmarks, int log_level = LOG_DEBUG); - -/** - @brief Prints a list of landmarks using the logging mechanism. - - The message is formatted as "Landmarks: [i: {x, y, z}, ...]" where i is the index of the - landmark in the list. - - This is a duplicate of above, but modified to handle the DataPoint class. - - @param landmarks The landmarks data to print. - @param log_level The optional log level to use; defaults to LOG_DEBUG so the message isn't - seen if on a higher logging level. - */ -static bool printLandmarks(const landmarks_t& landmarks, int log_level = LOG_DEBUG); - -Autonomous::Autonomous(const std::vector& gpsTargets, double controlHz) - : initialized(false), urc_targets({}), urc_targets_gps(gpsTargets), leg_idx(0), - search_target({}), - gate_targets({NAN, NAN, NAN}, {NAN, NAN, NAN}), gate_direction(true), - poseEstimator({0.2, 0.2}, gpsStdDev, Constants::WHEEL_BASE, 1.0 / controlHz), - map(10000), // TODO: the stride should be set higher when using the real lidar - calibrated(false), calibrationPoses({}), nav_state(NavState::GPS), - control_state(ControlState::FAR_FROM_TARGET_POSE), time_since_plan(0), plan(0, 2), - pending_plan(), pending_solve(), plan_cost(INFINITE_COST), plan_base({0, 0, 0}), - plan_idx(0), search_theta_increment(PI / 4), mapLoopCounter(0), mapBlindPeriod(15), - mapDoesOverlap(false), mapOverlapSampleThreshold(15), - pose_graph(0, 0, 0, 0, 0), // We'll re-initialize this in the function body - pose_id(0), prev_odom(toTransform({0, 0, 0})), smoothed_traj({}), - smoothed_landmarks({}) { -} - -void Autonomous::init() { - if (!robot::gpsHasFix()) { - return; - } - this->urc_targets.clear(); - // convert gps targets to map space - for (const auto& gpsTarget : urc_targets_gps) { - point_t p = robot::gpsToMeters(gpsTarget.gps).value(); - urc_targets.push_back({gpsTarget.left_post_id, gpsTarget.right_post_id, p}); - } - search_target = {urc_targets[0].approx_GPS(0), urc_targets[0].approx_GPS(1), -M_PI / 2}; - int num_landmarks = 0; - for (auto target : urc_targets) { - int num_landmarks_for_target = (target.right_post_id == -1) ? 1 : 2; - num_landmarks += num_landmarks_for_target; - } - // We use a gps_xy_std that's larger than the true std because empirically - // that leads to smoother changes in the pose estimate (at least in the simulator), - // which are easier for the controller to work with. - pose_graph = pose_graph::FriendlyGraph(num_landmarks, 10, 0.3, 5.0, 0.05); - prev_odom = robot::readOdom(); - // TODO how to make this start pose configurable? - transform_t start_pose_guess = toTransform({0, 0, 0}); - double pose_prior_xy_std = 3.0; - double pose_prior_th_std = 1.0; - pose_graph::covariance<3> pose_prior_cov = pose_graph::covariance<3>::Zero(); - pose_prior_cov << pose_prior_xy_std * pose_prior_xy_std, 0, 0, 0, - pose_prior_xy_std * pose_prior_xy_std, 0, 0, 0, pose_prior_th_std * pose_prior_th_std; - pose_graph.addPosePrior(0, start_pose_guess, pose_prior_cov); - double lm_std = 100.0; - for (auto target : urc_targets) { - pose_graph.addLandmarkPrior(target.left_post_id, target.approx_GPS, lm_std); - if (target.right_post_id != -1) { - pose_graph.addLandmarkPrior(target.right_post_id, target.approx_GPS, lm_std); - } - } - pose_graph.solve(); - smoothed_traj = pose_graph.getSmoothedTrajectory(); - smoothed_landmarks = pose_graph.getLandmarkLocations(); - initialized = true; -} - -double dist(const pose_t& p1, const pose_t& p2, double theta_weight) { - pose_t diff = p1 - p2; - // angles are modular in nature, so wrap at 2pi radians - double thetaDiff = std::fmod(fabs(diff(2)), 2 * PI); - // change domain from [0, 2pi) to (-pi, pi] - if (thetaDiff > PI) { - thetaDiff -= 2 * PI; - } - diff(2) = thetaDiff * theta_weight; - return diff.norm(); -} - -void Autonomous::endCurrentLeg() { - log(LOG_INFO, "Leg complete, exiting autonomous mode\n"); - robot::setCmdVel(0, 0); - Globals::AUTONOMOUS = false; - leg_idx += 1; - // clear the cached data points - - if (leg_idx == urc_targets.size()) { - log(LOG_INFO, "All legs complete\n"); - } else { - search_target = {urc_targets[leg_idx].approx_GPS(0) - PI, - urc_targets[leg_idx].approx_GPS(1), -PI / 2}; - setNavState(NavState::GPS); - log(LOG_INFO, "Enter autonomous mode to start next leg\n"); - } -} - -void Autonomous::setNavState(NavState s) { - NavState old_state = nav_state; - nav_state = s; - if (old_state != s) { - log(LOG_INFO, "Changing navigation state %s -> %s\n", NAV_STATE_NAMES[old_state], - NAV_STATE_NAMES[nav_state]); - plan_cost = INFINITE_COST; - } -} - -pose_t Autonomous::poseToDraw(const pose_t& pose, const pose_t& current_pose) const { - // Both poses are given in the same frame. (usually GPS frame) - // `current_pose` is the current location of the robot, `pose` is the pose we wish to draw - transform_t inv_curr = toTransform(current_pose).inverse(); - return toPose(toTransform(pose) * inv_curr * VIZ_BASE_TF, 0); -} - -void Autonomous::update_nav_state(const pose_t& pose, const pose_t& plan_target) { - if (nav_state == NavState::GPS || nav_state == NavState::SEARCH_PATTERN || - nav_state == NavState::SEARCH_PATTERN_SECOND_POST || - nav_state == NavState::POST_VISIBLE) { - bool foundLeftPost = getPostVisibility(LEFT); - bool foundRightPost = getPostVisibility(RIGHT); - if (foundLeftPost && foundRightPost) { - setNavState(NavState::GATE_ALIGN); - } else if ((foundLeftPost || foundRightPost) && - (nav_state != SEARCH_PATTERN_SECOND_POST)) { - setNavState(NavState::POST_VISIBLE); - } - } - - if (dist(pose, plan_target, 1.0) < 0.5) { - if (nav_state == NavState::GPS) { - log(LOG_WARN, "Reached GPS target without seeing post!\n"); - setNavState(NavState::SEARCH_PATTERN); - } else if (nav_state == NavState::POST_VISIBLE) { - if (urc_targets[leg_idx].right_post_id == -1) { - log(LOG_INFO, "Arrived at post\n"); - setNavState(NavState::DONE); - } else { - log(LOG_WARN, "Reached left post without seeing right post!\n"); - setNavState(NavState::SEARCH_PATTERN_SECOND_POST); - } - } else if (nav_state == NavState::GATE_ALIGN) { - setNavState(NavState::GATE_TRAVERSE); - } else if (nav_state == NavState::GATE_TRAVERSE) { - log(LOG_INFO, "Arrived at gate\n"); - setNavState(NavState::DONE); - } else if (nav_state == NavState::SEARCH_PATTERN || - nav_state == NavState::SEARCH_PATTERN_SECOND_POST) { - // Current search point has been reached - updateSearchTarget(); - } - } -} - -bool calibratePeriodic(std::vector& poses, const pose_t& pose, pose_t& out) { - poses.push_back(pose); - if (poses.size() == numSamples) { - pose_t sum = pose_t::Zero(); - for (const pose_t& p : poses) { - sum += p; - } - sum /= poses.size(); - out = sum; - return true; - } - return false; -} - -double getRelativeAngle(double currAngle, double targetAngle) { - double range = 2 * PI; - double dist = std::fmod(targetAngle - currAngle, range); - double absDist = fabs(dist); - - int sign = dist > 0 ? 1 : (dist < 0 ? -1 : 0); - - return currAngle + ((absDist > (range / 2)) ? -sign * (range - absDist) : dist); -} - -double Autonomous::getLinearVel(const pose_t& drive_target, const pose_t& pose, - double thetaErr) const { - double speed = FAST_DRIVE_SPEED; - if (dist(drive_target, pose, 0) < 1.0 || nav_state == NavState::GATE_TRAVERSE) { - // We may need to drive slower near the goal so that we don't overshoot the target - // (given our relatively low control frequency of 10 Hz) - speed = CAREFUL_DRIVE_SPEED; - } - if (fabs(thetaErr) > PI / 4 || control_state == ControlState::NEAR_TARGET_POSE) { - // don't drive forward if pointing away - // or if we're already very close to the target - speed = 0; - } - return speed; -} - -double Autonomous::getThetaVel(const pose_t& drive_target, const pose_t& pose, - double& thetaErr) const { - // If we're within 20cm of the target location, we want to turn the rover until - // we reach the target orientation. Otherwise, we want to turn the rover to - // aim it at the target location. - double targetAngle = drive_target(2); - if (control_state == ControlState::FAR_FROM_TARGET_POSE) { - double dx = drive_target(0) - pose(0); - double dy = drive_target(1) - pose(1); - targetAngle = atan2(dy, dx); - } - double relativeTargetAngle = getRelativeAngle(pose(2), targetAngle); - thetaErr = relativeTargetAngle - pose(2); - - return KP_ANGLE * thetaErr; -} - -int Autonomous::getPostID(bool left) { - int post_id = - left ? urc_targets[leg_idx].left_post_id : urc_targets[leg_idx].right_post_id; - return post_id; -} - -point_t Autonomous::getPostLocation(bool left, bool verbose) { - int post_id = getPostID(left); - if (post_id < 0 || static_cast(post_id) > smoothed_landmarks.size()) { - if (verbose) { - log(LOG_ERROR, "Invalid post_id %d\n", urc_targets[leg_idx].left_post_id); - } - return {0, 0, 0}; - } - return smoothed_landmarks[post_id]; -} - -bool Autonomous::getPostVisibility(bool left) { - // This is a bit of a hack. We decide whether a post is currently visible based - // on whether its current estimated location is different from its approximate - // GPS coordinates. Really what we should do is implement some pose graph function - // that tells us how uncertain we are about the location of the post, but that's - // more complicated to implement. - point_t prior_location = urc_targets[leg_idx].approx_GPS; - point_t location = getPostLocation(left, false); - double diff = (location - prior_location).norm(); - bool different_from_prior = (diff > 0.001); - bool visible = different_from_prior && (location(2) != 0); - log(LOG_DEBUG, - "Visibility %s is %d: diff %.3f loc (%.3f %.3f %.3f) prior (%.3f %.3f %.3f)\n", - left ? "LEFT " : "RIGHT", visible, diff, location(0), location(1), location(2), - prior_location(0), prior_location(1), prior_location(2)); - return visible; -} - -pose_t Autonomous::choose_plan_target(const pose_t& pose) { - // When traversing the gate, we don't want to reverse the order of the gate targets - computeGateTargets(pose, nav_state != NavState::GATE_TRAVERSE); - - if (nav_state == NavState::GPS) { - return getGPSTargetPose(); - } else if (nav_state == NavState::SEARCH_PATTERN || - nav_state == NavState::SEARCH_PATTERN_SECOND_POST) { - return search_target.value(); - } else if (nav_state == NavState::POST_VISIBLE) { - bool post_to_get = getPostVisibility(LEFT) ? LEFT : RIGHT; - point_t post = getPostLocation(post_to_get); - double angle = std::atan2(post(1) - pose(1), post(0) - pose(0)); - // Offset the target by two meters to avoid crashing into the post - pose_t plan_target({post(0) - 2.0 * cos(angle), post(1) - 2.0 * sin(angle), angle}); - return plan_target; - } else if (nav_state == NavState::GATE_ALIGN) { - return gate_targets.first; - } else if (nav_state == NavState::GATE_TRAVERSE) { - return gate_targets.second; - } else { // probably NavState::DONE - return {0, 0, 0}; - } -} - -void Autonomous::computeGateTargets(const pose_t& pose, bool choose_direction) { - if (!(getPostVisibility(LEFT) && getPostVisibility(RIGHT))) - return; - - point_t post_1 = getPostLocation(LEFT); - point_t post_2 = getPostLocation(RIGHT); - point_t center = (post_1 + post_2) / 2; - - point_t offset{(post_1(1) - post_2(1)) / 2, (post_2(0) - post_1(0)) / 2, 0}; - pose_t goal_1 = center + 2 * offset; - pose_t goal_2 = center - 2 * offset; - - // Choose the closest goal to be the first target - if (choose_direction) { - gate_direction = (dist(pose, goal_1, 1.0) < dist(pose, goal_2, 1.0)); - } - - if (gate_direction == true) { - gate_targets = std::make_pair(goal_1, goal_2); - } else { - gate_targets = std::make_pair(goal_2, goal_1); - } - - // Angle between targets - double angle = std::atan2(gate_targets.second(1) - gate_targets.first(1), - gate_targets.second(0) - gate_targets.first(0)); - gate_targets.first(2) = angle; - gate_targets.second(2) = angle; -} - -void Autonomous::updateSearchTarget() { - // Replan to avoid waiting at current search point - plan_cost = INFINITE_COST; - // Set the search target to the next point in the search pattern - pose_t& search_target = this->search_target.value(); - search_target -= urc_targets[leg_idx].approx_GPS; - double radius = hypot(search_target(0), search_target(1)); - double scale = (radius + search_theta_increment) / radius; - // Rotate the target counterclockwise by the theta increment - search_target.topRows(2) = - (Eigen::Matrix2d() << cos(search_theta_increment), -sin(search_theta_increment), - sin(search_theta_increment), cos(search_theta_increment)) - .finished() * - search_target.topRows(2) * scale; - search_target += urc_targets[leg_idx].approx_GPS; - // Adjust the target angle - search_target(2) += search_theta_increment; - if (search_target(2) > PI) { - search_target(2) -= 2 * PI; - // Decrease the theta increment every time a full rotation is made so the - // spiral shape is followed better - // This increases the denominator of the theta increment by 4 - search_theta_increment = PI / ((int)(PI / search_theta_increment + 4)); - } -} - -plan_t computePlan(transform_t invTransform, point_t goal) { - // We need to readLidar again from within the planning thread, for thread safety - auto lidarData = robot::readLidarScan(); - points_t lidar_scan; - std::function collide_func; - if (lidarData.isFresh(LIDAR_FRESH_PERIOD)) { - lidar_scan = lidarData.getData(); - collide_func = [&](double x, double y, double radius) -> bool { - // transform the point to check into map space - point_t relPoint = {x, y, 1}; - point_t p = invTransform * relPoint; - transform_t coll_tf = toTransform(relPoint); - // TODO implement thread-safe access to `map` if `USE_MAP` is true - // if (USE_MAP) return map.hasPointWithin(p, radius); - return util::collides(coll_tf, lidar_scan, radius); - }; - } else { - if (!lidarData) { - log(LOG_ERROR, "Tried to read from the lidar before it was initialized!\n"); - } else { - log(LOG_WARN, "Data is older than %dms!\n", LIDAR_FRESH_PERIOD); - } - collide_func = [](double, double, double) -> bool { return false; }; - } - return getPlan(collide_func, goal, PLANNING_GOAL_REGION_RADIUS); -} - -transform_t Autonomous::optimizePoseGraph(transform_t current_odom) { - DataPoint gpsData = robot::readGPS(); - bool new_gps_data = gpsData.isFresh(GPS_FRESH_PERIOD) && lastGPSTime != gpsData.getTime(); - if (new_gps_data) { - lastGPSTime = gpsData.getTime(); - } - - landmarks_t landmarks = robot::readLandmarks(); - printLandmarks(landmarks, LOG_DEBUG); - // check if any of the landmark data is new - bool new_landmark_data = false; - for (size_t i = 0; i < landmarks.size(); i++) { - const auto& lm = landmarks[i]; - // we don't check isFresh() because that's handled in the AR code - if (lm) { - auto lastTimeItr = lastLandmarkTimes.find(i); - // if we haven't gotten any data before, or if the current data is more recent, - // mark as new - if (lastTimeItr == lastLandmarkTimes.end() || - lastTimeItr->second != lm.getTime()) { - new_landmark_data = true; - lastLandmarkTimes[i] = lm.getTime(); - } else { - // we've seen this data point before, so mark as empty (so it won't be used - // later) - landmarks[i] = {}; - } - } - } - - double odom_diff = toPose(current_odom * prev_odom.inverse(), 0.0).norm(); - bool overwrite_landmark = (current_odom == prev_odom) && new_landmark_data; - bool new_data = - new_gps_data || (odom_diff > 0.5 && new_landmark_data) || overwrite_landmark; - - if (new_data) { - if (current_odom != prev_odom) { // Don't add poses to pose graph if we haven't moved - pose_id += 1; - pose_graph.addOdomMeasurement(pose_id, pose_id - 1, current_odom, prev_odom); - } - for (size_t lm_id = 0; lm_id < landmarks.size(); lm_id++) { - auto lmData = landmarks[lm_id]; - if (lmData) { - // data is assumed to be new here (since it should have been checked above) - point_t lm = lmData.getData(); - bool overwrite = overwrite_landmark || MOVING_LANDMARKS; - pose_graph.addLandmarkMeasurement(pose_id, (int)lm_id, lm, overwrite); - } - } - // TODO: change addGPSMeasurement() to accept a point_t instead of transform_t - if (new_gps_data) - pose_graph.addGPSMeasurement(pose_id, toTransform(gpsData.getData())); - - pose_graph.solve(); - int log_level = LOG_DEBUG; - log(log_level, "Found pose graph solution: "); - bool found_one = printLandmarks(pose_graph.getLandmarkLocations(), log_level); - if (!found_one) - log(log_level, "No landmarks found.\n"); - return current_odom; - } else { - return prev_odom; - } -} - -void Autonomous::autonomyIter() { - if (!Globals::AUTONOMOUS) { - return; - } else if (robot::gpsHasFix() && !initialized) { - init(); - } else if (!robot::gpsHasFix()) { - log(LOG_WARN, "Waiting for GPS fix...\n"); - return; - } - - transform_t odom = robot::readOdom(); - pose_t pose{0, 0, 0}; - - if (!pending_solve.valid()) { - // TODO even if we already have a pose graph optimization running, we should still - // add data to the pose graph (e.g. if we have new camera or GPS readings). - // However, this would be more complicated to implement, and the improvement is - // likely insignificant as long as pose graph solves take significantly less time - // than one second. - pending_solve = - std::async(std::launch::async, &Autonomous::optimizePoseGraph, this, odom); - } - - if (pending_solve.valid() && - pending_solve.wait_for(ZERO_DURATION) == std::future_status::ready) { - prev_odom = pending_solve.get(); - smoothed_traj = pose_graph.getSmoothedTrajectory(); - smoothed_landmarks = pose_graph.getLandmarkLocations(); - } - - transform_t current_tf = - odom * prev_odom.inverse() * smoothed_traj[smoothed_traj.size() - 1]; - pose = toPose(current_tf, 0.0); - log(LOG_DEBUG, "pose %f %f %f\n", pose(0), pose(1), pose(2)); - - for (transform_t& sm_tf : smoothed_traj) { - const pose_t sm_tf_transformed = poseToDraw(toPose(sm_tf, 0.0), pose); - rospub::publish(sm_tf_transformed, rospub::PointPub::POSE_GRAPH); - } - - transform_t invTransform = toTransform(pose).inverse(); - - const points_t landmarks_transformed = - util::transformReadings(smoothed_landmarks, invTransform * VIZ_BASE_TF); - rospub::publish_array(landmarks_transformed, rospub::ArrayPub::LANDMARKS); - - if (nav_state == NavState::DONE && Globals::AUTONOMOUS) { - endCurrentLeg(); - return; - } - - pose_t plan_target = choose_plan_target(pose); - log(LOG_DEBUG, "plan_target %f %f %f\n", plan_target(0), plan_target(1), plan_target(2)); - update_nav_state(pose, plan_target); - - auto lidarData = robot::readLidarScan(); - points_t lidar_scan; - if (lidarData.isFresh(LIDAR_FRESH_PERIOD)) { - if (lastLidarTime != lidarData.getTime()) { - lidar_scan = lidarData.getData(); - lastLidarTime = lidarData.getTime(); - } else { - log(LOG_DEBUG, "No new lidar data available!\n"); - } - } else if (!lidarData) { - log(LOG_ERROR, "Tried to read from the lidar before it was initialized!\n"); - } else { - log(LOG_DEBUG, "Data is older than %dms!\n", LIDAR_FRESH_PERIOD); - } - - if (USE_MAP) { - if (mapLoopCounter++ > mapBlindPeriod) { - map.addPoints(toTransform(pose), lidar_scan, mapDoesOverlap ? 0.4 : 0); - mapDoesOverlap = lidar_scan.size() > mapOverlapSampleThreshold; - } - } - - // if we don't have a plan pending, we should start computing one. - if (!pending_plan.valid()) { - point_t point_t_goal; - point_t_goal.topRows(2) = plan_target.topRows(2); - point_t_goal(2) = 1.0; - point_t_goal = toTransform(pose) * point_t_goal; - log(LOG_DEBUG, "point_t_goal %f %f %f\n", point_t_goal(0), point_t_goal(1), - point_t_goal(2)); - pending_plan = - std::async(std::launch::async, &computePlan, invTransform, point_t_goal); - } - // if there is a plan pending, check if it is ready. - else if (pending_plan.wait_for(ZERO_DURATION) == std::future_status::ready) { - // plan is ready; retrieve it and check if the cost is satisfactory. - // upon calling get(), pending_plan.valid() will return false, so if we reject this - // plan another will be computed. - plan_t new_plan = pending_plan.get(); - double new_plan_cost = planCostFromIndex(new_plan, 0); - // we want a significant improvement to avoid thrash - if (new_plan_cost < plan_cost * 0.8) { - log(LOG_DEBUG, "Got new plan: %ld steps, %f cost (previous: %ld steps, %f cost)\n", - new_plan.size(), new_plan_cost, plan.size(), plan_cost); - plan_idx = 0; - plan_base = pose; - plan_cost = new_plan_cost; - plan = new_plan; - time_since_plan = 0; - } - } - - // Send lidar points to visualization - const points_t lidar_scan_transformed = util::transformReadings(lidar_scan, VIZ_BASE_TF); - log(LOG_DEBUG, "Publishing %d lidar points\n", lidar_scan_transformed.size()); - rospub::publish_array(lidar_scan_transformed, rospub::ArrayPub::LIDAR_SCAN); - - // Send current pose and plan target to visualization - const pose_t curr_pose_transformed = poseToDraw(pose, pose); - rospub::publish(curr_pose_transformed, rospub::PointPub::CURRENT_POSE); - const pose_t plan_target_transformed = poseToDraw(plan_target, pose); - rospub::publish(plan_target_transformed, rospub::PointPub::PLAN_TARGET); - - pose_t driveTarget = pose; - if (nav_state == NavState::GATE_TRAVERSE || dist(plan_target, pose, 0.0) < 2.0) { - // We're probably within planning resolution of the goal, - // so using the goal as the drive target makes sense. - driveTarget = plan_target; - } else { - driveTarget = getDriveTargetFromPlan(pose, plan_target, lidar_scan); - } - // Send drive target to visualization - const pose_t drive_target_transformed = poseToDraw(driveTarget, pose); - rospub::publish(drive_target_transformed, rospub::PointPub::DRIVE_TARGET); - - // Send message to visualization that all data has been sent - rospub::publish({NAN, NAN, NAN}, rospub::PointPub::PLAN_VIZ); - - double d = dist(driveTarget, pose, 0); - // There's an overlap where either state might apply, to prevent rapidly switching - // back and forth between these two states. - if (d < 0.2 && control_state == ControlState::FAR_FROM_TARGET_POSE) { - control_state = ControlState::NEAR_TARGET_POSE; - } - if (d > 0.5 && control_state == ControlState::NEAR_TARGET_POSE) { - control_state = ControlState::FAR_FROM_TARGET_POSE; - } - double thetaErr; - double thetaVel = getThetaVel(driveTarget, pose, thetaErr); - double driveSpeed = getLinearVel(driveTarget, pose, thetaErr); - - if (!Globals::E_STOP && Globals::AUTONOMOUS) { - double scale_factor = robot::setCmdVel(thetaVel, driveSpeed); - } -} - -pose_t Autonomous::getDriveTargetFromPlan(const pose_t& pose, const pose_t& plan_target, - const points_t& lidar_scan) { - // Roll out the plan until we find a target pose a certain distance in front - // of the robot position `pose`. - // - // This code also handles visualizing the plan. - // - // It also handles estimating the cost of following the current plan, so that - // we can replace it with a better plan if we find one. - int plan_size = plan.rows(); - - pose_t plan_pose = plan_base; - pose_t driveTarget = pose; - transform_t invTransform = toTransform(pose).inverse(); - - bool found_target = false; - double accumulated_cost = 0; // Estimates the cost-to-go for following the current plan - for (int i = 0; i <= plan_size; i++) { - transform_t tf_plan_pose = toTransform(plan_pose) * invTransform; - // We don't care about collisions on already-executed parts of the plan - bool plan_pose_collides = (i >= plan_idx); - if (USE_MAP) { - plan_pose_collides &= map.hasPointWithin(plan_pose, PLAN_COLLISION_DIST); - } else { - plan_pose_collides &= util::collides(tf_plan_pose, lidar_scan, PLAN_COLLISION_DIST); - } - if (plan_pose_collides) { - // We'll replan next timestep - accumulated_cost += INFINITE_COST; - // TODO we should have a more sophisticated way of deciding whether a collision - // is imminent. - if (dist(plan_pose, pose, 0.0) < PLAN_COLLISION_STOP_DIST) { - log(LOG_WARN, "Collision imminent! Stopping the rover!\n"); - driveTarget = pose; // Don't move - found_target = true; - } - } - if (i >= plan_idx && !found_target && dist(plan_pose, pose, 1.0) > FOLLOW_DIST) { - found_target = true; - plan_idx = i; - accumulated_cost += planCostFromIndex(plan, i); - driveTarget = plan_pose; - if (dist(plan_pose, pose, 1.0) > 2 * FOLLOW_DIST) { - log(LOG_WARN, - "Drifted too far from plan. Stopping the rover and replanning.\n"); - driveTarget = pose; // Don't move - accumulated_cost += INFINITE_COST; - } - } else { - // Send current plan_pose to visualization - const pose_t curr_plan_transformed = poseToDraw(plan_pose, pose); - rospub::publish(curr_plan_transformed, rospub::PointPub::PLAN_VIZ); - } - if (i < plan_size) { - action_t action = plan.row(i); - plan_pose(2) += action(0); - plan_pose(0) += action(1) * cos(plan_pose(2)); - plan_pose(1) += action(1) * sin(plan_pose(2)); - } - } - pose_t final_plan_pose = plan_pose; // Renaming for clarity - if (!found_target) { - // We reached the end of the plan without finding a drive target far enough away. - // Set the drive target to the last point on the plan. - plan_idx = plan_size - 1; - driveTarget = final_plan_pose; - } - - if (dist(final_plan_pose, plan_target, 0.0) > 2 * PLANNING_GOAL_REGION_RADIUS) { - log(LOG_DEBUG, "Plan target moved (%f %f %f -> %f %f %f). Replanning.\n", - final_plan_pose(0), final_plan_pose(1), final_plan_pose(2), plan_target(0), - plan_target(1), plan_target(2)); - accumulated_cost += INFINITE_COST; - } - plan_cost = accumulated_cost; - - return driveTarget; -} - -pose_t Autonomous::getGPSTargetPose() const { - pose_t ret{urc_targets[leg_idx].approx_GPS(0), urc_targets[leg_idx].approx_GPS(1), 0.0}; - return ret; -} - -static bool printLandmarks(const points_t& landmarks, int log_level) { - std::ostringstream stream; - bool found_one = false; - for (size_t i = 0; i < landmarks.size(); i++) { - point_t lm = landmarks[i]; - if (lm[2] != 0 && lm.topRows(2).norm() != 0) { - found_one = true; - stream << "Landmark " << i << " at {" << lm[0] << ", " << lm[1] << "} "; - } - } - if (found_one) - stream << std::endl; - log(log_level, stream.str().c_str()); - return found_one; -} - -static bool printLandmarks(const landmarks_t& landmarks, int log_level) { - std::ostringstream stream; - bool found_one = false; - for (size_t i = 0; i < landmarks.size(); i++) { - auto lmData = landmarks[i]; - if (lmData && lmData.getData().topRows(2).norm() != 0) { - point_t lm = lmData.getData(); - found_one = true; - stream << "Landmark " << i << " at {" << lm[0] << ", " << lm[1] << "} "; - } - } - if (found_one) - stream << std::endl; - log(log_level, stream.str().c_str()); - return found_one; -} diff --git a/src/Autonomous.h b/src/Autonomous.h deleted file mode 100644 index fae73a4ba..000000000 --- a/src/Autonomous.h +++ /dev/null @@ -1,112 +0,0 @@ -#pragma once - -#include "Util.h" -#include "filters/PoseEstimator.h" -#include "filters/RollingAvgFilter.h" -#include "filters/pose_graph/friendly_graph.h" -#include "navtypes.h" -#include "planning/plan.h" -#include "world_interface/data.h" -#include "worldmap/GlobalMap.h" - -#include -#include -#include -#include -#include -#include - -enum ControlState { NEAR_TARGET_POSE, FAR_FROM_TARGET_POSE }; - -enum NavState { - GPS, - POST_VISIBLE, - SEARCH_PATTERN, - SEARCH_PATTERN_SECOND_POST, - GATE_ALIGN, - GATE_TRAVERSE, - DONE -}; - -constexpr std::array NAV_STATE_NAMES({"GPS", "POST_VISIBLE", "SEARCH_PATTERN", - "SEARCH_PATTERN_SECOND_POST", - "GATE_ALIGN", "GATE_TRAVERSE", "DONE"}); - -class Autonomous { -public: - explicit Autonomous(const std::vector& gpsTargets, double controlHz); - Autonomous(double controlHz, const navtypes::pose_t& startPose); - void init(); - // Returns a pair of floats, in heading, speed - // Accepts current heading of the robot as parameter - // Gets the target's coordinate - navtypes::pose_t getGPSTargetPose() const; - void autonomyIter(); - -private: - bool initialized; - std::vector urc_targets; - std::vector urc_targets_gps; - size_t leg_idx; // which of the urc_targets we're currently navigating toward - std::optional search_target; - // Gate targets are {NAN, NAN, NAN} if unset and {INF, INF, INF} if reached - // gate_targets.second(2) is NAN if targets have not been refined with more accurate - // landmark measurements - std::pair gate_targets; - bool gate_direction; // `true` if we go through the gate with left_post_id on our left - filters::PoseEstimator poseEstimator; - GlobalMap map; - bool calibrated = false; - std::vector calibrationPoses{}; - NavState nav_state; - ControlState control_state; - int time_since_plan; - plan_t plan; - std::future pending_plan; - std::future pending_solve; - double plan_cost; - navtypes::pose_t plan_base; - int plan_idx; - double search_theta_increment; - int mapLoopCounter; // number of times the map has been updated - int mapBlindPeriod; // the number of loops to wait before starting to build the map - bool mapDoesOverlap; - unsigned int - mapOverlapSampleThreshold; // at least these many points required to overlap map - - std::optional lastLidarTime; // last timestamp of lidar data - std::optional lastGPSTime; // last timestamp of gps data - std::map - lastLandmarkTimes; // maps landmark idx -> last datapoint time - - /* Variables for pose graph localization */ - filters::pose_graph::FriendlyGraph pose_graph; - int pose_id; // counter for how many poses we've added to the graph - navtypes::transform_t - prev_odom; // odom measurement at the time of the most recent pose in the graph - navtypes::trajectory_t - smoothed_traj; // cached solution to pose graph optimization (robot trajectory) - navtypes::points_t - smoothed_landmarks; // cached solution to pose graph optimization (AR tags) - - void setNavState(NavState s); - void update_nav_state(const navtypes::pose_t& pose, const navtypes::pose_t& plan_target); - navtypes::pose_t choose_plan_target(const navtypes::pose_t& pose); - navtypes::pose_t getDriveTargetFromPlan(const navtypes::pose_t& pose, - const navtypes::pose_t& plan_target, - const navtypes::points_t& lidar_scan); - int getPostID(bool left); - navtypes::point_t getPostLocation(bool left, bool verbose = true); - bool getPostVisibility(bool left); - void computeGateTargets(const navtypes::pose_t& pose, bool choose_direction); - void updateSearchTarget(); - void endCurrentLeg(); - navtypes::transform_t optimizePoseGraph(navtypes::transform_t current_odom); - - double getLinearVel(const navtypes::pose_t& drive_target, const navtypes::pose_t& pose, - double thetaErr) const; - double getThetaVel(const navtypes::pose_t& drive_target, const navtypes::pose_t& pose, - double& thetaErr) const; - navtypes::pose_t poseToDraw(const navtypes::pose_t& pose, - const navtypes::pose_t& current_pose) const; -}; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c968f6a60..93ecec63b 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -29,25 +29,12 @@ if(NOT CMAKE_EXPORT_COMPILE_COMMANDS) during generation." FORCE) endif() -# CAN should be disabled by default if we are not on a Linux system, since it depends on Linux -# kernel-specific header files. -if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") - set(WITH_CAN_DEFAULT ON) -else() - set(WITH_CAN_DEFAULT OFF) -endif() - ##====== Module settings ====== # These variables control whether various modules are enabled or disabled. The goal is to allow -# a user to selectively exclude modules that they do not need (and/or cannot compile on their -# system); for example, we need a way to disable CAN on non-Linux systems that don't support -# it, so this is a way to do that. -set(WITH_CAN ${WITH_CAN_DEFAULT} CACHE BOOL "Enable/Disable the CAN code. Disabled on non-Linux -systems.") +# a user to selectively exclude modules that they do not need. set(GPS "ARDUPILOT" CACHE STRING "GPS interface to use.") set(AVAILABLE_GPS "USB" "ARDUPILOT" "NONE") set_property(CACHE GPS PROPERTY STRINGS ${AVAILABLE_GPS}) -set(WITH_LIDAR ON CACHE BOOL "Enable/Disable the USB LiDAR interface.") set(WITH_TESTS ON CACHE BOOL "Enable/Disable the tests.") set(WORLD_INTERFACE "REAL" CACHE STRING "The world interface implementation to use.") set(AVAILABLE_WORLD_INTERFACES "REAL" "SIMULATOR" "NO-OP") @@ -65,20 +52,13 @@ endif() message("========================================\n\ Module settings:") -if(NOT WITH_CAN_DEFAULT) - message(" CAN Networking:\t${WITH_CAN} (non-Linux system - disabled by default)") -else() - message(" CAN Networking:\t${WITH_CAN}") -endif() message(" GPS:\t\t\t${GPS}") -message(" USB LiDAR code:\t${WITH_LIDAR}") message(" Test Suite:\t\t${WITH_TESTS}") message(" World Interface:\t${WORLD_INTERFACE}") message("========================================") ##====== Dependencies ====== -# Some of these are optional depending on module settings above. For example, the lidar -# libraries are not needed if using the simulator world interface. +# Some of these are optional depending on module settings above. # Get the Eigen linear algebra library. Eigen's CMake config includes its headers as user # headers instead of system headers by default, so explicitly force them to be system includes @@ -127,12 +107,6 @@ if(WORLD_INTERFACE STREQUAL "REAL") # Find the libgps USB GPS library. pkg_check_modules(libgps REQUIRED libgps) endif() - if(WITH_LIDAR) - # Find the URGLidar library. - # TODO: Decide if we want to use those RPLidars we found - find_package(RPLidar REQUIRED) - find_package(URGLidar REQUIRED) - endif() endif() # Enable all warnings except a few (unused variable/parameter/result) @@ -146,11 +120,12 @@ if(WITH_TESTS) enable_testing() endif() -list(APPEND camera_src +add_library(camera SHARED camera/Camera.cpp camera/CameraParams.cpp camera/CameraConfig.cpp - ) +) +target_link_libraries(camera PUBLIC ${OpenCV_LIBS}) # shared library for utility code add_library(utils SHARED @@ -189,13 +164,8 @@ target_sources(can_interface PRIVATE ) # Hardware specific source files -if(WITH_CAN) - target_sources(can_interface PRIVATE - CAN/CAN.cpp) -else() - target_sources(can_interface PRIVATE - CAN/CANStubs.cpp) -endif() +target_sources(can_interface PRIVATE + CAN/CAN.cpp) target_link_libraries(can_interface PUBLIC HindsightCAN::HindsightCAN Threads::Threads @@ -228,10 +198,9 @@ add_library(world_interface_core STATIC ar/Tag.cpp ar/read_landmarks.cpp world_interface/motor/base_motor.cpp - ${camera_src} ) target_include_directories(world_interface_core SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS}) -target_link_libraries(world_interface_core PUBLIC ${vision_libs} opencv_aruco ${OpenCV_LIBS} utils) +target_link_libraries(world_interface_core PUBLIC ${vision_libs} opencv_aruco ${OpenCV_LIBS} utils camera) # CAN library (above) requires some utilities from this target_link_libraries(can_interface PUBLIC utils) @@ -241,8 +210,6 @@ add_library(stub_world_interface STATIC add_library(real_world_interface STATIC world_interface/real_world_interface.cpp - lidar/read_hokuyo_lidar.cpp - lidar/read_rp_lidar.cpp world_interface/motor/can_motor.cpp Globals.cpp ) @@ -284,26 +251,13 @@ target_link_libraries(control utils) add_library(commands STATIC commands/DriveToWaypointCommand.cpp) -if(WITH_LIDAR) - add_subdirectory(lidar) -endif() - -list(APPEND real_world_libs - real_world_interface - lidar_interface - URGLidar::urg_c - RPLidar::RPLidar) - list(APPEND simulator_libs simulator_interface) add_executable(Rover Rover.cpp) add_library(rover_common SHARED - Autonomous.cpp Globals.cpp rospub.cpp - filters/PoseEstimator.cpp - filters/PoseEstimatorLinear.cpp network/websocket/WebSocketServer.cpp network/websocket/WebSocketProtocol.cpp worldmap/GlobalMap.cpp @@ -328,29 +282,26 @@ target_link_libraries(Rover ${rover_libs}) if(WORLD_INTERFACE STREQUAL "REAL") - target_link_libraries(Rover ${real_world_libs}) + target_link_libraries(Rover real_world_interface) elseif(WORLD_INTERFACE STREQUAL "SIMULATOR") target_link_libraries(Rover ${simulator_libs}) else() - target_link_libraries(Rover ${real_world_libs}) # TODO: support other interfaces + target_link_libraries(Rover real_world_interface) # TODO: support other interfaces endif() target_link_libraries(Rover ${vision_libs}) -if(WITH_CAN) - add_executable(TunePID TunePID.cpp) - target_link_libraries(TunePID - ${rover_libs} - can_interface - ${real_world_libs} - ) - target_link_libraries(TunePID ${vision_libs} ${sfml_libs}) -endif() +add_executable(TunePID TunePID.cpp) +target_link_libraries(TunePID + ${rover_libs} + can_interface + real_world_interface +) +target_link_libraries(TunePID ${vision_libs}) if(WITH_TESTS) add_executable(tests Tests.cpp - Autonomous.cpp rospub_test.cpp # AR Detection tests ar/DetectorTests.cpp @@ -387,7 +338,7 @@ if(WITH_TESTS) target_link_libraries(tests ${rover_libs} - ${real_world_libs} + stub_world_interface ${OpenCV_LIBS}) include(CTest) include(Catch) @@ -402,7 +353,7 @@ target_link_libraries(gpsd_test gps) if (WORLD_INTERFACE STREQUAL "REAL") add_executable(LimitSwitchCalibration LimitCalib.cpp) - target_link_libraries(LimitSwitchCalibration ${real_world_libs}) + target_link_libraries(LimitSwitchCalibration real_world_interface) endif() add_subdirectory(ar) diff --git a/src/Rover.cpp b/src/Rover.cpp index c768e870b..be4536f1f 100644 --- a/src/Rover.cpp +++ b/src/Rover.cpp @@ -1,4 +1,3 @@ -#include "Autonomous.h" #include "Constants.h" #include "Globals.h" #include "Util.h" diff --git a/src/lidar/CMakeLists.txt b/src/lidar/CMakeLists.txt deleted file mode 100644 index dfb4ec5f4..000000000 --- a/src/lidar/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -add_library(lidar_interface STATIC - URGLidar.cpp - PointCloudProcessing.cpp - read_hokuyo_lidar.cpp) - -find_package(RPLidar) - -add_executable(lidar_vis - MainVis.cpp - LidarVis.cpp) -target_include_directories(lidar_vis SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS}) -target_link_libraries(lidar_interface URGLidar::urg_c RPLidar::RPLidar utils) -target_link_libraries(lidar_vis lidar_interface ${OpenCV_LIBS}) diff --git a/src/lidar/LidarVis.cpp b/src/lidar/LidarVis.cpp deleted file mode 100644 index 5c12af758..000000000 --- a/src/lidar/LidarVis.cpp +++ /dev/null @@ -1,85 +0,0 @@ -#include "LidarVis.h" - -namespace lidar { -LidarVis::LidarVis(int win_width, int win_height, cv::Scalar bgr, int max_range) - : view(win_height, win_width, CV_8UC3, cv::Scalar(bgr[0], bgr[1], bgr[2])), bg_color(bgr), - win_width(win_width), win_height(win_height), lidar_max_range(max_range) { -} - -cv::Point LidarVis::worldToCvPoint(PointXY p) { - int x = (p.x / (2 * this->lidar_max_range) + 0.5) * this->win_width; - int y = 0.5 * this->win_width * (1 - p.y / this->lidar_max_range); - return cv::Point(x, y); -} - -void LidarVis::drawPoints(std::vector& pts, cv::Scalar bgr, int pt_radius) { - for (PointXY p : pts) { - cv::circle(this->view, worldToCvPoint(p), pt_radius, bgr, -1); - } -} - -void LidarVis::outlinePolygon(std::vector& vertices, cv::Scalar bgr) { - for (size_t i = 0; i < vertices.size(); i++) { - cv::Point p1 = worldToCvPoint(vertices[i]); - cv::Point p2 = worldToCvPoint(vertices[(i + 1) % vertices.size()]); - cv::line(this->view, p1, p2, bgr); - } -} - -void LidarVis::drawLidar(cv::Scalar bgr, int symb_px_size) { - cv::Point bot_left = - cv::Point((this->win_width - symb_px_size) / 2, (this->win_height + symb_px_size) / 2); - cv::Point bot_right = cv::Point((this->win_width + symb_px_size) / 2, bot_left.y); - cv::Point top_mid = cv::Point((bot_left.x + bot_right.x) / 2, bot_left.y - symb_px_size); - std::vector> pts = {{bot_left, bot_right, top_mid}}; - cv::fillPoly(this->view, pts, bgr); -} - -void LidarVis::setGrid(cv::Scalar bgr, int scale) { - PointXY p({0, 0}); - - // vertical lines to left of center - for (p.x = 0; worldToCvPoint(p).x >= 0; p.x -= scale) { - cv::Point p1 = worldToCvPoint(p); - p1.y = 0; - cv::Point p2 = worldToCvPoint(p); - p2.y = this->win_height; - cv::line(this->view, p1, p2, bgr); - } - - // vertical lines to the right of center - for (p.x = scale; worldToCvPoint(p).x < this->win_width; p.x += scale) { - cv::Point p1 = worldToCvPoint(p); - p1.y = 0; - cv::Point p2 = worldToCvPoint(p); - p2.y = this->win_height; - cv::line(this->view, p1, p2, bgr); - } - - // horizontal lines above center - for (p.y = 0; worldToCvPoint(p).y >= 0; p.y += scale) { - cv::Point p1 = worldToCvPoint(p); - p1.x = 0; - cv::Point p2 = worldToCvPoint(p); - p2.x = this->win_width; - cv::line(this->view, p1, p2, bgr); - } - - // horizontal lines below center - for (p.y = -scale; worldToCvPoint(p).y < this->win_height; p.y -= scale) { - cv::Point p1 = worldToCvPoint(p); - p1.x = 0; - cv::Point p2 = worldToCvPoint(p); - p2.x = this->win_width; - cv::line(this->view, p1, p2, bgr); - } -} - -void LidarVis::clear() { - this->view.setTo(this->bg_color); -} - -cv::Mat LidarVis::getView() { - return this->view; -} -} // namespace lidar diff --git a/src/lidar/LidarVis.h b/src/lidar/LidarVis.h deleted file mode 100644 index db2a9cdf5..000000000 --- a/src/lidar/LidarVis.h +++ /dev/null @@ -1,46 +0,0 @@ -#pragma once - -#include "PointCloudProcessing.h" -#include "PointGenerator.h" - -#include - -#include -#include -#include - -namespace lidar { -constexpr int lidar_max_range = 10000; -constexpr int vis_win_width = 600; -constexpr int vis_win_height = 600; -constexpr int vis_pt_radius = 3; -constexpr int cluster_sep_threshold = 1000; -const std::string vis_win_name = "Lidar Visualizer"; -constexpr char vis_win_esc = 'q'; -const cv::Scalar vis_bg_color(255, 255, 255); -const cv::Scalar vis_pt_color(0, 0, 0); -const cv::Scalar vis_conv_hull_color(255, 0, 0); -const cv::Scalar vis_lidar_color(0, 0, 255); -const cv::Scalar vis_grid_color(0, 128, 0); -constexpr int vis_grid_dist = 1000; -constexpr int vis_lidar_size = 10; - -class LidarVis { -private: - cv::Mat view; - cv::Scalar bg_color; - int win_width; - int win_height; - int lidar_max_range; - cv::Point worldToCvPoint(PointXY p); - -public: - LidarVis(int win_width, int win_height, cv::Scalar bgr, int max_range); - void drawPoints(std::vector& pts, cv::Scalar bgr, int ptRadius); - void outlinePolygon(std::vector& vertices, cv::Scalar bgr); - void drawLidar(cv::Scalar bgr, int symb_size_px); - void setGrid(cv::Scalar bgr, int scale); - void clear(); - cv::Mat getView(); -}; -} // namespace lidar diff --git a/src/lidar/MainVis.cpp b/src/lidar/MainVis.cpp deleted file mode 100644 index 9b4d3c244..000000000 --- a/src/lidar/MainVis.cpp +++ /dev/null @@ -1,154 +0,0 @@ -#include "LidarVis.h" -#include "URGLidar.h" -#include -#include "../Constants.h" - -#include -#include -#include -#include - -enum Lidars { - Hokuyo, - RP, - NONE -}; - -/** - * @brief Hokuyo Lidar - * Runs Main Visualizer with a plugged in Hokuyo Lidar - */ -int runHokuyo() { - using namespace lidar; - URGLidar lidar; - if (!lidar.open()) { - std::cout << "failed to open hokuyo lidar" << std::endl; - return lidar.getError(); - } - - LidarVis vis(vis_win_width, vis_win_height, vis_bg_color, lidar_max_range); - cv::namedWindow(vis_win_name); - while (true) { - vis.clear(); - if (!lidar.createFrame()) { - std::cout << "failed to create frame" << std::endl; - return lidar.getError(); - } - std::vector polarPts = lidar.getLastFrame(); - std::vector pts(polarPts.size()); - for (Polar2D p : polarPts) { - pts.push_back(lidar::polarToCartesian(p)); - } - - vis.setGrid(vis_grid_color, vis_grid_dist); - vis.drawPoints(pts, vis_pt_color, vis_pt_radius); - std::vector> clusters = clusterPoints(pts, cluster_sep_threshold); - for (std::vector cluster : clusters) { - std::vector bounds = convexHull(cluster); - vis.outlinePolygon(bounds, vis_conv_hull_color); - } - vis.drawLidar(vis_lidar_color, vis_lidar_size); - - cv::imshow(vis_win_name, vis.getView()); - if (cv::waitKey(5) == vis_win_esc) { - break; - } - } - - if (!lidar.close()) { - std::cout << "failed to close lidar device" << std::endl; - return lidar.getError(); - } - return 0; -} - -/** - * @brief RP Lidar - * Runs Main Visualizer on any RP Lidar that uses the RP LIdar SDK - */ -int runRPLidar(unsigned long baudrate) { - using namespace lidar; - RPLidar rp_lidar(Constants::Lidar::RP_PATH, baudrate); - - LidarVis vis(vis_win_width, vis_win_height, vis_bg_color, lidar_max_range); - cv::namedWindow(vis_win_name); - while (true) { - vis.clear(); - if (auto scan = rp_lidar.poll()) { - std::vector pts; - double dtheta = (scan.value().angle_max-scan.value().angle_min)/(scan.value().ranges.size()-1); - for (long unsigned i = 0; i < scan.value().ranges.size(); i++) { - double rad = dtheta*i; - double dist_mm = scan.value().ranges[i] * Constants::Lidar::MM_PER_M; - - Polar2D frame{dist_mm, rad}; - pts.push_back(lidar::polarToCartesian(frame)); - } - - vis.setGrid(vis_grid_color, vis_grid_dist); - vis.drawPoints(pts, vis_pt_color, vis_pt_radius); - std::vector> clusters = clusterPoints(pts, cluster_sep_threshold); - for (std::vector cluster : clusters) { - std::vector bounds = convexHull(cluster); - vis.outlinePolygon(bounds, vis_conv_hull_color); - } - vis.drawLidar(vis_lidar_color, vis_lidar_size); - - cv::imshow(vis_win_name, vis.getView()); - if (cv::waitKey(5) == vis_win_esc) { - break; - } - - } else { - std::cout << "failed to get frame" << std::endl; - return -1; - } - } - return 0; -} - -static void help() { - printf("Lidar Visualizer. \n" - "Usage: lidar_vis \n" - " # \"hokuyo\" \"rp\"\n" - " # default=115200\n" - ); -} - -Lidars hash(std::string lidar) { - if (lidar == "hokuyo") return Hokuyo; - if (lidar == "rp") return RP; - return NONE; -} - -/** - * @brief Main Visualizer - * Visualizes scanned data from inputted lidar by plotting objects - * on a graph - */ -int main(int argc, char** argv) { - unsigned long baudrate; - int errorCode = 0; - if (argc == 2) { - baudrate = Constants::Lidar::RPLIDAR_A1_BAUDRATE; - } else if (argc == 3) { - baudrate = (unsigned long) atol(argv[2]); - } else { - help(); - return -1; - } - - switch(hash(std::string(argv[1]))) { - case Hokuyo: - errorCode = runHokuyo(); - break; - case RP: - errorCode = runRPLidar(baudrate); - break; - case NONE: - help(); - return -1; - } - - return errorCode; -} diff --git a/src/lidar/PointCloudProcessing.cpp b/src/lidar/PointCloudProcessing.cpp deleted file mode 100644 index 74b465f2c..000000000 --- a/src/lidar/PointCloudProcessing.cpp +++ /dev/null @@ -1,173 +0,0 @@ -#include "PointCloudProcessing.h" - -#include "URGLidar.h" -#include "../navtypes.h" - -#include -#include -#include -#include - -using namespace navtypes; - -namespace lidar { - -const double LIDAR_METERS_PER_TICK = 0.001; // 1 mm resolution - -bool approxEqual(PointXY p, PointXY q) { - return fabs(p.x - q.x) < 1e-4 && fabs(p.y - q.y) < 1e-4; -} - -float distance(float x0, float y0, float x1, float y1) { - return sqrtf(powf(x0 - x1, 2) + powf(y0 - y1, 2)); -} - -PointXY polarToCartesian(Polar2D p) { - return PointXY( - {static_cast(p.r * cos(p.theta)), static_cast(p.r * sin(p.theta))}); -} - -point_t polarToCartesian2(Polar2D p) { - double x = p.r * cos(p.theta - M_PI / 2); - double y = p.r * sin(p.theta - M_PI / 2); - point_t ret({x * LIDAR_METERS_PER_TICK, y * LIDAR_METERS_PER_TICK, 1.0}); - return ret; -} - -// p is a point relative to the robot -// heading should be in radians, 0 is north, pi/2 is east, pi is south, 3*pi/2 is west -void localToGlobal(PointXY& p, float x_loc, float y_loc, float heading) { - p.x = x_loc - (cosf(heading) * p.x - sinf(heading) * p.y); - p.y = y_loc - (sinf(heading) * p.x + cosf(heading) * p.y); -} - -// clustering algorithm -// O(n^2) time -// does not assume any ordering or patterns among points -std::vector> clusterPoints(std::vector& pts, - float sep_threshold) { - std::vector> clusters; - for (PointXY curr : pts) { - int nearest_cluster = -1; - float min_dist = std::numeric_limits::infinity(); - for (size_t i = 0; i < clusters.size(); i++) { - for (PointXY xyPoint : clusters[i]) { - float dist = distance(curr.x, curr.y, xyPoint.x, xyPoint.y); - if (dist < min_dist) { - min_dist = dist; - nearest_cluster = i; - } - } - } - if (min_dist < sep_threshold) { - clusters[nearest_cluster].push_back(curr); - } else { - clusters.push_back(std::vector()); - clusters[clusters.size() - 1].push_back(curr); - } - } - return clusters; -} - -// another clustering algorithm -// O(n) time -// assumes points are in order by their polar degree -std::vector> clusterOrderedPoints(std::vector& points, - float sep_threshold) { - std::vector> clusters; - std::vector lastCluster; - PointXY lastPoint = points[0]; - for (size_t i = 1; i < points.size(); i++) { - float diff = distance(points[i].x, points[i].y, points[i - 1].x, points[i - 1].y); - if (diff < sep_threshold) { - if (approxEqual(lastPoint, points[i - 1])) { - lastCluster.push_back(points[i]); - } else { - std::vector cluster; - cluster.push_back(points[i - 1]); - cluster.push_back(points[i]); - lastPoint = points[i]; - clusters.push_back(cluster); - lastCluster = cluster; - } - } - PointXY zero_pt; - zero_pt.x = 0; - zero_pt.y = 0; - if (!approxEqual(lastPoint, zero_pt) && - distance(points[i].x, points[i].y, lastPoint.x, lastPoint.y) < sep_threshold) { - lastCluster.push_back(points[i]); - } - } - // checks last point and first point - float diff = distance(points[points.size() - 1].x, points[points.size() - 1].y, - points[0].x, points[0].y); - if (diff < sep_threshold) { - if (approxEqual(lastPoint, points[points.size() - 1])) { - lastCluster.push_back(points[0]); - } else { - std::vector cluster; - cluster.push_back(points[points.size() - 1]); - cluster.push_back(points[0]); - clusters.push_back(cluster); - } - } - return clusters; -} - -// removes points from the pts list that are ground points -// pitch_rad is positive if learning forward, else negative -// scan_height must be in the same units as the points distance -void filterGroundPoints(std::vector& pts, float scan_height, float pitch_rad, - float slope_tol_rad) { - std::vector::iterator itr = pts.begin(); - while (itr != pts.end()) { - float slope_rad = M_PI - pitch_rad - atan((*itr).r / scan_height); - if (slope_rad < slope_tol_rad) { - itr = pts.erase(itr); - } else { - itr++; - } - } -} - -std::vector convexHull(std::vector& cluster) { - if (cluster.size() <= 3) { - return cluster; - } - - // sort points by their x coordinates - std::sort(std::begin(cluster), std::end(cluster), - [](PointXY p1, PointXY p2) { return p1.x < p2.x; }); - - // 0 -> collinear, positive -> counterclockwise, negative -> clockwise - auto orientation = [](PointXY p, PointXY q, PointXY r) { - return (q.x * r.y) - (q.y * r.x) - (p.x * r.y) + (p.y * r.x) + (p.x * q.y) - - (p.y * q.x); - }; - - std::vector top_hull; - std::vector bot_hull; - for (size_t i = 0; i < cluster.size(); i++) { - while (top_hull.size() >= 2 && - orientation(top_hull[top_hull.size() - 2], top_hull[top_hull.size() - 1], - cluster[i]) > 0) { - top_hull.pop_back(); - } - top_hull.push_back(cluster[i]); - } - - for (int i = cluster.size() - 1; i >= 0; i--) { - while (bot_hull.size() >= 2 && - orientation(bot_hull[bot_hull.size() - 2], bot_hull[bot_hull.size() - 1], - cluster[i]) > 0) { - bot_hull.pop_back(); - } - bot_hull.push_back(cluster[i]); - } - - top_hull.insert(top_hull.end(), bot_hull.begin() + 1, bot_hull.end() - 1); - return top_hull; -} - -} // namespace lidar diff --git a/src/lidar/PointCloudProcessing.h b/src/lidar/PointCloudProcessing.h deleted file mode 100644 index 2a647716c..000000000 --- a/src/lidar/PointCloudProcessing.h +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once - -#include "../math/PointXY.h" -#include "../navtypes.h" - -#include - -class Polar2D; - -namespace lidar { - -typedef struct BoundingBox { - float xmin; - float ymin; - float xmax; - float ymax; -} BoundingBox; - -bool approxEqual(PointXY p, PointXY q); -float distance(float x0, float y0, float x1, float y1); -PointXY polarToCartesian(Polar2D p); -navtypes::point_t polarToCartesian2(Polar2D p); -void localToGlobal(PointXY& p, float x_loc, float y_loc, float heading); -std::vector> clusterPoints(std::vector& pts, - float sep_threshold); -std::vector> clusterOrderedPoints(std::vector& points, - float sep_threshold); -void filterGroundPoints(std::vector& pts, float scan_height, float slope_tol_rad); -std::vector convexHull(std::vector& cluster); - -}; // namespace lidar diff --git a/src/lidar/PointGenerator.cpp b/src/lidar/PointGenerator.cpp deleted file mode 100644 index 0cdd1723f..000000000 --- a/src/lidar/PointGenerator.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include "PointGenerator.h" - -#include -#include - -namespace lidar { -std::vector generateClusterRadius(float x0, float y0, float r, int num_pts) { - std::vector pts; - for (int i = 0; i < num_pts; i++) { - float x = - x0 - r + static_cast(rand()) / (static_cast(RAND_MAX / (2 * r))); - float yAbsMax = sqrtf(powf(r, 2) - powf(x - x0, 2)); - float y = y0 - yAbsMax + - static_cast(rand() / (static_cast(RAND_MAX / (2 * yAbsMax)))); - PointXY p; - p.x = x; - p.y = y; - pts.push_back(p); - } - return pts; -} - -std::vector generateClusterLinear(float x0, float y0, float x1, float y1, float tol, - int num_pts) { - // create vector for line, which we will multiply by a random value to get a value - float lin_x = x1 - x0; - float lin_y = y1 - y0; - - std::vector pts; - for (int i = 0; i < num_pts; i++) { - float r = static_cast(rand()) / static_cast(RAND_MAX); - float x = x0 + r * lin_x; - float y = y0 + r * lin_y; - pts.push_back(*generateClusterRadius(x, y, tol, 1).begin()); - } - return pts; -} -} // namespace lidar diff --git a/src/lidar/PointGenerator.h b/src/lidar/PointGenerator.h deleted file mode 100644 index 1c56fdf7a..000000000 --- a/src/lidar/PointGenerator.h +++ /dev/null @@ -1,9 +0,0 @@ -#pragma once - -#include "PointCloudProcessing.h" - -namespace lidar { -std::vector generateClusterRadius(float x0, float y0, float r, int num_pts); -std::vector generateClusterLinear(float x0, float y0, float x1, float y1, float tol, - int num_pts); -} // namespace lidar diff --git a/src/lidar/SyntheticLidar.cpp b/src/lidar/SyntheticLidar.cpp deleted file mode 100644 index e91fe7713..000000000 --- a/src/lidar/SyntheticLidar.cpp +++ /dev/null @@ -1,70 +0,0 @@ -#include "SyntheticLidar.h" - -SyntheticLidar::SyntheticLidar() - : lastAngle(0.0f), last(std::chrono::steady_clock::now()), - lastFrameTime(std::chrono::steady_clock::now()) { -} - -std::shared_ptr SyntheticLidar::nextPolarCircle(double radius) { - bool frame = updateAngle(); - Polar2D pd{radius, actingAngle()}; - return completeNext(frame, pd); -} - -std::shared_ptr SyntheticLidar::nextPolarSquare(double dimension) { - bool frame = updateAngle(); - double aa = fmod(actingAngle(), 90); - double hypotenuse; - if (aa > 45) { - hypotenuse = (dimension / 2) / cos((90 - aa) * PI / 180.0); - } else { - hypotenuse = (dimension / 2) / cos(aa * PI / 180.0); - } - Polar2D pd{hypotenuse, aa}; - return completeNext(frame, pd); -} - -std::shared_ptr SyntheticLidar::completeNext(bool frame, Polar2D pd) { - if (frame) { - createFrame(); - } - std::shared_ptr spd = std::make_shared(pd); - currentFrame.push_back(spd); - return spd; -} - -std::vector> SyntheticLidar::getLastFrame() { - return lastFrame; -} - -double SyntheticLidar::getLastAngle() { - return lastAngle; -} - -auto SyntheticLidar::getLastFrameTime() { - return lastFrameTime; -} - -bool SyntheticLidar::updateAngle() { - long millisSplit = std::chrono::duration_cast( - std::chrono::steady_clock::now() - last) - .count(); - last = std::chrono::steady_clock::now(); - double angle = (double)millisSplit * 3.6; - double lastOld = lastAngle; - lastAngle = fmod(lastAngle, 360) + angle; - return lastOld > lastAngle; -} - -double SyntheticLidar::actingAngle() { - double acting = - fmod(lastAngle + 360 + 90, 360); // Converts Lidar Coordinates to Polar standard. - return acting; -} - -void SyntheticLidar::createFrame() { - lastFrame = currentFrame; - std::vector> newFrame; - currentFrame = newFrame; - lastFrameTime = std::chrono::steady_clock::now(); -} \ No newline at end of file diff --git a/src/lidar/SyntheticLidar.h b/src/lidar/SyntheticLidar.h deleted file mode 100644 index 1bac9f448..000000000 --- a/src/lidar/SyntheticLidar.h +++ /dev/null @@ -1,65 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -struct Polar2D; - -typedef struct Polar2D { - double r, theta; -} Polar2D; - -class SyntheticLidar { -private: - const float PI = 3.14159265; - // Handles frame completion and addition. - std::shared_ptr completeNext(bool frame, Polar2D pd); - /** - * Updates the angle of the lidar based on the time that has passed since last it generated - * a point and the average speed. - */ - bool updateAngle(); - // Returns the angle which should be used in a cosine equation to convert to square - // coordinates. - double actingAngle(); - // Copies the currentFrame into lastFrame and then clears the currentFrame so it can be - // built again. - void createFrame(); - std::chrono::time_point - last; // The current system time in milliseconds - double lastAngle; // The angle the lidar was at for the last call to a next method. - std::vector> lastFrame; // The last completed frame. - std::vector> - currentFrame; // The frame that is currently being built. - std::chrono::time_point - lastFrameTime; // The system time at which the last frame was completed. - -public: - SyntheticLidar(); - /** - * Steps the Lidar for the time having passed since it was last called (Given the average - * time for a complete rotation of the lidar) and returns the next polar coordinate at the - * given radius for a circle. - * - * @param radius The radius for which the next point should be generated. - * @return A shared pointer to the next Polar2D coordinate. - */ - std::shared_ptr nextPolarCircle(double radius); - /** - * Steps the Lidar for the time having passed since it was last called (Given the average - * time for a complete rotation of the lidar) and returns the next polar coordinate at the - * given dimension for a axis-aligned square. - * - * @param radius The dimension for which the next point should be generated. - * @return A shared pointer to the next Polar2D coordinate. - */ - std::shared_ptr nextPolarSquare(double dimension); - // Gets the last completed frame or a nullptr if none have been completed. - std::vector> getLastFrame(); - // Gets the last angle the lidar generated a point at. - double getLastAngle(); - // Gets the system time at which the last frame was completed. - // auto getLastFrameTime(); -}; diff --git a/src/lidar/URGLidar.cpp b/src/lidar/URGLidar.cpp deleted file mode 100644 index 320518201..000000000 --- a/src/lidar/URGLidar.cpp +++ /dev/null @@ -1,74 +0,0 @@ -#include "URGLidar.h" - -#include - -constexpr char CONNECT_DEVICE[] = "/dev/ttyACM0"; -constexpr long CONNECT_BAUDRATE = 115200; - -URGLidar::URGLidar() : lastFrameTime(std::chrono::steady_clock::now()) { -} - -bool URGLidar::open() { - int ret; - ret = urg_open(&urg, URG_SERIAL, CONNECT_DEVICE, CONNECT_BAUDRATE); - if (ret != 0) { - error = ret; - return false; - } - // Tells the scanner to scan in its full range (-135 deg to 135 deg). - int first_step = urg_deg2step(&urg, -135); - int last_step = urg_deg2step(&urg, +135); - int skip_step = 0; - ret = urg_set_scanning_parameter(&urg, first_step, last_step, skip_step); - if (ret != 0) { - error = ret; - return false; - } - error = 0; - return true; -} - -std::vector URGLidar::getLastFrame() { - return lastFrame; -} - -std::chrono::time_point URGLidar::getLastFrameTime() { - return lastFrameTime; -} - -bool URGLidar::createFrame() { - // How many scans to complete (1) and how many to skip (0). - int ret = urg_start_measurement(&urg, URG_DISTANCE, 1, 0); - if (ret != 0) { - error = ret; - return false; - } - // Allocate memory to store the length data the scan will generate. - long* length_data = new long[urg_max_data_size(&urg)]; - lastFrameTime = std::chrono::steady_clock::now(); - // Record the time just before recording starts and start recording the frame. - int length_data_size = urg_get_distance(&urg, length_data, NULL); - - // Build each shared Polar2D point and store them in a std::vector - // Since we consider theta = 0 to be straight ahead with the robot facing the positive y - // axis Add PI / 2 to the angle - std::vector frame; - for (int i = 0; i < length_data_size; ++i) { - Polar2D pd{static_cast(length_data[i]), urg_index2rad(&urg, i) + M_PI / 2}; - frame.push_back(pd); - } - lastFrame = frame; - error = 0; - delete[] length_data; - return true; -} - -int URGLidar::getError() { - return error; -} - -bool URGLidar::close() { - urg_close(&urg); - error = 0; - return true; -} diff --git a/src/lidar/URGLidar.h b/src/lidar/URGLidar.h deleted file mode 100644 index dad67d43d..000000000 --- a/src/lidar/URGLidar.h +++ /dev/null @@ -1,68 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include -#include - -struct Polar2D; - -typedef struct Polar2D { - double r, theta; - double asCartesianX() { - return r * cos(theta); - } - double asCartesianY() { - return r * sin(theta); - } -} Polar2D; - -class URGLidar { -private: - urg_t urg; - int error = 0; - // The last completed frame. - std::vector lastFrame; - // The system time at which the last frame was completed. - std::chrono::time_point lastFrameTime; - -public: - /** - * Uses the Hokuyo URG sensor to read Lidar data and build frames. - */ - URGLidar(); - /** - * Opens a connection with the sensor on /dev/ttyACM0 and sets up - * scan range and skip defaults. - * @returns True if this operation was successful. - */ - bool open(); - /** - * @returns The last completed frame of nullptr if there is none. - */ - std::vector getLastFrame(); - /** - * @returns the steady clock timestamp just before the sensor was - * last queried for a new frame, or the time this URGLidar was - * created if no frame have been created. - */ - std::chrono::time_point getLastFrameTime(); - /** - * Builds the next frame and updates the last frame and time variables - * @returns True if this operation was successful. - */ - bool createFrame(); - /** - * @returns The last error code from the sensor. Generated by open, close, - * and getLastFrame. - */ - int getError(); - /** - * Closes and releases the connection to the sensor. - * @returns True if this operation was successful. - */ - bool close(); -}; diff --git a/src/lidar/read_hokuyo_lidar.cpp b/src/lidar/read_hokuyo_lidar.cpp deleted file mode 100644 index 919f019de..000000000 --- a/src/lidar/read_hokuyo_lidar.cpp +++ /dev/null @@ -1,82 +0,0 @@ - -#include "read_hokuyo_lidar.h" - -#include "../navtypes.h" -#include "../world_interface/world_interface.h" -#include "PointCloudProcessing.h" -#include "PointGenerator.h" -#include "URGLidar.h" - -#include -#include -#include - -using namespace navtypes; -using namespace robot::types; - -// This website may be helpful -// https://sourceforge.net/p/urgnetwork/wiki/top_en/ - -constexpr double LIDAR_MIN_RANGE = 0.15; - -std::atomic lidar_initialized(false); -std::atomic lidar_time(datatime_t{}); -URGLidar urg_lidar; -std::thread lidar_thread; -points_t last_points = {}; -std::mutex points_lock; - -namespace lidar { - -void readLidarLoop() { - while (true) { - if (!urg_lidar.createFrame()) { - perror("failed to create frame"); - continue; - } - - std::vector polarPts = urg_lidar.getLastFrame(); - lidar_time = dataclock::now(); - std::vector pts(polarPts.size()); - point_t origin({0, 0, 1}); - for (size_t i = 0; i < polarPts.size(); i++) { - pts[i] = lidar::polarToCartesian2(polarPts[i]); - if ((pts[i] - origin).norm() < LIDAR_MIN_RANGE) { - // We're inside lidar min range, so this is garbage data. - // We don't want to include this hit because otherwise the robot - // will think it's in collision with an obstacle. - pts[i](2) = 0.0; - } - } - points_lock.lock(); - last_points = pts; - points_lock.unlock(); - lidar_initialized = true; - } -} - -bool initializeLidar() { - if (!lidar_initialized) { - if (!urg_lidar.open()) { - perror("failed to open lidar"); - } else { - lidar_thread = std::thread(&readLidarLoop); - // we won't mark the lidar as initialized until the first scan has been read - } - } - return lidar_initialized; -} - -DataPoint readLidar() { - if (lidar_initialized) { - points_t pts; - points_lock.lock(); - pts = last_points; - points_lock.unlock(); - datatime_t time = lidar_time; - return {time, pts}; - } else { - return {}; - } -} -} // namespace lidar diff --git a/src/lidar/read_hokuyo_lidar.h b/src/lidar/read_hokuyo_lidar.h deleted file mode 100644 index 0cafa0688..000000000 --- a/src/lidar/read_hokuyo_lidar.h +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -#include "../navtypes.h" -#include "../world_interface/data.h" - -namespace lidar { -/** - * @brief Initialize the lidar sensor, must be called before any other lidar operations. - * - * @returns true iff the lidar was initialized successfully. - */ -bool initializeLidar(); - -/** - * @brief Check if the lidar was initialized. - * - * @returns true iff the lidar was initialized successfully. - */ -bool isLidarInitialized(); - -/** - * @brief Get the latest lidar data. - * May return the same data twice if called again too quickly, before new data was collected. - * - * @return The data from the lidar sensor, or an empty data point if the lidar wasn't initialized. - */ -robot::types::DataPoint readLidar(); -} // namespace lidar diff --git a/src/lidar/read_rp_lidar.cpp b/src/lidar/read_rp_lidar.cpp deleted file mode 100644 index 8bf5d7ee3..000000000 --- a/src/lidar/read_rp_lidar.cpp +++ /dev/null @@ -1,81 +0,0 @@ -#include "read_rp_lidar.h" - -#include "../navtypes.h" -#include "../world_interface/world_interface.h" -#include "../Constants.h" -#include "PointCloudProcessing.h" -#include "PointGenerator.h" - -#include -#include -#include -#include - -using namespace navtypes; - -bool lidar_initialized = false; -RPLidar rp_lidar(Constants::Lidar::RP_PATH, Constants::Lidar::RPLIDAR_S1_BAUDRATE); -std::thread lidar_thread; -robot::types::DataPoint last_points = {}; -std::mutex points_lock; - -namespace lidar { - -void readLidarLoop() { - std::vector polarPts; - while (true) { - auto scan = rp_lidar.poll(); - if (auto scan = rp_lidar.poll()) { - std::vector pts; - double dtheta = (scan.value().angle_max-scan.value().angle_min)/(scan.value().ranges.size()-1); - for (unsigned long i = 0; i < scan.value().ranges.size(); i++) { - double rad = dtheta*i; - double dist_m = scan.value().ranges[i]; - - Polar2D frame{dist_m, rad}; - pts.push_back(lidar::polarToCartesian2(frame)); - } - - points_lock.lock(); - last_points = {pts}; - points_lock.unlock(); - } else { - perror("failed to get frame"); - continue; - } - } -} - -/** - * @brief Startsup RPLidar with default settings - */ -bool initializeLidar(double max_dist) { - std::lock_guard lk(points_lock); - if (!lidar_initialized) { - if (!rp_lidar.checkHealth()) { - perror("failed to connect to rp lidar"); - } else { - rp_lidar.setMaxDistance(max_dist); - lidar_thread = std::thread(&readLidarLoop); - lidar_initialized = true; - } - } - return lidar_initialized; -} - -/** - * @brief Provides RP Lidar data at current timeframe - */ -robot::types::DataPoint readLidar() { - std::lock_guard lk(points_lock); - if (lidar_initialized) { - robot::types::DataPoint pts; - pts = last_points; - return pts; - } - return {}; -} - - -} // namespace lidar - diff --git a/src/lidar/read_rp_lidar.h b/src/lidar/read_rp_lidar.h deleted file mode 100644 index f00bba194..000000000 --- a/src/lidar/read_rp_lidar.h +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -#include - -#include "../navtypes.h" -#include "../world_interface/data.h" - -namespace lidar { - -/** - * @brief Startsup RPLidar with default settings - */ -bool initializeLidar(double max_dist=16); - -/** - * @brief Provides RP Lidar data at current timeframe - */ -robot::types::DataPoint readLidar(); - -/** - * @brief Polar struct that holds radius, and theta (radians) - */ -struct Polar2D { - double r, theta; - double asCartesianX() { - return r * cos(theta); - } - double asCartesianY() { - return r * sin(theta); - } -}; - -} \ No newline at end of file diff --git a/src/world_interface/noop_world.cpp b/src/world_interface/noop_world.cpp index f08c40c81..49460be77 100644 --- a/src/world_interface/noop_world.cpp +++ b/src/world_interface/noop_world.cpp @@ -35,10 +35,6 @@ std::shared_ptr getMotor(robot::types::motorid_t motor) { void emergencyStop() {} -DataPoint readLidarScan() { - return points_t{}; -} - landmarks_t readLandmarks() { return landmarks_t{}; } diff --git a/src/world_interface/real_world_interface.cpp b/src/world_interface/real_world_interface.cpp index ed181762a..a04836fca 100644 --- a/src/world_interface/real_world_interface.cpp +++ b/src/world_interface/real_world_interface.cpp @@ -235,10 +235,6 @@ landmarks_t readLandmarks() { return {}; } -DataPoint readLidarScan() { - return {}; -} - DataPoint readVisualOdomVel() { return DataPoint{}; } diff --git a/src/world_interface/simulator_interface.cpp b/src/world_interface/simulator_interface.cpp index 2c6bc7053..f4ab60cca 100644 --- a/src/world_interface/simulator_interface.cpp +++ b/src/world_interface/simulator_interface.cpp @@ -47,9 +47,6 @@ std::mutex headingMutex; DataPoint lastGPS; std::mutex gpsMutex; -DataPoint lastLidar; -std::mutex lidarMutex; - DataPoint lastTruePose; std::mutex truePoseMutex; @@ -129,19 +126,6 @@ void handleGPS(json msg) { lastGPS = {coords}; } -void handleLidar(json msg) { - points_t lidar; - datatime_t now = dataclock::now(); - const auto& arr = msg["points"]; - for (const auto& point : arr) { - double r = point["r"]; - double theta = point["theta"]; - lidar.push_back({r * std::cos(theta), r * std::sin(theta), 1}); - } - std::lock_guard guard(lidarMutex); - lastLidar = {now, lidar}; -} - void handleIMU(json msg) { double qx = msg["x"]; double qy = msg["y"]; @@ -232,7 +216,8 @@ void clientDisconnected() { void initSimServer() { auto protocol = std::make_unique(PROTOCOL_PATH); protocol->addMessageHandler("simImuOrientationReport", handleIMU); - protocol->addMessageHandler("simLidarReport", handleLidar); + // TODO: remove lidar report + protocol->addMessageHandler("simLidarReport", [](const nlohmann::json&){}); protocol->addMessageHandler("simGpsPositionReport", handleGPS); protocol->addMessageHandler("simCameraStreamReport", handleCamFrame); protocol->addMessageHandler("simMotorStatusReport", handleMotorStatus); @@ -355,11 +340,6 @@ landmarks_t readLandmarks() { return AR::readLandmarks(); } -DataPoint readLidarScan() { - std::lock_guard guard(lidarMutex); - return lastLidar; -} - DataPoint readVisualOdomVel() { return DataPoint{}; } diff --git a/src/world_interface/world_interface.h b/src/world_interface/world_interface.h index 83dcded77..862bb2f10 100644 --- a/src/world_interface/world_interface.h +++ b/src/world_interface/world_interface.h @@ -79,15 +79,6 @@ double setCmdVel(double dtheta, double dx); */ std::pair getCmdVel(); -/** - * @brief Get the last measurement from the lidar. - * - * The returned points are in the robot's reference frame. - * - * @return types::DataPoint The lidar scan, if available. - */ -types::DataPoint readLidarScan(); - /** * @brief Get the IDs of the currently supported cameras. * diff --git a/tests/lidar/PointCloudProcessingTest.cpp b/tests/lidar/PointCloudProcessingTest.cpp deleted file mode 100644 index ac4e035e9..000000000 --- a/tests/lidar/PointCloudProcessingTest.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#define CATCH_CONFIG_MAIN - -#include "lidar/PointCloudProcessing.h" -#include "lidar/PointGenerator.h" - -#include - -TEST_CASE("cluster many nearby points into 1 cluster") -{ - using namespace Lidar; - std::vector> pts = generateClusterRadius(0, 0, 1, 10); - std::vector>> clusters = clusterPoints(pts, 1.0); - REQUIRE(clusters.size() == 1); - REQUIRE(clusters[0].size() == 10); -} diff --git a/update_deps.sh b/update_deps.sh index 2a3fdd0b8..5cf83f980 100755 --- a/update_deps.sh +++ b/update_deps.sh @@ -4,5 +4,4 @@ sudo apt update sudo apt install -y build-essential unzip gnupg2 lsb-release git \ cmake libeigen3-dev libopencv-dev libopencv-contrib-dev \ libwebsocketpp-dev libboost-system-dev gpsd gpsd-clients libgps-dev nlohmann-json3-dev \ - catch2 urg-lidar rplidar ublox-linux hindsight-can=1.4.0 frozen libargparse-dev libavutil-dev \ - libx264-dev + catch2 ublox-linux hindsight-can=1.4.0 frozen libargparse-dev libavutil-dev libx264-dev