Added position_cmd example (used for Fresh) code and explanation to readme

examples/python/formant_module/agent/README.md

@@ -47,11 +47,17 @@ Examples:
 
 ## Handling commands
 
-Use the `register_command_request_callback` and `send_command_response` methods to handle commands sent from the Formant application on your device. Refer to the examples for usage.
+Use the `register_command_request_callback` and `send_command_response` methods to handle commands sent from the Formant application on your device. Refer to the examples for further usage. 
 
 Examples:
 
--   [Handling commands](./handle_commands.py)
+-   [Handling commands](./handle_commands.py) :   Outlines how to use the specific command type "get_file", which is a built-in command in the Formant UI. 
+-   [Create PoseStamped msg from commands](./position_cmd.py):  Outlines the basic idea of how to receive a command from the Formant UI and convert this command into a PoseStamped position for the robot. The code accepts a command with parameters (x,y,theta) from the Formant platform, unpacks the parameters to create a PoseStamped message which prints in the agent logs. To set up a command, go to Settings > Commands > Add Command. The script looks for "send_to_position" string from the command (as shown in the second screenshot below) and uses the parameters to accept x,y and theta.
+
+<img src="./images/create_command.png" width="1100" height="750" class="center"/>
+<img src="./images/configure_command.png" width="1100" height="750" class="center"/>
+
+
 
 ## Reading application configuration
 

examples/python/formant_module/agent/position_cmd.py

@@ -0,0 +1,47 @@
+import time
+from geometry_msgs.msg import PoseStamped
+from scipy.spatial.transform import Rotation
+
+from formant.sdk.agent.v1 import Client as FormantClient
+
+
+class FormantPositionCommander:
+    def __init__(self):
+        self._fclient = FormantClient(ignore_unavailable=True, ignore_throttled=True)
+        self._setup_position_command_handler()
+
+    def _setup_position_command_handler(self):
+        self._fclient.register_command_request_callback(
+            self._send_pose, command_filter=["send_to_position"]
+        )
+
+    def _send_pose(self, request):
+        print("Parameters sent from command: %s" % request.text)
+        try:
+            position_data = request.text.split(",")
+            x_pos = float(position_data[0])
+            y_pos = float(position_data[1])
+            theta = float(position_data[2])
+            goal_position = PoseStamped()
+            goal_position.pose.position.x = float(position_data[0])
+            goal_position.pose.position.y = float(position_data[1])
+            rot = Rotation.from_euler(
+                "xyz", [0, 0, float(position_data[2])], degrees=True
+            )
+            rot_quat = rot.as_quat()
+            goal_position.pose.orientation.x = rot_quat[0]
+            goal_position.pose.orientation.y = rot_quat[1]
+            goal_position.pose.orientation.z = rot_quat[2]
+            goal_position.pose.orientation.w = rot_quat[3]
+            print("Goal position: %s" % goal_position)
+        except Exception as e:
+            print(
+                "Ensure you send command with x, y, and theta as comma separated values."
+            )
+            print("Data sent: %s" % request.text)
+
+
+if __name__ == "__main__":
+    FPC = FormantPositionCommander()
+    while True:
+        time.sleep(1)

examples/python/formant_module/agent/requirements.txt

@@ -1 +1,3 @@
-formant
+formant
+scipy
+rospy