Comments

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;

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<std::size_t> 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<float>(msg->position.at(jointDe0Index.value())));
             auto rollWrapped = wrapAngle(static_cast<float>(msg->position.at(jointDe1Index.value())));
             mJointDePitchRoll = {pitchWrapped, rollWrapped};

src/esw/drive_bridge/main.cpp

@@ -5,6 +5,10 @@
 #include <motors_group.hpp>
 #include <params_utils.hpp>
 
+/*
+ *  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<Radians, inverse<Meters>> 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<Meters>(nh, "rover/width");
     WHEEL_DISTANCE_INNER = roverWidth / 2;
 
     auto ratioMotorToWheel = requireParamAsUnit<Dimensionless>(nh, "wheel/gear_ratio");
     auto wheelRadius = requireParamAsUnit<Meters>(nh, "wheel/radius");
-    // TODO(quintin) is dividing by ratioMotorToWheel right?
     WHEEL_LINEAR_TO_ANGULAR = Radians{1} / wheelRadius * ratioMotorToWheel;
 
-    auto maxLinearSpeed = requireParamAsUnit<MetersPerSecond>(nh, "rover/max_speed");
-    assert(maxLinearSpeed > 0_mps);
-
-    MAX_MOTOR_SPEED = maxLinearSpeed * WHEEL_LINEAR_TO_ANGULAR;
-
     auto leftGroup = std::make_unique<MotorsGroup>(nh, "drive_left");
     auto rightGroup = std::make_unique<MotorsGroup>(nh, "drive_right");
 
     leftVelocityPub = nh.advertise<Velocity>("drive_left_velocity_cmd", 1);
     rightVelocityPub = nh.advertise<Velocity>("drive_right_velocity_cmd", 1);
 
-    // Subscribe to the ROS topic for drive commands
     auto twistSubscriber = nh.subscribe<geometry_msgs::Twist>("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);
     }
 }