payload_deliverer: Add gripper state output to "status" CLI output

- Previously it was hard to check which state the gripper was in
(GRAB/GRABBED/ etc), not even whether it was correctly initialized, this
commit adds that functionality
This commit is contained in:
Junwoo Hwang 2022-09-21 13:55:04 +02:00 committed by Beat Küng
parent e65d48c070
commit 13d565d707
4 changed files with 43 additions and 0 deletions

View File

@ -127,3 +127,26 @@ void Gripper::publish_gripper_command(const int8_t gripper_command)
gripper.command = gripper_command;
_gripper_pub.publish(gripper);
}
const char *Gripper::get_state_str() const
{
switch (_state) {
case GripperState::GRABBING:
return "GRABBING";
case GripperState::GRABBED:
return "GRABBED";
case GripperState::RELEASING:
return "RELEASING";
case GripperState::RELEASED:
return "RELEASED";
case GripperState::IDLE:
return "IDLE";
default:
return "UNKNOWN";
}
}

View File

@ -87,8 +87,13 @@ public:
// Update gripper state
void update();
// Return griper's state in string format
const char *get_state_str() const;
// Returns true if in grabbed position either sensed or timeout based
bool grabbed() { return _state == GripperState::GRABBED; }
// Returns true if in grabbing position either sensed or timeout based
bool grabbing() { return _state == GripperState::GRABBING; }
// Returns true if in released position either sensed or timeout based

View File

@ -216,6 +216,18 @@ void PayloadDeliverer::gripper_close()
send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_GRAB);
}
int PayloadDeliverer::print_status()
{
// Gripper status
PX4_INFO("Gripper valid: %s", _gripper.is_valid() ? "True" : "False");
if (_gripper.is_valid()) {
PX4_INFO("Gripper state: %s", _gripper.get_state_str());
}
return 0;
}
int PayloadDeliverer::custom_command(int argc, char *argv[])
{
if (argc >= 1) {

View File

@ -77,6 +77,9 @@ public:
static int custom_command(int argc, char *argv[]);
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase. Override "status" output when invoked via Commandline, to give detailed status **/
int print_status() override;
// Initializes the module
bool init();