launch from desktop

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

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,47 +41,39 @@ 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)
 
         self.setCentralWidget(container)
         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,35 +93,73 @@ 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):
         if msg.data:
             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():

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

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