From ff7a587f80368392f0bdad523e00f8208871a1e9 Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Tue, 27 Feb 2018 17:39:58 -0800 Subject: [PATCH] Added pycharm files to gitignore. Started control coordinator refactore for multiple inputs. Last commit before made layout refactor --- .gitignore | 4 + .../control_coordinator.py | 9 +- .../coordinators/__init__.py | 0 .../coordinators/drive_coordinator.py | 85 +++++++++++++++++++ 4 files changed, 96 insertions(+), 2 deletions(-) create mode 100644 software/rover/rover_control/src/control_coordinator/coordinators/__init__.py create mode 100644 software/rover/rover_control/src/control_coordinator/coordinators/drive_coordinator.py diff --git a/.gitignore b/.gitignore index 7f787ce..64f0971 100644 --- a/.gitignore +++ b/.gitignore @@ -94,3 +94,7 @@ ground_station/resources/core/key # Map Images *.jpg *.png + +# Pycharm environment files +.idea/ +.directory diff --git a/software/rover/rover_control/src/control_coordinator/control_coordinator.py b/software/rover/rover_control/src/control_coordinator/control_coordinator.py index e3e0d1a..b2ee1f9 100755 --- a/software/rover/rover_control/src/control_coordinator/control_coordinator.py +++ b/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) diff --git a/software/rover/rover_control/src/control_coordinator/coordinators/__init__.py b/software/rover/rover_control/src/control_coordinator/coordinators/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/software/rover/rover_control/src/control_coordinator/coordinators/drive_coordinator.py b/software/rover/rover_control/src/control_coordinator/coordinators/drive_coordinator.py new file mode 100644 index 0000000..806628b --- /dev/null +++ b/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) \ No newline at end of file