Made LQR and PID ready for testing

vortexntnu · Apr 29, 2024 · 4a1110f · 4a1110f
1 parent cff5d3c
commit 4a1110f
Show file tree

Hide file tree

Showing 7 changed files with 13 additions and 13 deletions.
diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py
@@ -8,8 +8,8 @@
 from nav_msgs.msg import Odometry
 from transforms3d.euler import quat2euler
 
-from dp_guidance.conversions import odometrymsg_to_state, state_to_odometrymsg
-from dp_guidance.reference_filter import ReferenceFilter
+from conversions import odometrymsg_to_state, state_to_odometrymsg
+from reference_filter import ReferenceFilter
 
 class Guidance(Node):
     def __init__(self):
@@ -64,9 +64,9 @@ def guidance_callback(self):
             self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, 0])
             x_next = self.reference_filter.step(self.eta_ref, self.xd)
             self.xd = x_next
-            self.get_logger().info(f'x_next[0]: {x_next[0]}')
-            self.get_logger().info(f'x_next[0]: {x_next[1]}')
-            self.get_logger().info(f'x_next[0]: {x_next[2]}')
+            # self.get_logger().info(f'x_next[0]: {x_next[0]}')
+            # self.get_logger().info(f'x_next[0]: {x_next[1]}')
+            # self.get_logger().info(f'x_next[0]: {x_next[2]}')
 
             odom_msg = Odometry()
             odom_msg = state_to_odometrymsg(x_next[:3])

diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py
@@ -34,7 +34,7 @@ def eta_callback(self, msg: Odometry):
     def send_request(self):
         if self.eta_received:
             req = Waypoint.Request()
-            wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1]]]
+            wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1] + 3]]
             req.waypoint = [Point(x=float(wp[0]), y=float(wp[1]), z=0.0) for wp in wp_list]
             self.get_logger().info(f'Sending request: {req}')
             self.future = self.client.call_async(req)

diff --git a/motion/lqr_controller/CMakeLists.txt b/motion/lqr_controller/CMakeLists.txt
@@ -20,7 +20,7 @@ install(DIRECTORY
 )
 
 install(PROGRAMS
-lqr_controller/lqr_controller_node.py
+  lqr_controller/lqr_controller_node.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py
@@ -5,8 +5,8 @@
 from rclpy.node import Node
 from geometry_msgs.msg import Wrench
 from nav_msgs.msg import Odometry
-from lqr_controller.lqr_controller import LQRController
-from lqr_controller.conversions import odometrymsg_to_state
+from lqr_controller import LQRController
+from conversions import odometrymsg_to_state
 from time import sleep
 
 from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy

diff --git a/motion/pid_controller/CMakeLists.txt b/motion/pid_controller/CMakeLists.txt
@@ -20,7 +20,7 @@ install(DIRECTORY
 )
 
 install(PROGRAMS
-pid_controller/pid_controller_node.py
+  pid_controller/pid_controller_node.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

diff --git a/motion/pid_controller/launch/pid_controller.launch.py b/motion/pid_controller/launch/pid_controller.launch.py
diff --git a/motion/pid_controller/pid_controller/pid_controller_node.py b/motion/pid_controller/pid_controller/pid_controller_node.py
@@ -5,8 +5,8 @@
 from rclpy.node import Node
 from geometry_msgs.msg import Wrench
 from nav_msgs.msg import Odometry
-from pid_controller.pid_controller import PID
-from pid_controller.conversions import odometrymsg_to_state
+from pid_controller import PID
+from conversions import odometrymsg_to_state
 from time import sleep
 
 from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy
@@ -67,7 +67,7 @@ def guidance_cb(self, msg):
     def controller_callback(self):
         if hasattr(self, 'state') and hasattr(self, 'x_ref'):
             control_input = self.pid.calculate_control_input(self.state[:3], self.x_ref, self.state[3:], self.controller_period)
-            self.get_logger().info(f"Control input: {control_input}")
+            # self.get_logger().info(f"Control input: {control_input}")
             wrench_msg = Wrench()
             wrench_msg.force.x  = control_input[0]
             wrench_msg.force.y  = control_input[1]