diff --git a/launch.sh b/launch.sh new file mode 100755 index 0000000..72209b9 --- /dev/null +++ b/launch.sh @@ -0,0 +1,8 @@ +#!/usr/bin/env bash + +echo "hello world" + +source /opt/ros/humble/setup.bash +source /robotics/workspace/install/setup.bash + +ros2 launch grr_bringup bloodstone.launch.py \ No newline at end of file diff --git a/src/grr_guis/grr_guis/main_gui.py b/src/grr_guis/grr_guis/main_gui.py index 50569e4..f4c43f0 100644 --- a/src/grr_guis/grr_guis/main_gui.py +++ b/src/grr_guis/grr_guis/main_gui.py @@ -7,8 +7,10 @@ from PyQt5.QtMultimedia import QMediaContent, QMediaPlayer from PyQt5.QtMultimediaWidgets import QVideoWidget import sys -from std_msgs.msg import Bool, String -from PyQt5.QtGui import QPixmap +from std_msgs.msg import Bool, String, Float64MultiArray +from sensor_msgs.msg import JointState +from geometry_msgs.msg import Twist +from PyQt5.QtGui import QPixmap, QColor @@ -17,12 +19,20 @@ def __init__(self) -> None: super().__init__() self.setWindowTitle("GRR SECON APP") - self.button = QPushButton("Ready to start? No ") - self.button.setCheckable(True) - - - self.label = QLabel() + self.start_button = QPushButton("Ready to start? No ") + self.start_button.setFixedHeight(60) + self.start_button.setCheckable(True) + + self.kill_motors_button = QPushButton("Drivetrain Goals: Yes") + self.kill_motors_button.setCheckable(True) + self.kill_motors_button.setFixedHeight(120) + self.reset_servos_button = QPushButton("Reset Servos") + self.reset_servos_button.setFixedHeight(60) + + self.state_machine_button = QPushButton("Launch State Machine") + self.state_machine_button.setCheckable(True) + self.state_machine_button.setFixedHeight(60) #media player instance None, QMediaPlayer.VideoSurface self.player = QMediaPlayer(None, QMediaPlayer.VideoSurface) @@ -31,34 +41,32 @@ def __init__(self) -> None: self.videoWidget = QVideoWidget() - self.videoWidget.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - + self.middle_pane = QVBoxLayout() self.node_list = QListWidget() + self.node_list.setFixedWidth(200) + + self.label = QLabel("State Machine not running") + + self.middle_pane.addWidget(self.node_list) + self.middle_pane.addWidget(self.label) self.main_pane = QVBoxLayout() - self.main_pane.addWidget(self.label) - self.main_pane.addWidget(self.button) + self.main_pane.addWidget(self.kill_motors_button) + self.main_pane.addWidget(self.reset_servos_button) + self.main_pane.addWidget(self.state_machine_button) + self.main_pane.addWidget(self.start_button) # self.main_pane.addWidget(self.videoWidget) - - self.loadVideoButton = QPushButton('Load and Play Video') - self.main_pane.addWidget(self.loadVideoButton) - - horiz_layout.addLayout(self.main_pane) - horiz_layout.addWidget(self.node_list) + horiz_layout.addLayout(self.middle_pane) horiz_layout.addWidget(self.videoWidget) container = QWidget() container.setLayout(horiz_layout) self.player.setVideoOutput(self.videoWidget) #self.setCentralWidget(videoWidget) - - - self.loadVideoButton.clicked.connect(self.promoVid2) - # Connect the error signal to a slot #self.player.error.connect(self.handleError) @@ -66,12 +74,6 @@ def __init__(self) -> None: self.showMaximized() self.show() - def promoVid(self,main_pane): - pixmap = QPixmap('Logo_MainGreenBorder.png') - self.label.setPixmap(pixmap) - self.resize(pixmap.width(), pixmap.height()) - self.main_pane.addWidget(self.label) - # Define a method to handle errors def handleError(self, error): print("Error:", error) @@ -91,18 +93,42 @@ class Gui(Node): def __init__(self, application:QApplication, window:GRR_Window) -> None: super().__init__("GUI") self.start_button_pub = self.create_publisher(Bool, "/gui/start", 10) - self.test_label = self.create_subscription(String, "/gui/info", self.display_test, 10) + self.drivetrain_enable = self.create_publisher(Bool, "/drivetrain/publish", 10) + self.test_label = self.create_subscription(String, "/grr/state_name", self.display_test, 10) self.gui_loop_timer = self.create_timer(.1, self.gui_loop) + self.gui_run_video = self.create_subscription(Bool, "/grr/promo_display", self.run_video, 10) self.app = application self.window = window self.previous_nodes = [] + self.drive_train_enable = True + + self.reset_command = JointState(name=['small_package_sweeper_joint', 'bridge_latch_joint', 'mechanism_package_joint', 'mechanism_thruster_joint', 'mechanism_lift_joint'], position=[0.0, 100.0, 50.0, 0.0, 100.0]) + + self.servo_pub = self.create_publisher(JointState, "/grr/joint_command", 10) + + self.effort_pub = self.create_publisher(Float64MultiArray, '/effort_controller/commands', 10) + self.raw_cmd = self.create_publisher(Twist, "/grr_cmake_controller/cmd_vel_unstamped", 10) + + + self.expected_nodes = ["/roboclaw_wrapper", "/controller_manager", "/isaac_hardware_interface", "/effort_controller", "/joint_state_broadcaster", "/grr_cmake_controller", "/grr/robot", "/grr/drivetrain", "/grr/gui", "/robot_state_publisher", "/odometry", "/State_Machine", "/grr/fake"] self.bind() def bind(self): - self.window.button.clicked.connect(self.send_start) + self.window.start_button.clicked.connect(self.send_start) + self.window.kill_motors_button.clicked.connect(self.kill_motor) + self.window.reset_servos_button.clicked.connect(self.reset_servos) + + def reset_servos(self): + self.servo_pub.publish(self.reset_command) + + def kill_motor(self, checked): + self.window.kill_motors_button.setText(f"Drivetrain Goals: {'No ' if checked else 'Yes'}") + self.drive_train_enable = not checked + self.effort_pub.publish(Float64MultiArray(data=[0.0, 0.0])) + self.raw_cmd.publish(Twist()) def send_start(self, checked): - self.window.button.setText(f"Ready to Start? {'Yes' if checked else 'No '}") + self.window.start_button.setText(f"Ready to Start? {'Yes' if checked else 'No '}") self.start_button_pub.publish(Bool(data=checked)) def run_video(self, msg:Bool): @@ -110,16 +136,30 @@ def run_video(self, msg:Bool): self.window.promoVid2() def display_test(self, msg:String): - print(msg) self.window.label.setText(msg.data) + def display_nodes(self, nodes: list[str]): + self.window.node_list.clear() + maluable_nodes = nodes.copy() + for node in self.expected_nodes: + item = QListWidgetItem(node) + if not node in maluable_nodes: + item.setBackground(QColor(255, 0, 0, 100)) + else: + maluable_nodes.pop(maluable_nodes.index(node)) + self.window.node_list.addItem(item) + for node in maluable_nodes: + item = QListWidgetItem(node) + item.setBackground(QColor(255, 255, 0, 100)) + self.window.node_list.addItem(item) + self.previous_nodes = nodes + def gui_loop(self): self.app.processEvents(flags=QEventLoop.ProcessEventsFlag.AllEvents) nodes = [((y + "/") if not y == "/" else "/") + x for x,y in self.get_node_names_and_namespaces()] if not nodes == self.previous_nodes: - self.window.node_list.clear() - self.window.node_list.addItems(nodes) - self.previous_nodes = nodes + self.display_nodes(nodes) + self.drivetrain_enable.publish(Bool(data=self.drive_train_enable)) def main(): diff --git a/src/grr_python_controllers/grr_python_controllers/state_machine.py b/src/grr_python_controllers/grr_python_controllers/state_machine.py index 25a161c..958102e 100644 --- a/src/grr_python_controllers/grr_python_controllers/state_machine.py +++ b/src/grr_python_controllers/grr_python_controllers/state_machine.py @@ -7,7 +7,7 @@ from nav_msgs.msg import Odometry from geometry_msgs.msg import Twist, Pose, Point, Quaternion, Twist, Vector3 from trajectory_msgs.msg import JointTrajectory -from std_msgs.msg import Bool, Float64, Float64MultiArray +from std_msgs.msg import Bool, Float64, Float64MultiArray, String from sensor_msgs.msg import JointState, LaserScan from collections import defaultdict @@ -27,10 +27,16 @@ class StateMachine(Node): def __init__(self): super().__init__('State_Machine') + self.promo_display = False + + def set_true(): self.promo_display=True + # self.action_tree = ServoAction(JointState(name=['small_package_sweeper_joint', 'bridge_latch_joint', 'mechansim_thruster_joint', 'mechanism_lift_joint'], position=[0.0, 100.0, 0.0, 100.0]), 1, name="starting positions") self.action_tree = ServoAction(JointState(name=['small_package_sweeper_joint', 'bridge_latch_joint', 'mechanism_package_joint', 'mechanism_thruster_joint', 'mechanism_lift_joint'], position=[0.0, 100.0, 50.0, 0.0, 100.0]), 1, name="starting positions") self.action_tree.setNext( CornerReset(Pose()) + ).setNext( + Action(function=set_true) ).setNext( WaitForStart() ).setNext( @@ -163,8 +169,6 @@ def __init__(self): self.new_pose = self.create_publisher(Pose, "/pose", 10) self.effort_pub = self.create_publisher(Float64MultiArray, '/effort_controller/commands', 10) self.raw_cmd = self.create_publisher(Twist, "/grr_cmake_controller/cmd_vel_unstamped", 10) - - self.position = Pose() @@ -180,6 +184,14 @@ def __init__(self): self.current_action_name = "" + self.current_action_pub = self.create_publisher(String, "/grr/state_name", 10) + + self.promo_pub = self.create_publisher(Bool, "/grr/promo_display", 10) + self.promo_timer = self.create_timer(1, self.promo_timer_loop) + + def promo_timer_loop(self): + self.promo_pub.publish(Bool(data=self.promo_display)) + def magnent_callback(self, msg:Vector3): self.get_logger().info(f"{msg}") self.magnent_data = msg @@ -195,6 +207,7 @@ def runner(self): if self.action_tree.name != self.current_action_name: self.get_logger().info(f"Starting: {self.action_tree.name}") self.current_action_name = self.action_tree.name + self.current_action_pub.publish(String(data=self.current_action_name)) def bool_callback(self, msg:Bool, name:str): self.bools[name] = msg.data diff --git a/src/grr_python_controllers/state_actions/action.py b/src/grr_python_controllers/state_actions/action.py index 886e597..3a56155 100644 --- a/src/grr_python_controllers/state_actions/action.py +++ b/src/grr_python_controllers/state_actions/action.py @@ -10,12 +10,16 @@ from geometry_msgs.msg import Twist, Pose from trajectory_msgs.msg import JointTrajectory from std_msgs.msg import Bool, Float32 + +from typing import Callable + class Action: nextAction : Action name: str - def __init__(self) -> None: + def __init__(self, function: Callable = None) -> None: self.name = self.__class__.__name__ self.nextAction = self + self.function = function def setNext(self, nextAction: Action): nextAction.nextAction = self.nextAction @@ -30,4 +34,7 @@ def setLast(self, lastAction: Action): return self.nextAction def run(self, node:Node): + if self.function: + self.function() + return self.nextAction return self \ No newline at end of file