Merge branch 'master' into Elevation-data

src/TunePID.cpp

@@ -18,7 +18,8 @@ extern "C" {
 }
 
 enum class targetmode_t {
-	step, sinusoidal
+	step,
+	sinusoidal
 };
 
 using namespace robot::types;
@@ -145,7 +146,8 @@ int main(int argc, char** argv) {
 
 	time_point<steady_clock> tp = steady_clock::now();
 	time_point<steady_clock> startTime = tp;
-	while (steady_clock::now() - startTime < 3 * milliseconds((int)(period * 1000))) {
+	while (steady_clock::now() - startTime <
+		   3 * milliseconds(static_cast<int>(period * 1000))) {
 		int32_t current_angle = can::motor::getMotorPosition(serial).getData();
 		double difference = (current_angle - angle_target) / 1000.0;
 		acc_error += difference * difference;
@@ -159,7 +161,7 @@ int main(int argc, char** argv) {
 		if (mode == targetmode_t::step) {
 			prescaled_target = round(prescaled_target);
 		}
-		angle_target = (int32_t) round(amplitude * prescaled_target) + starting_angle;
+		angle_target = (int32_t)round(amplitude * prescaled_target) + starting_angle;
 
 		can::motor::setMotorPIDTarget(serial, angle_target);
 

src/ar/ARTester.cpp

@@ -97,7 +97,7 @@ int main(int argc, char* argv[]) {
 	if (!cam_config[cam::KEY_FILENAME].empty() && !parser.has("fo")) {
 		cam_file = (std::string)cam_config[cam::KEY_FILENAME];
 	} else if (!cam_config[cam::KEY_CAMERA_ID].empty() && !parser.has("co")) {
-		cam_id = (int)cam_config[cam::KEY_CAMERA_ID];
+		cam_id = static_cast<int>(cam_config[cam::KEY_CAMERA_ID]);
 	} else if (!parser.has("fo") && !parser.has("co")) {
 		std::cerr << "Error: no file or camera_id was provided in the configuration file or "
 					 "on the command line!"
@@ -196,8 +196,8 @@ int main(int argc, char* argv[]) {
 		if (show_grid) {
 			std::vector<cv::Point2f> grid = projectGrid(imageSize, grid_spacing);
 			for (cv::Point2f pt : grid) {
-				cv::Point2f newPt(pt.x * (float)imageSize.width,
-								  pt.y * (float)imageSize.height);
+				cv::Point2f newPt(pt.x * static_cast<float>(imageSize.width),
+								  pt.y * static_cast<float>(imageSize.height));
 				cv::drawMarker(frame, newPt, cv::Scalar(255, 0, 0), cv::MARKER_CROSS, 10, 1);
 			}
 		}
@@ -252,14 +252,15 @@ std::vector<cv::Point2d> projectCube(float len, cv::Vec3d rvec, cv::Vec3d tvec)
 }
 
 std::vector<cv::Point2f> projectGrid(cv::Size imageSize, int spacing) {
-	cv::Point2f center((float)imageSize.width / 2.0f, (float)imageSize.height / 2.0f);
+	cv::Point2f center(static_cast<float>(imageSize.width) / 2.0f,
+					   static_cast<float>(imageSize.height) / 2.0f);
 	std::vector<cv::Point2f> grid_points;
 	std::vector<cv::Point2f> projected_points;
 	float xf, yf;
 	for (int x = 0; x < imageSize.width / 2; x += spacing) {
 		for (int y = 0; y < imageSize.height / 2; y += spacing) {
-			xf = (float)x;
-			yf = (float)y;
+			xf = static_cast<float>(x);
+			yf = static_cast<float>(y);
 			grid_points.push_back(cv::Point2f(xf, yf) + center);
 			if (x != 0 || y != 0) {
 				grid_points.push_back(cv::Point2f(-xf, -yf) + center);

src/autonomous/AutonomousTask.cpp

@@ -36,7 +36,7 @@ void AutonomousTask::navigate() {
 										 SLOW_DRIVE_THRESHOLD, DONE_THRESHOLD,
 										 CLOSE_TO_TARGET_DUR_VAL);
 
-	DiffDriveKinematics diffDriveKinematics(Constants::EFF_WHEEL_BASE);
+	kinematics::DiffDriveKinematics diffDriveKinematics(Constants::EFF_WHEEL_BASE);
 
 	auto sleepUntil = std::chrono::steady_clock().now();
 	while (!cmd.isDone()) {
@@ -50,8 +50,8 @@ void AutonomousTask::navigate() {
 			cmd.setState(latestPos, robot::types::dataclock::now());
 			commands::command_t output = cmd.getOutput();
 			auto scaledVels = diffDriveKinematics.ensureWithinWheelSpeedLimit(
-				DiffDriveKinematics::PreferredVelPreservation::PreferThetaVel, output.xVel,
-				output.thetaVel, Constants::MAX_WHEEL_VEL);
+				kinematics::DiffDriveKinematics::PreferredVelPreservation::PreferThetaVel,
+				output.xVel, output.thetaVel, Constants::MAX_WHEEL_VEL);
 			navtypes::point_t relTarget = util::toTransform(latestPos) * _waypoint_coords;
 			LOG_F(INFO, "Relative Target: (%lf, %lf)", relTarget(0), relTarget(1));
 			LOG_F(INFO, "thetaVel: %lf", scaledVels(2));

src/camera/CameraParams.cpp

@@ -93,8 +93,8 @@ void CameraParams::readFromFileNode(const cv::FileNode& file_node) {
 	file_node[KEY_CAMERA_MATRIX] >> cam;
 	file_node[KEY_DIST_COEFFS] >> dist;
 	int w, h;
-	w = (int)file_node[KEY_IMAGE_WIDTH];
-	h = (int)file_node[KEY_IMAGE_HEIGHT];
+	w = static_cast<int>(file_node[KEY_IMAGE_WIDTH]);
+	h = static_cast<int>(file_node[KEY_IMAGE_HEIGHT]);
 	cv::Size size(w, h);
 	// call init to do validation
 	init(cam, dist, size);

src/camera/calibration.cpp

@@ -121,8 +121,16 @@ static void help(char** argv) {
 	printf("\n%s", liveCaptureHelp);
 }
 
-enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };
-enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
+enum {
+	DETECTION = 0,
+	CAPTURING = 1,
+	CALIBRATED = 2
+};
+enum Pattern {
+	CHESSBOARD,
+	CIRCLES_GRID,
+	ASYMMETRIC_CIRCLES_GRID
+};
 
 static double
 computeReprojectionErrors(const std::vector<std::vector<cv::Point3f>>& objectPoints,
@@ -135,12 +143,12 @@ computeReprojectionErrors(const std::vector<std::vector<cv::Point3f>>& objectPoi
 	double totalErr = 0, err;
 	perViewErrors.resize(objectPoints.size());
 
-	for (i = 0; i < (int)objectPoints.size(); i++) {
+	for (i = 0; i < static_cast<int>(objectPoints.size()); i++) {
 		projectPoints(cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix, distCoeffs,
 					  imagePoints2);
 		err = norm(cv::Mat(imagePoints[i]), cv::Mat(imagePoints2), cv::NORM_L2);
-		int n = (int)objectPoints[i].size();
-		perViewErrors[i] = (float)std::sqrt(err * err / n);
+		int n = static_cast<int>(objectPoints[i].size());
+		perViewErrors[i] = static_cast<float>(std::sqrt(err * err / n));
 		totalErr += err * err;
 		totalPoints += n;
 	}
@@ -264,7 +272,7 @@ saveCameraParams(const std::string& filename, cv::Size imageSize, cv::Size board
 		fs << "per_view_reprojection_errors" << cv::Mat(reprojErrs);
 
 	if (!rvecs.empty() || !reprojErrs.empty())
-		fs << "nframes" << (int)std::max(rvecs.size(), reprojErrs.size());
+		fs << "nframes" << static_cast<int>(std::max(rvecs.size(), reprojErrs.size()));
 
 	if (flags & cv::CALIB_FIX_ASPECT_RATIO)
 		fs << "aspectRatio" << aspectRatio;
@@ -293,8 +301,8 @@ saveCameraParams(const std::string& filename, cv::Size imageSize, cv::Size board
 
 	if (!rvecs.empty() && !tvecs.empty()) {
 		CV_Assert(rvecs[0].type() == tvecs[0].type());
-		cv::Mat bigmat((int)rvecs.size(), 6, rvecs[0].type());
-		for (int i = 0; i < (int)rvecs.size(); i++) {
+		cv::Mat bigmat(static_cast<int>(rvecs.size()), 6, rvecs[0].type());
+		for (int i = 0; i < static_cast<int>(rvecs.size()); i++) {
 			cv::Mat r = bigmat(cv::Range(i, i + 1), cv::Range(0, 3));
 			cv::Mat t = bigmat(cv::Range(i, i + 1), cv::Range(3, 6));
 
@@ -310,8 +318,9 @@ saveCameraParams(const std::string& filename, cv::Size imageSize, cv::Size board
 	}
 
 	if (!imagePoints.empty()) {
-		cv::Mat imagePtMat((int)imagePoints.size(), (int)imagePoints[0].size(), CV_32FC2);
-		for (int i = 0; i < (int)imagePoints.size(); i++) {
+		cv::Mat imagePtMat(static_cast<int>(imagePoints.size()),
+						   static_cast<int>(imagePoints[0].size()), CV_32FC2);
+		for (int i = 0; i < static_cast<int>(imagePoints.size()); i++) {
 			cv::Mat r = imagePtMat.row(i).reshape(2, imagePtMat.cols);
 			cv::Mat imgpti(imagePoints[i]);
 			imgpti.copyTo(r);
@@ -489,7 +498,7 @@ int main(int argc, char** argv) {
 		return fprintf(stderr, "Could not initialize video (%d) capture\n", cameraId), -2;
 
 	if (!imageList.empty())
-		nframes = (int)imageList.size();
+		nframes = static_cast<int>(imageList.size());
 
 	if (capture.isOpened())
 		printf("%s", liveCaptureHelp);
@@ -509,7 +518,7 @@ int main(int argc, char** argv) {
 			cv::Mat view0;
 			capture >> view0;
 			view0.copyTo(view);
-		} else if (i < (int)imageList.size())
+		} else if (i < static_cast<int>(imageList.size()))
 			view = cv::imread(imageList[i], 1);
 
 		if (view.empty()) {
@@ -563,19 +572,20 @@ int main(int argc, char** argv) {
 		if (found)
 			drawChessboardCorners(view, boardSize, cv::Mat(pointbuf), found);
 
-		std::string msg = mode == CAPTURING
-							  ? "100/100"
-							  : mode == CALIBRATED ? "Calibrated" : "Press 'g' to start";
+		std::string msg = mode == CAPTURING	   ? "100/100"
+						  : mode == CALIBRATED ? "Calibrated"
+											   : "Press 'g' to start";
 		int baseLine = 0;
 		cv::Size textSize = cv::getTextSize(msg, 1, 1, 1, &baseLine);
 		cv::Point textOrigin(view.cols - 2 * textSize.width - 10,
 							 view.rows - 2 * baseLine - 10);
 
 		if (mode == CAPTURING) {
 			if (undistortImage)
-				msg = cv::format("%d/%d Undist", (int)imagePoints.size(), nframes);
+				msg =
+					cv::format("%d/%d Undist", static_cast<int>(imagePoints.size()), nframes);
 			else
-				msg = cv::format("%d/%d", (int)imagePoints.size(), nframes);
+				msg = cv::format("%d/%d", static_cast<int>(imagePoints.size()), nframes);
 		}
 
 		putText(view, msg, textOrigin, 1, 1,
@@ -590,7 +600,7 @@ int main(int argc, char** argv) {
 		}
 
 		imshow("Image View", view);
-		char key = (char)cv::waitKey(capture.isOpened() ? 50 : 500);
+		char key = static_cast<char>(cv::waitKey(capture.isOpened() ? 50 : 500));
 
 		if (key == 27)
 			break;
@@ -622,14 +632,14 @@ int main(int argc, char** argv) {
 			getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0),
 			imageSize, CV_16SC2, map1, map2);
 
-		for (i = 0; i < (int)imageList.size(); i++) {
+		for (i = 0; i < static_cast<int>(imageList.size()); i++) {
 			view = cv::imread(imageList[i], 1);
 			if (view.empty())
 				continue;
 			// undistort( view, rview, cameraMatrix, distCoeffs, cameraMatrix );
 			remap(view, rview, map1, map2, cv::INTER_LINEAR);
 			imshow("Image View", rview);
-			char c = (char)cv::waitKey();
+			char c = static_cast<char>(cv::waitKey());
 			if (c == 27 || c == 'q' || c == 'Q')
 				break;
 		}

src/control_interface.cpp

@@ -24,7 +24,7 @@ using namespace std::chrono_literals;
 
 namespace robot {
 namespace {
-jointpos_t commandedWristPos{0, 0};
+kinematics::jointpos_t commandedWristPos{0, 0};
 std::mutex wristPosMutex;
 
 void setJointMotorPower(robot::types::jointid_t joint, double power);
@@ -57,7 +57,7 @@ double setCmdVel(double dtheta, double dx) {
 		}
 	}
 
-	wheelvel_t wheelVels = driveKinematics().robotVelToWheelVel(dx, dtheta);
+	kinematics::wheelvel_t wheelVels = driveKinematics().robotVelToWheelVel(dx, dtheta);
 	double lPWM = wheelVels.lVel / Constants::MAX_WHEEL_VEL;
 	double rPWM = wheelVels.rVel / Constants::MAX_WHEEL_VEL;
 	double maxAbsPWM = std::max(std::abs(lPWM), std::abs(rPWM));
@@ -92,7 +92,7 @@ double setTankCmdVel(double left, double right) {
 	if (!Globals::swerveController.checkWheelRotation(DriveMode::Normal, curr_wheel_rots))
 		return 0;
 
-	wheelvel_t wheelVels = {left, right};
+	kinematics::wheelvel_t wheelVels = {left, right};
 	double lPWM = wheelVels.lVel / Constants::MAX_WHEEL_VEL;
 	double rPWM = wheelVels.rVel / Constants::MAX_WHEEL_VEL;
 	double maxAbsPWM = std::max(std::abs(lPWM), std::abs(rPWM));

src/kinematics/SwerveDriveKinematics.h

@@ -3,7 +3,6 @@
 #include "../navtypes.h"
 
 namespace kinematics {
-using namespace navtypes;
 
 /**
  * Represents robot wheel velocities in robot reference frame,
@@ -61,7 +60,7 @@ class SwerveDriveKinematics {
 	 * robot's reference frame.
 	 * @return the robot's translational and angular velocity in the robot's reference frame.
 	 */
-	pose_t wheelVelToRobotVel(swervewheelvel_t wheelVel) const;
+	navtypes::pose_t wheelVelToRobotVel(swervewheelvel_t wheelVel) const;
 
 	/**
 	 * Calculate the pose update in the local reference frame given the velocities of the

src/world_interface/simulator_interface.cpp

@@ -249,10 +249,10 @@ void initSimServer(net::websocket::SingleClientWSServer& ws) {
 namespace robot {
 
 namespace {
-DiffDriveKinematics drive_kinematics(Constants::EFF_WHEEL_BASE);
+kinematics::DiffDriveKinematics drive_kinematics(Constants::EFF_WHEEL_BASE);
 } // namespace
 
-const DiffDriveKinematics& driveKinematics() {
+const kinematics::DiffDriveKinematics& driveKinematics() {
 	return drive_kinematics;
 }
 

src/world_interface/world_interface.h

@@ -11,7 +11,6 @@
 #include <optional>
 #include <unordered_set>
 
-using namespace kinematics;
 // forward declare cam::CameraParams instead of including it
 // we do this to avoid unnecessarily including OpenCV in all build targets
 namespace cam {