971 2019 software release

#1

971 is proud to release our 2019 software. We’ve been doing some cleanup which delayed the release. We had some great students this year.

http://frc971.org/content/2019-software

We had an on-field localizer which fused 5 IR cameras together and was used to follow trajectories and used in conjunction with our new spline code for our 2 disc cargo-ship auto. This was on top of the fast statespace controls for the superstructure.

We are in the process of revamping our pubsub system to make it more flexible. This should set us up for even higher levels of automation for the future. Watch out!

34 Likes
What teams have the most impressive software?
Making Code and Designs Public for 2020 season
#2

Long time fan, first time questioner. Can you provide some additional details about the IR localizer system?

Also, if you had a magic wand and could change the field to make systems like that easier for more teams to implement, what would you change?

3 Likes
#3

The IR portion just refers to the fact that the cameras had IR filters and we used IR lights for illumination, which we then used to detect the retroreflective targets on the field.

For localization, we wrote an Extended Kalman Filter to take the targets returned from all the cameras, the drivetrain encoders, and the yaw axis of the IMU as sensor inputs, along with the same basic model of the drivetrain that we’ve always used in our controls, to estimate the current state of the robot. If people are interested, the year-agnostic code for this is at frc971/control_loops/drivetrain/hybrid_ekf.h in the codebase, and the year-specific implementation (with the logic for handling the processed camera targets and the such) is at y2019/control_loops/drivetrain/localizer.h. The code for doing the vision processing is mostly in y2019/vision/. We are happy to answer more detailed questions about what it’s doing internally.

