caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 18:21:15 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Corwin Perren
2018-02-27 17:39:58 -08:00
parent b0258bc100
commit ff7a587f80
4 changed files with 96 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
software/rover/rover_control/src/control_coordinator/control_coordinator.py
View File

@@ -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)

0
software/rover/rover_control/src/control_coordinator/coordinators/__init__.py Normal file
View File

85
software/rover/rover_control/src/control_coordinator/coordinators/drive_coordinator.py Normal file
View File

@@ -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)