diff --git a/jetson/nav/.clang-format b/jetson/nav/.clang-format new file mode 100644 index 0000000000..88ec6c09be --- /dev/null +++ b/jetson/nav/.clang-format @@ -0,0 +1,66 @@ +# Generated from CLion C/C++ Code Style settings +BasedOnStyle: LLVM +AccessModifierOffset: -4 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: None +AlignOperands: Align +AllowAllArgumentsOnNextLine: false +AllowAllConstructorInitializersOnNextLine: false +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortBlocksOnASingleLine: Always +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: Always +AllowShortLambdasOnASingleLine: All +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterReturnType: None +AlwaysBreakTemplateDeclarations: Yes +BreakBeforeBraces: Custom +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: false + SplitEmptyRecord: true +BreakBeforeBinaryOperators: None +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: BeforeColon +BreakInheritanceList: BeforeColon +ColumnLimit: 0 +CompactNamespaces: false +ContinuationIndentWidth: 8 +IndentCaseLabels: true +IndentPPDirectives: None +IndentWidth: 4 +KeepEmptyLinesAtTheStartOfBlocks: true +MaxEmptyLinesToKeep: 2 +NamespaceIndentation: All +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PointerAlignment: Left +ReflowComments: false +SpaceAfterCStyleCast: true +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: false +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 0 +SpacesInAngles: false +SpacesInCStyleCastParentheses: false +SpacesInContainerLiterals: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +TabWidth: 4 +UseTab: Never diff --git a/jetson/nav/README.md b/jetson/nav/README.md index 0d2247ac8d..980cbe2872 100644 --- a/jetson/nav/README.md +++ b/jetson/nav/README.md @@ -1,156 +1,203 @@ # MRover Logo MRover Navigation Codebase ## Table of Contents -[Project Overview](#project-overview)
-[Top-Level Code](#Top-Level-Code)
-[Gate Search (`gate_search/`)](#Gate-Search)
-[Obstacle Avoidance (`obstacle_avoidance/`)](#obstacle-avoidance)
-[Search (`search/`)](#search)
-[Variables and Utilities](#vars-and-utils)
+[Project Overview](#project-overview) \ +[Top-Level Code](#top-level-code) \ +[Gate Search (`gate_search/`)](#gate-search) \ +[Obstacle Avoidance (`obstacle_avoidance/`)](#obstacle-avoidance) \ +[Search (`search/`)](#search) \ +[Utilities](#utilities) -**Created:** Ana Warner, Oct 3, 2021 -**Updated:** Zachary Goldston, December 7, 2021 +**Created:** Ana Warner, Oct 3, 2021 \ +**Updated:** Zachary Goldston, December 7, 2021 \ +**Updated:** Quintin Dwight, May 26, 2022 --- - ## Project Overview -The nav codebase contains logic for commanding the rover during the Autonomous Traversal task. The task consists of several legs that increase in difficulty. During a leg, the operator inputs the destination GPS waypoint and end behavior for the leg (search and/or gate), then starts the Auton system from the base station. The rover will first drive autonomously to the destination waypoint without responding to target data from Perception. It will avoid obstacles as they appear. Once the waypoint is reached, the rover will start a search pattern if the leg was a search leg. The search algorithm first attempts a spiral out search pattern and approaches the tag once it is found. Finally, if the operator indicated the leg is a gate leg, there is another post to find, so the gate search and traversal algorithm will start. ---- +The nav codebase contains logic for commanding the rover during the Autonomous Traversal task. The task consists of +several legs that increase in difficulty. During a leg, the operator inputs the destination GPS waypoint and end +behavior for the leg (search and/or gate), then starts the Auton system from the base station. The rover will first +drive autonomously to the destination waypoint without responding to target data from Perception. It will avoid +obstacles as they appear. Once the waypoint is reached, the rover will start a search pattern if the leg was a search +leg. The search algorithm first attempts a spiral out search pattern and approaches the tag once it is found. Finally, +if the operator indicated the leg is a gate leg, there is another post to find, so the gate search and traversal +algorithm will start. - -## Top-Level Code +--- -#### `main.cpp` -The `main.cpp` file contains the `main()` function. In the main function, we create an instance of the state machine, create the LCM handlers class, and subscribe to the LCM channels that we will read messages from. (For more about LCM’s, see below.) Then we call the outermost function of the state machine, `run()`, which begins executing the state machine logic. +## Top Level Code -#### `stateMachine.hpp` -This is an example of a header file, commonly used in C and C++. The header file for a class (an object) contains the class declaration. A class declaration lists the class’s member variables and declares the member functions, which are then implemented (“defined”) in the .cpp file. The `stateMachine.hpp` file contains the state machine variables, including pointers to the search state machine and obstacle avoidance state machine, which are derived classes from the regular state machine. +#### [`main.cpp`](./main.cpp) -#### `stateMachine.cpp` -This file contains implementations of the stateMachine object’s member functions, including the `run()` function, which executes the logic for switching between navigation states and calling the functions to run in each state. +Contains the `main()` entrypoint. Here [LCM](https://github.com/lcm-proj/lcm) callbacks are set up to receive commands +and status from the base station. +The rover class, environment class (physical surroundings), course progress, and configuration are all initialized. +These are passed to the state machine construction. +Then the state machine is run every time we receive a new LCM message from the network. -#### `rover.cpp` -This file defines the rover and rover status objects. The rover object is used throughout the codebase to interact with real-life capabilities of the rover. Notably, the object contains functions like `drive()` and `turn()`. The rover status object/class is nested in the rover class, and it contains information about the current state of the rover and relevant features like targets and obstacles. Most variables in the rover status are populated from LCM messages. +#### [`stateMachine.cpp`](./stateMachine.cpp) ---- +Main state machine logic, defines how states transition to one another. +Waypoint traversal logic is implemented here since it is simple. +For search (single tag) and gate search states, sub state machines are invoked to keep code properly encapsulated. +Note that the actual state variable is held by the rover class. - -## Gate Search (`gate_search/` folder) +#### [`rover.cpp`](./rover.cpp) -#### `diamondGateSearch.cpp` -This file creates the search waypoints in the shape of a diamond for completing a search for the second gate post. +Information about the rover and functionality for controlling the drivetrain. +Contains the current state, odometry (position + rotation), the PID controllers for bearing (turning) and distance. +Has behavior for driving to a waypoint which handles turning and driving to within a threshold. -#### `gateStateMachine.cpp` -Defines gate search/traversal states and functions. +#### [`environment.cpp`](./environment.cpp) +Holds processed information about the physical surroundings. +Raw and unreliable data is received from perception and passed through several caches and filters. +The caches ensure that briefly missing data or briefly seeing false positives does not have a large effect on the +overall state. +The filters combine multiple readings into a more accurate one via chopping out extreme values and then taking the +average of the remaining. +Gate location is calculated as global positioning by combing camera and odometry information. +Often times we do not have the targets in view, so this makes them persistent in space. --- - -## Obstacle Avoidance +## Search -#### `obstacleAvoidanceStateMachine.cpp` -Defines an obstacle avoidance state machine with minimal functionality, intended to be a parent class for different types of obstacle avoidance strategies +#### [`searchStateMachine.cpp`](./search/searchStateMachine.cpp) -#### `simpleAvoidance.cpp` -This contains what is currently our only implementation of obstacle avoidance behavior. Inherited from the obstacle state machine, it is a very simple algorithm and just drops a waypoint at the front of the queue, with a position at a safe location away from the obstacle, for the rover to drive to before continuing to its previous destination. If we developed another behavior algorithm, we would make another obstacle state machine child class like simpleAvoidance. +#### [`searchFromPathFile.cpp`](./search/searchFromPathFile.cpp) +Sub state machines for driving to a single post/tag. +First we need to find the tag by driving in a spiral. +The spiral is pre-generated and loaded from a file. +The idea is that eventually we will spot the tag, then transition to the other sub state machine. +Now we can handle the logic of discerning which of the left/right tag (in screen space) from the environment is the +correct ID. --- - -## Search -Similar to the `gate_search/` folder, this folder for search logic contains a `searchStateMachine` object and files to define the waypoints for different types of searches. First we follow a square spiral outwards with points generated in spiralOutSearch.cpp, then if the search completes and the target is not found, we will move onto trying the lawnmower search and the spiral in search. +## Gate Search + +[`gateStateMachine.cpp`](./gate_search/gateStateMachine.cpp) +Sub state machine for gate search and traversal. +Checking the source code is highly recommended as this has the most vector arithmetic. +Basically we first line up with the gate, so we can cleanly drive straight through it. +Sometimes this involves an extra step if both gate posts are in front of us (worst case). + +--- -#### `searchPointGeneration.py` -This script is used to generate search patterns relative to the start point of the rover. The results are then written into `search/spiral_search_points.txt` as pairs of polar coordinates. The pairs are each "distance bearing". The reasoning for using polar coordinates is to use our pre-made utility function that generates points. +## Obstacle Avoidance -To use this script, you can simply run it and choose what you want it to do according to the prompts that you recieve. The first prompt is for the search type, and the rest are for the parameters. For testing, we currently use square spiral search with the parameters (13, 3). +Not yet implemented. --- - -## Variables and Utilities +## Utilities -#### Nav State -The nav state specifies what state of the state machine we are in. It is implemented as an enum (a C++ type) where named states are associated with a number behind the scenes. +[`utilities.cpp`](./utilities.cpp) Misc. math, conversions between GPS and cartesian coordinates \ +[`filter.cpp`](./filter.hpp) Generic median + mean filter, useful for combining multiple readings into a more accurate +one \ +[`cache.cpp`](./cache.hpp) Generic storage resilient to brief readings misses and false positives -#### Auton State -This is just a boolean hidden as a type called AutonState and it tells us if Auton is on or off. It is read from LCMs published by the GUI or simulator. +--- -##### `utilities.cpp` -Contains functions used commonly throughout auton code. +## State Diagram +![image](https://user-images.githubusercontent.com/20666629/170621044-2055089d-cba9-4164-8472-e669dfc1ee66.png) --- - -## LCM Channels Publishing/Subscribed To +## LCM Channels Publishing/Subscribed To + **Target [subscriber]** \ -Messages: [ AutonState.lcm ](https://github.com/umrover/mrover-workspace/blob/master/rover_msgs/AutonState.lcm) “/auton_state” \ +Messages: [Enable.lcm](https://github.com/umrover/mrover-workspace/blob/main/rover_msgs/Enable.lcm) +“/auton_enabled” \ Publishers: jetson/teleop \ Subscribers: jetson/nav **Course [subscriber]** \ -Messages: [ Course.lcm ](https://github.com/umrover/mrover-workspace/blob/master/rover_msgs/Course.lcm) “/course” \ +Messages: [Course.lcm](https://github.com/umrover/mrover-workspace/blob/master/rover_msgs/Course.lcm) “/course” \ Publishers: simulators/nav, base_station/gui \ Subscribers: jetson/nav **Obstacle [subscriber]** \ -Messages: [ Obstacle.lcm ](https://github.com/umrover/mrover-workspace/blob/master/rover_msgs/Obstacle.lcm) “/obstacle” \ +Messages: [Obstacle.lcm](https://github.com/umrover/mrover-workspace/blob/master/rover_msgs/Obstacle.lcm) +“/obstacle” \ Publishers: jetson/percep \ Subscribers: jetson/nav **Odometry [subscriber]** \ -Messages: [ Odometry.lcm ](https://github.com/umrover/mrover-workspace/blob/master/rover_msgs/Odometry.lcm) “/odometry” \ +Messages: [Odometry.lcm](https://github.com/umrover/mrover-workspace/blob/master/rover_msgs/Odometry.lcm) +“/odometry” \ Publishers: jetson/filter \ Subscribers: jetson/nav **Target List [subscriber]** \ -Messages: [ TargetList.lcm ](https://github.com/umrover/mrover-workspace/blob/master/rover_msgs/TargetList.lcm) “/target_list” \ +Messages: [TargetList.lcm](https://github.com/umrover/mrover-workspace/blob/master/rover_msgs/TargetList.lcm) +“/target_list” \ Publishers: jetson/percep \ Subscribers: jetson/nav -**ZED Gimbal Data [subscriber]** \ -Messages: [ ZedGimablPosition.lcm ](https://github.com/umrover/mrover-workspace/blob/master/rover_msgs/TargetList.lcm) “/zed_gimbal_data” \ -Publishers: simulators/nav, raspi/zed_gimbal, jetson/nav (TODO) \ -Subscribers: jetson/nav - **Joystick [publisher]** \ -Messages: [ Joystick.lcm ](https://github.com/umrover/mrover-workspace/blob/master/rover_msgs/Joystick.lcm) “/autonomous” \ +Messages: [Joystick.lcm](https://github.com/umrover/mrover-workspace/blob/master/rover_msgs/Joystick.lcm) +“/autonomous” \ Publishers: jetson/nav \ Subscribers: jetson/teleop, simulators/nav, base_station/gui **NavStatus [publisher]** \ -Messages: [ NavStatus.lcm ](https://github.com/umrover/mrover-workspace/blob/master/rover_msgs/NavStatus.lcm) “/nav_status” \ +Messages: [NavStatus.lcm](https://github.com/umrover/mrover-workspace/blob/master/rover_msgs/NavStatus.lcm) +“/nav_status” \ Publishers: jetson/nav \ Subscribers: simulators/nav, base_station/gui, jetson/science_bridge +**NavStatus [publisher]** \ +Messages: [ProjectedPoints.lcm](https://github.com/umrover/mrover-workspace/blob/master/rover_msgs/ProjectedPoints.lcm) +“/projected_points” \ +Publishers: jetson/nav --- - ## Simulator Usage -To run the Navigation Simulator, you will need to open at least 3 terminals from the `mrover-workspace` directory (at the top level). Run `vagrant up` in one of the terminals, and once that completes, run `vagrant ssh` in all three of them. You will then follow these commands in order, one set of commands for each terminal. Once the three sets of commands are run and in use, go to the [ Navigation Simulator Website ](http://localhost:8010/). Note, this will not show anything until the last set of commands are run. + +To run the Navigation Simulator, you will need to open at least 3 terminals from the `mrover-workspace` directory (at +the top level). Run `vagrant up` in one of the terminals, and once that completes, run `vagrant ssh` in all three of +them. You will then follow these commands in order, one set of commands for each terminal. Once the three sets of +commands are run and in use, go to the [Navigation Simulator Website](http://localhost:8010/). Note, this will not +show anything until the last set of commands are run. ### Terminal 1 (`jetson/nav` code) -Run the command `$./jarvis build jetson/nav`, this command will compile all the navigation directory code. If it compiles succecssful, no errors will be returned. Then, run `$./jarvis exec jetson/nav` to start the navigation code. + +Run the command `./jarvis build jetson/nav`, this command will compile all the navigation directory code. If it +compiles successful, no errors will be returned. Then, run `./jarvis exec jetson/nav` to start the navigation code. ### Terminal 2 (LCMs) -Run the command `$./jarvis build lcm_bridge/server`, this command will build the LCM messages. If it compiles successful, no errors will be returned. Then, run `$./jarvis exec lcm_bridge/server` to run the LCM messages. + +Run the command `./jarvis build lcm_bridge/server`, this command will build the LCM messages. If it compiles +successful, no errors will be returned. Then, run `./jarvis exec lcm_bridge/server` to run the LCM messages. ### Terminal 3 (Navigation Simulator) -Run the command `$./jarvis build simulators/nav`, this command will build the Navigation Simulator. If it compiles successful, no errors will be returned. Then, run `$./jarvis exec simulators/nav` to run the Simulator. -If desired, you can run a fourth terminal for debugging purposes via LCM messages. To do so, make sure you have another terminal, and starting in the `mrover-workspace` directory, run `vagrant ssh`. Once we are ssh'ed into the virtual machine, run `$./jarvis build lcm_tools/echo` to build the echo tool for LCMs. This will return the messages that are being communicated between publishers and subscribers. To run, enter the command `$./jarvis exec lcm_tools/echo TYPE_NAME CHANNEL` to echo the specified LCM and channel. (These are described in our LCM section and ICDs on the Drive) +Run the command `./jarvis build simulators/nav`, this command will build the Navigation Simulator. If it compiles +successful, no errors will be returned. Then, run `./jarvis exec simulators/nav` to run the Simulator. +If desired, you can run a fourth terminal for debugging purposes via LCM messages. To do so, make sure you have another +terminal, and starting in the `mrover-workspace` directory, run `vagrant ssh`. Once we are ssh'ed into the virtual +machine, run `./jarvis build lcm_tools/echo` to build the echo tool for LCMs. This will return the messages that are +being communicated between publishers and subscribers. To run, enter the +command `./jarvis exec lcm_tools/echo TYPE_NAME CHANNEL` to echo the specified LCM and channel. (These are described in +our LCM section and ICDs on the Drive) --- - ## Rover Testing (in-person) -To run the rover at testing, all that needs to be executing is `$./jarvis build jetson/nav` and `$./jarvis exec jetson/nav` to run our code. This should be done AFTER the Perception and GPS executables and necessary programs are running. To start, use the Base Station GUI to create waypoints via dropping. (dropping will place a waypoint at the current GPS location of the rover) Additionally, the GUI has an option to create a waypoint by directly inputting the GPS information Then, drag the newly created waypoint to the course. Repeat this process until all desired waypoints are added. Finally, to start the Navigation code, hit the "Autonomy" button on the GUI. \ No newline at end of file + +To run the rover at testing, all that needs to be executing is `./jarvis build jetson/nav` +and `./jarvis exec jetson/nav` to run our code. This should be done AFTER the Perception and GPS executables and +necessary programs are running. To start, use the Base Station GUI to create waypoints via dropping. (dropping will +place a waypoint at the current GPS location of the rover) Additionally, the GUI has an option to create a waypoint by +directly inputting the GPS information Then, drag the newly created waypoint to the course. Repeat this process until +all desired waypoints are added. Finally, to start the Navigation code, hit the "Autonomy" button on the GUI. diff --git a/jetson/nav/cache.hpp b/jetson/nav/cache.hpp index 3d6719db26..72a72f3a4c 100644 --- a/jetson/nav/cache.hpp +++ b/jetson/nav/cache.hpp @@ -1,7 +1,5 @@ #pragma once -#include - /*** * Readings from the sensor may occasionally have invalid readings. * A cache attempts to resolve this by saving old values for a bit @@ -12,10 +10,10 @@ template class Cache { public: - Cache(int neededHits, int neededMisses, T invalidDefault) : - mNeededHits(neededHits), - mNeededMisses(neededMisses), - mInvalidDefault(invalidDefault) {} + Cache(int neededHits, int neededMisses, T invalidDefault) + : mNeededHits(neededHits), + mNeededMisses(neededMisses), + mInvalidDefault(invalidDefault) {} T get() const { return mValid ? mCurrentReading : mInvalidDefault; @@ -25,7 +23,7 @@ class Cache { return mValid; } - void reset(){ + void reset() { mCurrentReading = mInvalidDefault; mValid = false; mHits = 0; diff --git a/jetson/nav/courseProgress.cpp b/jetson/nav/courseProgress.cpp index a54a04ff97..5c7e116223 100644 --- a/jetson/nav/courseProgress.cpp +++ b/jetson/nav/courseProgress.cpp @@ -6,34 +6,34 @@ void CourseProgress::setCourse(Course const& course) { mCourse = course; clearProgress(); } -} +}// setCourse() std::deque const& CourseProgress::getRemainingWaypoints() const { return mRemainingWaypoints; -} +}// getRemainingWaypoints() Waypoint const& CourseProgress::getCurrentWaypoint() const { return mRemainingWaypoints.front(); -} +}// getCurrentWaypoint() Waypoint CourseProgress::completeCurrentWaypoint() { Waypoint waypoint = mRemainingWaypoints.front(); mRemainingWaypoints.pop_front(); return waypoint; -} +}// completeCurrentWaypoint() void CourseProgress::clearProgress() { mRemainingWaypoints.assign(mCourse.waypoints.begin(), mCourse.waypoints.end()); -} +}// clearProgress() Course const& CourseProgress::getCourse() const { return mCourse; -} +}// getCourse() int32_t CourseProgress::getCompletedWaypointCount() const { return static_cast(mCourse.num_waypoints - mRemainingWaypoints.size()); -} +}// getCompletedWaypointCount() Waypoint const& CourseProgress::getLastCompletedWaypoint() const { return mCourse.waypoints.at(getCompletedWaypointCount() - 1); -} +}// getLastCompletedWaypoint() diff --git a/jetson/nav/courseProgress.hpp b/jetson/nav/courseProgress.hpp index 8ac199cb13..8757cb33e8 100644 --- a/jetson/nav/courseProgress.hpp +++ b/jetson/nav/courseProgress.hpp @@ -1,7 +1,7 @@ #pragma once -#include #include "rover_msgs/Course.hpp" +#include using namespace rover_msgs; diff --git a/jetson/nav/environment.cpp b/jetson/nav/environment.cpp index 9ca6163113..95b98f4bd4 100644 --- a/jetson/nav/environment.cpp +++ b/jetson/nav/environment.cpp @@ -1,20 +1,19 @@ -#include +#include "environment.hpp" #include "utilities.hpp" -#include "environment.hpp" -Environment::Environment(const rapidjson::Document& config) : - mConfig(config), - mTargetLeft(mConfig["navThresholds"]["cacheHitMax"].GetInt(), mConfig["navThresholds"]["cacheMissMax"].GetInt(), {-1, -1, -1}), - mTargetRight(mConfig["navThresholds"]["cacheHitMax"].GetInt(), mConfig["navThresholds"]["cacheMissMax"].GetInt(), {-1, -1, -1}), - mPostOneLat(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()), - mPostOneLong(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()), - mPostTwoLat(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()), - mPostTwoLong(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()), - mLeftTargetBearing(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()), - mLeftTargetDistance(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()), - mRightTargetBearing(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()), - mRightTargetDistance(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()) {} +Environment::Environment(const rapidjson::Document& config) + : mConfig(config), + mTargetLeft(mConfig["navThresholds"]["cacheHitMax"].GetInt(), mConfig["navThresholds"]["cacheMissMax"].GetInt(), {-1, -1, -1}), + mTargetRight(mConfig["navThresholds"]["cacheHitMax"].GetInt(), mConfig["navThresholds"]["cacheMissMax"].GetInt(), {-1, -1, -1}), + mPostOneLat(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()), + mPostOneLong(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()), + mPostTwoLat(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()), + mPostTwoLong(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()), + mLeftTargetBearing(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()), + mLeftTargetDistance(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()), + mRightTargetBearing(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()), + mRightTargetDistance(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()) {} /*** * @param targets New target data from perception to update our filters. @@ -25,24 +24,21 @@ void Environment::setTargets(TargetList const& targets) { mTargetLeft.put((leftTargetRaw.distance != -1 && leftTargetRaw.id != -1), leftTargetRaw); mTargetRight.put((rightTargetRaw.distance != -1 && rightTargetRaw.id != -1), rightTargetRaw); - if (mTargetLeft.isValid()){ + if (mTargetLeft.isValid()) { mLeftTargetBearing.push(mTargetLeft.get().bearing); mLeftTargetDistance.push(mTargetLeft.get().distance); - } - else{ + } else { mLeftTargetDistance.reset(); mLeftTargetBearing.reset(); } - if (mTargetRight.isValid()){ + if (mTargetRight.isValid()) { mRightTargetBearing.push(mTargetRight.get().bearing); mRightTargetDistance.push(mTargetRight.get().distance); - } - else{ + } else { mRightTargetDistance.reset(); mRightTargetBearing.reset(); } - -} +}// setTargets() /*** * Update our estimate for where the post is with our current filtered values. @@ -95,64 +91,64 @@ void Environment::updatePost(std::shared_ptr const& rover, std::shared_pt double cosine = cos(degreeToRadian(rover->odometry().latitude_deg, rover->odometry().latitude_min)); rover->setLongMeterInMinutes(60 / (EARTH_CIRCUM * cosine / 360)); } -} +}// updatePost() void Environment::setObstacle(Obstacle const& obstacle) { mObstacle = obstacle; -} +}// setObstacle() Obstacle Environment::getObstacle() { return mObstacle; -} +}// getObstacle() Target Environment::getLeftTarget() const { return {mLeftTargetDistance.get(), mLeftTargetBearing.get(), mTargetLeft.get().id};//mTargetLeft.get(); -} +}// getLeftTarget() Target Environment::getRightTarget() const { return {mRightTargetDistance.get(), mRightTargetBearing.get(), mTargetRight.get().id};//mTargetRight.get(); -} +}// getRightTarget() void Environment::setBaseGateID(int baseGateId) { mBaseGateId = baseGateId; -} +}// setBaseGateID() bool Environment::hasNewPostUpdate() const { return mHasNewPostUpdate; -} +}// hasNewPostUpdate() bool Environment::hasGateLocation() const { return hasPostOneLocation() && hasPostTwoLocation(); -} +}// hasGateLocation() bool Environment::hasPostOneLocation() const { return mPostOneLat.ready(); -} +}// hasPostOneLocation() bool Environment::hasPostTwoLocation() const { return mPostTwoLat.ready(); -} +}// hasPostTwoLocation() -void Environment::setShouldLookForGate(bool gate){ +void Environment::setShouldLookForGate(bool gate) { mLookForGate = gate; -} +}// setShouldLookForGate() Odometry Environment::getPostOneLocation() const { return createOdom(mPostOneLat.get(), mPostOneLong.get()); -} +}// getPostOneLocation() Odometry Environment::getPostTwoLocation() const { return createOdom(mPostTwoLat.get(), mPostTwoLong.get()); -} +}// getPostTwoLocation() // Offset of the post in our linearized cartesian space. Vector2d Environment::getPostOneOffsetInCartesian(Odometry cur) const { return getOffsetInCartesian(cur, getPostOneLocation()); -} +}// getPostOneOffsetInCartesian() Vector2d Environment::getPostTwoOffsetInCartesian(Odometry cur) const { return getOffsetInCartesian(cur, getPostTwoLocation()); -} +}// getPostTwoOffsetInCartesian() std::optional Environment::tryGetTargetWithId(int32_t id) const { if (mTargetLeft.get().id == id && mTargetLeft.get().distance > 0.0) { @@ -161,4 +157,4 @@ std::optional Environment::tryGetTargetWithId(int32_t id) const { return {mTargetRight.get()}; } return std::nullopt; -} +}// tryGetTargetWithId() diff --git a/jetson/nav/environment.hpp b/jetson/nav/environment.hpp index 3fe1773123..28428e82fe 100644 --- a/jetson/nav/environment.hpp +++ b/jetson/nav/environment.hpp @@ -1,13 +1,13 @@ #pragma once -#include +#include #include #include -#include "rover.hpp" -#include "filter.hpp" #include "cache.hpp" +#include "filter.hpp" +#include "rover.hpp" #include "rover_msgs/Obstacle.hpp" #include "rover_msgs/TargetList.hpp" diff --git a/jetson/nav/filter.hpp b/jetson/nav/filter.hpp index ec98a1403f..01c0fdded6 100644 --- a/jetson/nav/filter.hpp +++ b/jetson/nav/filter.hpp @@ -1,8 +1,9 @@ #pragma once +#include #include #include -#include +#include /*** * A filter that combines multiple readings into one. @@ -37,46 +38,46 @@ class Filter { mFilterCount = std::min(mFilterCount + 1, size()); mSortedValues.assign(mValues.begin(), mValues.end()); std::sort(mSortedValues.begin(), mSortedValues.end()); - } + }// push() void reset() { mFilterCount = 0; - } + }// reset() //if we have values decrease count by 1 void decrementCount() { mFilterCount = std::max(mFilterCount - 1, size_t{}); - } + }// decrementCount() [[nodiscard]] size_t size() const { return mValues.size(); - } + }// size() [[nodiscard]] size_t filterCount() const { return mFilterCount; - } + }// filterCount() /*** * @return If we have enough readings to use the filter */ [[nodiscard]] bool ready() const { return mFilterCount > 0; - } + }// ready() [[nodiscard]] bool full() const { return mFilterCount == size(); - } + }// full() /*** * @return Filtered reading if full, or else the most recent reading if we don't have enough readings yet. */ [[nodiscard]] T get() const { -// return mValues[mHead]; + // return mValues[mHead]; if (!full()) { return mValues[mHead]; } auto begin = mSortedValues.begin() + (mProportion * size() / 4); auto end = mSortedValues.end() - (mProportion * size() / 4); return std::accumulate(begin, end, T{}) / (end - begin); - } + }// get() }; diff --git a/jetson/nav/gate_search/gateStateMachine.cpp b/jetson/nav/gate_search/gateStateMachine.cpp index aeeef247b0..e8cacd62ce 100644 --- a/jetson/nav/gate_search/gateStateMachine.cpp +++ b/jetson/nav/gate_search/gateStateMachine.cpp @@ -9,7 +9,6 @@ using Eigen::Vector2d; -// Constructs a GateStateMachine object with mStateMachine GateStateMachine::GateStateMachine(std::weak_ptr stateMachine, const rapidjson::Document& roverConfig) : mStateMachine(move(stateMachine)), mConfig(roverConfig) { @@ -155,12 +154,11 @@ void GateStateMachine::makeSpiderPath(std::shared_ptr const& rover, std:: mPath.push_back(victoryOdom); } // makeSpiderPath() -// Creates an GateStateMachine object std::shared_ptr GateFactory(const std::weak_ptr& sm, const rapidjson::Document& roverConfig) { return std::make_shared(sm, roverConfig); } // GateFactory() -// Sends search path rover takes when trying to find posts +/** @brief Sends search path rover takes when trying to find posts */ void GateStateMachine::publishGatePath() { std::shared_ptr sm = mStateMachine.lock(); std::shared_ptr env = sm->getEnv(); diff --git a/jetson/nav/main.cpp b/jetson/nav/main.cpp index 3be94f42e7..9180d87e22 100644 --- a/jetson/nav/main.cpp +++ b/jetson/nav/main.cpp @@ -1,17 +1,15 @@ -#include -#include #include #include -// #include does not compile with ubuntu18 +#include +#include #include -#include "rapidjson/document.h" -#include "rapidjson/istreamwrapper.h" +#include +#include -#include "stateMachine.hpp" -#include "environment.hpp" #include "courseProgress.hpp" - +#include "environment.hpp" +#include "stateMachine.hpp" using namespace rover_msgs; @@ -19,27 +17,19 @@ rapidjson::Document readConfig() { std::ifstream configFile; char* mrover_config = getenv("MROVER_CONFIG"); std::string path = std::string(mrover_config) + "/config_nav/config.json"; -// std::filesystem::path path; -// if (mrover_config) { -// path = std::filesystem::path{mrover_config} / "config_nav" / "config.json"; -// } else { -// path = std::filesystem::current_path() / "config" / "nav" / "config.json"; -// } configFile.open(path); -// if (!configFile) throw std::runtime_error("Could not open config file at: " + path.string()); if (!configFile) throw std::runtime_error("Could not open config file at: " + path); rapidjson::Document document; rapidjson::IStreamWrapper isw(configFile); document.ParseStream(isw); return document; -} +}// readConfig() // Runs the autonomous navigation of the rover. int main() { lcm::LCM lcm; if (!lcm.good()) throw std::runtime_error("Cannot create LCM"); - auto courseProgress = std::make_shared(); auto config = readConfig(); auto env = std::make_shared(config); @@ -71,8 +61,7 @@ int main() { }; lcm.subscribe("/target_list", &decltype(targetCallback)::operator(), &targetCallback)->setQueueCapacity(3); while (lcm.handle() == 0) { - //while (lcm.handleTimeout(0) > 0) {} stateMachine->run(); } return 0; -} // main() +}// main() diff --git a/jetson/nav/pid.cpp b/jetson/nav/pid.cpp index 77b3a03b35..14425a2e58 100644 --- a/jetson/nav/pid.cpp +++ b/jetson/nav/pid.cpp @@ -4,11 +4,7 @@ #include -PidLoop::PidLoop(double p, double i, double d) : - mP(p), - mI(i), - mD(d) { -} +PidLoop::PidLoop(double p, double i, double d) : mP(p), mI(i), mD(d) {} double PidLoop::error(double current, double desired) const { if (mMaxInputBeforeWrap) { @@ -18,13 +14,14 @@ double PidLoop::error(double current, double desired) const { double error = desired - current; if (std::fabs(error) > maxInput / 2) { if (error > 0) error -= maxInput; - else error += maxInput; + else + error += maxInput; } return error; } else { return desired - current; } -} +}// error() double PidLoop::update(double current, double desired, double dt) { if (dt < 1e-6) { @@ -45,30 +42,30 @@ double PidLoop::update(double current, double desired, double dt) { if (effort > mMaxOut) effort = mMaxOut; return effort; -} +}// update() void PidLoop::reset() { mFirst = true; mTotalError = 0.0; mLastError = 0.0; -} +}// reset() PidLoop& PidLoop::withThreshold(double threshold) { mThreshold = threshold; return *this; -} +}// withThreshold() PidLoop& PidLoop::withMaxInput(double maxInputBeforeWrap) { mMaxInputBeforeWrap = maxInputBeforeWrap; return *this; -} +}// withMaxInput() PidLoop& PidLoop::withOutputRange(double minOut, double maxOut) { mMinOut = minOut; mMaxOut = maxOut; return *this; -} +}// withOutputRange() bool PidLoop::isOnTarget(double current, double desired) const { return std::fabs(error(current, desired)) < mThreshold; -} +}// isOnTarget() diff --git a/jetson/nav/rover.cpp b/jetson/nav/rover.cpp index 478617da8b..0adbaf6c9c 100644 --- a/jetson/nav/rover.cpp +++ b/jetson/nav/rover.cpp @@ -1,30 +1,32 @@ #include "rover.hpp" -#include "utilities.hpp" -#include "rover_msgs/TargetBearing.hpp" +#include #include #include +#include +#include + +#include "rover_msgs/TargetBearing.hpp" +#include "utilities.hpp" -// Constructs a rover object with the given configuration file and lcm -// object with which to use for communications. Rover::Rover(const rapidjson::Document& config, lcm::LCM& lcmObject) - : mConfig(config), mLcmObject(lcmObject), - mTurningBearingPid(PidLoop(config["bearingPid"]["kP"].GetDouble(), - config["bearingPid"]["kI"].GetDouble(), - config["bearingPid"]["kD"].GetDouble()) - .withMaxInput(360.0) - .withThreshold(mConfig["navThresholds"]["turningBearing"].GetDouble())), - mDriveBearingPid(PidLoop(config["driveBearingPid"]["kP"].GetDouble(), - config["driveBearingPid"]["kI"].GetDouble(), - config["driveBearingPid"]["kD"].GetDouble()) - .withMaxInput(360.0) - .withThreshold(mConfig["navThresholds"]["turningBearing"].GetDouble())), - mLongMeterInMinutes(-1), - mTurning(true), - mDriving(false), - mBackingUp(false), - mNeedToSetTurnStart(true) { -} // Rover( + : mConfig(config), mLcmObject(lcmObject), + mTurningBearingPid(PidLoop(config["bearingPid"]["kP"].GetDouble(), + config["bearingPid"]["kI"].GetDouble(), + config["bearingPid"]["kD"].GetDouble()) + .withMaxInput(360.0) + .withThreshold(mConfig["navThresholds"]["turningBearing"].GetDouble())), + mDriveBearingPid(PidLoop(config["driveBearingPid"]["kP"].GetDouble(), + config["driveBearingPid"]["kI"].GetDouble(), + config["driveBearingPid"]["kD"].GetDouble()) + .withMaxInput(360.0) + .withThreshold(mConfig["navThresholds"]["turningBearing"].GetDouble())), + mLongMeterInMinutes(-1), + mTurning(true), + mDriving(false), + mBackingUp(false), + mNeedToSetTurnStart(true) { +}// Rover() /*** * Drive to the global position defined by destination, turning if necessary. @@ -38,7 +40,7 @@ bool Rover::drive(const Odometry& destination, double stopDistance, double dt) { double distance = estimateDistance(mOdometry, destination); double bearing = estimateBearing(mOdometry, destination); return drive(distance, bearing, stopDistance, dt); -} // drive() +}// drive() bool Rover::drive(double distance, double bearing, double threshold, double dt) { if (distance < threshold) { @@ -48,7 +50,7 @@ bool Rover::drive(double distance, double bearing, double threshold, double dt) mNeedToSetTurnStart = true; return true; } - if (mNeedToSetTurnStart){ + if (mNeedToSetTurnStart) { mTurnStartTime = NOW; std::cout << "staring turn clock" << std::endl; mNeedToSetTurnStart = false; @@ -59,44 +61,40 @@ bool Rover::drive(double distance, double bearing, double threshold, double dt) auto straightTime = std::chrono::milliseconds(2000); auto totalTime = std::chrono::milliseconds(4000); auto turnTimeout = std::chrono::milliseconds(15000); - if (mBackingUp){ + if (mBackingUp) { std::cout << "backing out" << std::endl; - if (NOW - mBackingUpStartTime > totalTime){ + if (NOW - mBackingUpStartTime > totalTime) { mBackingUp = false; mTurning = true; mDriving = false; mTurnStartTime = NOW; std::cout << "staring turn clock" << std::endl; - } - else if (NOW - mBackingUpStartTime > straightTime){ + } else if (NOW - mBackingUpStartTime > straightTime) { publishAutonDriveCmd(-1.0, -0.5); - } - else{ + } else { publishAutonDriveCmd(-1.0, -1.0); } - } - else if (mTurning){ + } else if (mTurning) { std::cout << "turning" << std::endl; - if (turn(bearing, dt)){ + if (turn(bearing, dt)) { mTurning = false; mDriving = true; mBackingUp = false; } - if (!mBackingUp){ + if (!mBackingUp) { //std::cout << NOW - mTurnStartTime << std::endl; - if (NOW - mTurnStartTime > turnTimeout){ + if (NOW - mTurnStartTime > turnTimeout) { mBackingUpStartTime = NOW; mBackingUp = true; mTurning = false; mDriving = false; } } - } - else{ + } else { std::cout << "driving" << std::endl; double destinationBearing = mod(bearing, 360); double turningEffort = mDriveBearingPid.update(mOdometry.bearing_deg, destinationBearing, dt); - if (mDriveBearingPid.error(mOdometry.bearing_deg, destinationBearing) > mConfig["navThresholds"]["drivingBearing"].GetDouble()){ + if (mDriveBearingPid.error(mOdometry.bearing_deg, destinationBearing) > mConfig["navThresholds"]["drivingBearing"].GetDouble()) { mTurning = true; mTurnStartTime = NOW; std::cout << "starting turn clock" << std::endl; @@ -106,26 +104,21 @@ bool Rover::drive(double distance, double bearing, double threshold, double dt) // if we need to turn clockwise, turning effort will be positive, so leftVel will be 1, and rightVel will be in between 0 and 1 // if we need to turn ccw, turning effort will be negative, so rightVel will be 1 and leftVel will be in between 0 and 1 // TODO: use std::clamp -// double leftVel = std::clamp(1.0 + turningEffort, 0.0, 1.0); -// double rightVel = std::clamp(1.0 - turningEffort, 0.0, 1.0); double leftVel = std::min(1.0, std::max(0.0, 1.0 + turningEffort)); double rightVel = std::min(1.0, std::max(0.0, 1.0 - turningEffort)); publishAutonDriveCmd(leftVel, rightVel); } - - return false; -} // drive() +}// drive() -// Turn to a global point +/** @brief Turn to a global point */ bool Rover::turn(Odometry const& destination, double dt) { double bearing = estimateBearing(mOdometry, destination); return turn(bearing, dt); -} // turn() +}// turn() - -// Turn to an absolute bearing +/** @brief Turn to an absolute bearing */ bool Rover::turn(double absoluteBearing, double dt) { if (mTurningBearingPid.isOnTarget(mOdometry.bearing_deg, absoluteBearing)) { return true; @@ -136,7 +129,7 @@ bool Rover::turn(double absoluteBearing, double dt) { double rightVel = std::max(std::min(1.0, -turningEffort), -1.0); publishAutonDriveCmd(leftVel, rightVel); return false; -} // turn() +}// turn() void Rover::stop() { mTurning = true; @@ -144,21 +137,20 @@ void Rover::stop() { mDriving = false; mNeedToSetTurnStart = true; publishAutonDriveCmd(0.0, 0.0); -} // stop() +}// stop() -// Calculates the conversion from minutes to meters based on the -// rover's current latitude. +/** @brief Calculates the conversion from minutes to meters based on the rover's current latitude */ double Rover::longMeterInMinutes() const { if (mLongMeterInMinutes <= 0.0) { throw std::runtime_error("Invalid conversion"); } return mLongMeterInMinutes; -} +}// longMeterInMinutes() -// Gets the rover's turning pid object. +/** @return Rover's turning PID controller */ PidLoop& Rover::turningBearingPid() { return mTurningBearingPid; -} // bearingPid() +}// turningBearingPid() // Gets the rover's turning pid while driving object PidLoop& Rover::drivingBearingPid() { @@ -168,39 +160,35 @@ PidLoop& Rover::drivingBearingPid() { void Rover::publishAutonDriveCmd(const double leftVel, const double rightVel) { AutonDriveControl driveControl{ .left_percent_velocity = leftVel, - .right_percent_velocity = rightVel - }; + .right_percent_velocity = rightVel}; std::string autonDriveControlChannel = mConfig["lcmChannels"]["autonDriveControlChannel"].GetString(); mLcmObject.publish(autonDriveControlChannel, &driveControl); } -// Gets a reference to the rover's current navigation state. NavState const& Rover::currentState() const { return mCurrentState; -} // currentState() +}// currentState() -// Gets a reference to the rover's current auton state. Enable const& Rover::autonState() const { return mAutonState; -} // autonState() +}// autonState() -// Gets a reference to the rover's current odometry information. Odometry const& Rover::odometry() const { return mOdometry; -} // odometry() +}// odometry() void Rover::setAutonState(Enable state) { mAutonState = state; -} +}// setAutonState() void Rover::setOdometry(const Odometry& odometry) { mOdometry = odometry; -} +}// setOdometry() void Rover::setState(NavState state) { mCurrentState = state; -} +}// setState() void Rover::setLongMeterInMinutes(double l) { mLongMeterInMinutes = l; -} +}//setLongMeterInMinutes() diff --git a/jetson/nav/rover.hpp b/jetson/nav/rover.hpp index 96b3db004c..5b7c17108d 100644 --- a/jetson/nav/rover.hpp +++ b/jetson/nav/rover.hpp @@ -1,31 +1,29 @@ #pragma once -#include -#include #include - +#include #include +#include -#include "rover_msgs/Enable.hpp" +#include "courseProgress.hpp" +#include "environment.hpp" +#include "pid.hpp" +#include "rover_msgs/AutonDriveControl.hpp" #include "rover_msgs/Bearing.hpp" #include "rover_msgs/Course.hpp" +#include "rover_msgs/Enable.hpp" #include "rover_msgs/Obstacle.hpp" #include "rover_msgs/Odometry.hpp" +#include "rover_msgs/ProjectedPoints.hpp" #include "rover_msgs/TargetList.hpp" #include "rover_msgs/Waypoint.hpp" -#include "rover_msgs/AutonDriveControl.hpp" -#include "rover_msgs/ProjectedPoints.hpp" -#include "rapidjson/document.h" -#include "courseProgress.hpp" -#include "environment.hpp" -#include "pid.hpp" -#include -#include + +// TODO: replace with function #define NOW std::chrono::high_resolution_clock::now() + using namespace rover_msgs; -// This class is the representation of the navigation states. enum class NavState { // Base States Off = 0, @@ -52,10 +50,8 @@ enum class NavState { // Unknown State Unknown = 255 -}; // AutonState +};// AutonState -// This class creates a Rover object which can perform operations that -// the real rover can perform. class Rover { public: Rover(const rapidjson::Document& config, lcm::LCM& lcm_in); @@ -100,11 +96,9 @@ class Rover { /* Private Member Variables */ /*************************************************************************/ - // A reference to the configuration file. const rapidjson::Document& mConfig; - // A reference to the lcm object that will be used for - // communicating with the actual rover and the base station. + // Interface for communicating messages to and from the base station lcm::LCM& mLcmObject; // The pid loop for turning. @@ -113,20 +107,16 @@ class Rover { // The pid loop for turning while driving PidLoop mDriveBearingPid; - // The conversion factor from arcminutes to meters. This is based - // on the rover's current latitude. + // The conversion factor from arcminutes to meters. This is based n the rover's current latitude. double mLongMeterInMinutes; - // The rover's current navigation state. NavState mCurrentState{NavState::Off}; - // The rover's current auton state. Enable mAutonState{}; - // The rover's current odometry information. Odometry mOdometry{}; bool mTurning, mDriving, mBackingUp, mNeedToSetTurnStart; std::chrono::_V2::system_clock::time_point mTurnStartTime, mBackingUpStartTime; -}; +};// Rover diff --git a/jetson/nav/search/searchStateMachine.cpp b/jetson/nav/search/searchStateMachine.cpp index 01eb3c6b75..362a647630 100644 --- a/jetson/nav/search/searchStateMachine.cpp +++ b/jetson/nav/search/searchStateMachine.cpp @@ -11,15 +11,10 @@ using Eigen::Rotation2Dd; -// Constructs an SearchStateMachine object with mStateMachine, mConfig, and mRover SearchStateMachine::SearchStateMachine(std::weak_ptr sm, const rapidjson::Document& roverConfig) : mStateMachine(move(sm)), mConfig(roverConfig) {} -// Runs the search state machine through one iteration. This will be called by -// StateMachine when NavState is in a search state. This will call the corresponding -// function based on the current state and return the next NavState NavState SearchStateMachine::run() { - //mStateMachine.lock()->publishProjectedPoints(mSearchPoints, "search"); switch (mStateMachine.lock()->getRover()->currentState()) { case NavState::Search: { return executeSearch(); @@ -175,8 +170,11 @@ NavState SearchStateMachine::executeDriveToTarget() { return NavState::DriveToTarget; } // executeDriveToTarget() -// add intermediate points between the existing search points in a path generated by a search algorithm. -// The maximum separation between any points in the search point list is determined by the rover's sight distance. +/** + * Add intermediate points between the existing search points in a path generated by a search algorithm. + * The maximum separation between any points in the search point list is determined by the rover's sight distance. + * TODO: commented out since it does not really help + */ void SearchStateMachine::insertIntermediatePoints() { // double visionDistance = mConfig["computerVision"]["visionDistance"].GetDouble(); // const double maxDifference = 2 * visionDistance; @@ -200,8 +198,6 @@ void SearchStateMachine::insertIntermediatePoints() { // } } // insertIntermediatePoints() -// The search factory allows for the creation of search objects and -// an ease of transition between search algorithms std::shared_ptr SearchFactory(const std::weak_ptr& sm, SearchType type, const std::shared_ptr& rover, const rapidjson::Document& roverConfig) { std::shared_ptr search = nullptr; @@ -218,9 +214,3 @@ SearchFactory(const std::weak_ptr& sm, SearchType type, const std: } return search; } // SearchFactory - -/******************/ -/* TODOS */ -/******************/ -// save location of target then go around object? ( Execute drive to target ) -// look into removing the waiting when turning to target or at least doing this a better way. This should at very least be it's own state diff --git a/jetson/nav/search/searchStateMachine.hpp b/jetson/nav/search/searchStateMachine.hpp index 0a7913b91b..49c72ffbcc 100644 --- a/jetson/nav/search/searchStateMachine.hpp +++ b/jetson/nav/search/searchStateMachine.hpp @@ -7,10 +7,8 @@ class StateMachine; -// This class is the representation of different -// search algorithms enum class SearchType { - FROM_PATH_FILE = 0, + FROM_PATH_FILE = 0, FROM_PATH_FILE_GATE = 1 }; @@ -25,8 +23,6 @@ class SearchStateMachine { NavState run(); -// bool targetReachable(std::shared_ptr rover, double distance, double bearing); - virtual void initializeSearch(const rapidjson::Document& roverConfig, double pathWidth) = 0; // TODO protected: @@ -64,8 +60,5 @@ class SearchStateMachine { bool mDrivenToFirstPost = false; }; -// Creates an ObstacleAvoidanceStateMachine object based on the inputted obstacle -// avoidance algorithm. This allows for an an ease of transition between obstacle -// avoidance algorithms std::shared_ptr SearchFactory(const std::weak_ptr& sm, SearchType type, const std::shared_ptr& rover, const rapidjson::Document& roverConfig); diff --git a/jetson/nav/searchPointGeneration.py b/jetson/nav/searchPointGeneration.py index 9c5a944b6e..533e09d0d9 100644 --- a/jetson/nav/searchPointGeneration.py +++ b/jetson/nav/searchPointGeneration.py @@ -4,41 +4,45 @@ import enum from math import sin, cos, pi + def generateSpiralSearchPoints(radius, sides, coils): awayStep = radius/sides aroundStep = coils/sides aroundRadians = aroundStep * 2 * math.pi - coordinates = [(0,0)] + coordinates = [(0, 0)] for i in range(1, sides+1): away = i * awayStep around = i * aroundRadians - x = math.cos(around) * away + x = math.cos(around) * away y = math.sin(around) * away - coordinates.append((x,y)) + coordinates.append((x, y)) return coordinates - + + def generateEquidistantSpiralSearchPoints(radius, distance, coils, rotation): thetaMax = coils * 2 * math.pi awayStep = radius / thetaMax theta = distance / awayStep - coordinates = [(0,0)] + coordinates = [(0, 0)] while theta <= thetaMax: away = awayStep * theta around = theta + rotation x = math.cos(around) * away y = math.sin(around) * away - coordinates.append((x,y)) + coordinates.append((x, y)) theta += distance / away return coordinates -def generateSquareSpiral (points, distance): - directions = [(0,1), (-1,0), (0,-1), (1,0)] - coordinates = [(0,0)] + +def generateSquareSpiral(points, distance): + directions = [(0, 1), (-1, 0), (0, -1), (1, 0)] + coordinates = [(0, 0)] new_distance = distance - for i in range(0,points): - coordinates.append( (coordinates[-1][0]+new_distance*directions[i%4][0], coordinates[-1][1]+new_distance*directions[i%4][1]) ) - new_distance += (i%2)*distance - #divide up each segment into intermediate segments + for i in range(0, points): + coordinates.append((coordinates[-1][0]+new_distance*directions[i % + 4][0], coordinates[-1][1]+new_distance*directions[i % 4][1])) + new_distance += (i % 2)*distance + # divide up each segment into intermediate segments intermediate_len = 6 new_coordinates = [] for i in range(0, len(coordinates) - 1): @@ -56,29 +60,32 @@ def generateSquareSpiral (points, distance): def generateHexagonSpiral(points, distance): - #lowkey hacky + # lowkey hacky forward = np.array([0, 1]) directions = [] print(np.linspace(0, 2 * pi, num=6)) for theta in np.linspace(0, 2 * pi, num=7): print(theta) - rot_mat = np.array([[cos(theta), -sin(theta)], [sin(theta), cos(theta)]]) + rot_mat = np.array( + [[cos(theta), -sin(theta)], [sin(theta), cos(theta)]]) dir_vec = np.dot(forward, rot_mat) directions.append(dir_vec) - #return directions + # return directions coordinates = [(0, 0)] new_distance = distance for i in range(points): - cur = np.array([coordinates[-1][0], coordinates[-1][1]]) - new_distance += (distance / 3) - nex = cur + (directions[i%6] * new_distance) - coordinates.append((nex[0], nex[1])) - + cur = np.array([coordinates[-1][0], coordinates[-1][1]]) + new_distance += (distance / 3) + nex = cur + (directions[i % 6] * new_distance) + coordinates.append((nex[0], nex[1])) + return coordinates -def generateSquareSpiralInward (points, distance): - return generateSquareSpiral(points, distance)[::-1] + +def generateSquareSpiralInward(points, distance): + return generateSquareSpiral(points, distance)[::-1] + def showCoords(coordinates): x_coords = [i[0] for i in coordinates] @@ -89,49 +96,56 @@ def showCoords(coordinates): plt.grid() plt.show() + def cart2pol(x, y): rho = np.sqrt(x**2 + y**2) phi = -1 * (np.arctan2(y, x) * 180 / math.pi - 90) return(rho, phi) + class SearchType(enum.Enum): point_equidistant_spiral = '0' radially_equidistant_spiral = '1' square_spiral = '2' hex_spiral = '3' + print("\n-----Search types-----\nRadially Equidistant Spiral: 0\nPoint Equidistant Spiral: 1\nSquare Spiral: 2\n Hex Spiral: 3 \n") search_type = input("Select a search type: ") -if search_type == SearchType.point_equidistant_spiral.value: # POINT EQUIDISTANCE SPIRAL - # generateSpiralSearchPoints +if search_type == SearchType.point_equidistant_spiral.value: # POINT EQUIDISTANCE SPIRAL + # generateSpiralSearchPoints # (Radius of spiral, Number of Points, Number of coils) radius = float(input("Enter a radius: ")) sides = int(input("Enter the number of points: ")) coils = int(input("Enter the number of coils: ")) - coords = generateSpiralSearchPoints(radius, sides, coils) # Try (20, 200, 10) to see basic example -elif search_type == SearchType.radially_equidistant_spiral.value: # RADIALLY EQUIDISTANT SPIRAL - # generateSpiralSearchPoints + # Try (20, 200, 10) to see basic example + coords = generateSpiralSearchPoints(radius, sides, coils) +elif search_type == SearchType.radially_equidistant_spiral.value: # RADIALLY EQUIDISTANT SPIRAL + # generateSpiralSearchPoints # (Radius of spiral, Distance between points, Number of coils, Rotation from start) radius = float(input("Enter a radius: ")) distance = float(input("Enter distance between points: ")) coils = int(input("Enter number of coils: ")) rotation = float(input("Enter rotation of spiral start (degrees): ")) - coords = generateEquidistantSpiralSearchPoints(radius, distance, coils, rotation) # Try (20, 1.2, 10, 90) to see basic example -elif search_type == SearchType.square_spiral.value: # SQUARE SPIRAL SEARCH + coords = generateEquidistantSpiralSearchPoints( + radius, distance, coils, rotation) # Try (20, 1.2, 10, 90) to see basic example +elif search_type == SearchType.square_spiral.value: # SQUARE SPIRAL SEARCH # generateSquareSpiral # (number of points, distance between each coil of the spiral) number_of_points = int(input("Enter Number of Points: ")) distance = float(input("Enter Distance Between Points: ")) - coords = generateSquareSpiral(number_of_points, distance) # Currently, we use (13,3) as arguements + # Currently, we use (13,3) as arguements + coords = generateSquareSpiral(number_of_points, distance) spiral_in = (input("Do You Want to Spiral In? (y/n)") == 'y') if spiral_in: coords += generateSquareSpiralInward(number_of_points, distance) elif search_type == SearchType.hex_spiral.value: number_of_points = int(input("Enter Number of Points: ")) distance = float(input("Enter Distance Between Points: ")) - coords = generateHexagonSpiral(number_of_points, distance) # Currently, we use (13,3) as arguements + # Currently, we use (13,3) as arguements + coords = generateHexagonSpiral(number_of_points, distance) else: - print ("Not a valid type") + print("Not a valid type") exit(1) show_coords = input("Do you want to display the search coordinates? (y/n)") if show_coords == 'y': @@ -144,7 +158,7 @@ class SearchType(enum.Enum): with open('search/'+name, 'w') as f: # print (len(coords), file=f) - for x,y in coords: - polar_coord = cart2pol(x,y) + for x, y in coords: + polar_coord = cart2pol(x, y) # (Rho (distance), Phi (angle)) - print(polar_coord[0], polar_coord[1], file=f) \ No newline at end of file + print(polar_coord[0], polar_coord[1], file=f) diff --git a/jetson/nav/stateMachine.cpp b/jetson/nav/stateMachine.cpp index 57df7064bc..d504298e4c 100644 --- a/jetson/nav/stateMachine.cpp +++ b/jetson/nav/stateMachine.cpp @@ -1,41 +1,39 @@ #include "stateMachine.hpp" -#include -#include #include +#include #include +#include -#include "utilities.hpp" -#include "rover_msgs/NavStatus.hpp" #include "obstacle_avoidance/simpleAvoidance.hpp" +#include "rover_msgs/NavStatus.hpp" +#include "utilities.hpp" -// Constructs a StateMachine object with the input lcm object. -// Reads the configuration file and constructs a Rover objet with this -// and the lcmObject. Sets mStateChanged to true so that on the first -// iteration of run the rover is updated. StateMachine::StateMachine( rapidjson::Document& config, std::shared_ptr rover, std::shared_ptr env, std::shared_ptr courseProgress, - lcm::LCM& lcmObject -) : mConfig(config), - mRover(move(rover)), mEnv(move(env)), mCourseProgress(move(courseProgress)), mLcmObject(lcmObject), - mTimePoint(std::chrono::high_resolution_clock::now()), - mPrevTimePoint(mTimePoint) { - // TODO: fix, weak_from_this() should not be called in ctor, will always be null + lcm::LCM& lcmObject) : mConfig(config), + mRover(move(rover)), mEnv(move(env)), mCourseProgress(move(courseProgress)), mLcmObject(lcmObject), + mTimePoint(std::chrono::high_resolution_clock::now()), + mPrevTimePoint(mTimePoint) { + // TODO weak_from_this() should not be called in ctor, will always be null. Fine at the moment since obstacle detection is not used mObstacleAvoidanceStateMachine = ObstacleAvoiderFactory(weak_from_this(), ObstacleAvoidanceAlgorithm::SimpleAvoidance, mRover, mConfig); -} // StateMachine() +}// StateMachine() -// Runs the state machine through one iteration. The state machine will -// run if the state has changed or if the rover's status has changed. -// Will call the corresponding function based on the current state. +/** + * Run a single iteration of the top level state machine. + * Executes the correct function or sub state machine based on the current state machine to calculate the next state. + * In this way the state machine can be seen as a tree, with search and gate search being subtrees. + */ void StateMachine::run() { mPrevTimePoint = mTimePoint; auto now = std::chrono::high_resolution_clock::now(); mTimePoint = now; // Diagnostic info: take average loop time + // TODO move static to instance variable. Fine at the moment since we don't have multiple instances of the state machine static std::array readings{}; static int i = 0; readings[i] = getDtSeconds(); @@ -100,8 +98,9 @@ void StateMachine::run() { case NavState::Unknown: { throw std::runtime_error("Entered unknown state."); } - } // switch + }// switch + // Avoid old PID values from previous states if (nextState != mRover->currentState()) { mRover->setState(nextState); mRover->turningBearingPid().reset(); @@ -109,30 +108,26 @@ void StateMachine::run() { } } else { nextState = NavState::Off; - mRover->setState(executeOff()); // Turn off immediately + mRover->setState(executeOff()); if (nextState != mRover->currentState()) { mRover->setState(nextState); } } + // TODO no longer needed after switching to waiting for LCM messages? std::this_thread::sleep_until(mTimePoint + LOOP_DURATION); -} // run() +}// run() -// Publishes the current navigation state to the nav status lcm channel. +/*** @brief Publishes the current navigation state to the nav status lcm channel. */ void StateMachine::publishNavState() const { NavStatus navStatus{ .nav_state_name = stringifyNavState(), .completed_wps = static_cast(mCourseProgress->getRemainingWaypoints().size()), - .total_wps = mCourseProgress->getCourse().num_waypoints - }; + .total_wps = mCourseProgress->getCourse().num_waypoints}; const std::string& navStatusChannel = mConfig["lcmChannels"]["navStatusChannel"].GetString(); mLcmObject.publish(navStatusChannel, &navStatus); -} // publishNavState() +}// publishNavState() -// Executes the logic for off. If the rover is turned on, it updates -// the roverStatus. If the course is empty, the rover is done with -// the course otherwise it will turn to the first waypoint. Else the -// rover is still off. NavState StateMachine::executeOff() { if (mRover->autonState().enabled) { NavState nextState = mCourseProgress->getCourse().num_waypoints ? NavState::DriveWaypoints : NavState::Done; @@ -141,13 +136,12 @@ NavState StateMachine::executeOff() { } mRover->stop(); return NavState::Off; -} // executeOff() +}// executeOff() -// Executes the logic for the done state. Stops and turns off the rover. NavState StateMachine::executeDone() { mRover->stop(); return NavState::Done; -} // executeDone() +}// executeDone() /** * Drive through the waypoints defined by course progress. @@ -175,62 +169,54 @@ NavState StateMachine::executeDrive() { return NavState::BeginSearch; } return NavState::DriveWaypoints; -} // executeDrive() +}// executeDrive() -// Gets the string representation of a nav state. std::string StateMachine::stringifyNavState() const { static const std::unordered_map navStateNames = { - {NavState::Off, "Off"}, - {NavState::Done, "Done"}, - {NavState::DriveWaypoints, "Drive Waypoints"}, - {NavState::BeginSearch, "Change Search Algorithm"}, - {NavState::Search, "Search"}, - {NavState::DriveToTarget, "Drive to Target"}, - {NavState::TurnAroundObs, "Turn Around Obstacle"}, - {NavState::DriveAroundObs, "Drive Around Obstacle"}, - {NavState::SearchTurnAroundObs, "Search Turn Around Obstacle"}, + {NavState::Off, "Off"}, + {NavState::Done, "Done"}, + {NavState::DriveWaypoints, "Drive Waypoints"}, + {NavState::BeginSearch, "Change Search Algorithm"}, + {NavState::Search, "Search"}, + {NavState::DriveToTarget, "Drive to Target"}, + {NavState::TurnAroundObs, "Turn Around Obstacle"}, + {NavState::DriveAroundObs, "Drive Around Obstacle"}, + {NavState::SearchTurnAroundObs, "Search Turn Around Obstacle"}, {NavState::SearchDriveAroundObs, "Search Drive Around Obstacle"}, - {NavState::BeginGateSearch, "Gate Prepare"}, - {NavState::GateTraverse, "Gate Traverse"}, + {NavState::BeginGateSearch, "Gate Prepare"}, + {NavState::GateTraverse, "Gate Traverse"}, - {NavState::Unknown, "Unknown"} - }; + {NavState::Unknown, "Unknown"}}; return navStateNames.at(mRover->currentState()); -} // stringifyNavState() +}// stringifyNavState() void StateMachine::setSearcher(SearchType type) { mSearchStateMachine = SearchFactory(weak_from_this(), type, mRover, mConfig); mSearchStateMachine->initializeSearch(mConfig, mConfig["computerVision"]["visionDistance"].GetDouble()); -} +}// setSearcher() void StateMachine::setGateSearcher() { mGateStateMachine = GateFactory(weak_from_this(), mConfig); -} - -// Allows outside objects to set the original obstacle angle -// This will allow the variable to be set before the rover turns -void StateMachine::updateObstacleDistance(double distance) { - mObstacleAvoidanceStateMachine->updateObstacleDistance(distance); -} +}//setGateSearcher() std::shared_ptr StateMachine::getEnv() { return mEnv; -} +}//getEnv() std::shared_ptr StateMachine::getCourseState() { return mCourseProgress; -} +}//getCourseState() std::shared_ptr StateMachine::getRover() { return mRover; -} +}//getRover() lcm::LCM& StateMachine::getLCM() { return mLcmObject; -} +}//getLCM() double StateMachine::getDtSeconds() { return std::chrono::duration(mTimePoint - mPrevTimePoint).count(); -} +}//getDtSeconds() diff --git a/jetson/nav/stateMachine.hpp b/jetson/nav/stateMachine.hpp index df19b6423f..2614d25473 100644 --- a/jetson/nav/stateMachine.hpp +++ b/jetson/nav/stateMachine.hpp @@ -1,26 +1,24 @@ #pragma once -#include #include +#include #include -#include "rover.hpp" -#include "environment.hpp" #include "courseProgress.hpp" -#include "search/searchStateMachine.hpp" +#include "environment.hpp" #include "gate_search/gateStateMachine.hpp" #include "obstacle_avoidance/simpleAvoidance.hpp" +#include "rover.hpp" +#include "search/searchStateMachine.hpp" using namespace rover_msgs; using namespace std::chrono_literals; using time_point = std::chrono::high_resolution_clock::time_point; -static auto const LOOP_DURATION = 0.01s; +constexpr auto LOOP_DURATION = 0.01s; -// This class implements the logic for the state machine for the -// autonomous navigation of the rover. class StateMachine : public std::enable_shared_from_this { public: /*************************************************************************/ @@ -32,8 +30,6 @@ class StateMachine : public std::enable_shared_from_this { void run(); - void updateObstacleDistance(double distance); - void setSearcher(SearchType type); void setGateSearcher(); @@ -91,4 +87,4 @@ class StateMachine : public std::enable_shared_from_this { std::shared_ptr mObstacleAvoidanceStateMachine; time_point mTimePoint, mPrevTimePoint; -}; // StateMachine +};// StateMachine diff --git a/jetson/nav/utilities.cpp b/jetson/nav/utilities.cpp index a5e2892681..f6c2587bf4 100644 --- a/jetson/nav/utilities.cpp +++ b/jetson/nav/utilities.cpp @@ -1,18 +1,21 @@ #include "utilities.hpp" -#include // remove + #include -// Coverts the input degree (and optional minute) to radians. +// You probably don't want to copy anything in this file +// Everything is implemented in a very silly way, but hey it works + +/** @brief Coverts the input degree (and optional minute) to radians */ double degreeToRadian(const double degree, const double minute) { return (PI / 180) * (degree + minute / 60); -} // degreeToRadian +}// degreeToRadian() -// Converts the input radians to degrees. +/** @brief Converts the input radians to degrees */ double radianToDegree(const double radian) { return radian * 180 / PI; -} // radianToDegree +}// radianToDegree() -// create a new odom with coordinates offset from current odom by a certain lat and lon change +/** @brief create a new odom with coordinates offset from current odom by a certain lat and lon change */ Odometry addMinToDegrees(const Odometry& current, const double lat_minutes, const double lon_minutes) { Odometry newOdom = current; double total_lat_min = current.latitude_min + lat_minutes; @@ -25,9 +28,9 @@ Odometry addMinToDegrees(const Odometry& current, const double lat_minutes, cons newOdom.longitude_deg += static_cast(total_lon_min) / 60; return newOdom; -} // addMinToDegrees +}// addMinToDegrees() -// Estimate approximate distance using euclidean methods +/** @brief Estimate approximate distance using euclidean methods */ double estimateDistance(const Odometry& current, const Odometry& dest) { double currentLat = degreeToRadian(current.latitude_deg, current.latitude_min); double currentLon = degreeToRadian(current.longitude_deg, current.longitude_min); @@ -37,7 +40,7 @@ double estimateDistance(const Odometry& current, const Odometry& dest) { double diffLat = (destLat - currentLat); double diffLon = (destLon - currentLon) * cos((currentLat + destLat) / 2); return sqrt(diffLat * diffLat + diffLon * diffLon) * EARTH_RADIUS; -} // estimateDistance +}// estimateDistance() /*** * @param current Current position @@ -51,7 +54,7 @@ Odometry createOdom(const Odometry& current, double absoluteBearing, double dist double lonChange = distance * sin(absoluteBearing) * rover->longMeterInMinutes(); Odometry newOdom = addMinToDegrees(current, latChange, lonChange); return newOdom; -} // createOdom +}// createOdom() /*** * @param current Current position @@ -62,10 +65,12 @@ Odometry createOdom(const Odometry& current, Vector2d offset, const std::shared_ double bearing = radianToDegree(atan2(offset.y(), offset.x())); double distance = offset.norm(); return createOdom(current, bearing, distance, rover); -} // createOdom +}// createOdom() -// Approximate the LHS bearing (clockwise rotation in positive) between two global odometries. -// The linearization that occurs is implicitly defined relative to the destination. +/** + * Approximate the LHS bearing (clockwise rotation in positive) between two global odometries. + * The linearization that occurs is implicitly defined relative to the destination. + */ double estimateBearing(const Odometry& start, const Odometry& dest) { double currentLat = degreeToRadian(start.latitude_deg, start.latitude_min); double currentLon = degreeToRadian(start.longitude_deg, start.longitude_min); @@ -88,14 +93,16 @@ double estimateBearing(const Odometry& start, const Odometry& dest) { } } return radianToDegree(bearing); -} // estimateBearing +}// estimateBearing() -// Calculates the modulo of value with the given modulus. -// This handles the case where value is negatively properly. +/** + * Calculates the modulo of value with the given modulus. + * This handles the case where value is negatively properly. + */ double mod(double value, double modulus) { double mod = fmod(value, modulus); return mod < 0 ? mod + modulus : mod; -} // mod() +}// mod() Odometry createOdom(double latitude, double longitude) { double latitudeDeg; @@ -106,9 +113,8 @@ Odometry createOdom(double latitude, double longitude) { longitudeMin *= 60.0; return Odometry{ static_cast(latitudeDeg), latitudeMin, - static_cast(longitudeDeg), longitudeMin - }; -} // createOdom + static_cast(longitudeDeg), longitudeMin}; +}// createOdom() /*** * @param current From position @@ -119,4 +125,4 @@ Vector2d getOffsetInCartesian(Odometry current, Odometry target) { double bearing = degreeToRadian(estimateBearing(current, target)); double distance = estimateDistance(current, target); return {distance * cos(bearing), distance * sin(bearing)}; -} // getOffsetInCartesian +}// getOffsetInCartesian() diff --git a/jetson/nav/utilities.hpp b/jetson/nav/utilities.hpp index a1054481b1..36f7e60531 100644 --- a/jetson/nav/utilities.hpp +++ b/jetson/nav/utilities.hpp @@ -1,19 +1,19 @@ #pragma once -#include #include -#include "rover_msgs/Waypoint.hpp" -#include "rover_msgs/Odometry.hpp" -#include "rover.hpp" + #include "environment.hpp" +#include "rover.hpp" +#include "rover_msgs/Odometry.hpp" +#include "rover_msgs/Waypoint.hpp" using namespace rover_msgs; -const int EARTH_RADIUS = 6371000; // meters -const int EARTH_CIRCUM = 40075000; // meters -const double PI = 3.141592654; // radians -const double LAT_METER_IN_MINUTES = 0.0005389625; // minutes/meters +const int EARTH_RADIUS = 6371000; // meters +const int EARTH_CIRCUM = 40075000; // meters +const double PI = 3.141592654; // radians +const double LAT_METER_IN_MINUTES = 0.0005389625;// minutes/meters double degreeToRadian(double degree, double minute = 0); @@ -40,5 +40,3 @@ double doesVectorIntersectCircle(); Odometry createOdom(double latitude, double longitude); Vector2d getOffsetInCartesian(Odometry current, Odometry target); - -