From 4a1110f7b888947c148152fd155e8b6e7b3ccce1 Mon Sep 17 00:00:00 2001 From: Andeshog <157955984+Andeshog@users.noreply.github.com> Date: Mon, 29 Apr 2024 08:38:22 +0200 Subject: [PATCH] Made LQR and PID ready for testing --- guidance/dp_guidance/dp_guidance/dp_guidance_node.py | 10 +++++----- .../hybridpath_guidance/waypoint_node.py | 2 +- motion/lqr_controller/CMakeLists.txt | 2 +- .../lqr_controller/lqr_controller_node.py | 4 ++-- motion/pid_controller/CMakeLists.txt | 2 +- motion/pid_controller/launch/pid_controller.launch.py | 0 .../pid_controller/pid_controller_node.py | 6 +++--- 7 files changed, 13 insertions(+), 13 deletions(-) mode change 100644 => 100755 motion/lqr_controller/lqr_controller/lqr_controller_node.py mode change 100644 => 100755 motion/pid_controller/launch/pid_controller.launch.py mode change 100644 => 100755 motion/pid_controller/pid_controller/pid_controller_node.py diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py index a416f952..29a87a13 100755 --- 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 index bae7f73b..ff8dc843 100755 --- 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 index cafa3ad4..874cf293 100644 --- 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 old mode 100644 new mode 100755 index 18bebe0c..9261d41a --- 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 index 0c45a92b..dffae1df 100644 --- 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 old mode 100644 new mode 100755 diff --git a/motion/pid_controller/pid_controller/pid_controller_node.py b/motion/pid_controller/pid_controller/pid_controller_node.py old mode 100644 new mode 100755 index 4c3e187c..700a9d4d --- 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]