Major changes. Refactored. Drive now arbitrates between FrSky and ground station.

This commit is contained in:
2018-02-27 21:47:23 -08:00
parent 934dd3eeb4
commit 230f64f4a3
20 changed files with 240 additions and 239 deletions

3
.gitignore vendored
View File

@@ -98,3 +98,6 @@ ground_station/resources/core/key
# Pycharm environment files
.idea/
.directory
# Don't commit key file
key

View File

@@ -1 +0,0 @@
/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake

View File

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

View File

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

View File

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

View File

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

View File

@@ -1,4 +1,3 @@
#!/usr/bin/env python
#####################################
# Imports
#####################################

View File

@@ -1,45 +1,6 @@
<launch>
<arg name="target" default="192.168.1.15" />
<!--&lt;!&ndash; The UDP sender node &ndash;&gt;-->
<!--<node name="udp_sender" pkg="nimbro_topic_transport" type="udp_sender" output="screen">-->
<!--&lt;!&ndash; The destination host name or IP address &ndash;&gt;-->
<!--<param name="destination_addr" value="$(arg target)" />-->
<!--<param name="destination_port" value="17001" />-->
<!--&lt;!&ndash; Load the list of topics from a YAML file &ndash;&gt;-->
<!--<rosparam param="topics">-->
<!--[{name: "/cameras/camera_chassis/image_1280x720/compressed", compress: false, rate: 30.0}]-->
<!--</rosparam>-->
<!--</node>-->
<!--&lt;!&ndash; The UDP sender node &ndash;&gt;-->
<!--<node name="udp_sender2" pkg="nimbro_topic_transport" type="udp_sender" output="screen">-->
<!--&lt;!&ndash; The destination host name or IP address &ndash;&gt;-->
<!--<param name="destination_addr" value="$(arg target)" />-->
<!--<param name="destination_port" value="17002" />-->
<!--&lt;!&ndash; Load the list of topics from a YAML file &ndash;&gt;-->
<!--<rosparam param="topics">-->
<!--[{name: "/cameras/camera_undercarriage/image_1280x720/compressed", compress: false, rate: 30.0}]-->
<!--</rosparam>-->
<!--</node>-->
<!--&lt;!&ndash; The UDP sender node &ndash;&gt;-->
<!--<node name="udp_sender3" pkg="nimbro_topic_transport" type="udp_sender" output="screen">-->
<!--&lt;!&ndash; The destination host name or IP address &ndash;&gt;-->
<!--<param name="destination_addr" value="$(arg target)" />-->
<!--<param name="destination_port" value="17003" />-->
<!--&lt;!&ndash; Load the list of topics from a YAML file &ndash;&gt;-->
<!--<rosparam param="topics">-->
<!--[{name: "/cameras/main_navigation/image_1280x720/compressed", compress: false, rate: 30.0}]-->
<!--</rosparam>-->
<!--</node>-->
<!-- The UDP sender node -->
<node name="udp_sender4" pkg="nimbro_topic_transport" type="udp_sender" output="screen">

View File

@@ -1,15 +1,15 @@
<launch>
<group ns="cameras">
<node name="camera_undercarriage" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 1" output="screen">
<node name="undercarriage" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 1" output="screen">
<param name="device_path" value="/dev/rover/camera_undercarriage"/>
</node>
<node name="main_navigation" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 2" output="screen" >
<node name="navigation" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 2" output="screen" >
<param name="device_path" value="/dev/rover/camera_main_navigation" />
</node>
<node name="camera_chassis" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 3" output="screen" >
<node name="chassis" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 3" output="screen" >
<param name="device_path" value="/dev/rover/camera_chassis" />
</node>

View File

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

View File

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

View File

