From 34e1c5e3397211adafb965b97619543a209eb96b Mon Sep 17 00:00:00 2001 From: DinoSage Date: Sat, 2 Mar 2024 22:14:35 +0000 Subject: [PATCH] car.launch.py fixed as well as corresponding nodes controllery.py and particle_odom_filter.py --- src/rktl_control/rktl_control/controller.py | 26 +++--- .../rktl_control/keyboard_interface.py | 6 +- .../rktl_control/particle_odom_filter.py | 81 ++++++++++--------- src/rktl_control/setup.py | 5 +- 4 files changed, 64 insertions(+), 54 deletions(-) diff --git a/src/rktl_control/rktl_control/controller.py b/src/rktl_control/rktl_control/controller.py index 55071c2c..3fe47a33 100644 --- a/src/rktl_control/rktl_control/controller.py +++ b/src/rktl_control/rktl_control/controller.py @@ -97,9 +97,9 @@ def __init__(self): self.node = rclpy.create_node('controller') # Constants - self.MAX_SPEED = self.node.declare_parameter('/cars/throttle/max_speed').value - self.STEERING_THROW = self.node.declare_parameter('/cars/steering/max_throw').value - self.BODY_LENGTH = self.node.declare_parameter('/cars/length').value + self.MAX_SPEED = self.node.declare_parameter('/cars/throttle/max_speed', 2.3).value + self.STEERING_THROW = self.node.declare_parameter('/cars/steering/max_throw', 0.1826).value + self.BODY_LENGTH = self.node.declare_parameter('/cars/length', 0.12).value self.MIN_THROTTLE_EFFORT = self.node.declare_parameter('~limits/throttle/min', -1.0).value self.MAX_THROTTLE_EFFORT = self.node.declare_parameter('~limits/throttle/max', 1.0).value @@ -108,15 +108,17 @@ def __init__(self): self.PUBLISH_EARLY = self.node.declare_parameter('~open_loop/publish_early', True).value + self.CONTROLLER_TYPE = self.node.declare_parameter('~controller_type', 'none').value + # Make closed loop velocity controller - if self.node.declare_parameter('~controller_type').value == 'lead_lag': + if self.CONTROLLER_TYPE == 'lead_lag': self.controller = LeadLagController( self.node.declare_parameter('~lead/gain').value, self.node.declare_parameter('~lead/alpha').value, self.node.declare_parameter('~lead/beta').value, self.node.declare_parameter('~lag/alpha').value, self.node.declare_parameter('~lag/beta').value) - elif self.node.declare_parameter('~controller_type').value == 'pid': + elif self.CONTROLLER_TYPE == 'pid': self.controller = PIDController( self.node.declare_parameter('~pid/kp').value, self.node.declare_parameter('~pid/ki').value, @@ -124,7 +126,7 @@ def __init__(self): self.node.declare_parameter('~pid/anti_windup').value, self.node.declare_parameter('~pid/deadband').value, 1.0 / self.node.declare_parameter('~rate', 10.0).value) - elif self.node.declare_parameter('~controller_type').value == 'none': + elif self.CONTROLLER_TYPE == 'none': self.controller = None else: raise NotImplementedError(f"unrecognized controller type: {self.node.declare_parameter('controller_type').value}") @@ -134,14 +136,14 @@ def __init__(self): self.psi_ref = None # Publishers - self.pub = self.node.create_publisher(ControlEffort, 'effort', queue_size=1) + self.pub = self.node.create_publisher(ControlEffort, 'effort', 1) # Subscribers - self.node.create_subscription(ControlCommand, 'command', self.command_cb) - self.node.create_subscription(Odometry, 'odom', self.odom_cb) + self.node.create_subscription(ControlCommand, 'command', self.command_cb, 1) + self.node.create_subscription(Odometry, 'odom', self.odom_cb, 1) # trust that odom_cb runs at proper rate - rclpy.spin() + rclpy.spin(self.node) def command_cb(self, cmd_msg): """Callback for command messages for car.""" @@ -185,6 +187,8 @@ def odom_cb(self, odom_msg): msg.steering = steering_effort self.pub.publish(msg) +def main(): + Controller() if __name__ == "__main__": - Controller() \ No newline at end of file + main() \ No newline at end of file diff --git a/src/rktl_control/rktl_control/keyboard_interface.py b/src/rktl_control/rktl_control/keyboard_interface.py index 0eb31e58..1cf3d076 100644 --- a/src/rktl_control/rktl_control/keyboard_interface.py +++ b/src/rktl_control/rktl_control/keyboard_interface.py @@ -63,7 +63,7 @@ def main(win): win.addstr("q to quit") sleep(0.01) -if __name__ == "__main__": +def main(): rclpy.init(args=sys.argv) node = rclpy.create_node('keyboard') @@ -71,3 +71,7 @@ def main(win): reset_srv = node.create_client(Empty, '/sim_reset') wrapper(main) + +if __name__ == "__main__": + main() + diff --git a/src/rktl_control/rktl_control/particle_odom_filter.py b/src/rktl_control/rktl_control/particle_odom_filter.py index 2c4201c4..34221f29 100644 --- a/src/rktl_control/rktl_control/particle_odom_filter.py +++ b/src/rktl_control/rktl_control/particle_odom_filter.py @@ -31,67 +31,68 @@ def __init__(self): self.node = rclpy.create_node('particle_odom_filter') # physical constants (global) - self.FIELD_WIDTH = self.node.declare_parameter('/field/width') - self.FIELD_HEIGHT = self.node.declare_parameter('/field/length') - self.CAR_LENGTH = self.node.declare_parameter('/cars/length') - self.MAX_SPEED = self.node.declare_parameter('/cars/throttle/max_speed') - self.THROTTLE_TAU = self.node.declare_parameter('/cars/throttle/tau') - self.STEERING_THROW = self.node.declare_parameter('/cars/steering/max_throw') - self.STEERING_RATE = self.node.declare_parameter('/cars/steering/rate') + self.FIELD_WIDTH = self.node.declare_parameter('/field/width', 3).value + self.FIELD_HEIGHT = self.node.declare_parameter('/field/length', 4.25).value + self.CAR_LENGTH = self.node.declare_parameter('/cars/length', 0.12).value + self.MAX_SPEED = self.node.declare_parameter('/cars/throttle/max_speed', 2.3).value + self.THROTTLE_TAU = self.node.declare_parameter('/cars/throttle/tau', 0.2).value + self.STEERING_THROW = self.node.declare_parameter('/cars/steering/max_throw', 0.1826).value + self.STEERING_RATE = self.node.declare_parameter('/cars/steering/rate', 0.9128).value # node configuration - self.MAP_FRAME = self.node.declare_parameter('~frame_ids/map', 'map') - self.BODY_FRAME = self.node.declare_parameter('~frame_ids/body', 'base_link') - self.DELTA_T = rclpy.time.Time(1.0/self.node.declare_parameter('~rate', 10.0)) - self.SUPERSAMPLING = self.node.declare_parameter('~supersampling', 1) - self.PUB_PARTICLES = self.node.declare_parameter('~publish_particles', False) - self.WATCHDOG_DELTA_T = self.DELTA_T * \ - self.node.declare_parameter('~allowable_latency', 1.2) - self.OPEN_LOOP_LIMIT = self.node.declare_parameter('~open_loop_limit', 10) + self.MAP_FRAME = self.node.declare_parameter('~frame_ids/map', 'map').value + self.BODY_FRAME = self.node.declare_parameter('~frame_ids/body', 'base_link').value + self.TEMP_RATE = self.node.declare_parameter('~rate', 10.0).value + self.DELTA_T = rclpy.duration.Duration(nanoseconds=1.0/self.TEMP_RATE) + self.SUPERSAMPLING = self.node.declare_parameter('~supersampling', 1).value + self.PUB_PARTICLES = self.node.declare_parameter('~publish_particles', False).value + self.WATCHDOG_DELTA_T = self.DELTA_T.nanoseconds * \ + self.node.declare_parameter('~allowable_latency', 1.2).value + self.OPEN_LOOP_LIMIT = self.node.declare_parameter('~open_loop_limit', 10).value # should the filter compensate for delay by trying to predict the future? - self.PREDICT_ENABLE = self.node.declare_parameter('~delay/compensate', False) - self.PREDICT_TIME = rclpy.time.Time( - self.node.declare_parameter('~delay/duration', 0.0)) + self.PREDICT_ENABLE = self.node.declare_parameter('~delay/compensate', False).value + self.PREDICT_TIME = rclpy.time.Time(seconds= + self.node.declare_parameter('~delay/duration', 0.0).value) # should the filter weigh particles based on a boundary check? - boundary_check = self.node.declare_parameter('~boundary_check', False) + boundary_check = self.node.declare_parameter('~boundary_check', False).value # filter tuning options - num_particles = self.node.declare_parameter('~num_particles', 1000) - resample_proportion = self.node.declare_parameter('~resample_proportion', 0.1) + num_particles = self.node.declare_parameter('~num_particles', 1000).value + resample_proportion = self.node.declare_parameter('~resample_proportion', 0.1).value # standard deviation of incoming measurements used to assign particle weights self.MEAS_LOC_STD_DEV = self.node.declare_parameter( - '~measurement_error/location', 0.05) + '~measurement_error/location', 0.05).value self.MEAS_DIR_STD_DEV = self.node.declare_parameter( - '~measurement_error/orientation', np.deg2rad(5)) + '~measurement_error/orientation', np.deg2rad(5)).value # standard deviation when generating random states based off current guess self.GEN_LOC_STD_DEV = self.node.declare_parameter( - '~generator_noise/location', 0.05) + '~generator_noise/location', 0.05).value self.GEN_DIR_STD_DEV = self.node.declare_parameter( - '~generator_noise/orientation', 0.05) + '~generator_noise/orientation', 0.05).value self.GEN_VEL_STD_DEV = self.node.declare_parameter( - '~generator_noise/velocity', 0.05) + '~generator_noise/velocity', 0.05).value self.GEN_PSI_STD_DEV = self.node.declare_parameter( - '~generator_noise/steering_angle', np.deg2rad(1)) + '~generator_noise/steering_angle', np.deg2rad(1)).value # should the filter use historic effort data to get a more accurate idea of where the car is? - use_efforts = self.node.declare_parameter('~efforts/enable', False) - effort_buffer_size = self.node.declare_parameter('~efforts/buffer_size', 0) + use_efforts = self.node.declare_parameter('~efforts/enable', False).value + effort_buffer_size = self.node.declare_parameter('~efforts/buffer_size', 0).value # standard deviation to add noise to effort when effort is known and enabled self.THR_EFFORT_STD_DEV = self.node.declare_parameter( - '~efforts/throttle/noise', 0.05) + '~efforts/throttle/noise', 0.05).value self.STR_EFFORT_STD_DEV = self.node.declare_parameter( - '~efforts/steering/noise', 0.05) + '~efforts/steering/noise', 0.05).value # max and min for uniform effort distribution when effort is not known or disabled - self.MAX_THROTTLE = self.node.declare_parameter('~efforts/throttle/max', 1.0) - self.MIN_THROTTLE = self.node.declare_parameter('~efforts/throttle/min', -1.0) - self.MAX_STEERING = self.node.declare_parameter('~efforts/steering/max', 1.0) - self.MIN_STEERING = self.node.declare_parameter('~efforts/steering/min', -1.0) + self.MAX_THROTTLE = self.node.declare_parameter('~efforts/throttle/max', 1.0).value + self.MIN_THROTTLE = self.node.declare_parameter('~efforts/throttle/min', -1.0).value + self.MAX_STEERING = self.node.declare_parameter('~efforts/steering/max', 1.0).value + self.MIN_STEERING = self.node.declare_parameter('~efforts/steering/min', -1.0).value # variables self.effort_buffer = deque(maxlen=effort_buffer_size) @@ -113,16 +114,16 @@ def __init__(self): self.filter.mean_state = None # pubs / subs - self.odom_pub = self.node.create_publisher(Odometry, 'odom', queue_size=1) - self.node.create_subscription(PoseWithCovarianceStamped, 'pose_sync', self.pose_cb) + self.odom_pub = self.node.create_publisher(Odometry, 'odom', 1) + self.node.create_subscription(PoseWithCovarianceStamped, 'pose_sync', self.pose_cb, 1) if use_efforts: - self.node.create_subscription(ControlEffort, 'effort', self.effort_cb) + self.node.create_subscription(ControlEffort, 'effort', self.effort_cb, 1) if self.PUB_PARTICLES: self.cloud_pub = self.node.create_publisher(PoseArray, - 'odom_particles', queue_size=1) + 'odom_particles', 1) # main loop - rclpy.spin() + rclpy.spin(self.node) def effort_cb(self, effort_msg): """Callback for new efforts.""" diff --git a/src/rktl_control/setup.py b/src/rktl_control/setup.py index 4cb8c758..bf91df99 100644 --- a/src/rktl_control/setup.py +++ b/src/rktl_control/setup.py @@ -27,8 +27,9 @@ "controller = rktl_control.controller:main", "mean_odom_filter = rktl_control.mean_odom_filter:main", "particle_odom_filter = rktl_control.particle_odom_filter:main", - "launch_test = rktl_control.launch_test:main", - "topic_delay = rktl_control.topic_delay:main" + "topic_delay = rktl_control.topic_delay:main", + "keyboard_interface = rktl_control.keyboard_interface:main", + "pose_synchronizer.py = rktl_control.pose_synchronizer:main" ], }, )