Add lolo_move_to action server

All Topics.msg.md

@@ -1,5 +1,82 @@
 # Topics in the system defined under all Topics.msg files
 
+## Topics in file: [messages/lolo_msgs/msg/Topics.msg](messages/lolo_msgs/msg/Topics.msg)
+
+-  RUDDER_CMD :  "actuators/rudder_cmd"
+-  ELEVATOR_CMD :  "actuators/elevator_cmd"
+-  ELEVON_PORT_CMD :  "actuators/elevon_port_cmd"
+-  ELEVON_STRB_CMD :  "actuators/elevon_strb_cmd"
+-  RUDDER_FB :  "actuators/rudder_fb"
+-  ELEVATOR_FB :  "actuators/elevator_fb"
+-  ELEVON_PORT_FB :  "actuators/elevon_port_fb"
+-  ELEVON_STRB_FB :  "actuators/elevon_strb_fb"
+-  THRUSTER_PORT_CMD :  "actuators/thruster_port_cmd"
+-  THRUSTER_STRB_CMD :  "actuators/thruster_strb_cmd"
+-  VERTICAL_THRUSTER_BACK_PORT_CMD :  "actuators/vertical_thruster_back_port_cmd"
+-  VERTICAL_THRUSTER_BACK_STRB_CMD :  "actuators/vertical_thruster_back_strb_cmd"
+-  VERTICAL_THRUSTER_FRONT_PORT_CMD :  "actuators/vertical_thruster_front_port_cmd"
+-  VERTICAL_THRUSTER_FRONT_STRB_CMD :  "actuators/vertical_thruster_front_strb_cmd"
+-  THRUSTER_PORT_FB :  "actuators/thruster_port_fb"
+-  THRUSTER_STRB_FB :  "actuators/thruster_strb_fb"
+-  VERTICAL_THRUSTER_BACK_PORT_FB :  "actuators/vertical_thruster_back_port_fb"
+-  VERTICAL_THRUSTER_BACK_STRB_FB :  "actuators/vertical_thruster_back_strb_fb"
+-  VERTICAL_THRUSTER_FRONT_PORT_FB :  "actuators/vertical_thruster_front_strb_fb"
+-  VERTICAL_THRUSTER_FRONT_STRB_FB :  "actuators/vertical_thruster_front_strb_fb"
+-  HEARTBEAT_TOPIC :  "heartbeat"
+-  LEAK_TOPIC :  "leak"
+-  BATTERY_1_TOPIC :  "extended/battery1"
+-  BATTERY_2_TOPIC :  "extended/battery2"
+-  EXTENDED_STATUS_TOPIC :  "extended/status"
+-  EXTENDED_SETTINGS_TOPIC :  "extended/settings_cmd"
+-  EXTENDED_INTERNAL_TEMPERATURE_TOPIC :  "extended/internal_temperature"
+-  EXTENDED_INTERNAL_PRESSURE_TOPIC :  "extended/internal_pressure"
+-  EXTENDED_RUDDER_FB_TOPIC :  "extended/actuators/rudder_fb"
+-  EXTENDED_ELEVATOR_FB_TOPIC :  "extended/actuators/elevator_fb"
+-  EXTENDED_PORT_ELEVON_FB_TOPIC :  "extended/actuators/elevon_port_fb"
+-  EXTENDED_STRB_ELEVON_FB_TOPIC :  "extended/actuators/elevon_strb_fb"
+-  EXTENDED_PORT_THRUSTER_FB_TOPIC :  "extended/actuators/thruster_port_fb"
+-  EXTENDED_STRB_THRUSTER_FB_TOPIC :  "extended/actuators/thruster_strb_fb"
+-  EXTENDED_VERTICAL_THRUSTER_FRONT_PORT_FB_TOPIC :  "extended/actuators/vertical_thruster_front_port_fb"
+-  EXTENDED_VERTICAL_THRUSTER_FRONT_STRB_FB_TOPIC :  "extended/actuators/vertical_thruster_front_strb_fb"
+-  EXTENDED_VERTICAL_THRUSTER_BACK_PORT_FB_TOPIC :  "extended/actuators/vertical_thruster_back_port_fb"
+-  EXTENDED_VERTICAL_THRUSTER_BACK_STRB_FB_TOPIC :  "extended/actuators/vertical_thruster_back_strb_fb"
+-  SATELITE_RECEIVED_TOPIC :  "communication/satelite_received"
+-  USBL_RECEIVED_TOPIC :  "communication/usbl/received"
+-  USBL_RECEIVED_CHR_TOPIC :  "communication/usbl/received/chr"
+-  INS_ODOM_TOPIC :  "ins_odom"
+-  INS_RAW_TOPIC :  "/ix/ins"
+-  INS_IMU_TOPIC :  "/standard/imu"
+## Topics in file: [messages/dead_reckoning_msgs/msg/Topics.msg](messages/dead_reckoning_msgs/msg/Topics.msg)
+
+-  DR_ODOM_TOPIC :  'dr/odom'  
+-  DR_DEPTH_POSE_TOPIC :  'dr/depth_pose'  
+-  DR_GPS_ODOM_TOPIC :  'dr/gps_odom'  
+-  DR_ROLL_TOPIC :  'dr/roll'  
+-  DR_PITCH_TOPIC :  'dr/pitch'  
+-  DR_YAW_TOPIC :  'dr/yaw'  
+-  DR_DEPTH_TOPIC :  'dr/depth'  
+-  DR_COMPASS_HEADING_TOPIC :  'dr/compass_heading'  
+-  DR_LAT_LON_TOPIC :  'dr/lat_lon'  
+-  DR_ODOM_X_TOPIC :  'dr/x'  
+-  DR_ODOM_Y_TOPIC :  'dr/y'  
+-  DR_ODOM_Z_TOPIC :  'dr/z'  
+-  DR_ODOM_U_TOPIC :  'dr/u'  
+-  DR_ODOM_V_TOPIC :  'dr/v'  
+-  DR_ODOM_W_TOPIC :  'dr/w'  
+-  DR_ODOM_P_TOPIC :  'dr/p'  
+-  DR_ODOM_Q_TOPIC :  'dr/q'  
+-  DR_ODOM_R_TOPIC :  'dr/r'  
+-  DR_ODOM_ALT_TOPIC :  'dr/alt'  
+## Topics in file: [messages/smarc_mission_msgs/msg/Topics.msg](messages/smarc_mission_msgs/msg/Topics.msg)
+
+-  DUBINS_SERVICE :  'mission/dubins_service'
+-  UTM_LATLON_CONVERSION_SERVICE :  'mission/utm_latlon_conversion_service'
+-  MISSION_COMPLETE_TOPIC :  'mission/complete'
+-  MISSION_CONTROL_TOPIC :  'mission/mission_control'
+-  BT_COMMAND_TOPIC :  'mission/bt_command'
+-  BT_TIP_TOPIC :  'mission/bt_tip'
+-  BT_LAST_WP_TOPIC :  'mission/last_wp'
+-  GOTO_WP_ACTION :  'mission/goto_wp_action'
 ## Topics in file: [messages/sam_msgs/msg/Topics.msg](messages/sam_msgs/msg/Topics.msg)
 
 -  VBS_CMD_TOPIC :  'core/vbs_cmd'
