diff --git a/src/esw/arm_hw_bridge/main.cpp b/src/esw/arm_hw_bridge/main.cpp index 7a3762427..28480cc13 100644 --- a/src/esw/arm_hw_bridge/main.cpp +++ b/src/esw/arm_hw_bridge/main.cpp @@ -13,7 +13,6 @@ auto armLaserCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Respon } auto main(int argc, char** argv) -> int { - // Initialize the ROS node ros::init(argc, argv, "arm_hw_bridge"); ros::NodeHandle nh; @@ -23,7 +22,6 @@ auto main(int argc, char** argv) -> int { ros::ServiceServer armLaserService = nh.advertiseService("enable_arm_laser", armLaserCallback); - // Enter the ROS event loop ros::spin(); return EXIT_SUCCESS; diff --git a/src/esw/arm_translator_bridge/arm_translator.cpp b/src/esw/arm_translator_bridge/arm_translator.cpp index 8d1b91924..0be5f66fe 100644 --- a/src/esw/arm_translator_bridge/arm_translator.cpp +++ b/src/esw/arm_translator_bridge/arm_translator.cpp @@ -15,7 +15,7 @@ namespace mrover { Dimensionless static constexpr PITCH_ROLL_TO_01_SCALE{40}; // How often we send an adjust command to the DE motors - // This updates the quadrature motor source on the Moteus based on the absolute encoder readings + // This corrects the HALL-effect motor source on the Moteus based on the absolute encoder readings double static constexpr DE_OFFSET_TIMER_PERIOD = 1; ArmTranslator::ArmTranslator(ros::NodeHandle& nh) { @@ -136,6 +136,8 @@ namespace mrover { std::optional jointDe0Index = findJointByName(msg->name, "joint_de_0"), jointDe1Index = findJointByName(msg->name, "joint_de_1"); if (jointDe0Index && jointDe1Index) { + // The Moteus reports auxiliary motor positions in the range [0, tau) instead of [-pi, pi) + // Wrap to better align with IK conventions auto pitchWrapped = wrapAngle(static_cast(msg->position.at(jointDe0Index.value()))); auto rollWrapped = wrapAngle(static_cast(msg->position.at(jointDe1Index.value()))); mJointDePitchRoll = {pitchWrapped, rollWrapped}; diff --git a/src/esw/drive_bridge/main.cpp b/src/esw/drive_bridge/main.cpp index cc30f11b5..4bcbe86d5 100644 --- a/src/esw/drive_bridge/main.cpp +++ b/src/esw/drive_bridge/main.cpp @@ -5,6 +5,10 @@ #include #include +/* + * Converts from a Twist (linear and angular velocity) to the individual wheel velocities + */ + using namespace mrover; void moveDrive(geometry_msgs::Twist::ConstPtr const& msg); @@ -13,37 +17,26 @@ ros::Publisher leftVelocityPub, rightVelocityPub; Meters WHEEL_DISTANCE_INNER; compound_unit> WHEEL_LINEAR_TO_ANGULAR; -RadiansPerSecond MAX_MOTOR_SPEED; auto main(int argc, char** argv) -> int { - // Initialize the ROS node ros::init(argc, argv, "drive_bridge"); ros::NodeHandle nh; - // Load rover dimensions and other parameters from the ROS parameter server auto roverWidth = requireParamAsUnit(nh, "rover/width"); WHEEL_DISTANCE_INNER = roverWidth / 2; auto ratioMotorToWheel = requireParamAsUnit(nh, "wheel/gear_ratio"); auto wheelRadius = requireParamAsUnit(nh, "wheel/radius"); - // TODO(quintin) is dividing by ratioMotorToWheel right? WHEEL_LINEAR_TO_ANGULAR = Radians{1} / wheelRadius * ratioMotorToWheel; - auto maxLinearSpeed = requireParamAsUnit(nh, "rover/max_speed"); - assert(maxLinearSpeed > 0_mps); - - MAX_MOTOR_SPEED = maxLinearSpeed * WHEEL_LINEAR_TO_ANGULAR; - auto leftGroup = std::make_unique(nh, "drive_left"); auto rightGroup = std::make_unique(nh, "drive_right"); leftVelocityPub = nh.advertise("drive_left_velocity_cmd", 1); rightVelocityPub = nh.advertise("drive_right_velocity_cmd", 1); - // Subscribe to the ROS topic for drive commands auto twistSubscriber = nh.subscribe("cmd_vel", 1, moveDrive); - // Enter the ROS event loop ros::spin(); return 0; @@ -62,14 +55,14 @@ void moveDrive(geometry_msgs::Twist::ConstPtr const& msg) { { Velocity leftVelocity; leftVelocity.names = {"front_left", "middle_left", "back_left"}; - // TODO(quintin): Invert in firmware - leftVelocity.velocities = {-left.get(), -left.get(), -left.get()}; + left *= -1; // TODO(quintin): Invert in firmware instead + leftVelocity.velocities.resize(leftVelocity.names.size(), left.get()); leftVelocityPub.publish(leftVelocity); } { Velocity rightVelocity; rightVelocity.names = {"front_right", "middle_right", "back_right"}; - rightVelocity.velocities = {right.get(), right.get(), right.get()}; + rightVelocity.velocities.resize(rightVelocity.names.size(), right.get()); rightVelocityPub.publish(rightVelocity); } } diff --git a/src/esw/motor_library/linear_joint_translation.hpp b/src/esw/motor_library/linear_joint_translation.hpp deleted file mode 100644 index 1d7924ec3..000000000 --- a/src/esw/motor_library/linear_joint_translation.hpp +++ /dev/null @@ -1,15 +0,0 @@ -#pragma once - -#include - -namespace mrover { - - auto inline convertLinVel(float velocity, float multiplier) { - return velocity * multiplier; - } - - auto inline convertLinPos(float position, float multiplier) { - return position * multiplier; - } - -} // namespace mrover