payload_deliverer & gripper: Improve intermediate state & vcmd_ack

Gripper:
- Don't command gripper (via uORB `gripper` topic, which maps into an
actuator via Control Allocation) if we are already at the state we want
(e.g. Grabbed / Released) or in the intermediate state to the state we
want -> This prevents spamming on `gripper` topic

Payload Deliverer:
- Add read-once function for Gripper's released / grabbed state
- Send vehicle_command_ack for both release/grab actions.

TODO: target_system & target_component for the released/grabbed vcmd_ack
is incomplete, since we are not keeping track of the vehicle_command
that corresponds to this. This needs to be dealt with in the future, not
sure what the best solution it is for now.

Possible solutions:
- Queue-ing the vehicle command?
- Tying the gripper's action to specific vehicle command one-on-one, to make sure if we send multiple vehicle commands, we know
which command resulted in the action exactly?)

src/modules/payload_deliverer/gripper.cpp

@@ -68,6 +68,10 @@ void Gripper::init(const GripperConfig &config)
 
 void Gripper::grab()
 {
+	if (_state == GripperState::GRABBING || _state == GripperState::GRABBED) {
+		return;
+	}
+
 	publish_gripper_command(gripper_s::COMMAND_GRAB);
 	_state = GripperState::GRABBING;
 	_last_command_time = hrt_absolute_time();
@@ -75,6 +79,10 @@ void Gripper::grab()
 
 void Gripper::release()
 {
+	if (_state == GripperState::RELEASING || _state == GripperState::RELEASED) {
+		return;
+	}
+
 	publish_gripper_command(gripper_s::COMMAND_RELEASE);
 	_state = GripperState::RELEASING;
 	_last_command_time = hrt_absolute_time();
@@ -89,11 +97,13 @@ void Gripper::update()
 	case GripperState::GRABBING:
 		if (_has_feedback_sensor) {
 			// Handle feedback sensor input, return true for now (not supported)
+			_grabbed_state_cache = true;
 			_state = GripperState::GRABBED;
 			break;
 		}
 
 		if (command_timed_out) {
+			_grabbed_state_cache = true;
 			_state = GripperState::GRABBED;
 		}
 

src/modules/payload_deliverer/gripper.h

@@ -99,9 +99,17 @@ public:
 	// Returns true if in released position either sensed or timeout based
 	bool released() { return _state == GripperState::RELEASED; }
 
-	// Returns true only once after the state transitioned into released
-	// Used for sending vehicle command ack only when the gripper actually releases
-	// in payload deliverer
+	/**
+	 * @brief Returns true once after gripper succeededs in releasing the payload
+	 *
+	 * Note, that this is a single-read for a one successful release operation. Meaning that
+	 * if you command `release()` and then call this function multiple times in the future,
+	 * it will only return `true` only once, and then it will return `false` after it returns
+	 * `true` once.
+	 *
+	 * This is used to detect a single event of successful releasing of a gripper in `payload_deliverer`
+	 * module, which then would send `vehicle_command_ack` message, indicating the successful release.
+	 */
 	bool released_read_once()
 	{
 		if (_released_state_cache) {
@@ -113,6 +121,20 @@ public:
 		}
 	}
 
+	/**
+	 * @brief Returns true once after gripper succeededs in grabbing the payload
+	 */
+	bool grabbed_read_once()
+	{
+		if (_grabbed_state_cache) {
+			_grabbed_state_cache = false;
+			return true;
+
+		} else {
+			return false;
+		}
+	}
+
 	// Returns true if Gripper output function is assigned properly
 	bool is_valid() const { return _valid; }
 
@@ -128,7 +150,10 @@ private:
 	// Internal state
 	GripperState _state{GripperState::IDLE};
 	hrt_abstime _last_command_time{0};
-	bool _released_state_cache{false}; // Cached flag that is set once gripper release was successful, gets reset once read
+
+	// Cached flag that is set once gripper release/grab was successful, it must get reset when read.
+	bool _released_state_cache{false};
+	bool _grabbed_state_cache{false};
 
 	uORB::Publication<gripper_s> _gripper_pub{ORB_ID(gripper)};
 };

src/modules/payload_deliverer/payload_deliverer.cpp

@@ -131,7 +131,7 @@ void PayloadDeliverer::gripper_update(const hrt_abstime &now)
 
 	_gripper.update();
 
-	// Publish a successful gripper release acknowledgement
+	// Publish a successful gripper release / grab acknowledgement
 	if (_gripper.released_read_once()) {
 		vehicle_command_ack_s vcmd_ack{};
 		vcmd_ack.timestamp = now;
@@ -140,7 +140,18 @@ void PayloadDeliverer::gripper_update(const hrt_abstime &now)
 		// Ideally, we need to fill out target_system and target_component to match the vehicle_command we are acknowledging for
 		// But since we are not tracking the vehicle command's source system & component, we don't fill it out for now.
 		_vehicle_command_ack_pub.publish(vcmd_ack);
-		PX4_DEBUG("Payload Drop Successful Ack Sent!");
+		PX4_DEBUG("Payload Release Successful Ack Sent!");
+
+	} else if (_gripper.grabbed_read_once()) {
+		vehicle_command_ack_s vcmd_ack{};
+		vcmd_ack.timestamp = now;
+		vcmd_ack.command = vehicle_command_s::VEHICLE_CMD_DO_GRIPPER;
+		vcmd_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+		// Ideally, we need to fill out target_system and target_component to match the vehicle_command we are acknowledging for
+		// But since we are not tracking the vehicle command's source system & component, we don't fill it out for now.
+		_vehicle_command_ack_pub.publish(vcmd_ack);
+		PX4_DEBUG("Payload Grab Successful Ack Sent!");
+
 	}
 }