@@ -52,40 +129,6 @@
 -  GET_TRANSPORT_STATS_SRV :  'core/uavcan_get_transport_stats'
 -  RESTART_NODE_SRV :  'core/uavcan_restart_node'
 -  UPDATE_BATTERY_SRV :  'core/uavcan_update_battery'
-## Topics in file: [messages/dead_reckoning_msgs/msg/Topics.msg](messages/dead_reckoning_msgs/msg/Topics.msg)
-
--  DR_ODOM_TOPIC :  'dr/odom'  
--  DR_DEPTH_POSE_TOPIC :  'dr/depth_pose'  
--  DR_GPS_ODOM_TOPIC :  'dr/gps_odom'  
--  DR_ROLL_TOPIC :  'dr/roll'  
--  DR_PITCH_TOPIC :  'dr/pitch'  
--  DR_YAW_TOPIC :  'dr/yaw'  
--  DR_DEPTH_TOPIC :  'dr/depth'  
--  DR_COMPASS_HEADING_TOPIC :  'dr/compass_heading'  
--  DR_LAT_LON_TOPIC :  'dr/lat_lon'  
--  DR_ODOM_X_TOPIC :  'dr/x'  
--  DR_ODOM_Y_TOPIC :  'dr/y'  
--  DR_ODOM_Z_TOPIC :  'dr/z'  
--  DR_ODOM_U_TOPIC :  'dr/u'  
--  DR_ODOM_V_TOPIC :  'dr/v'  
--  DR_ODOM_W_TOPIC :  'dr/w'  
--  DR_ODOM_P_TOPIC :  'dr/p'  
--  DR_ODOM_Q_TOPIC :  'dr/q'  
--  DR_ODOM_R_TOPIC :  'dr/r'  
--  DR_ODOM_ALT_TOPIC :  'dr/alt'  
-## Topics in file: [messages/sam_graph_slam_2_msgs/msg/Topics.msg](messages/sam_graph_slam_2_msgs/msg/Topics.msg)
-
--  MAP_LINE_DEPTH_TOPIC :  'map/line_depth'
--  MAP_POINT_FEATURE_TOPIC :  'map/point_features'
--  MAP_LINE_FEATURE_TOPIC :  'map/line_features'
--  MAP_MARKED_LINE_SPHERES_TOPIC :  'map/marked_lines_spheres'
--  MAP_MARKED_LINE_LINES_TOPIC :  'map/marked_lines_lines'
--  DETECTOR_HYPOTH_TOPIC :  'payload/sidescan/detection_hypothesis'  
--  DETECTOR_MARKER_TOPIC :  'payload/sidescan/detection_markers'  
--  DETECTOR_RAW_SSS_TOPIC :  'payload/sidescan/image'  
--  DETECTOR_MARKED_SSS_TOPIC :  'payload/sidescan/detection_hypothesis_image'  
--  DR_ODOM_TOPIC :  'graph_dr/dr_odom'
--  GT_ODOM_TOPIC :  'graph_dr/gt_odom'
 ## Topics in file: [messages/smarc_control_msgs/msg/Topics.msg](messages/smarc_control_msgs/msg/Topics.msg)
 
 -  STATES :  'core/odom_gt' 
