Repository navigation
Pan Tilt Attention
There will be 4 nodes involved in controlling the pan-tilt mechanism:
- A vision node which detects an object of attention.
- A PID attention node which maps the object pose to pan-tilt servo efforts.
- A pan-tilt controller that converts efforts into servo PWM targets.
- A servo driver that sends PWM targets to the hardware.
The vision node detects an object of interest in the visual field. It publishes intermittent messages describing an object of attention. The messages should include the 6-dof pose (with covariance) of the object relative to the sensor, and a single number characterizing how "interesting" the object is (i.e., how urgent it is to look at it). It should handle multiple objects of interest internally, such that it outputs only one pose at a time. For the pan-tilt only the position matters (not the orientation); however, we choose 6-dof messages for the API to work easily with AR markers and such.
- PoseWithCovariance of object of interest. Eventually should add the "interesting" parameter.
This node maps 6-DOF pose messages from the vision node to pan-tilt efforts.
It converts the position of the object to declination/ascension angles (relative to sensor normal). Assuming the sensor is aligned with the rotation axes, we can treat these angles as errors, feed them into PID controllers, which to get efforts. Finally, we need to squash the efforts to -1...1, which can be done with a sigmoid, or just by clipping the output.
Can add a calibration that determines the transform from the pan-tilt base to the sensor coordinates.
- the pan PID parameters
- the tilt PID parameters
- PoseWithCovariance (from vision node)
- Pan, tilt servo efforts, both floats from -1...1
Node which maps the effort -1...1 to a PWM target. This node should load up servo calibrations, since it needs to know the max/min PWM targets. It receives intermittent effort messages when a target is detected in a frame, but it publishes PWM targets continuously. It takes 2 parameters: a sensitivity parameter and a forget parameter. The effort messages provide acceleration in a particular direction, the forget parameter acts as friction, reducing the speed.
The node has variables to keep track of the current target, and a speed. Initialize servos to its home position, initialize speed to zero. Receipt of an effort message increments the speed by sensitivity X effort. In the publish function, the target is incremented by the speed, the target is set (using the driver's service) and then the speed is multiplied by the friction parameter. Thus, if the target is lost, the speed is exponentially damped. If the target is detected in every frame, it's a PID-controller, with effort representing torque.
It must smoothly reduce the speed when it nears the edge of the servos' range(s).
- friction (forget)
- sensitivity
- the pan, tilt pin numbers
- the pan, tilt servo calibrations
Note that the friction and sensitivity parameters should be scaled to account for the publishing rate.
- Pan, tilt servo efforts
It doesn't actually publish anything, but rather uses the servo driver's action server to set the pan/tilt targets.
The low-level driver to set the PWM target. In the case of the maestro, we also need to set hardware controlled speed and acceleration. I will set the speed and accel to 0 (unlimited), and trust the above nodes to implement appropriate control.
Note that the maestro ROS driver only works if the board mode is set to "USB dual port". This can be done in the Maestro control center > Serial Settings tab. Select "USB dual port" and click "Apply Settings." TODO: is there a way to do this programmatically?