@@ -1,52 +1,8 @@
<launch>
<!-- ########## Start Hardware Interface Nodes ########## -->
<!-- ### Start Drive Interfaces ### -->
<!-- Start Left Bogie Interface -->
<!-- Start Right Bogie Interface -->
<!-- Start Rear Bogie Interface -->
<!-- ### Start Arm Interfaces ### -->
<!-- Start Arm Base Interface -->
<!-- Start Arm End Effector Interface -->
<!-- ### Start Miscellaneous Interfaces ### -->
<!-- Start Sample Containment Interface -->
<!-- Start Tower Interface -->
<!-- Start Chassis Pan/Tilt Interface -->
<!-- Start Iris Interface -->
<!-- ########## Start All Rover Camera Nodes ########## -->
<!-- ### Start 2D Cameras ### -->
<group ns="cameras">
<!-- Start Undercarriage Camera -->
<node name="undercarriage" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 1" respawn="true" output="screen">
<param name="device_path" value="/dev/rover/camera_undercarriage"/>
</node>
<!-- Start Main Navigation Camera -->
<node name="main_navigation" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 2" respawn="true" output="screen" >
<param name="device_path" value="/dev/rover/camera_main_navigation" />
</node>
<!-- Start Chassis Camera -->
<node name="chassis" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 3" respawn="true" output="screen" >
<param name="device_path" value="/dev/rover/camera_chassis" />
</node>
<!-- ### Start 3D Cameras ### -->
<!-- Start ZED Camera -->
<group ns="zed">
<include file="$(find zed_wrapper)/launch/zed_camera.launch">
<!-- compliant mode for rviz -->
<arg name="odometry_frame" value="map" />
</include>
</group>
</group>
<!-- ########## Start System Status Monitoring Nodes ########## -->
<!-- Start System CPU / RAM / Filesystem Monitor -->
<!-- Start System Temperature Monitor -->
<!-- ########## Start Nimbro Topic Transport Nodes ########## -->
<include file="$(find rover_main)/launch/ground_station/topic_transport_senders.launch"/>
<include file="$(find rover_main)/launch/ground_station/topic_transport_receivers.launch"/>
<!-- ########## Start Ground Station Interface ########## -->
<node name="ground_station" pkg="ground_station" type="ground_station_launch.sh" output="screen"/>
</launch>

View File

@@ -0,0 +1,27 @@
<launch>
<group ns="cameras">
<node name="chassis_640x360" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
<param name="port" value="17001" />
</node>
<node name="undercarriage_640x360" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
<param name="port" value="17002" />
</node>
<node name="main_navigation_640x360" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
<param name="port" value="17003" />
</node>
<node name="chassis_256x144" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
<param name="port" value="17004" />
</node>
<node name="undercarriage_256x144" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
<param name="port" value="17005" />
</node>
<node name="main_navigation_256x144" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
<param name="port" value="17006" />
</node>
</group>
</launch>

View File

@@ -0,0 +1,13 @@
<launch>
<group ns="sender_transports">
<arg name="target" default="192.168.1.10" />
<node name="udp_sender4" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17020" />
<rosparam param="topics">
[{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0}]
</rosparam>
</node>
</group>
</launch>

View File

@@ -1,52 +1,14 @@
<launch>
<!-- ########## Start Hardware Interface Nodes ########## -->
<!-- ### Start Drive Interfaces ### -->
<!-- Start Left Bogie Interface -->
<!-- Start Right Bogie Interface -->
<!-- Start Rear Bogie Interface -->
<!-- ### Start Arm Interfaces ### -->
<!-- Start Arm Base Interface -->
<!-- Start Arm End Effector Interface -->
<!-- ### Start Miscellaneous Interfaces ### -->
<!-- Start Sample Containment Interface -->
<!-- Start Tower Interface -->
<!-- Start Chassis Pan/Tilt Interface -->
<!-- Start Iris Interface -->
<!-- ########## Start Rover Control Nodes ########## -->
<include file="$(find rover_main)/launch/rover/control.launch"/>
<!-- ########## Start All Rover Camera Nodes ########## -->
<!-- ### Start 2D Cameras ### -->
<group ns="cameras">
<!-- Start Undercarriage Camera -->
<node name="undercarriage" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 1" respawn="true" output="screen">
<param name="device_path" value="/dev/rover/camera_undercarriage"/>
</node>
<!-- Start Main Navigation Camera -->
<node name="main_navigation" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 2" respawn="true" output="screen" >
<param name="device_path" value="/dev/rover/camera_main_navigation" />
</node>
<!-- Start Chassis Camera -->
<node name="chassis" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 3" respawn="true" output="screen" >
<param name="device_path" value="/dev/rover/camera_chassis" />
</node>
<!-- ### Start 3D Cameras ### -->
<!-- Start ZED Camera -->
<group ns="zed">
<include file="$(find zed_wrapper)/launch/zed_camera.launch">
<!-- compliant mode for rviz -->
<arg name="odometry_frame" value="map" />
</include>
</group>
</group>
<include file="$(find rover_main)/launch/rover/cameras.launch"/>
<!-- ########## Start System Status Monitoring Nodes ########## -->
<!-- Start System CPU / RAM / Filesystem Monitor -->
<!-- Start System Temperature Monitor -->
<!-- ########## Start Nimbro Topic Transport Nodes ########## -->
<include file="$(find rover_main)/launch/rover/topic_transport_senders.launch"/>
<include file="$(find rover_main)/launch/rover/topic_transport_receivers.launch"/>
</launch>