@@ -108,49 +151,26 @@
 -  TELEOP_ENABLE :  'teleop/enable'
 -  ASSIST_ENABLE :  'teleop/drive_assist_enable'
 -  ELEVATOR_PID_CTRL :  'ctrl/elevator_pid/ctrl_effort'
-## Topics in file: [messages/lolo_msgs/msg/Topics.msg](messages/lolo_msgs/msg/Topics.msg)
+-  CONTROL_YAW_TOPIC :  "ctrl/yaw"              
+-  CONTROL_YAW_RATE_TOPIC :  "ctrl/yawrate"     
+-  CONTROL_PITCH_TOPIC :  "ctrl/pitch"          
+-  CONTROL_PITCH_RATE_TOPIC :  "ctrl/pitchrate" 
+-  CONTROL_ROLL_TOPIC :  "ctrl/roll"            
+-  CONTROL_ROLL_RATE_TOPIC :  "ctrl/rollrate"   
+-  CONTROL_SURGE_RATE_TOPIC :  "ctrl/surgerate" 
+## Topics in file: [messages/sam_graph_slam_2_msgs/msg/Topics.msg](messages/sam_graph_slam_2_msgs/msg/Topics.msg)
 
--  RUDDER_CMD :  "actuators/rudder_cmd"
--  ELEVATOR_CMD :  "actuators/elevator_cmd"
--  ELEVON_PORT_CMD :  "actuators/elevon_port_cmd"
--  ELEVON_STRB_CMD :  "actuators/elevon_strb_cmd"
--  RUDDER_FB :  "actuators/rudder_fb"
--  ELEVATOR_FB :  "actuators/elevator_fb"
--  ELEVON_PORT_FB :  "actuators/elevon_port_fb"
--  ELEVON_STRB_FB :  "actuators/elevon_strb_fb"
--  THRUSTER_PORT_CMD :  "actuators/thruster_port_cmd"
--  THRUSTER_STRB_CMD :  "actuators/thruster_strb_cmd"
--  VERTICAL_THRUSTER_BACK_PORT_CMD :  "actuators/vertical_thruster_back_port_cmd"
--  VERTICAL_THRUSTER_BACK_STRB_CMD :  "actuators/vertical_thruster_back_strb_cmd"
--  VERTICAL_THRUSTER_FRONT_PORT_CMD :  "actuators/vertical_thruster_front_port_cmd"
--  VERTICAL_THRUSTER_FRONT_STRB_CMD :  "actuators/vertical_thruster_front_strb_cmd"
--  THRUSTER_PORT_FB :  "actuators/thruster_port_fb"
--  THRUSTER_STRB_FB :  "actuators/thruster_strb_fb"
--  VERTICAL_THRUSTER_BACK_PORT_FB :  "actuators/vertical_thruster_back_port_fb"
--  VERTICAL_THRUSTER_BACK_STRB_FB :  "actuators/vertical_thruster_back_strb_fb"
--  VERTICAL_THRUSTER_FRONT_PORT_FB :  "actuators/vertical_thruster_front_strb_fb"
--  VERTICAL_THRUSTER_FRONT_STRB_FB :  "actuators/vertical_thruster_front_strb_fb"
--  HEARTBEAT_TOPIC :  "heartbeat"
--  LEAK_TOPIC :  "leak"
--  BATTERY_1_TOPIC :  "extended/battery1"
--  BATTERY_2_TOPIC :  "extended/battery2"
--  EXTENDED_STATUS_TOPIC :  "extended/status"
--  EXTENDED_SETTINGS_TOPIC :  "extended/settings_cmd"
--  EXTENDED_INTERNAL_TEMPERATURE_TOPIC :  "extended/internal_temperature"
--  EXTENDED_INTERNAL_PRESSURE_TOPIC :  "extended/internal_pressure"
--  EXTENDED_RUDDER_FB_TOPIC :  "extended/actuators/rudder_fb"
--  EXTENDED_ELEVATOR_FB_TOPIC :  "extended/actuators/elevator_fb"
--  EXTENDED_PORT_ELEVON_FB_TOPIC :  "extended/actuators/elevon_port_fb"
--  EXTENDED_STRB_ELEVON_FB_TOPIC :  "extended/actuators/elevon_strb_fb"
--  EXTENDED_PORT_THRUSTER_FB_TOPIC :  "extended/actuators/thruster_port_fb"
--  EXTENDED_STRB_THRUSTER_FB_TOPIC :  "extended/actuators/thruster_strb_fb"
--  EXTENDED_VERTICAL_THRUSTER_FRONT_PORT_FB_TOPIC :  "extended/actuators/vertical_thruster_front_port_fb"
--  EXTENDED_VERTICAL_THRUSTER_FRONT_STRB_FB_TOPIC :  "extended/actuators/vertical_thruster_front_strb_fb"
--  EXTENDED_VERTICAL_THRUSTER_BACK_PORT_FB_TOPIC :  "extended/actuators/vertical_thruster_back_port_fb"
--  EXTENDED_VERTICAL_THRUSTER_BACK_STRB_FB_TOPIC :  "extended/actuators/vertical_thruster_back_strb_fb"
--  SATELITE_RECEIVED_TOPIC :  "communication/satelite_received"
--  USBL_RECEIVED_TOPIC :  "communication/usbl/received"
--  USBL_RECEIVED_CHR_TOPIC :  "communication/usbl/received/chr"
+-  MAP_LINE_DEPTH_TOPIC :  'map/line_depth'
+-  MAP_POINT_FEATURE_TOPIC :  'map/point_features'
+-  MAP_LINE_FEATURE_TOPIC :  'map/line_features'
+-  MAP_MARKED_LINE_SPHERES_TOPIC :  'map/marked_lines_spheres'
+-  MAP_MARKED_LINE_LINES_TOPIC :  'map/marked_lines_lines'
+-  DETECTOR_HYPOTH_TOPIC :  'payload/sidescan/detection_hypothesis'  
+-  DETECTOR_MARKER_TOPIC :  'payload/sidescan/detection_markers'  
+-  DETECTOR_RAW_SSS_TOPIC :  'payload/sidescan/image'  
+-  DETECTOR_MARKED_SSS_TOPIC :  'payload/sidescan/detection_hypothesis_image'  
+-  DR_ODOM_TOPIC :  'graph_dr/dr_odom'
+-  GT_ODOM_TOPIC :  'graph_dr/gt_odom'
 ## Topics in file: [messages/smarc_msgs/msg/Topics.msg](messages/smarc_msgs/msg/Topics.msg)
 
 -  GPS_TOPIC :  'core/gps'
