Merge pull request #181 from DoernerD/planner

Planner

Author: ignaciotb

behaviours/go_to_hydrobaticpoint/go_to_hydrobaticpoint/hydrobaticpoint_action.py

@@ -35,8 +35,8 @@ def decode(
             hydropoint = PoseStamped()
             hydropoint.header.frame_id = str(fmt_dict["hydropoint"]["frame_id"])
             hydropoint.pose.position.x = float(fmt_dict["hydropoint"]["position"]["x"])
-            hydropoint.pose.position.x = float(fmt_dict["hydropoint"]["position"]["y"])
-            hydropoint.pose.position.x = float(fmt_dict["hydropoint"]["position"]["z"])
+            hydropoint.pose.position.y = float(fmt_dict["hydropoint"]["position"]["y"])
+            hydropoint.pose.position.z = float(fmt_dict["hydropoint"]["position"]["z"])
             hydropoint.pose.orientation.x = float(fmt_dict["hydropoint"]["orientation"]["x"])
             hydropoint.pose.orientation.y = float(fmt_dict["hydropoint"]["orientation"]["y"])
             hydropoint.pose.orientation.z = float(fmt_dict["hydropoint"]["orientation"]["z"])

behaviours/sam_diving_controller/config/sam_diving_controller_config.yaml

@@ -15,7 +15,7 @@
     lcg_pid_ki: 5.0
     lcg_pid_kd: 1.0
     lcg_pid_kaw: 1.0
-    lcg_u_neutral: 75.0
+    lcg_u_neutral: 50.0
     lcg_u_min: 0.0
     lcg_u_max: 100.0
     tv_pid_kp: 1.0

behaviours/sam_diving_controller/launch/mpc_trajectory_tracking.launch

@@ -0,0 +1,16 @@
+<launch>
+
+    <arg name="robot_name" default="sam0"/>
+    <arg name="tf_suffix" default=""/>
+    <arg name="control_yaml" default="$(find-pkg-share sam_diving_controller)/config/sam_diving_controller_config.yaml"/>
+   
+    <group>
+    <push-ros-namespace namespace="$(var robot_name)"/>
+       <node name="diving_node" pkg="sam_diving_controller" exec="mpc_trajectory_tracking" output="screen">
+        <param name="robot_name" value="$(var robot_name)"/>
+        <param name="tf_suffix" value="$(var tf_suffix)"/>
+	<param from="$(var control_yaml)"/>
+       </node>
+    </group>
+
+</launch>

behaviours/sam_diving_controller/sam_diving_controller/ActionServerDiveSub.py

@@ -4,6 +4,8 @@
 import json
 import threading
 
+import numpy as np
+
 from rclpy.node import Node
 from rclpy.action import ActionServer, CancelResponse, GoalResponse
 from rclpy.action.server import ServerGoalHandle
@@ -28,6 +30,7 @@
 from action_msgs.srv import CancelGoal
 
 # ROS Imports
+import rclpy
 from rclpy.action import ActionClient, ActionServer, CancelResponse, GoalResponse
 from rclpy.action.client import ClientGoalHandle
 from rclpy.action.server import ServerGoalHandle
@@ -46,6 +49,8 @@
     SMARCActionServer,
 )
 
+from sam_path_following.path_action import ActionComponent as ActC
+from sam_path_following.path_action import PathAction
 
 try:
     from .DiveSub import DiveSub
@@ -231,3 +236,203 @@ def cancel_callback(self, goal_handle) -> CancelResponse:
         return CancelResponse.ACCEPT
 
 
