From 13d565d7077d2c1f5c8364602f70f098454486ea Mon Sep 17 00:00:00 2001
From: Junwoo Hwang <junwoo091400@gmail.com>
Date: Wed, 21 Sep 2022 13:55:04 +0200
Subject: [PATCH] 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
---
 src/modules/payload_deliverer/gripper.cpp     | 23 +++++++++++++++++++
 src/modules/payload_deliverer/gripper.h       |  5 ++++
 .../payload_deliverer/payload_deliverer.cpp   | 12 ++++++++++
 .../payload_deliverer/payload_deliverer.h     |  3 +++
 4 files changed, 43 insertions(+)

diff --git a/src/modules/payload_deliverer/gripper.cpp b/src/modules/payload_deliverer/gripper.cpp
index fa97452ca2..323929648f 100644
--- a/src/modules/payload_deliverer/gripper.cpp
+++ b/src/modules/payload_deliverer/gripper.cpp
@@ -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";
+	}
+}
diff --git a/src/modules/payload_deliverer/gripper.h b/src/modules/payload_deliverer/gripper.h
index 08b8293655..3c0ab0cef5 100644
--- a/src/modules/payload_deliverer/gripper.h
+++ b/src/modules/payload_deliverer/gripper.h
@@ -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
diff --git a/src/modules/payload_deliverer/payload_deliverer.cpp b/src/modules/payload_deliverer/payload_deliverer.cpp
index 0de0af86a4..9f6e6c070a 100644
--- a/src/modules/payload_deliverer/payload_deliverer.cpp
+++ b/src/modules/payload_deliverer/payload_deliverer.cpp
@@ -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) {
diff --git a/src/modules/payload_deliverer/payload_deliverer.h b/src/modules/payload_deliverer/payload_deliverer.h
index 584c2a9fe1..c003eb3777 100644
--- a/src/modules/payload_deliverer/payload_deliverer.h
+++ b/src/modules/payload_deliverer/payload_deliverer.h
@@ -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();