One of the biggest challenges we had this year was that all the targets look the same–in particular, when you see targets along the side of the cargo ship, you have to disambiguate them. If your current position estimate is off too much when you get camera readings (or the camera readings have noise or biases), then you latch onto the wrong set of targets, which is a difficult situation to address. There are certainly ways we could go about addressing that robot-side, but if we could change things field-side, there’s a few levels that I’d go for, depending on how many resources we had to change the field:

  • Given a magic wand, I’d probably ask for something like the zebra tracking system that was at Chezy Champs to be set up, and then provide a realtime feed, with timestamps, to the teams. Given good timestamps, that would provide an excellent way to prevent our localizer from ever getting too lost, although due to the latencies and reliance on WiFi, we’d still use other sensors to handle finer adjustments or outages.
  • More realistically, I’d ask to have a field where there are nearly always relatively easy to identify targets within view, wherever you are on the field (this year did a relatively good job of that). For years like 2019 where accuracy is important in scoring, having targets on the scoring locations themselves is valuable. However, instead of the retroreflective targets as they are, I’d probably prefer something like ArUco markers so that we could readily determine which target we were looking at. Given a sufficiently obstacle-free field (like in 2014), these goals might even be achieved by just having large markers mounted to the top of the driverstation wall in each corner of the field. Some WPI students tried out using ArUco markers to do FRC-like localization (https://digitalcommons.wpi.edu/mqp-all/4087/) a few years ago, and as I recall it worked out relatively well, although that was not in a competition environment.
6 Likes
#4

In lieu of something like the zebra tracking system provided by FIRST, teams could also probably implement a similar, but less effective, tracking system using a camera at their driver station

#5

Yeah, there’s various ways that you could try to make use of the driver’s station as a ground truth. However, depending on the year and the field setup, that’s going to be varying levels of difficulty. E.g., with the sandstorm this year, you probably couldn’t be running a camera (usefully) from the driverstation. One thing that I had been considering was the possibility of propping up a large target behind the driverstation glass, although that also doesn’t help with sandstorm, and still wouldn’t be as convenient as having the field built in a way that supported this things.

#6

We can dream.

#7

What’s your release workflow? What sort of requirements do you put in place before allowing code to go to “prod” (a match)?

1 Like
#8

Hi Connor,

There are 2 workflows worth talking about. There is our standard development workflow during the season, and there is the between match emergency workflow.

For the standard workflow, we follow a pretty standard process. We do basic in-person design reviews before people start. This is pretty light weight and mostly involves having the person discuss what they are going to do and make sure we are on the same page. For our overall superstructure design this year, this involved whiteboard sketches and a design proposal, which really helped get the team all moving in the same direction.

Once that happens, we do test driven development. We have gerrit and jenkins setup to do code reviews and run our continuous integration tests. These include full simulations of every subsystem. We maintain support for the all the robots that exist in the shop by continuing to run the tests for each robot and keep them up to date in our monorepo.

Once this finishes, we start testing on a real robot. From there, it is all about trying to make sure we’ve seen all the cases. We don’t have great tests for the joystick interface and autonomous, so these have to get developed and tested on the real bot. It isn’t too hard to build up a mental code coverage model and make sure you’ve seen all the code paths run. For autonomous, this means 10 perfect runs in a row, minimum.

We haven’t set up any official project management tools. My personal preferences for projects this small is to keep it all in people’s heads, and maybe keep a small spreadsheet. The project lead checks in daily or every other day with everyone, keeps track of the critical path, and re-allocate resources as needed. Bug tracking works the same way. The critical bugs get fixed as they get found, and we only really can remember the top 10 bugs. Anything else wasn’t important enough. Some stuff slips through the cracks, but it largely works, and doesn’t consume a bunch of time. This only works if you have good people who work well together and a simple project.

At competition, we have to short circuit a lot of this because everything needs to move fast. :slight_smile: We don’t have much time between matches to make changes. Things tend to start with a small risk assessment. We pretty much are asking the question of what could go wrong with a change, and what will happen if we don’t make the change. This helps inform us of how thorough to be or if we should hold off until we have the time to properly test it. If it is a simple calibration change, we’ll just do it, confirm with logs that the cal change took, and go. If it is a logic change, we have to think it through more. We’ll likely do an informal code review on the spot or pair program it, and then either run the unit tests locally (rarely), or do a functional test to confirm. We won’t push code and run in a match without a functional test at a minimum.

8 Likes
#9

But running an Auton you’ve tested before is no fun. :wink:

1 Like
#10

It’s cool to see your on-the-fly DARE solver get utilized more since it was introduced in 2018, this time for drivetrain control. What kind of solve times have you been seeing with it? I’ve been thinking of adding one (specifically https://drake.mit.edu/ 's) to wpilib at some point for use with the 2020 system identification stuff, and a ballpark for ideal solve times would be helpful.

How did your cubic spline trajectory impl work out for you? I noticed there were some todos regarding more accurate drivetrain constraints. Also, did 5th order Gaussian quadrature provide a noticeable difference in integration accuracy over RK4?

The EKF impl and target selection seemed like a pretty standard and sensible approach to me, but this interface could use a bit of refactoring. :slight_smile:

  void Correct(
      const Output &z, const Input *U,
      ::std::function<
          void(const State &, const StateSquare &,
               ::std::function<Output(const State &, const Input &)> *,
               ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
                   const State &)> *)>
          make_h,
      ::std::function<Output(const State &, const Input &)> h,
      ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
          dhdx,
      const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
      aos::monotonic_clock::time_point t);

That has the potential to incur a lot of heap allocations in std::function on every call if you’re not careful (like capturing a lot of stuff in lambda closures). On that note, do you guys do any resource utilization profiling (task execution duration, etc.) to catch potential issues in the hotpath?

#11

We are seeing like 20% CPU usage with the full EKF running. I remember something like 3-6% last year for the arm with the on-the-fly DARE solver. I’m going to say that it takes negligible CPU to compute, and that was for a 4 state system.

The cubic spline trajectory implementation is working really well. I think we are seeing 1mm or so error for a 1+ meter simulated move. So it is doing a very good job at inverting the dynamics and solving for the constraints.

RK4 is for ODEs. Gaussian Quadrature is for integrating equations. I can’t remember doing extensive analysis on the correct order, other than a 5th order quadrature was fast enough and good to significantly less than 1 mm path length. So I stopped doing analysis.

You are missing that that is a templated class, and therefore a templated function, and the definition is available in the header. There is a very high likelihood that the compiler will inline the whole mess. If it were in a separate compilation unit, you would be right.

We take a reasonable amount of care to make sure we are doing things deterministicly when designing them so we don’t have surprise hot spots. Then, we run top and see what the CPU usage looks like and make sure we have some headroom on the CPU. I think the drivetrain came out at like 20-30% CPU, which was good enough.

I’d like to beef up our run time analysis a bit more. I’d like to add some handler runtime and scheduling latency logging, and also add detection for any mallocs when running RT.

#12

Since a lot of stuff is in headers due to matrix dimension templating, what kind of compilation times do you see for fresh and incremental builds? I ask because I made a controls library recently with DARE, LQR, EKF, and UKF support and it takes like 10+ minutes for the subsystems using it to compile (1 minute for incremental on just controller classes). I was wondering if that’s typical for template-heavy stuff like this using Eigen or if I need to figure out some source-level build optimizations. For reference, this was on a Thinkpad X280 using a makefile, because Gradle takes three times as long and uses twice the RAM.

Edit: Thinking about it more, having a unique class for each subsystem’s EKF impl as opposed to a generic templated EKF might be helping compilation times.

#13

The core libraries take ages. I think it is something like 6k fortran -> C files that need to be built. That ends up at about 10 minutes for us. Eigen from there doesn’t add much. We don’t include the DARE solvers in headers that get propagated very far.

Our drivetrain takes like 40 seconds right now to compile. Annoying, but not bad enough to do anything about it.

I keep hearing that C++ modules will solve world hunger but haven’t investigated any.

3 Likes
#14

I have clang cross-compilation working (as well as llvm’s libc++ compiled) for the Rio, so if you want to try clang’s very experimental C++ modules TS support (or any other clang C++2a features), just let me know and I’ll set you up…

2 Likes
#15

  1. What specific information did your Jevois feed the EKF, target vectors and pose?

  2. Also how did you develop your EKF, are there any resources you used or anything you would have done differently than you did?

  3. How do you like making CV software on the Jevois? Why do you like them?

  4. Why do you use SPI instead of UART to communicate to the RoboRIO from the teensy?

Thanks!
-1311

#16

The inputs were the “raw” measurements from the camera. This was the computed angle to the target and distance to the target. These were the result of using the ceres solver to back out the extrinsics.

We built a simulator and started playing. It was kinda cool. You can simulate noise, etc, and make sure your algorithms are sane.

We really needed better log replay. When something went wrong in the field, it was hard to replay the log files and rerun algorithms. We have spent the off-season building up better infrastructure to support that.

The EKF is pretty much just off wikipedia. Nothing special.

Cheap, small, and you have a MIPI in image sensor. We will be switching off the Jevois next year very likely and looking at the Pi4.

They were actually pretty annoying to work with. The CPU performance isn’t great to start with, and deploying code involved unplugging the cameras and plugging them into a laptop.

It is a bandwidth and latency problem. We know how to tune the roboRIO to work well with SPI.

30 fps * 5 cameras * 41 bytes/camera frame -> 6kbyte/sec. This is over 1/2 of the max serial speed available.

The max speed for serial is 115200 kbit/s, and SPI is 8 mbit/s. We were a bit uncertain when we started about how much data we needed.

1 Like