[WIP] Calibrate KJS force sensor

[708yamaguchi]

Description

This PR implements calibration, unit conversion (ADC to Newton), and drift detection for the KJS force sensors (Alps Alpine HSFPAR003A).

Previously, the bridge only published raw ADC values for individual sensor elements. This update aggregates the 4-element sensor values, applies a conversion formula based on the hardware specification, and provides a mechanism to zero-out the preload via a ROS service.

Key Changes

1. Unit Conversion (ADC to Newton)

• Implemented convert_adc_to_newton method.
• Parameters:
  • Sensor: Alps Alpine HSFPAR003A
  • Gain Resistor ($R_G$): 15 kΩ (resulting in Gain $\approx$ 7.67)
  • Sensitivity: 3.7 mV/V/N
  • ADC: 12-bit resolution at 3.3V Vdd

2. Calibration Service

• Service Name: ~calibrate_force_sensor (std_srvs/Trigger)
• Logic:
  • Uses the internal history buffer (recent_adc_history) to calculate the average offset for each of the 4 sensor elements.
  • Optimization: Uses cached history instead of blocking serial_call_with_retry for faster response.
  • Safety: Checks if the sensor values are too low (threshold < 10.0) to ensure proper preload is applied before calibrating.

3. Drift Detection (Warmup Check)

• Implemented a background timer (drift_check_callback) running at 1-minute intervals.
• Checks if the average force has drifted by more than 0.1 N compared to the previous minute.
• Publishes the status to .../force_drifted to indicate if the sensor is still warming up.

4. Updated Topics
   For each sensor ID (e.g., /kjs/19/...), the following topics are now available:

Topic Suffix	Message Type	Description
/force	WrenchStamped	Raw sum of ADC values (always published).
/force_corrected	WrenchStamped	Net ADC sum (Raw - Offset). Only published after calibration.
/force_N	WrenchStamped	Force converted to Newtons. Only published after calibration.
/force_drifted	Bool (Latched)	True if drift > 0.1N/min (unstable), False otherwise.

Notes

Sensor Warm-up: The force sensor values (HSFPAR003A) tend to drift immediately after power-on due to initial temperature changes. It typically takes about 10 minutes for the values to stabilize. Please wait for the force_drifted topic to become False before performing critical calibration.

How to Test

1. Launch the bridge node.
2. Monitor /kjs/{id}/force to see raw values.
3. Call the service: rosservice call /rcb4_ros_bridge/calibrate_force_sensor.
4. Verify that /kjs/{id}/force_N is now being published and is near 0.0 N.
5. Monitor /kjs/{id}/force_drifted to check the stability of the sensor.

[github-actions]

🎬️ Watch Hardware CI Test Video