@@ -190,19 +210,12 @@
 -  BUOY_DETECTOR_ESTIMATE_TOPIC :  'auv/buoy_est'
 -  SAM_LOWEST_POINT_ESTIMATE_TOPIC :  'auv/sam_lowest_est'
 -  CAMERA_PROCESSED_TOPIC :  'core/fpcamera/image_processed'
-## Topics in file: [messages/smarc_mission_msgs/msg/Topics.msg](messages/smarc_mission_msgs/msg/Topics.msg)
-
--  DUBINS_SERVICE :  'mission/dubins_service'
--  UTM_LATLON_CONVERSION_SERVICE :  'mission/utm_latlon_conversion_service'
--  MISSION_COMPLETE_TOPIC :  'mission/complete'
--  MISSION_CONTROL_TOPIC :  'mission/mission_control'
--  BT_COMMAND_TOPIC :  'mission/bt_command'
--  BT_TIP_TOPIC :  'mission/bt_tip'
--  BT_LAST_WP_TOPIC :  'mission/last_wp'
--  GOTO_WP_ACTION :  'mission/goto_wp_action'
 
 # Duplicates
 
+-  "actuators/vertical_thruster_front_strb_fb": 
+    - [ VERTICAL_THRUSTER_FRONT_PORT_FB ](messages/lolo_msgs/msg/Topics.msg)
+    - [ VERTICAL_THRUSTER_FRONT_STRB_FB ](messages/lolo_msgs/msg/Topics.msg)
 -  'core/depth20_pressure': 
     - [ DEPTH_TOPIC ](messages/sam_msgs/msg/Topics.msg)
     - [ PRESS_DEPTH20_TOPIC ](messages/sam_msgs/msg/Topics.msg)

