All actuation-related files are handled in the move_gun package. The main programs for the moving target actuation are shoot_moving.py, extrapolation.py, and shoot.py.
In order to shoot a moving target, we need to get some data on the target's movement to track its trajectory. To do this, we retrieve all the transforms between /base and /target_new with unique timestamps within a 0.5 second period, since a naive loop will retrieve the same transform multiple times before a newer target position has a chance to be published.
In order to perform extrapolation, we created a custom ExtrapolationQueue class, which takes a series of points in its queue, interpolates using Least Squares Regression to find the most probable path, and performs extrapolation given a certain time. By default, the class interpolates the data with a polynomial of degree 2.
First, we calculate the time T it would take for the arm to reach the last recorded position of the target, by letting the planner plan the path. We then use the extrapolation queue to extrapolate to the time T plus a time offset, which we set to 2.5 seconds, which should give enough time for the arm to execute the longer path and shoot the target in time.
Using the predicted target position, we can move the arm to the desired location. For moving targets, we always assume the target will be within the reachable workspace, which means the arm always moves to the location (X, Y, Z). The planner will plan and execute the motion plan, shoot the gun automatically, and open the gripper slightly to allow for reloading.