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

Got point and shoot control working. Needs zooming. Backup scale node added. Lots of random things, super tired.

Browse Source
This commit is contained in:
Corwin Perren
2018-08-11 07:12:06 -07:00
parent 12ed32ef54
commit 03cdd2e3a6
27 changed files with 768 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
software/ros_packages/rover_control/CMakeLists.txt
View File

@@ -59,6 +59,7 @@ find_package(catkin REQUIRED COMPONENTS
MiningStatusMessage.msg
GripperControlMessage.msg
GripperStatusMessage.msg
CameraControlMessage.msg
)
## Generate services in the 'srv' folder

0
software/ros_packages/rover_control/__init__.py Normal file
View File

8
software/ros_packages/rover_control/msg/CameraControlMessage.msg Normal file
View File

@@ -0,0 +1,8 @@
uint8 camera_mode
bool zoom_in
bool zoom_out
bool full_zoom_in
bool full_zoom_out
bool focus
bool shoot

0
software/ros_packages/rover_control/src/__init__.py Normal file
View File

0
software/ros_packages/rover_control/src/effectors_control/__init__.py Normal file
View File

82
software/ros_packages/rover_control/src/effectors_control/effectors_control.py
View File

@@ -12,7 +12,7 @@ import minimalmodbus
# from std_msgs.msg import UInt8, UInt16
# Custom Imports
from rover_control.msg import MiningControlMessage, MiningStatusMessage, GripperControlMessage, GripperStatusMessage
from rover_control.msg import MiningControlMessage, MiningStatusMessage, GripperControlMessage, GripperStatusMessage, CameraControlMessage
#####################################
# Global Variables
@@ -45,6 +45,8 @@ GRIPPER_STATUS_PUBLISHER_TOPIC = "gripper/status"
MINING_CONTROL_SUBSCRIBER_TOPIC = "mining/control"
MINING_STATUS_PUBLISHER_TOPIC = "mining/status"
CAMERA_CONTROL_SUBSCRIBER_TOPIC = "camera/control"
# ##### Gripper Defines #####
GRIPPER_MODBUS_REGISTERS = {
"MODE": 0,
@@ -94,9 +96,17 @@ MINING_MODBUS_REGISTERS = {
"TARE": 7,
"CAL_FACTOR": 8,
"LIFT_POSITION": 9,
"TILT_POSITION": 10,
"MEASURED_WEIGHT": 11
"CHANGE_VIEW_MODE": 9,
"ZOOM_IN": 10,
"ZOOM_OUT": 11,
"FULL_ZOOM_IN": 12,
"FULL_ZOOM_OUT": 13,
"FOCUS": 14,
"SHOOT": 15,
"CURRENT_POSITION_LIFT": 16,
"CURRENT_POSITION_TILT": 17,
"MEASURED_WEIGHT": 18
}
@@ -139,6 +149,9 @@ class EffectorsControl(object):
self.mining_status_publisher_topic = rospy.get_param("~mining_status_publisher_topic",
MINING_STATUS_PUBLISHER_TOPIC)
self.camera_control_subscriber_topic = rospy.get_param("~camera_control_subscriber_topic",
CAMERA_CONTROL_SUBSCRIBER_TOPIC)
self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
self.gripper_node = None # type:minimalmodbus.Instrument
@@ -153,14 +166,14 @@ class EffectorsControl(object):
# ##### Subscribers #####
self.gripper_control_subscriber = rospy.Subscriber(self.gripper_control_subscriber_topic, GripperControlMessage, self.gripper_control_message_received__callback)
self.mining_control_subscriber = rospy.Subscriber(self.mining_control_subscriber_topic, MiningControlMessage,
self.mining_control_message_received__callback)
self.mining_control_subscriber = rospy.Subscriber(self.mining_control_subscriber_topic, MiningControlMessage, self.mining_control_message_received__callback)
self.camera_control_subscriber = rospy.Subscriber(self.camera_control_subscriber_topic, CameraControlMessage, self.camera_control_message_received__callback)
# ##### Publishers #####
self.gripper_status_publisher = rospy.Publisher(self.gripper_status_publisher_topic, GripperStatusMessage, queue_size=1)
self.mining_status_publisher = rospy.Publisher(self.mining_status_publisher_topic, MiningStatusMessage,
queue_size=1)
self.mining_status_publisher = rospy.Publisher(self.mining_status_publisher_topic, MiningStatusMessage, queue_size=1)
# ##### Misc #####
self.modbus_nodes_seen_time = time()
@@ -175,6 +188,9 @@ class EffectorsControl(object):
self.gripper_control_message = None
self.new_gripper_control_message = False
self.camera_control_message = None # type: CameraControlMessage
self.new_camera_control_message = False
self.failed_gripper_modbus_count = 0
self.failed_mining_modbus_count = 0
@@ -210,7 +226,8 @@ class EffectorsControl(object):
try:
self.run_mining()
self.failed_mining_modbus_count = 0
except:
except Exception, e:
print e
self.failed_mining_modbus_count += 1
if self.failed_mining_modbus_count == FAILED_MINING_MODBUS_LIMIMT:
@@ -224,6 +241,7 @@ class EffectorsControl(object):
def run_mining(self):
self.process_mining_control_message()
self.send_mining_status_message()
self.process_camera_control_message()
def connect_to_nodes(self):
self.gripper_node = minimalmodbus.Instrument(self.port, int(self.gripper_node_id))
@@ -268,13 +286,39 @@ class EffectorsControl(object):
self.modbus_nodes_seen_time = time()
self.new_mining_control_message = False
def process_camera_control_message(self):
if self.new_camera_control_message:
print self.camera_control_message
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_ABSOLUTE"]] = 1024
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_POSITIVE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_NEGATIVE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_ABSOLUTE"]] = 1024
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_POSITIVE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_NEGATIVE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["MEASURE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["TARE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["CHANGE_VIEW_MODE"]] = self.camera_control_message.camera_mode
self.mining_registers[MINING_MODBUS_REGISTERS["ZOOM_IN"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["ZOOM_OUT"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["FULL_ZOOM_IN"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["FULL_ZOOM_OUT"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["FOCUS"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["SHOOT"]] = 0
self.mining_node.write_registers(0, self.mining_registers)
self.modbus_nodes_seen_time = time()
self.new_camera_control_message = False
def send_mining_status_message(self):
if self.mining_node_present:
self.mining_registers = self.mining_node.read_registers(0, len(MINING_MODBUS_REGISTERS))
message = MiningStatusMessage()
message.lift_position = self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_POSITION"]]
message.tilt_position = self.mining_registers[MINING_MODBUS_REGISTERS["TILT_POSITION"]]
message.lift_position = self.mining_registers[MINING_MODBUS_REGISTERS["CURRENT_POSITION_LIFT"]]
message.tilt_position = self.mining_registers[MINING_MODBUS_REGISTERS["CURRENT_POSITION_TILT"]]
message.measured_weight = self.mining_registers[MINING_MODBUS_REGISTERS["MEASURED_WEIGHT"]]
self.mining_status_publisher.publish(message)
@@ -296,16 +340,17 @@ class EffectorsControl(object):
homing_complete = False
while not homing_complete:
registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS))
# print registers
self.gripper_registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS))
self.send_gripper_status_message()
if registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION_OUTPUT"]] == 0:
if self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION_OUTPUT"]] == 0:
homing_complete = True
self.gripper_registers = None
else:
if self.gripper_control_message.toggle_light:
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] = not self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]]
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] = 0 if self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] else 1
self.gripper_control_message.toggle_light = False
gripper_absolute = self.gripper_control_message.gripper_position_absolute
gripper_relative = self.gripper_control_message.gripper_position_relative
@@ -320,7 +365,8 @@ class EffectorsControl(object):
self.gripper_node.write_registers(0, DEFAULT_GRIPPER_REGISTERS)
self.new_gripper_control_message = False
self.gripper_control_message = None
self.new_gripper_control_message = False
def send_gripper_status_message(self):
registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS))
@@ -348,6 +394,10 @@ class EffectorsControl(object):
self.mining_control_message = control_message
self.new_mining_control_message = True
def camera_control_message_received__callback(self, control_message):
self.camera_control_message = control_message
self.new_camera_control_message = True
if __name__ == "__main__":
EffectorsControl()