behaviours/lolo_move_to/README.md

@@ -0,0 +1,59 @@
+<!--toc:start-->
+- [go_to_geopoint](#gotogeopoint)
+- [Implementing an Action Server/Client](#implementing-an-action-serverclient)
+  - [Creating an Action](#creating-an-action)
+    - [ROS2 Humble Doc's](#ros2-humble-docs)
+  - [Implementing with smarc-action-base](#implementing-with-smarc-action-base)
+    - [Server](#server)
+    - [Client](#client)
+  - [ROS2 Humble Documentation on Action Server Client](#ros2-humble-documentation-on-action-server-client)
+<!--toc:end-->
+
+# go_to_geopoint
+
+Non-trivial example implementation of action client and action server with `smarc_action_base`. 
+
+# Implementing an Action Server/Client
+
+## Creating an Action
+
+Action server/client's utilize custom ROS2 message types to communicate. This message consists of three items:
+- Goal
+- Feedback 
+- Result
+
+These concepts are easy to google and read in the docs so to not avoid duplication a starting point is provided below
+
+### ROS2 Humble Doc's
+- [What are Actions?](https://docs.ros.org/en/humble/Concepts/Basic/About-Actions.html) 
+- [Creating an Action](https://docs.ros.org/en/humble/Tutorials/Intermediate/Creating-an-Action.html) 
+
+## Implementing with smarc-action-base
+
+### Server
+In order to implement a server using the provided framework the following functions must be filled out:
+
+- `goal_callback`
+    - The action server needs to manage what goals it accepts and rejects. This is handled in this callback. To see how to formulate the response such that ROS understand consult the docstring on the `goal_callback` or the example.
+- `cancel_callback`
+    - The action server needs to manage how it cancels goals. This is handled in this callback. This may seem trivial, but imagine a simple case where the action server is moving an AUV to a new waypoint and the client requests to cancel. The action server needs to stop the AUV's motion and indicate whether or not that was successful to the client. This is exactly the purpose of this callback.
+    To see how to formulate the response such that ROS understand consult the docstring on the `cancel_callback` or the example.
+- `execution_callback`
+    - The execution callback is simpler than the previous examples. This is simply the callback where the user parses the Goal message and does what it needs to do with it to begin the action. Additionally feedback can be provided here by the action server to the client if the action takes a significant amount of time to complete.
+
+### Client
+In order to implement a client using the provided framework the following functions must be filled out:
+
+- `feedback_callback`
+- `goal_response_callback`
+- `result_callback`
+
+These are much simpler to understand than the server version above. The client must manage `ACCEPT` or `REJECT` goal requests via the `goal_response_callback`. Feedback can be parsed, logged, or dealt with in any manner through the `feedback_callback`. Finally, the success of the action is provided and can be parsed and used for other actions, servers, topics, etc in the `result_callback`.
+
+**WARNING** If the action server does not accept the goal, there will be no feedback or response as it was rejected.
+
+
+## ROS2 Humble Documentation on Action Server Client
+This simple action-server-client tutorial provides the basis of what an action server-client is 
+- [ROS2 Tutorial](https://docs.ros.org/en/humble/Tutorials/Intermediate/Writing-an-Action-Server-Client/Py.html) 
+

behaviours/lolo_move_to/lolo_move_to/action_parsing.py

@@ -0,0 +1,67 @@
+from enum import Enum
+import json
+
+from lolo_move_to.move_to_goal import MoveToGoal
+from std_msgs.msg import String
+
+
+class ActionSubMsg(Enum):
+    GOAL = 0
+    FEEDBACK = 2
+
+
+class MoveToActionParsing:
+    def __init__(self):
+        pass
+
+    def decode(
+        self,
+        serialized_fmt: String,
+        component: ActionSubMsg,
+    ) -> MoveToGoal | float:
+        """Decodes action message from json to Python / ROS types.
+
+        Note: this is done for the convenience of higher level operations and is not necessary.
+        Args:
+            serialized_fmt: string format from action
+            component: The desired action component that is being parsed (defines how it will be parsed)
+
+        Returns:
+            Python and MoveToGoal types for usage in client and server.
+
+        """
+        fmt_dict = json.loads(serialized_fmt.data)
+        if component is ActionSubMsg.GOAL:
+            goal = MoveToGoal()
+            goal.geopoint.latitude = float(fmt_dict["geopoint"]["latitude"])
+            goal.geopoint.longitude = float(fmt_dict["geopoint"]["longitude"])
+            goal.target_depth = float(fmt_dict["target_depth"])
+            goal.min_altitude = float(fmt_dict["min_altitude"])
+            goal.rpm = float(fmt_dict["rpm"])
+            goal.timeout = float(fmt_dict["timeout"])
+            return goal
+        elif component is ActionSubMsg.FEEDBACK:
+            return float(fmt_dict["distance_remaining"])
+
+    def encode(
+        self,
+        val: MoveToGoal | float,
+    ) -> String | None:
+        """Encodes action message into string."""
+        str_msg = String()
+        fmt_dict = {}
+        if isinstance(val, (MoveToGoal,)):
+            fmt_dict["geopoint"] = {}
+            fmt_dict["geopoint"]["latitude"] = val.geopoint.latitude
+            fmt_dict["geopoint"]["longitude"] = val.geopoint.longitude
+            fmt_dict["target_depth"] = val.target_depth
+            fmt_dict["min_altitude"] = val.min_altitude
+            fmt_dict["rpm"] = val.rpm
+            fmt_dict["timeout"] = val.timeout
+        elif isinstance(val, (float,)):
+            fmt_dict["distance_remaining"] = val
+        else:
+            return None
+        str_val = json.dumps(fmt_dict)
+        str_msg.data = str_val
+        return str_msg