diff --git a/src/TunePID.cpp b/src/TunePID.cpp index 9f6675d3..d0f4c46b 100644 --- a/src/TunePID.cpp +++ b/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 tp = steady_clock::now(); time_point startTime = tp; - while (steady_clock::now() - startTime < 3 * milliseconds((int)(period * 1000))) { + while (steady_clock::now() - startTime < + 3 * milliseconds(static_cast(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); diff --git a/src/ar/ARTester.cpp b/src/ar/ARTester.cpp index 866682dd..0f52006a 100644 --- a/src/ar/ARTester.cpp +++ b/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(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 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(imageSize.width), + pt.y * static_cast(imageSize.height)); cv::drawMarker(frame, newPt, cv::Scalar(255, 0, 0), cv::MARKER_CROSS, 10, 1); } } @@ -252,14 +252,15 @@ std::vector projectCube(float len, cv::Vec3d rvec, cv::Vec3d tvec) } std::vector projectGrid(cv::Size imageSize, int spacing) { - cv::Point2f center((float)imageSize.width / 2.0f, (float)imageSize.height / 2.0f); + cv::Point2f center(static_cast(imageSize.width) / 2.0f, + static_cast(imageSize.height) / 2.0f); std::vector grid_points; std::vector 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(x); + yf = static_cast(y); grid_points.push_back(cv::Point2f(xf, yf) + center); if (x != 0 || y != 0) { grid_points.push_back(cv::Point2f(-xf, -yf) + center); diff --git a/src/autonomous/AutonomousTask.cpp b/src/autonomous/AutonomousTask.cpp index e3067abe..82fb8839 100644 --- a/src/autonomous/AutonomousTask.cpp +++ b/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)); diff --git a/src/camera/CameraParams.cpp b/src/camera/CameraParams.cpp index 5cb41973..f83ea50e 100644 --- a/src/camera/CameraParams.cpp +++ b/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(file_node[KEY_IMAGE_WIDTH]); + h = static_cast(file_node[KEY_IMAGE_HEIGHT]); cv::Size size(w, h); // call init to do validation init(cam, dist, size); diff --git a/src/camera/calibration.cpp b/src/camera/calibration.cpp index 4e12c949..c36f3a37 100644 --- a/src/camera/calibration.cpp +++ b/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>& objectPoints, @@ -135,12 +143,12 @@ computeReprojectionErrors(const std::vector>& objectPoi double totalErr = 0, err; perViewErrors.resize(objectPoints.size()); - for (i = 0; i < (int)objectPoints.size(); i++) { + for (i = 0; i < static_cast(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(objectPoints[i].size()); + perViewErrors[i] = static_cast(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(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(rvecs.size()), 6, rvecs[0].type()); + for (int i = 0; i < static_cast(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(imagePoints.size()), + static_cast(imagePoints[0].size()), CV_32FC2); + for (int i = 0; i < static_cast(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(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(imageList.size())) view = cv::imread(imageList[i], 1); if (view.empty()) { @@ -563,9 +572,9 @@ 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, @@ -573,9 +582,10 @@ int main(int argc, char** argv) { if (mode == CAPTURING) { if (undistortImage) - msg = cv::format("%d/%d Undist", (int)imagePoints.size(), nframes); + msg = + cv::format("%d/%d Undist", static_cast(imagePoints.size()), nframes); else - msg = cv::format("%d/%d", (int)imagePoints.size(), nframes); + msg = cv::format("%d/%d", static_cast(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(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(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(cv::waitKey()); if (c == 27 || c == 'q' || c == 'Q') break; } diff --git a/src/control_interface.cpp b/src/control_interface.cpp index 79c91242..15b139cd 100644 --- a/src/control_interface.cpp +++ b/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)); diff --git a/src/kinematics/SwerveDriveKinematics.h b/src/kinematics/SwerveDriveKinematics.h index 05173719..2c282621 100644 --- a/src/kinematics/SwerveDriveKinematics.h +++ b/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 diff --git a/src/world_interface/simulator_interface.cpp b/src/world_interface/simulator_interface.cpp index f8348baf..cbeb1ae4 100644 --- a/src/world_interface/simulator_interface.cpp +++ b/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; } diff --git a/src/world_interface/world_interface.h b/src/world_interface/world_interface.h index e9044802..b9507cf9 100644 --- a/src/world_interface/world_interface.h +++ b/src/world_interface/world_interface.h @@ -11,7 +11,6 @@ #include #include -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 {