Added pycharm files to gitignore. Started control coordinator refactore for multiple inputs. Last commit before made layout refactor

.gitignore

@@ -94,3 +94,7 @@ ground_station/resources/core/key
 # Map Images
 *.jpg
 *.png
+
+# Pycharm environment files
+.idea/
+.directory

software/rover/rover_control/src/control_coordinator/control_coordinator.py

@@ -16,6 +16,7 @@ from rover_control.msg import DriveCommandMessage, DriveControlMessage
 NODE_NAME = "control_coordinator"
 
 DEFAULT_IRIS_DRIVE_COMMAND_TOPIC = "command_control/iris_drive"
+DEFAULT_GROUND_STATION_DRIVE_COMMAND_TOPIC = "command_control/ground_station_drive"
 DEFAULT_REAR_BOGIE_TOPIC = "drive_control/rear"
 DEFAULT_LEFT_BOGIE_TOPIC = "drive_control/left"
 DEFAULT_RIGHT_BOGIE_TOPIC = "drive_control/right"
@@ -30,13 +31,17 @@ class ControlCoordinator(object):
     def __init__(self):
         rospy.init_node(NODE_NAME)
 
-        self.iris_topic = rospy.get_param("~iris_drive_command_topic", DEFAULT_IRIS_DRIVE_COMMAND_TOPIC)
+        self.iris_drive_command_topic = rospy.get_param("~iris_drive_command_topic", DEFAULT_IRIS_DRIVE_COMMAND_TOPIC)
+        self.ground_station_drive_command_topic = \
+            rospy.get_param("~iris_drive_command_topic", DEFAULT_IRIS_DRIVE_COMMAND_TOPIC)
         self.rear_bogie_topic = rospy.get_param("~rear_bogie_control_topic", DEFAULT_REAR_BOGIE_TOPIC)
         self.left_bogie_topic = rospy.get_param("~left_bogie_control_topic", DEFAULT_LEFT_BOGIE_TOPIC)
         self.right_bogie_topic = rospy.get_param("~right_bogie_control_topic", DEFAULT_RIGHT_BOGIE_TOPIC)
 
         self.iris_drive_command_subscriber = \
-            rospy.Subscriber(self.iris_topic, DriveCommandMessage, self.iris_drive_command_callback)
+            rospy.Subscriber(self.iris_drive_command_topic, DriveCommandMessage, self.iris_drive_command_callback)
+
+        self.ground_station_command_subscriber = rospy.Subscriber(self.ground_station_drive_command_topic, )
 
         self.rear_bogie_publisher = rospy.Publisher(self.rear_bogie_topic, DriveControlMessage, queue_size=1)
         self.left_bogie_publisher = rospy.Publisher(self.left_bogie_topic, DriveControlMessage, queue_size=1)

software/rover/rover_control/src/control_coordinator/coordinators/drive_coordinator.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python
+#####################################
+# Imports
+#####################################
+# Python native imports
+import rospy
+
+from time import time, sleep
+
+# Custom Imports
+from rover_control.msg import DriveCommandMessage, DriveControlMessage
+
+#####################################
+# Global Variables
+#####################################
+NODE_NAME = "control_coordinator"
+
+DEFAULT_IRIS_DRIVE_COMMAND_TOPIC = "command_control/iris_drive"
+DEFAULT_GROUND_STATION_DRIVE_COMMAND_TOPIC = "command_control/ground_station_drive"
+DEFAULT_REAR_BOGIE_TOPIC = "drive_control/rear"
+DEFAULT_LEFT_BOGIE_TOPIC = "drive_control/left"
+DEFAULT_RIGHT_BOGIE_TOPIC = "drive_control/right"
+
+UINT16_MAX = 65535
+
+
+#####################################
+# ControlCoordinator Class Definition
+#####################################
+class DriveCoordinator(object):
+    def __init__(self):
+        self.iris_drive_command_topic = rospy.get_param("~iris_drive_command_topic", DEFAULT_IRIS_DRIVE_COMMAND_TOPIC)
+        self.ground_station_drive_command_topic = \
+            rospy.get_param("~iris_drive_command_topic", DEFAULT_IRIS_DRIVE_COMMAND_TOPIC)
+        self.rear_bogie_topic = rospy.get_param("~rear_bogie_control_topic", DEFAULT_REAR_BOGIE_TOPIC)
+        self.left_bogie_topic = rospy.get_param("~left_bogie_control_topic", DEFAULT_LEFT_BOGIE_TOPIC)
+        self.right_bogie_topic = rospy.get_param("~right_bogie_control_topic", DEFAULT_RIGHT_BOGIE_TOPIC)
+
+        self.iris_drive_command_subscriber = \
+            rospy.Subscriber(self.iris_drive_command_topic, DriveCommandMessage, self.iris_drive_command_callback)
+
+        self.ground_station_command_subscriber = rospy.Subscriber(self.ground_station_drive_command_topic, )
+
+        self.rear_bogie_publisher = rospy.Publisher(self.rear_bogie_topic, DriveControlMessage, queue_size=1)
+        self.left_bogie_publisher = rospy.Publisher(self.left_bogie_topic, DriveControlMessage, queue_size=1)
+        self.right_bogie_publisher = rospy.Publisher(self.right_bogie_topic, DriveControlMessage, queue_size=1)
+
+        self.run()
+
+    def run(self):
+        while not rospy.is_shutdown():
+            sleep(0.25)
+
+    def iris_drive_command_callback(self, drive_command):
+        rear_drive = DriveControlMessage()
+        left_drive = DriveControlMessage()
+        right_drive = DriveControlMessage()
+
+        left = drive_command.drive_twist.linear.x - drive_command.drive_twist.angular.z
+        right = drive_command.drive_twist.linear.x + drive_command.drive_twist.angular.z
+
+        left_direction = 1 if left >= 0 else 0
+        rear_drive.first_motor_direction = left_direction
+        left_drive.first_motor_direction = left_direction
+        left_drive.second_motor_direction = left_direction
+
+        right_direction = 1 if right >= 0 else 0
+        rear_drive.second_motor_direction = right_direction
+        right_drive.first_motor_direction = right_direction
+        right_drive.second_motor_direction = right_direction
+
+        left_speed = min(abs(left * UINT16_MAX), UINT16_MAX)
+        right_speed = min(abs(right * UINT16_MAX), UINT16_MAX)
+
+        rear_drive.first_motor_speed = left_speed
+        left_drive.first_motor_speed = left_speed
+        left_drive.second_motor_speed = left_speed
+
+        rear_drive.second_motor_speed = right_speed
+        right_drive.first_motor_speed = right_speed
+        right_drive.second_motor_speed = right_speed
+
+        self.rear_bogie_publisher.publish(rear_drive)
+        self.left_bogie_publisher.publish(left_drive)
+        self.right_bogie_publisher.publish(right_drive)