diff --git a/src/modules/payload_deliverer/gripper.cpp b/src/modules/payload_deliverer/gripper.cpp index 323929648f..5ceacd78b3 100644 --- a/src/modules/payload_deliverer/gripper.cpp +++ b/src/modules/payload_deliverer/gripper.cpp @@ -62,12 +62,38 @@ void Gripper::init(const GripperConfig &config) _timeout_us = config.timeout_us; + // Reset internal states + _state = GripperState::IDLE; + _last_command_time = 0; + _released_state_cache = false; + _grabbed_state_cache = false; + // We have valid gripper type & sensor configuration _valid = true; } +void Gripper::deinit() +{ + // Reset Config variables + _has_feedback_sensor = false; + _timeout_us = 0; + + // Reset internal states + _state = GripperState::IDLE; + _last_command_time = 0; + _released_state_cache = false; + _grabbed_state_cache = false; + + // Mark gripper instance as invalid + _valid = false; +} + 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 +101,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 +119,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..f90378647b 100644 --- a/src/modules/payload_deliverer/gripper.h +++ b/src/modules/payload_deliverer/gripper.h @@ -78,6 +78,9 @@ public: // Initialize the gripper void init(const GripperConfig &config); + // De-initialize the gripper + void deinit(); + // Actuate the gripper to the grab position void grab(); @@ -99,9 +102,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 +124,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 +153,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..4aec8940b5 100644 --- a/src/modules/payload_deliverer/payload_deliverer.cpp +++ b/src/modules/payload_deliverer/payload_deliverer.cpp @@ -48,47 +48,50 @@ bool PayloadDeliverer::init() return false; } - initialize_gripper(); + configure_gripper(); return true; } -bool PayloadDeliverer::initialize_gripper() +void PayloadDeliverer::configure_gripper() { - // If gripper instance is invalid, try initializing it + // If gripper instance is valid, but user disabled the gripper, de-initialize the gripper + if (_gripper.is_valid() && !_param_gripper_enable.get()) { + _gripper.deinit(); + return; + } + + // If gripper instance is invalid, and user enabled the gripper, try initializing it if (!_gripper.is_valid() && _param_gripper_enable.get()) { GripperConfig config{}; config.type = (GripperConfig::GripperType)_param_gripper_type.get(); config.sensor = GripperConfig::GripperSensorType::NONE; // Feedback sensor isn't supported for now config.timeout_us = hrt_abstime(_param_gripper_timeout_s.get() * 1000000ULL); _gripper.init(config); + + if (!_gripper.is_valid()) { + PX4_DEBUG("Gripper object initialization failed!"); + return; + + } else { + // Command the gripper to grab position by default + if (!_gripper.grabbed() && !_gripper.grabbing()) { + PX4_DEBUG("Gripper intialize: putting to grab position!"); + send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_GRAB); + } + + return; + } } // NOTE: Support for changing gripper sensor type / gripper type configuration when // the parameter change is detected isn't added as we don't have actual use case for that // yet! - - if (!_gripper.is_valid()) { - PX4_DEBUG("Gripper object initialization invalid!"); - return false; - - } else { - _gripper.update(); - - // If gripper wasn't commanded to go to grab position, command. - if (!_gripper.grabbed() && !_gripper.grabbing()) { - PX4_DEBUG("Gripper intialize: putting to grab position!"); - send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_GRAB); - } - - return true; - } - } void PayloadDeliverer::parameter_update() { updateParams(); - initialize_gripper(); + configure_gripper(); } void PayloadDeliverer::Run() @@ -125,13 +128,13 @@ void PayloadDeliverer::gripper_update(const hrt_abstime &now) { if (!_gripper.is_valid()) { // Try initializing gripper - initialize_gripper(); + configure_gripper(); return; } _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 +143,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!"); + } } diff --git a/src/modules/payload_deliverer/payload_deliverer.h b/src/modules/payload_deliverer/payload_deliverer.h index c003eb3777..ff0ee41010 100644 --- a/src/modules/payload_deliverer/payload_deliverer.h +++ b/src/modules/payload_deliverer/payload_deliverer.h @@ -100,9 +100,12 @@ private: void parameter_update(); /** - * @brief Initialize gripper with current parameter settings + * @brief Initialize or deinitialize gripper instance based on parameter settings + * + * Depending on `PD_GRIPPER_EN` and the state of `_gripper` instance, this function will + * either try to initialize or de-initialize the gripper appropriately. */ - bool initialize_gripper(); + void configure_gripper(); /** * @brief Update gipper instance's state and send vehicle command ack