View File

@@ -0,0 +1,18 @@
<launch>
<group ns="cameras">
<!-- Start Undercarriage Camera -->
<node name="undercarriage" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 1" respawn="true" output="screen">
<param name="device_path" value="/dev/rover/camera_undercarriage"/>
</node>
<!-- Start Main Navigation Camera -->
<node name="main_navigation" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 2" respawn="true" output="screen" >
<param name="device_path" value="/dev/rover/camera_main_navigation" />
</node>
<!-- Start Chassis Camera -->
<node name="chassis" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 3" respawn="true" output="screen" >
<param name="device_path" value="/dev/rover/camera_chassis" />
</node>
</group>
</launch>

View File

@@ -0,0 +1,28 @@
<launch>
<group ns="rover_control">
<node name="iris_controller" pkg="rover_control" type="iris_controller.py" output="screen">
<param name="port" value="/dev/rover/ttyIRIS"/>
<param name="hertz" value="16"/>
</node>
<node name="rear_bogie" pkg="rover_control" type="drive_control.py" output="screen">
<param name="port" value="/dev/rover/ttyBogieRear"/>
<param name="drive_control_topic" value="drive_control/rear"/>
</node>
<node name="left_bogie" pkg="rover_control" type="drive_control.py" output="screen">
<param name="port" value="/dev/rover/ttyBogieLeft"/>
<param name="drive_control_topic" value="drive_control/left"/>
<param name="invert_first_motor" value="True"/>
<param name="invert_second_motor" value="True"/>
</node>
<node name="right_bogie" pkg="rover_control" type="drive_control.py" output="screen">
<param name="port" value="/dev/rover/ttyBogieRight"/>
<param name="drive_control_topic" value="drive_control/right"/>
</node>
<node name="drive_coordinator" pkg="rover_control" type="drive_coordinator.py" output="screen"/>
</group>
</launch>

View File

@@ -0,0 +1,7 @@
<launch>
<group ns="rover_control">
<node name="ground_station_drive_command" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
<param name="port" value="17020" />
</node>
</group>
</launch>

View File

@@ -0,0 +1,53 @@
<launch>
<group ns="sender_transports">
<arg name="target" default="192.168.1.15" />
<node name="chassis_640x360" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17001" />
<rosparam param="topics">
[{name: "/cameras/chassis/image_640x360/compressed", compress: false, rate: 30.0}]
</rosparam>
</node>
<node name="undercarriage_640x360" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17002" />
<rosparam param="topics">
[{name: "/cameras/undercarriage/image_640x360/compressed", compress: false, rate: 30.0}]
</rosparam>
</node>
<node name="main_navigation_640x360" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17003" />
<rosparam param="topics">
[{name: "/cameras/main_navigation/image_640x360/compressed", compress: false, rate: 30.0}]
</rosparam>
</node>
<node name="chassis_256x144" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17004" />
<rosparam param="topics">
[{name: "/cameras/chassis/image_256x144/compressed", compress: false, rate: 30.0}]
</rosparam>
</node>
<node name="undercarriage_256x144" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17005" />
<rosparam param="topics">
[{name: "/cameras/undercarriage/image_256x144/compressed", compress: false, rate: 30.0}]
</rosparam>
</node>
<node name="main_navigation_256x144" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17006" />
<rosparam param="topics">
[{name: "/cameras/main_navigation/image_256x144/compressed", compress: false, rate: 30.0}]
</rosparam>
</node>
</group>
</launch>