+class PathServer(SMARCActionServer, DiveSub):
+    """Action point server that handle GotoGeopoint messages.
+
+    Attributes:
+        logger: shorthand for `node.get_logger()`
+        robot_name: provided robot name from launch file
+        target_frame: frame that goal's should be transformed to
+    """
+
+    def __init__(self, node: Node, action_name, action_type: ActionType, param):
+
+        self.param = param
+        self._node = node
+
+        SMARCActionServer.__init__(self,
+            node,
+            action_name,
+            action_type,
+            SMaRCTopics.WARA_PS_ACTION_SERVER_HB_TOPIC,
+        )
+        DiveSub.__init__(self,self._node, self.param)
+
+        self.logger = node.get_logger()
+
+        self.path = None
+        self.path_len = None
+        self.current_idx = 0
+
+        #self.declare_parameters()
+
+        self.logger.set_level(rclpy.logging.LoggingSeverity.INFO)
+
+        self._json_ops: PathAction = PathAction()
+
+        self._loginfo("Path Action Server started")
+
+
+    # TODO: Cancel process. Need to stop the controller as well, similar to the wp action server.
+
+#    def declare_parameters(self):
+#        """Declares all of node's parameters in a single location."""
+#
+#        # TODO: Which ones are actually needed?
+#
+#        node = self._node
+#        self.robot_name = node.declare_parameter("robot_name", "Quadrotor").value
+#        self._target_frame_param = node.declare_parameter("target_frame", "odom").value
+#
+#        self._distance_frame_param = node.declare_parameter(
+#            "distance_frame",
+#            "base_link",
+#            ParameterDescriptor(
+#                description="Frame for which the distance to target will be computed (usually base_link)"
+#            ),
+#        ).value
+#
+#        self._distance_frame_suffix = node.declare_parameter(
+#            "distance_frame_suffix",
+#            "_gt",
+#            ParameterDescriptor(
+#                description="Frame suffix for distance frame. Commonly is '_gt' for ground truth if applicable"
+#            ),
+#        ).value
+#
+#        self._frame_suffix = node.declare_parameter(
+#            "frame_suffix",
+#            "_gt",
+#            ParameterDescriptor(
+#                description="Frame suffix for transform. Commonly is '_gt' for ground truth if applicable"
+#            ),
+#        ).value
+#
+#        self._setpoint_tol: float = node.declare_parameter(
+#            "setpoint_tolerance",
+#            0.25,
+#            ParameterDescriptor(
+#                description="Setpoint tolerance for when the goal is considered achieved (Euclidean norm)."
+#            ),
+#        ).value
+#
+#        # self._setpoint_topic = node.declare_parameter(
+#        #     "setpoint_topic",
+#        #     "go_to_setpoint",
+#        #     ParameterDescriptor(
+#        #         description="Topic to publish setpoint targets to. Will be prepended with 'robot_name'"
+#        #     ),
+#        # ).value
+#
+#        self._goal_threshold = (
+#            node.declare_parameter(
+#                "goal_threshold",
+#                10,
+#                ParameterDescriptor(
+#                    description="Distance threshold in meters where a goal should be rejected. (Euclidean Norm)"
+#                ),
+#            ).value
+#        )
+#
+#        self.target_frame = (
+#            f"{self.robot_name}/{self._target_frame_param}{self._frame_suffix}"
+#        )
+#        self.logger.info(f"Target frame {self.target_frame}")
+#
+#        self.distance_frame = f"{self.robot_name}/{self._distance_frame_param}{self._distance_frame_suffix}"
+#        self.logger.info(f"Distance frame {self.distance_frame}")
+
+    def _save_path(self, path):
+        """
+        Convert path from list to numpy array.
+        """
+        self.path = np.asarray(path)
+        self.logger.info(f"AS: saved path")
+
+
+    def goal_callback(self, goal_request: ActionType.Goal) -> GoalResponse:
+        """Considers a goal validity and evaluates whether it should be accepted or not.
+
+        Args:
+            goal_request (ActionType.Goal): Goal message
+
+        Returns:
+            response: Either GoalResponse.Accept or GoalResponse.Reject
+
+        """
+        # TODO: Think of whether you want to reject any goal. For now, accept
+        # everything, as we assume the planner to know what it's doing.
+        goal_request = goal_request.goal
+        path = self._json_ops.decode(goal_request, ActC.GOAL)
+        self.logger.info(f"Recieved path")
+        self._save_path(path)
+        self.path_len = len(self.path) # TODO: Check which index to use, 0 or 1
+
+        # Accepts as all criteria fulfilled
+        return GoalResponse.ACCEPT
+
+
+    def execution_callback(self, goal_handle: ServerGoalHandle) -> ActionResult:
+        """Primary execution callback where goal's are handled after acceptance.
+
+        Args:
+            goal_handle: handle to control server and add callbacks
+
+        Returns:
+            A populated ActionResult message
+        """
+        result_msg = self.action_type.Result
+        status = self.feedback_loop(goal_handle)
+        if status == "cancelled":
+            self.logger.info("Goal was cancelled by client.")
+            result_msg.success = False
+            return result_msg
+        
+        self.set_mission_state(MissionStates.COMPLETED, "AS")
+        result_msg.success = True
+        return result_msg
+
+    def feedback_loop(self, goal_handle: ServerGoalHandle):
+        """Abstracted feedback loop where tolerance checks are conducted.
+
+        Args:
+            pose_stamped: target location
+            goal_handle: passed in to enable feedback publishing
+        """
+        rate = self._node.create_rate(2)
+        feedback = self.action_type.Feedback
+
+        while self.current_idx <= self.path_len:
+
+            self.set_mission_state(MissionStates.RUNNING, "AS")
+
+            if goal_handle.is_cancel_requested:
+                self.logger.info("Goal was cancelled by client.")
+                goal_handle.canceled()
+                return "cancelled"
+            
+            feedback.feedback = self._json_ops.encode(self.current_idx)
+            goal_handle.publish_feedback(feedback)
+            rate.sleep()
+
+        goal_handle.succeed()
+        rate.destroy()
+        return "done"
+
+
+    def cancel_callback(self, goal_handle: ServerGoalHandle) -> CancelResponse:
+        """Handles canceling of goal requests.
+
+        Args:
+            goal_handle: handle
+
+        Returns:
+            Cancel response as ACCEPT
+        """
+
+        self._loginfo("Cancelled")
+
+        self.set_mission_state(MissionStates.CANCELLED, "AS")
+
+        return CancelResponse.ACCEPT
+

