Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 06:36:45 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

payload_deliverer & gripper: Improve intermediate state & vcmd_ack

Browse Source 
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?)

Only command Gripper grab when we are actually initializing gripper

- Previously, on every parameter update, gripper grab was being
commanded
- This commit narrows that scope to only when we are actually
initializing the gripper

Handle gripper de-initialization upon parameter change

- Also added some local state initialization code to init() function of
Gripper
- This will now make init / de-init more graceful & controlled compared
to before
This commit is contained in:
Junwoo Hwang
2022-09-30 11:25:38 +02:00
committed by Beat Küng
parent 36a3c716d6
commit 6529e39f8b
4 changed files with 107 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/payload_deliverer/gripper.cpp
+32
View File
@@ -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;
}
src/modules/payload_deliverer/gripper.h
+32 -4
View File
@@ -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_s> _gripper_pub{ORB_ID(gripper)};
};
src/modules/payload_deliverer/payload_deliverer.cpp
+38 -24
View File
@@ -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!");
}
}
src/modules/payload_deliverer/payload_deliverer.h
+5 -2
View File
@@ -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