Merge branch 'integration' of github.com:umrover/mrover-ros into inte…

…gration

config/esw.yaml

@@ -4,7 +4,7 @@ right_gps_driver:
   frame_id: "base_link"
 
 left_gps_driver:
-  port: "/dev/gps_0"
+  port: "/dev/gps"
   baud: 38400
   frame_id: "base_link"
 

config/localization.yaml

@@ -4,8 +4,8 @@ rover_frame: "base_link"
 use_odom_frame: false
 
 gps_linearization:
-  reference_point_latitude: 42.30008806193693
-  reference_point_longitude: -83.6931540297569
+  reference_point_latitude: 42.293195
+  reference_point_longitude: -83.7096706
   reference_point_altitude: 234.1
   reference_heading: 90.0
   use_both_gps: false

launch/autonomy.launch

@@ -3,7 +3,7 @@
   <arg name="run_object_detector" default="false" />
 
   <arg name="sim" default="false" />
-  <arg name="use_ekf" default="true" />
+  <arg name="use_ekf" default="false" />
   <arg name="ekf_start_delay" default="0" />
 
   <include file="$(find mrover)/launch/perception.launch">

src/esw/drive_bridge/main.cpp

@@ -4,6 +4,7 @@
 #include <can_device.hpp>
 #include <motors_group.hpp>
 #include <params_utils.hpp>
+#include <units/units.hpp>
 
 /*
  *  Converts from a Twist (linear and angular velocity) to the individual wheel velocities
@@ -16,14 +17,17 @@ void moveDrive(geometry_msgs::Twist::ConstPtr const& msg);
 ros::Publisher leftVelocityPub, rightVelocityPub;
 
 Meters WHEEL_DISTANCE_INNER;
+Meters WHEEL_DISTANCE_OUTER;
 compound_unit<Radians, inverse<Meters>> WHEEL_LINEAR_TO_ANGULAR;
 
 auto main(int argc, char** argv) -> int {
     ros::init(argc, argv, "drive_bridge");
     ros::NodeHandle nh;
 
     auto roverWidth = requireParamAsUnit<Meters>(nh, "rover/width");
+    auto roverLength = requireParamAsUnit<Meters>(nh, "rover/length");
     WHEEL_DISTANCE_INNER = roverWidth / 2;
+    WHEEL_DISTANCE_OUTER = sqrt((roverWidth / 2) * (roverWidth / 2) + (roverLength / 2) * (roverLength / 2));
 
     auto ratioMotorToWheel = requireParamAsUnit<Dimensionless>(nh, "wheel/gear_ratio");
     auto wheelRadius = requireParamAsUnit<Meters>(nh, "wheel/radius");
@@ -44,26 +48,38 @@ auto main(int argc, char** argv) -> int {
 
 void moveDrive(geometry_msgs::Twist::ConstPtr const& msg) {
     // See 13.3.1.4 in "Modern Robotics" for the math
-    // Link: https://hades.mech.northwestern.edu/images/7/7f/MR.pdf
+    // Link: https://hades.mech.northwestern.edu/images/7/7f/MR.pdf 
     auto forward = MetersPerSecond{msg->linear.x};
     auto turn = RadiansPerSecond{msg->angular.z};
     // TODO(quintin)    Don't ask me to explain perfectly why we need to cancel out a meters unit in the numerator
     //                  I think it comes from the fact that there is a unit vector in the denominator of the equation
-    auto delta = turn / Radians{1} * WHEEL_DISTANCE_INNER; // should be in m/s
-    RadiansPerSecond left = (forward - delta) * WHEEL_LINEAR_TO_ANGULAR;
-    RadiansPerSecond right = (forward + delta) * WHEEL_LINEAR_TO_ANGULAR;
+    auto delta_inner = turn / Radians{1} * WHEEL_DISTANCE_INNER; // should be in m/s
+    auto delta_outer = turn / Radians{1} * WHEEL_DISTANCE_OUTER;
+    RadiansPerSecond left_inner = (forward - delta_inner) * WHEEL_LINEAR_TO_ANGULAR;
+    RadiansPerSecond right_inner = (forward + delta_inner) * WHEEL_LINEAR_TO_ANGULAR;
+    RadiansPerSecond left_outer = (forward - delta_outer) * WHEEL_LINEAR_TO_ANGULAR;
+    RadiansPerSecond right_outer = (forward + delta_outer) * WHEEL_LINEAR_TO_ANGULAR;
+
+    constexpr auto MAX_SPEED = RadiansPerSecond{15 * 2 * M_PI};
+    RadiansPerSecond maximal = std::max(abs(left_outer), abs(right_outer));
+    if (maximal > MAX_SPEED) {
+        Dimensionless changeRatio = MAX_SPEED / maximal;
+        left_inner = left_inner * changeRatio;
+        right_inner = right_inner * changeRatio;
+        left_outer = left_outer * changeRatio;
+        right_outer = right_outer * changeRatio;
+    }
 
     {
         Velocity leftVelocity;
         leftVelocity.names = {"front_left", "middle_left", "back_left"};
-        leftVelocity.velocities.resize(leftVelocity.names.size(), left.get());
-        leftVelocity.velocities.resize(leftVelocity.names.size(), left.get());
+        leftVelocity.velocities = {left_outer.get(), left_inner.get(), left_outer.get()};
         leftVelocityPub.publish(leftVelocity);
     }
     {
         Velocity rightVelocity;
         rightVelocity.names = {"front_right", "middle_right", "back_right"};
-        rightVelocity.velocities.resize(rightVelocity.names.size(), right.get());
+        rightVelocity.velocities = {right_outer.get(), right_inner.get(), right_outer.get()};
         rightVelocityPub.publish(rightVelocity);
     }
 }

src/navigation/drive.py

@@ -10,6 +10,8 @@
 from util.np_utils import angle_to_rotate, normalized
 from util.ros_utils import get_rosparam
 
+import rospy
+
 default_constants = {
     "max_driving_effort": 1.0,
     "min_driving_effort": -1.0,
@@ -212,6 +214,8 @@ def get_drive_command(
         if drive_back:
             output[0].linear.x *= -1
 
+        rospy.logerr(f"output: {output}, target: {target_pos}, diff: {target_pos - rover_pos}")
+
         self._last_angular_error = angular_error
         self._last_target = target_pos
         return output