12
software/ros_packages/rover_control/src/effectors_control/effectors_tester.py
View File

@@ -2,20 +2,18 @@
import rospy
import time
from rover_control.msg import GripperControlMessage
from rover_control.msg import CameraControlMessage
TOPIC = "/gripper/control"
TOPIC = "/rover_control/camera/control"
rospy.init_node("effectors_tester")
publisher = rospy.Publisher(TOPIC, GripperControlMessage, queue_size=1)
publisher = rospy.Publisher(TOPIC, CameraControlMessage, queue_size=1)
time.sleep(2)
message = GripperControlMessage()
message.gripper_mode = 1
message.gripper_position = 0
message.should_home = 0
message = CameraControlMessage()
message.camera_mode = 2
publisher.publish(message)

58
software/ros_packages/rover_control/src/effectors_control/emergency_scale.py Normal file
View File

@@ -0,0 +1,58 @@
#!/usr/bin/env python
#####################################
# Imports
#####################################
# Python native imports
import rospy
from time import time, sleep
import serial
from std_msgs.msg import Float64
#####################################
# Global Variables
#####################################
NODE_NAME = "effectors_control"
# ##### Communication Defines #####
DEFAULT_PORT = "/dev/rover/ttyScale"
# DEFAULT_PORT = "/dev/ttyUSB3"
DEFAULT_BAUD = 115200
DEFAULT_TOPIC = "scale/measurement"
CAL_FACTOR = 89500
#####################################
# DriveControl Class Definition
#####################################
class EmergencyScale(object):
def __init__(self):
rospy.init_node(NODE_NAME)
self.scale_serial = serial.Serial(port=DEFAULT_PORT, baudrate=DEFAULT_BAUD, timeout=200)
self.publisher = rospy.Publisher(DEFAULT_TOPIC, Float64, queue_size=1)
self.run()
def run(self):
sleep(.2)
self.scale_serial.write(str(CAL_FACTOR))
sleep(.2)
while not rospy.is_shutdown():
value = self.scale_serial.readline()
try:
message = Float64()
message.data = float(value)
self.publisher.publish(message)
print
except:
print "No data"
if __name__ == "__main__":
EmergencyScale()