Merge branch 'master' into updatetelemtry

README.md

@@ -27,8 +27,6 @@ your distro.
 
 # Project Setup
 
-## Option 1: Easy Install
-
 Make sure software is up to date (`sudo apt upgrade` is optional) and install required tools:
 ```bash
 sudo apt update
@@ -48,145 +46,6 @@ cd Resurgence
 ```
 
 Done! Continue on to the "IDE Setup" section.
-## Option 2: Manual Install
-
-### Install System Tools
-
-First, update your software with 
-```bash
-sudo apt update && sudo apt upgrade
-```
-Then, to install the tools, run
-```bash
-sudo apt install build-essential cmake git
-```
-
-### Setup Ubuntu repository
-The project depends on many third-party libraries (mostly for sensors,
-distributed by the sensor manufacturers) that we have packaged in a third-party
-Ubuntu package repository. *Before continuing*, you should follow the
-instructions at <https://huskyroboticsteam.github.io/ubuntu-repo> to set it
-up. Afterwards, it should be fairly trivial to install the dependencies. 
-
-If you are on another Linux distribution, you may still have to build these
-dependencies from source (especially if your system does not use the APT package
-manager).
-
-### Setup Project Repository
-
-```bash
-# Enter the home directory (or wherever you would like to put the project files).
-# If you would like to put the project somewhere else, replace '~' with the folder path.
-cd ~
-# Clone the repository.
-git clone https://github.com/huskyroboticsteam/Resurgence/
-``` 
-
-### Install Team Repositories
-We have a few team libraries that we maintain internally.
-HindsightCAN, for example, is a library developed by Electronics for interfacing with their
-motors and sensors over the
-[CAN](https://en.wikipedia.org/wiki/CAN_bus) bus; it is needed for packet definitions and utility functions and doesn't actually require support for a physical CAN bus.
-
-
-Assuming the Ubuntu repository has been set up:
-```bash
-sudo apt install hindsight-can
-```
-
-### Install OpenCV
-
-OpenCV and its contrib modules are packaged for Ubuntu. For other distributions, check to
-see if `libopencv` and `libopencv-contrib` are included in your distribution's package
-catalog.
-
-To install, run the following command:
-```bash
-sudo apt install libopencv-dev libopencv-contrib-dev
-```
-
-### Install Catch2 (for unit testing)
-
-If you are on Ubuntu, you can use our Ubuntu package repository.
-Follow the instructions on <https://huskyroboticsteam.github.io/ubuntu-repo> first, and 
-then once you're finished, you can run
-```bash
-sudo apt install catch2
-```
-
-### Install URG library (Hokuyo lidar)
-
-On Ubuntu, make sure you have followed the
-[instructions to install our Ubuntu
-repo](https://huskyroboticsteam.github.io/ubuntu-repo) and then just run:
-```bash
-sudo apt install urg-lidar
-```
-
-If you need to actually access the hardware (e.g. you're provisioning the jetson, don't do this on your personal machine unless you need to)
-
-```bash
-sudo cp src/lidar/50-usb-hokuyo-lidar.rules /etc/udev/rules.d
-sudo udevadm control --reload-rules && sudo udevadm trigger
-```
-
-### Install RPLidar
-
-```bash
-sudo apt install rplidar
-```
-
-### Install [Eigen](http://eigen.tuxfamily.org)
-
-On Ubuntu, you will be able to install via
-
-```bash
-sudo apt-get install libeigen3-dev
-```
-
-### Install JSON library
-
-```bash
-sudo apt-get install nlohmann-json3-dev
-```
-
-### Install WebSocket library
-
-Websocket++ depends on a module from boost, so we'll need to install that too.
-```bash
-sudo apt install libwebsocketpp-dev libboost-system-dev
-```
-
-### Install GPS libraries
-
-
-On Ubuntu, make sure you have followed the
-[instructions to install our Ubuntu
-repo](https://huskyroboticsteam.github.io/ubuntu-repo) and then just run:
-
-```bash
-sudo apt-get install ublox-linux gpsd gpsd-clients libgps-dev
-```
-(**NOTE**: `gpsd` is still required for legacy purposes, but will likely be
-phased out when we completely switch to the new Ublox GPS hardware, so this
-command installs `ublox-linux` as well to interact with that. Even `ublox-linux`
-is a temporary solution; Electronics plans to integrate the GPS into a board
-which we will query over CAN.)
-
-### Install other libraries
-
-We need the Frozen library for making immutable compile-time constant
-containers, and argparse for parsing command-line arguments.
-
-On Ubuntu, make sure you have followed the
-[instructions to install our Ubuntu
-repo](https://huskyroboticsteam.github.io/ubuntu-repo) and then just run:
-
-```bash
-sudo apt-get install frozen libargparse-dev libavutil-dev libx264-dev
-```
-
-Done!
 
 # IDE Setup
 

src/Constants.h

@@ -56,17 +56,6 @@ constexpr double MAX_WHEEL_VEL = WHEEL_RADIUS * MAX_DRIVE_PWM / PWM_PER_RAD_PER_
 const double MAX_DTHETA = kinematics::DiffDriveKinematics(EFF_WHEEL_BASE)
 							  .wheelVelToRobotVel(-MAX_WHEEL_VEL, MAX_WHEEL_VEL)(2);
 
-// Joint limits
-// TODO: make sure these are still accurate with the new arm.
-constexpr double ARM_BASE_MIN = -M_PI / 2;
-constexpr double ARM_BASE_MAX = M_PI / 2;
-// constexpr double SHOULDER_MIN = M_PI / 2; // TODO mechanical problem with the moon gear.
-//                                             Use this value during actual rover operation.
-constexpr double SHOULDER_MIN = 0.0;
-constexpr double SHOULDER_MAX = M_PI * 5. / 6.; // Can't quite lie flat
-constexpr double ELBOW_MIN = 0.0;
-constexpr double ELBOW_MAX = M_PI * 29. / 30.; // I think this should prevent self-collisions
-
 // TODO: We need to recalibrate the camera, since we replaced it with a different one.
 // TODO: rename cameras (in MC as well) as appropriate
 constexpr const char* AR_CAMERA_CONFIG_PATH = "../camera-config/MastCameraCalibration.yml";
@@ -146,6 +135,10 @@ constexpr frozen::unordered_map<robot::types::jointid_t, robot::types::motorid_t
 
 // Arm inverse kinematics
 namespace arm {
+/**
+ * Percentage of fully extended overall arm length to limit arm extension within.
+ */
+constexpr double SAFETY_FACTOR = 0.95;
 
 /**
  * Maximum commanded end-effector velocity, in m/s

src/Globals.cpp

@@ -41,6 +41,7 @@ robot::types::mountedperipheral_t mountedPeripheral = robot::types::mountedperip
 const kinematics::PlanarArmKinematics<Constants::arm::IK_MOTORS.size()>
 	planarArmKinematics(getSegLens(), getJointLimits(true), getJointLimits(false),
 						IK_SOLVER_THRESH, IK_SOLVER_MAX_ITER);
-control::PlanarArmController<2> planarArmController({0, 0}, planarArmKinematics);
+control::PlanarArmController<2> planarArmController({0, 0}, planarArmKinematics,
+													Constants::arm::SAFETY_FACTOR);
 std::atomic<bool> armIKEnabled = false;
 } // namespace Globals

src/Rover.cpp

@@ -1,6 +1,5 @@
 #include "Constants.h"
 #include "Globals.h"
-#include "Util.h"
 #include "log.h"
 #include "navtypes.h"
 #include "network/MissionControlProtocol.h"

src/commands/DriveToWaypointCommand.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "../navtypes.h"
+#include "../utils/time.h"
 #include "../world_interface/data.h"
 #include "CommandBase.h"
 
@@ -11,44 +12,47 @@ namespace commands {
 
 /**
  * Provides robot drive velocity commands to navigate the robot to a given waypoint given
- * the velocity parameters to drive the robot at, the proportional factor to steer the robot with
- * and the 2D Cartesian position of the target in the world reference frame.
+ * the velocity parameters to drive the robot at, the proportional factor to steer the robot
+ * with and the 2D Cartesian position of the target in the world reference frame.
  */
 class DriveToWaypointCommand : CommandBase {
 public:
 	/**
 	 * Creates a new DriveToWaypointCommand with the specified targeting information.
-	 * 
+	 *
 	 * @param target, the position of the target in world coordinates.
 	 * @param thetaKP, the scaling factor to use for robot steering proportional control.
 	 * @param driveVel, the speed to drive the robot at.
 	 * @param slowDriveVel, the slower speed to drive the robot at when near the target.
 	 * @param doneThresh, the threshold for minimum robot-target distance to complete command.
-	 * @param closeToTargetDurVal, the duration within doneThresh after which robot navigation 
+	 * @param closeToTargetDurVal, the duration within doneThresh after which robot navigation
 	 * 							   is considered done, seconds.
 	 */
 	DriveToWaypointCommand(const navtypes::point_t& target, double thetaKP, double driveVel,
-						   double slowDriveVel, double doneThresh, util::dseconds closeToTargetDur);
+						   double slowDriveVel, double doneThresh,
+						   util::dseconds closeToTargetDur);
 
 	/**
-	 * Must be called before getOutput() for each iteration. Updates the current pose of the robot in 
-	 * the global reference frame used by DriveToWaypointCommand.
-	 * 
+	 * Must be called before getOutput() for each iteration. Updates the current pose of the
+	 * robot in the global reference frame used by DriveToWaypointCommand.
+	 *
 	 * @param pose, the current pose of the robot in the global reference frame.
 	 * @param time, the current time from robot::types::dataclock.
 	 */
 	void setState(const navtypes::pose_t& pose, robot::types::datatime_t time);
 
 	/**
-	 * Gets the angular velocity and forward speed to run the robot at in order to navigate robot to target.
-	 * 
-	 * @return the angular velocity and forward speed to run the robot at in order to navigate robot to target.
+	 * Gets the angular velocity and forward speed to run the robot at in order to navigate
+	 * robot to target.
+	 *
+	 * @return the angular velocity and forward speed to run the robot at in order to navigate
+	 * robot to target.
 	 */
 	command_t getOutput() override;
 
 	/**
 	 * Returns whether the DriveToWaypointCommand has finished its navigation task.
-	 * 
+	 *
 	 * @return whether the DriveToWaypointCommand has finished its navigation task.
 	 */
 	bool isDone() override;

src/control/JacobianVelController.h

@@ -1,9 +1,9 @@
 #pragma once
 
-#include "../Util.h"
 #include "../filters/StateSpaceUtil.h"
 #include "../navtypes.h"
 #include "../utils/math.h"
+#include "../utils/time.h"
 #include "../world_interface/data.h"
 
 #include <functional>
@@ -19,7 +19,8 @@
  * @tparam inputDim The dimension of the input vector of the kinematic function.
  * @see https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant
  */
-template <int outputDim, int inputDim> class JacobianVelController {
+template <int outputDim, int inputDim>
+class JacobianVelController {
 public:
 	/**
 	 * @brief Construct a new controller.

src/control/PIDControl.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../Util.h"
+#include "../utils/time.h"
 #include "../world_interface/data.h"
 
 #include <chrono>
@@ -27,9 +27,11 @@ struct PIDGains {
  * @tparam dim The number of dimensions to control.
  * @see https://en.wikipedia.org/wiki/PID_controller
  */
-template <int dim> class PIDController {
+template <int dim>
+class PIDController {
 public:
-	template <int d> using Arrayd = Eigen::Array<double, d, 1>;
+	template <int d>
+	using Arrayd = Eigen::Array<double, d, 1>;
 
 	/**
 	 * @brief Construct a new PIDController.