You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
68 lines
1.9 KiB
68 lines
1.9 KiB
import threading
|
|
import time
|
|
|
|
import rclpy
|
|
|
|
from decoupled_wbc.control.main.constants import NAV_CMD_TOPIC
|
|
from decoupled_wbc.control.policy.keyboard_navigation_policy import KeyboardNavigationPolicy
|
|
from decoupled_wbc.control.utils.keyboard_dispatcher import KeyboardListenerSubscriber
|
|
from decoupled_wbc.control.utils.ros_utils import ROSMsgPublisher
|
|
|
|
FREQUENCY = 10
|
|
NAV_NODE_NAME = "NavigationPolicy"
|
|
|
|
|
|
def main():
|
|
rclpy.init(args=None)
|
|
node = rclpy.create_node(NAV_NODE_NAME)
|
|
|
|
# Start ROS spin in a separate thread
|
|
thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
|
|
thread.start()
|
|
time.sleep(0.5)
|
|
|
|
dict_publisher = ROSMsgPublisher(NAV_CMD_TOPIC)
|
|
keyboard_listener = KeyboardListenerSubscriber()
|
|
|
|
# Initialize navigation policy
|
|
navigation_policy = KeyboardNavigationPolicy()
|
|
|
|
# Create rate controller
|
|
rate = node.create_rate(FREQUENCY)
|
|
|
|
try:
|
|
while rclpy.ok():
|
|
t_now = time.monotonic()
|
|
# get keyboard input
|
|
|
|
navigation_policy.handle_keyboard_button(keyboard_listener.read_msg())
|
|
# Get action from navigation policy
|
|
action = navigation_policy.get_action(time=t_now)
|
|
|
|
# Add timestamp to the data
|
|
action["timestamp"] = t_now
|
|
|
|
# Create and publish ByteMultiArray message
|
|
dict_publisher.publish(action)
|
|
|
|
# Print status periodically (optional)
|
|
if int(t_now * 10) % 10 == 0:
|
|
nav_cmd = action["navigate_cmd"]
|
|
node.get_logger().info(
|
|
f"Nav cmd: linear=({nav_cmd[0]:.2f}, {nav_cmd[1]:.2f}), "
|
|
f"angular={nav_cmd[2]:.2f}"
|
|
)
|
|
|
|
rate.sleep()
|
|
|
|
except KeyboardInterrupt:
|
|
print("Navigation control loop terminated by user")
|
|
|
|
finally:
|
|
# Clean shutdown
|
|
node.destroy_node()
|
|
rclpy.shutdown()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|