diff --git a/examples/python/formant_module/agent/README.md b/examples/python/formant_module/agent/README.md index 76f8e29..e539466 100755 --- a/examples/python/formant_module/agent/README.md +++ b/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. + + + + + ## Reading application configuration diff --git a/examples/python/formant_module/agent/images/configure_command.png b/examples/python/formant_module/agent/images/configure_command.png new file mode 100644 index 0000000..23ba03b Binary files /dev/null and b/examples/python/formant_module/agent/images/configure_command.png differ diff --git a/examples/python/formant_module/agent/images/create_command.png b/examples/python/formant_module/agent/images/create_command.png new file mode 100644 index 0000000..39a4dc1 Binary files /dev/null and b/examples/python/formant_module/agent/images/create_command.png differ diff --git a/examples/python/formant_module/agent/position_cmd.py b/examples/python/formant_module/agent/position_cmd.py new file mode 100644 index 0000000..2c100dd --- /dev/null +++ b/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) diff --git a/examples/python/formant_module/agent/requirements.txt b/examples/python/formant_module/agent/requirements.txt index eae67a9..eb611f4 100755 --- a/examples/python/formant_module/agent/requirements.txt +++ b/examples/python/formant_module/agent/requirements.txt @@ -1 +1,3 @@ -formant \ No newline at end of file +formant +scipy +rospy \ No newline at end of file