diff --git a/src/modules/payload_deliverer/gripper.cpp b/src/modules/payload_deliverer/gripper.cpp index 323929648f..5192beba4f 100644 --- a/src/modules/payload_deliverer/gripper.cpp +++ b/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; } diff --git a/src/modules/payload_deliverer/gripper.h b/src/modules/payload_deliverer/gripper.h index 3c0ab0cef5..7fd56d7c4a 100644 --- a/src/modules/payload_deliverer/gripper.h +++ b/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_pub{ORB_ID(gripper)}; }; diff --git a/src/modules/payload_deliverer/payload_deliverer.cpp b/src/modules/payload_deliverer/payload_deliverer.cpp index 5be494b0e5..5958c024f2 100644 --- a/src/modules/payload_deliverer/payload_deliverer.cpp +++ b/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!"); + } }