diff --git a/.gitignore b/.gitignore index 64f0971..015af31 100644 --- a/.gitignore +++ b/.gitignore @@ -98,3 +98,6 @@ ground_station/resources/core/key # Pycharm environment files .idea/ .directory + +# Don't commit key file +key diff --git a/software/ros_packages/CMakeLists.txt b/software/ros_packages/CMakeLists.txt deleted file mode 120000 index 581e61d..0000000 --- a/software/ros_packages/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/software/ros_packages/ground_station/scripts/ground_station_launch.sh b/software/ros_packages/ground_station/scripts/ground_station_launch.sh new file mode 100755 index 0000000..ba27a66 --- /dev/null +++ b/software/ros_packages/ground_station/scripts/ground_station_launch.sh @@ -0,0 +1,17 @@ +#!/usr/bin/env bash + +current_folder_name="scripts" +current_folder_name_length=`expr length $current_folder_name` + +launch_dir="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +launch_dir_length=`expr length $launch_dir` + +launch_dir_length_without_current_folder=$(($launch_dir_length-$current_folder_name_length)) + +script_launch_path="${launch_dir:0:$launch_dir_length_without_current_folder}/src" +cd $script_launch_path + +sleep 5 + +export DISPLAY=:0 +python ground_station.py \ No newline at end of file diff --git a/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py b/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py index 13c6c6f..e323554 100644 --- a/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py +++ b/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py @@ -15,7 +15,7 @@ from rover_control.msg import DriveCommandMessage ##################################### GAME_CONTROLLER_NAME = "Logitech Logitech Extreme 3D Pro" -DEFAULT_DRIVE_COMMAND_TOPIC = "command_control/groundstation_drive" +DEFAULT_DRIVE_COMMAND_TOPIC = "/rover_control/command_control/ground_station_drive" DRIVE_COMMAND_HERTZ = 15 @@ -149,8 +149,6 @@ class RoverDriveSender(QtCore.QThread): self.run_thread_flag = True self.joystick = LogitechJoystick() - while not self.joystick.ready: - self.msleep(100) # ########## Class Variables ########## # Publishers diff --git a/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py b/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py index 03969d3..cd5acf5 100644 --- a/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py +++ b/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py @@ -127,6 +127,8 @@ class RoverVideoCoordinator(QtCore.QThread): for topics_group in topics: main_topic = topics_group[0] + if "heartbeat" in main_topic: + continue camera_name = main_topic.split("/")[2] names.append(camera_name) diff --git a/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoReceiver.py b/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoReceiver.py index 4ee7a4d..361e10b 100644 --- a/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoReceiver.py +++ b/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoReceiver.py @@ -127,6 +127,8 @@ class RoverVideoReceiver(QtCore.QThread): for topics_group in topics: main_topic = topics_group[0] + if "heartbeat" in main_topic: + continue camera_name = main_topic.split("/")[3] resolution_options.append(camera_name) diff --git a/software/ros_packages/ground_station/src/RoverGroundStation.py b/software/ros_packages/ground_station/src/ground_station.py old mode 100755 new mode 100644 similarity index 99% rename from software/ros_packages/ground_station/src/RoverGroundStation.py rename to software/ros_packages/ground_station/src/ground_station.py index 3265685..8655744 --- a/software/ros_packages/ground_station/src/RoverGroundStation.py +++ b/software/ros_packages/ground_station/src/ground_station.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python ##################################### # Imports ##################################### diff --git a/software/ros_packages/nimbro_topic_transport/launch/udp_camera_sender.launch b/software/ros_packages/nimbro_topic_transport/launch/udp_camera_sender.launch index 60f8abe..bd74ef1 100644 --- a/software/ros_packages/nimbro_topic_transport/launch/udp_camera_sender.launch +++ b/software/ros_packages/nimbro_topic_transport/launch/udp_camera_sender.launch @@ -1,45 +1,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/software/ros_packages/rover_camera/launch/example.launch b/software/ros_packages/rover_camera/launch/example.launch index a9fe144..d657d4d 100644 --- a/software/ros_packages/rover_camera/launch/example.launch +++ b/software/ros_packages/rover_camera/launch/example.launch @@ -1,15 +1,15 @@ - + - + - + diff --git a/software/ros_packages/rover_control/src/control_coordinator/coordinators/__init__.py b/software/ros_packages/rover_control/src/control_coordinator/coordinators/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/software/ros_packages/rover_control/src/control_coordinator/coordinators/drive_coordinator.py b/software/ros_packages/rover_control/src/control_coordinator/coordinators/drive_coordinator.py deleted file mode 100644 index 806628b..0000000 --- a/software/ros_packages/rover_control/src/control_coordinator/coordinators/drive_coordinator.py +++ /dev/null @@ -1,85 +0,0 @@ -#!/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 diff --git a/software/ros_packages/rover_control/src/control_coordinator/control_coordinator.py b/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py similarity index 62% rename from software/ros_packages/rover_control/src/control_coordinator/control_coordinator.py rename to software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py index b2ee1f9..1083e37 100755 --- a/software/ros_packages/rover_control/src/control_coordinator/control_coordinator.py +++ b/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py @@ -4,7 +4,7 @@ ##################################### # Python native imports import rospy - +from PyQt5 import QtCore from time import time, sleep # Custom Imports @@ -13,7 +13,7 @@ from rover_control.msg import DriveCommandMessage, DriveControlMessage ##################################### # Global Variables ##################################### -NODE_NAME = "control_coordinator" +NODE_NAME = "drive_coordinator" DEFAULT_IRIS_DRIVE_COMMAND_TOPIC = "command_control/iris_drive" DEFAULT_GROUND_STATION_DRIVE_COMMAND_TOPIC = "command_control/ground_station_drive" @@ -23,37 +23,68 @@ DEFAULT_RIGHT_BOGIE_TOPIC = "drive_control/right" UINT16_MAX = 65535 +DEFAULT_HERTZ = 15 + ##################################### # ControlCoordinator Class Definition ##################################### -class ControlCoordinator(object): +class DriveCoordinator(object): def __init__(self): + rospy.init_node(NODE_NAME) 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) + rospy.get_param("~ground_station_drive_command_topic", DEFAULT_GROUND_STATION_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.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ) - self.ground_station_command_subscriber = rospy.Subscriber(self.ground_station_drive_command_topic, ) + # ########## Class Variables ########## + 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, + DriveCommandMessage, + self.ground_station_drive_command_callback) 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) + # Other Vars TODO: fix this later + self.drive_commands = { + "iris": DriveCommandMessage(), + "ground_station": DriveCommandMessage() + } + + # ########## Run the Class ########## self.run() def run(self): while not rospy.is_shutdown(): - sleep(0.25) + start_time = time() - def iris_drive_command_callback(self, drive_command): + try: + self.process_drive_commands() + except Exception, error: + print "Error occurred:", error + + time_diff = time() - start_time + + sleep(max(self.wait_time - time_diff, 0)) + + def process_drive_commands(self): + if not self.drive_commands["iris"].ignore_drive_control: + self.send_drive_control_command(self.drive_commands["iris"]) + else: + self.send_drive_control_command(self.drive_commands["ground_station"]) + + def send_drive_control_command(self, drive_command): rear_drive = DriveControlMessage() left_drive = DriveControlMessage() right_drive = DriveControlMessage() @@ -86,5 +117,15 @@ class ControlCoordinator(object): self.left_bogie_publisher.publish(left_drive) self.right_bogie_publisher.publish(right_drive) -if __name__ == "__main__": - ControlCoordinator() + + def iris_drive_command_callback(self, drive_command): + self.drive_commands["iris"] = drive_command + return + + def ground_station_drive_command_callback(self, drive_command): + self.drive_commands["ground_station"] = drive_command + return + + +if __name__ == '__main__': + DriveCoordinator() diff --git a/software/ros_packages/rover_main/launch/ground_station.launch b/software/ros_packages/rover_main/launch/ground_station.launch index 2590035..e88ca62 100644 --- a/software/ros_packages/rover_main/launch/ground_station.launch +++ b/software/ros_packages/rover_main/launch/ground_station.launch @@ -1,52 +1,8 @@ - - - - - + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + \ No newline at end of file diff --git a/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch b/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch new file mode 100644 index 0000000..5cc8a5b --- /dev/null +++ b/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch new file mode 100644 index 0000000..5de3a5a --- /dev/null +++ b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch @@ -0,0 +1,13 @@ + + + + + + + + + [{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0}] + + + + diff --git a/software/ros_packages/rover_main/launch/rover.launch b/software/ros_packages/rover_main/launch/rover.launch index 2590035..e3af5e8 100644 --- a/software/ros_packages/rover_main/launch/rover.launch +++ b/software/ros_packages/rover_main/launch/rover.launch @@ -1,52 +1,14 @@ - - - - - + + - - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + diff --git a/software/ros_packages/rover_main/launch/rover/cameras.launch b/software/ros_packages/rover_main/launch/rover/cameras.launch new file mode 100644 index 0000000..7afa57f --- /dev/null +++ b/software/ros_packages/rover_main/launch/rover/cameras.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_main/launch/rover/control.launch b/software/ros_packages/rover_main/launch/rover/control.launch new file mode 100644 index 0000000..40cd749 --- /dev/null +++ b/software/ros_packages/rover_main/launch/rover/control.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_main/launch/rover/topic_transport_receivers.launch b/software/ros_packages/rover_main/launch/rover/topic_transport_receivers.launch new file mode 100644 index 0000000..3a8de85 --- /dev/null +++ b/software/ros_packages/rover_main/launch/rover/topic_transport_receivers.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch new file mode 100644 index 0000000..db549b5 --- /dev/null +++ b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch @@ -0,0 +1,53 @@ + + + + + + + + + [{name: "/cameras/chassis/image_640x360/compressed", compress: false, rate: 30.0}] + + + + + + + + [{name: "/cameras/undercarriage/image_640x360/compressed", compress: false, rate: 30.0}] + + + + + + + + [{name: "/cameras/main_navigation/image_640x360/compressed", compress: false, rate: 30.0}] + + + + + + + + [{name: "/cameras/chassis/image_256x144/compressed", compress: false, rate: 30.0}] + + + + + + + + [{name: "/cameras/undercarriage/image_256x144/compressed", compress: false, rate: 30.0}] + + + + + + + + [{name: "/cameras/main_navigation/image_256x144/compressed", compress: false, rate: 30.0}] + + + +