behaviours/sam_diving_controller/sam_diving_controller/ConveniencePub.py

@@ -20,6 +20,12 @@ class ConveniencePub(IDivePub):
     Implements convenience topic publishers for debugging
     """
     def __init__(self, node: Node, dive_sub, dive_controller) -> None:
+
+        self._node = node
+
+        #self._node.declare_parameter('robot_name', 'sam0')
+        self._robot_name = self._node.get_parameter('robot_name').get_parameter_value().string_value
+
         self._state_pub = node.create_publisher(ControlState, ControlTopics.STATES_CONV, 10)
         self._ref_pub = node.create_publisher(ControlReference, ControlTopics.REF_CONV, 10)
         self._error_pub = node.create_publisher(ControlError, ControlTopics.CONTROL_ERROR_CONV, 10)
@@ -80,15 +86,15 @@ def _update_input(self) -> None:
         self._input_pub.publish(self._input_msg)
 
     def _update_waypoint(self) -> None:
-        self._waypoint = self._dive_sub.get_waypoint()
+        self._waypoint = self._dive_sub.get_odom_waypoint()
         self._goal_tolerance = self._dive_sub.get_goal_tolerance()
 
         if self._waypoint is None:
             return
 
         self._waypoint_msg = PoseWithCovarianceStamped()
-        self._waypoint_msg.header = self._waypoint.header
-        self._waypoint_msg.pose.pose = self._waypoint.pose
+        self._waypoint_msg.header.frame_id = self._robot_name + 'odom'
+        self._waypoint_msg.pose.pose = self._waypoint
         self._waypoint_msg.pose.pose.orientation.w = 1.0
         cov = np.zeros([6,6])
         cov[0][0] = np.sqrt(self._goal_tolerance)
@@ -166,5 +172,5 @@ def update(self) -> None:
         self._update_error()
         self._update_input()
         self._update_waypoint()
-        self._print_state()
+        #self._print_state()