Skip to content

Commit

Permalink
car.launch.py fixed as well as corresponding nodes controllery.py and…
Browse files Browse the repository at this point in the history 
… particle_odom_filter.py
  • Loading branch information
@DinoSage
DinoSage committed Mar 2, 2024
1 parent 5088eac commit 34e1c5e
Show file tree
Hide file tree
Showing 4 changed files with 64 additions and 54 deletions.
26 changes: 15 additions & 11 deletions src/rktl_control/rktl_control/controller.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -108,23 +108,25 @@ 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,
self.node.declare_parameter('~pid/kd').value,
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}")
Expand All @@ -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."""
Expand Down Expand Up @@ -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()
main()
6 changes: 5 additions & 1 deletion src/rktl_control/rktl_control/keyboard_interface.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,11 +63,15 @@ 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')

effort_pub = node.create_publisher(ControlEffort, 'effort', queue_size=1)
reset_srv = node.create_client(Empty, '/sim_reset')

wrapper(main)

if __name__ == "__main__":
main()

81 changes: 41 additions & 40 deletions src/rktl_control/rktl_control/particle_odom_filter.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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."""
Expand Down
5 changes: 3 additions & 2 deletions src/rktl_control/setup.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
],
},
)

0 comments on commit 34e1c5e

Please sign in to comment.