diff --git a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules b/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules index 901ff80..8bce37c 100644 --- a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules +++ b/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules @@ -20,7 +20,7 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{seria SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyOdometry" SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyChassisPanTilt" -SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyIRIS_2_0" +SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyRDF_SoilProbe" SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyIRIS_2_1" SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyIRIS" SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyExtraUART" diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py index 0412966..c71feba 100644 --- a/software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py +++ b/software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py @@ -9,7 +9,7 @@ from time import time import rospy from rover_arm.msg import ArmControlMessage -from rover_control.msg import MiningControlMessage +from rover_control.msg import MiningControlMessage, GripperControlMessage ##################################### # Global Variables @@ -18,6 +18,7 @@ GAME_CONTROLLER_NAME = "Afterglow Gamepad for Xbox 360" DRIVE_COMMAND_HERTZ = 20 +GRIPPER_CONTROL_TOPIC = "/rover_control/gripper/control" RELATIVE_ARM_CONTROL_TOPIC = "/rover_arm/control/relative" MINING_CONTROL_TOPIC = "/rover_control/mining/control" @@ -28,6 +29,8 @@ ROLL_SCALAR = 0.003 WRIST_PITCH_SCALAR = 0.003 WRIST_ROLL_SCALAR = 0.006 +GRIPPER_MOVEMENT_SCALAR = 300 + LEFT_X_AXIS_DEADZONE = 1500 LEFT_Y_AXIS_DEADZONE = 1500 @@ -39,7 +42,6 @@ THUMB_STICK_MAX = 32768.0 MINING_LIFT_SCALAR = 5 MINING_TILT_SCALAR = 5 - COLOR_GREEN = "background-color:darkgreen; border: 1px solid black;" COLOR_NONE = "border: 1px solid black;" @@ -158,11 +160,18 @@ class XBOXController(QtCore.QThread): # Controller Class Definition ##################################### class XBOXControllerControlSender(QtCore.QThread): - STATES = [ + XBOX_CONTROL_STATES = [ "ARM", "MINING" ] + GRIPPER_CONTROL_MODES = { + "NORMAL": 1, + "TWO_FINGER_PINCH": 2, + "WIDE": 3, + "SCISSOR": 4 + } + xbox_control_arm_stylesheet_update_ready__signal = QtCore.pyqtSignal(str) xbox_control_mining_stylesheet_update_ready__signal = QtCore.pyqtSignal(str) @@ -190,14 +199,20 @@ class XBOXControllerControlSender(QtCore.QThread): self.wait_time = 1.0 / DRIVE_COMMAND_HERTZ + self.gripper_control_publisher = rospy.Publisher(GRIPPER_CONTROL_TOPIC, GripperControlMessage, queue_size=1) + self.relative_arm_control_publisher = rospy.Publisher(RELATIVE_ARM_CONTROL_TOPIC, ArmControlMessage, queue_size=1) self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1) - self.current_state = self.STATES.index("ARM") - self.state_just_changed = False + self.current_state = self.XBOX_CONTROL_STATES.index("ARM") + self.xbox_control_state_just_changed = False self.last_xbox_button_state = 0 + self.last_left_bumper_state = 0 + self.last_right_bumper_state = 0 + + self.gripper_control_mode = self.GRIPPER_CONTROL_MODES["NORMAL"] def run(self): self.logger.debug("Starting Joystick Thread") @@ -207,9 +222,9 @@ class XBOXControllerControlSender(QtCore.QThread): self.change_control_state_if_needed() - if self.current_state == self.STATES.index("ARM"): + if self.current_state == self.XBOX_CONTROL_STATES.index("ARM"): self.process_and_send_arm_control() - elif self.current_state == self.STATES.index("MINING"): + elif self.current_state == self.XBOX_CONTROL_STATES.index("MINING"): self.send_mining_commands() time_diff = time() - start_time @@ -223,31 +238,51 @@ class XBOXControllerControlSender(QtCore.QThread): self.xbox_control_mining_stylesheet_update_ready__signal.connect(self.xbox_mode_mining_label.setStyleSheet) def change_control_state_if_needed(self): - if self.state_just_changed: - self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_NONE) - self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_GREEN) - self.state_just_changed = False - xbox_state = self.controller.controller_states["xbox_button"] + left_bumper_state = self.controller.controller_states["left_bumper"] + right_bumper_state = self.controller.controller_states["right_bumper"] if self.last_xbox_button_state == 0 and xbox_state == 1: self.current_state += 1 - self.current_state = self.current_state % len(self.STATES) - self.state_just_changed = True + self.current_state = self.current_state % len(self.XBOX_CONTROL_STATES) + self.xbox_control_state_just_changed = True self.last_xbox_button_state = 1 - elif self.last_xbox_button_state == 1 and xbox_state == 0: self.last_xbox_button_state = 0 + if self.last_left_bumper_state == 0 and left_bumper_state == 1: + self.gripper_control_mode = ((self.gripper_control_mode - 1) % len(self.GRIPPER_CONTROL_MODES)) + # self.gripper_control_mode += 1 + self.last_left_bumper_state = 1 + elif self.last_left_bumper_state == 1 and left_bumper_state == 0: + self.last_left_bumper_state = 0 + + if self.last_right_bumper_state == 0 and right_bumper_state == 1: + self.gripper_control_mode = ((self.gripper_control_mode + 1) % len(self.GRIPPER_CONTROL_MODES)) + # self.gripper_control_mode += 1 + self.last_right_bumper_state = 1 + elif self.last_right_bumper_state == 1 and right_bumper_state == 0: + self.last_right_bumper_state = 0 + + if self.xbox_control_state_just_changed: + self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_NONE) + self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_GREEN) + self.xbox_control_state_just_changed = False + def process_and_send_arm_control(self): - if self.state_just_changed: + if self.xbox_control_state_just_changed: self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_GREEN) self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_NONE) - self.state_just_changed = False + self.xbox_control_state_just_changed = False - message = ArmControlMessage() + arm_control_message = ArmControlMessage() - should_publish = False + gripper_control_message = GripperControlMessage() + gripper_control_message.gripper_position_absolute = -1 + gripper_control_message.gripper_mode = self.gripper_control_mode + 1 + + should_publish_arm = False + should_publish_gripper = False left_trigger = self.controller.controller_states["left_trigger"] right_trigger = self.controller.controller_states["right_trigger"] @@ -266,29 +301,36 @@ class XBOXControllerControlSender(QtCore.QThread): left_trigger_ratio = left_trigger / 255.0 right_trigger_ratio = right_trigger / 255.0 - if left_trigger > 25: - should_publish = True - message.base = ((left_x_axis / THUMB_STICK_MAX) * BASE_SCALAR) * left_trigger_ratio - message.shoulder = ((left_y_axis / THUMB_STICK_MAX) * SHOULDER_SCALAR) * left_trigger_ratio - message.elbow = (-(right_y_axis / THUMB_STICK_MAX) * ELBOW_SCALAR) * left_trigger_ratio - message.roll = (-(right_x_axis / THUMB_STICK_MAX) * ROLL_SCALAR) * left_trigger_ratio + if left_trigger > 0: + should_publish_arm = True + arm_control_message.base = ((left_x_axis / THUMB_STICK_MAX) * BASE_SCALAR) * left_trigger_ratio + arm_control_message.shoulder = ((left_y_axis / THUMB_STICK_MAX) * SHOULDER_SCALAR) * left_trigger_ratio + arm_control_message.elbow = (-(right_y_axis / THUMB_STICK_MAX) * ELBOW_SCALAR) * left_trigger_ratio + arm_control_message.roll = (-(right_x_axis / THUMB_STICK_MAX) * ROLL_SCALAR) * left_trigger_ratio - elif right_trigger > 25: - should_publish = True - message.wrist_roll = ((right_x_axis / THUMB_STICK_MAX) * WRIST_ROLL_SCALAR) * right_trigger_ratio - message.wrist_pitch = (-(left_y_axis / THUMB_STICK_MAX) * WRIST_PITCH_SCALAR) * right_trigger_ratio + elif right_trigger > 0: + should_publish_arm = True + should_publish_gripper = True - if should_publish: - self.relative_arm_control_publisher.publish(message) + arm_control_message.wrist_roll = ((right_x_axis / THUMB_STICK_MAX) * WRIST_ROLL_SCALAR) * right_trigger_ratio + arm_control_message.wrist_pitch = (-(left_y_axis / THUMB_STICK_MAX) * WRIST_PITCH_SCALAR) * right_trigger_ratio + + gripper_control_message.gripper_position_relative = (-(right_y_axis / THUMB_STICK_MAX) * GRIPPER_MOVEMENT_SCALAR) * right_trigger_ratio + + if should_publish_arm: + self.relative_arm_control_publisher.publish(arm_control_message) + + if should_publish_gripper: + self.gripper_control_publisher.publish(gripper_control_message) def send_mining_commands(self): - left_y_axis = self.controller.controller_states["left_y_axis"] if abs(self.controller.controller_states["left_y_axis"]) > LEFT_Y_AXIS_DEADZONE else 0 + left_y_axis = self.controller.controller_states["left_y_axis"] if abs( + self.controller.controller_states["left_y_axis"]) > LEFT_Y_AXIS_DEADZONE else 0 right_y_axis = self.controller.controller_states["right_y_axis"] if abs(self.controller.controller_states[ - "right_y_axis"]) > RIGHT_Y_AXIS_DEADZONE else 0 + "right_y_axis"]) > RIGHT_Y_AXIS_DEADZONE else 0 if left_y_axis or right_y_axis: - message = MiningControlMessage() message.lift_set_absolute = 1024 diff --git a/software/ros_packages/rover_control/CMakeLists.txt b/software/ros_packages/rover_control/CMakeLists.txt index d9dde07..bbaa0c0 100644 --- a/software/ros_packages/rover_control/CMakeLists.txt +++ b/software/ros_packages/rover_control/CMakeLists.txt @@ -57,6 +57,8 @@ find_package(catkin REQUIRED COMPONENTS TowerPanTiltControlMessage.msg MiningControlMessage.msg MiningStatusMessage.msg + GripperControlMessage.msg + GripperStatusMessage.msg ) ## Generate services in the 'srv' folder diff --git a/software/ros_packages/rover_control/msg/GripperControlMessage.msg b/software/ros_packages/rover_control/msg/GripperControlMessage.msg new file mode 100644 index 0000000..87dad76 --- /dev/null +++ b/software/ros_packages/rover_control/msg/GripperControlMessage.msg @@ -0,0 +1,7 @@ +uint8 gripper_mode + +int32 gripper_position_absolute +int16 gripper_position_relative + +bool should_home +bool toggle_light \ No newline at end of file diff --git a/software/ros_packages/rover_control/msg/GripperStatusMessage.msg b/software/ros_packages/rover_control/msg/GripperStatusMessage.msg new file mode 100644 index 0000000..968a056 --- /dev/null +++ b/software/ros_packages/rover_control/msg/GripperStatusMessage.msg @@ -0,0 +1,13 @@ +uint16 pinch_position_raw +uint16 forefinger_position_raw +uint16 thumb_position_raw +uint16 middlefinger_position_raw + +uint16 pinch_current +uint16 forefinger_current +uint16 thumb_current +uint16 middlefinger_current + +bool light_on +uint8 current_mode +uint16 current_finger_position \ No newline at end of file diff --git a/software/ros_packages/rover_control/src/effectors_control/effectors_control.py b/software/ros_packages/rover_control/src/effectors_control/effectors_control.py index 4d99380..42b4cb3 100755 --- a/software/ros_packages/rover_control/src/effectors_control/effectors_control.py +++ b/software/ros_packages/rover_control/src/effectors_control/effectors_control.py @@ -12,7 +12,7 @@ import minimalmodbus # from std_msgs.msg import UInt8, UInt16 # Custom Imports -from rover_control.msg import MiningControlMessage, MiningStatusMessage +from rover_control.msg import MiningControlMessage, MiningStatusMessage, GripperControlMessage, GripperStatusMessage ##################################### # Global Variables @@ -21,7 +21,7 @@ NODE_NAME = "effectors_control" # ##### Communication Defines ##### # DEFAULT_PORT = "/dev/rover/ttyEffectors" -DEFAULT_PORT = "/dev/ttyUSB0" +DEFAULT_PORT = "/dev/ttyUSB1" DEFAULT_BAUD = 115200 GRIPPER_NODE_ID = 1 @@ -38,13 +38,47 @@ TX_DELAY = 0.01 DEFAULT_HERTZ = 40 GRIPPER_CONTROL_SUBSCRIBER_TOPIC = "gripper/control" +GRIPPER_STATUS_PUBLISHER_TOPIC = "gripper/status" MINING_CONTROL_SUBSCRIBER_TOPIC = "mining/control" MINING_STATUS_PUBLISHER_TOPIC = "mining/status" SCIENCE_CONTROL_SUBSCRIBER_TOPIC = "science/control" -# ##### Arm Defines ##### +# ##### Gripper Defines ##### +GRIPPER_MODBUS_REGISTERS = { + "MODE": 0, + "FINGER_POSITION": 1, + "HOME": 2, + "LIGHT_STATE": 3, + + "CURRENT_PINCH": 4, + "CURRENT_FOREFINGER": 5, + "CURRENT_THUMB": 6, + "CURRENT_MIDDLEFINGER": 7, + "POSITION_PINCH": 8, + "POSITION_FOREFINGER": 9, + "POSITION_THUMB": 10, + "POSITION_MIDDLEFINGER": 11, + "LIGHT_STATE_OUPUT": 12, + "MODE_OUTPUT": 13, + "FINGER_POSITION_OUTPUT": 14 +} + +GRIPPER_MODES = { + "NO_CHANGE": 0, + "NORMAL": 1, + "TWO_FINGER_PINCH": 2, + "WIDE ": 3, + "SCISSOR": 4 +} + +DEFAULT_GRIPPER_REGISTERS = [ + 0, # No change + 0, # No positional update + 0, # Do not home + 0, # Light off +] # ##### Mining Defines ##### MINING_MODBUS_REGISTERS = { @@ -89,6 +123,8 @@ class EffectorsControl(object): self.gripper_control_subscriber_topic = rospy.get_param("~gripper_control_subscriber_topic", GRIPPER_CONTROL_SUBSCRIBER_TOPIC) + self.gripper_status_publisher_topic = rospy.get_param("~gripper_status_publisher_topic", + GRIPPER_STATUS_PUBLISHER_TOPIC) self.mining_control_subscriber_topic = rospy.get_param("~mining_control_subscriber_topic", MINING_CONTROL_SUBSCRIBER_TOPIC) @@ -113,22 +149,30 @@ class EffectorsControl(object): # self.check_which_nodes_present() # ##### 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_subscriber = rospy.Subscriber(self.mining_control_subscriber_topic, MiningStatusMessage, self.mining_control_message_received__callback) # ##### Publishers ##### - self.mining_status_publisher = rospy.Publisher(self.mining_status_publisher_topic, MiningStatusMessage, queue_size=1) + 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) # ##### Misc ##### self.modbus_nodes_seen_time = time() # ##### Mining Variables ##### self.mining_registers = [0 for _ in MINING_MODBUS_REGISTERS] + self.gripper_registers = [0 for _ in GRIPPER_MODBUS_REGISTERS] self.mining_control_message = None # type:MiningControlMessage self.new_mining_control_message = False + self.gripper_control_message = None + self.new_gripper_control_message = False + self.run() def __setup_minimalmodbus_for_485(self): @@ -150,6 +194,11 @@ class EffectorsControl(object): def run(self): # self.initialize_mining_system() + try: + self.gripper_registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS)) + except: + pass + while not rospy.is_shutdown(): # try: # self.process_mining_control_message() @@ -172,7 +221,8 @@ class EffectorsControl(object): # pass try: - print self.gripper_node.read_registers(0, 15) + self.process_gripper_control_message() + self.send_gripper_status_message() except Exception, error: print error @@ -253,6 +303,52 @@ class EffectorsControl(object): self.modbus_nodes_seen_time = time() + def process_gripper_control_message(self): + if self.new_gripper_control_message: + + 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["HOME"]] = self.gripper_control_message.should_home + self.gripper_registers[GRIPPER_MODBUS_REGISTERS["MODE"]] = self.gripper_control_message.gripper_mode + + gripper_absolute = self.gripper_control_message.gripper_position_absolute + gripper_relative = self.gripper_control_message.gripper_position_relative + + if -1 < gripper_absolute < UINT16_MAX: + self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = gripper_absolute + else: + new_position = self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] + gripper_relative + self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = min(max(new_position, 0), 10000) + + self.gripper_node.write_registers(0, self.gripper_registers) + + self.gripper_node.write_registers(0, DEFAULT_GRIPPER_REGISTERS) + + self.new_gripper_control_message = False + + def send_gripper_status_message(self): + registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS)) + + message = GripperStatusMessage() + message.pinch_position_raw = registers[GRIPPER_MODBUS_REGISTERS["POSITION_PINCH"]] + message.forefinger_position_raw = registers[GRIPPER_MODBUS_REGISTERS["POSITION_FOREFINGER"]] + message.thumb_position_raw = registers[GRIPPER_MODBUS_REGISTERS["POSITION_THUMB"]] + message.middlefinger_position_raw = registers[GRIPPER_MODBUS_REGISTERS["POSITION_MIDDLEFINGER"]] + message.pinch_current = registers[GRIPPER_MODBUS_REGISTERS["CURRENT_PINCH"]] + message.forefinger_current = registers[GRIPPER_MODBUS_REGISTERS["CURRENT_FOREFINGER"]] + message.thumb_current = registers[GRIPPER_MODBUS_REGISTERS["CURRENT_THUMB"]] + message.middlefinger_current = registers[GRIPPER_MODBUS_REGISTERS["CURRENT_MIDDLEFINGER"]] + message.light_on = registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE_OUPUT"]] + message.current_mode = registers[GRIPPER_MODBUS_REGISTERS["MODE_OUTPUT"]] + message.current_finger_position = registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION_OUTPUT"]] + + self.gripper_status_publisher.publish(message) + + def gripper_control_message_received__callback(self, control_message): + self.gripper_control_message = control_message + self.new_gripper_control_message = True + def mining_control_message_received__callback(self, control_message): self.mining_control_message = control_message self.new_mining_control_message = True diff --git a/software/ros_packages/rover_control/src/effectors_control/effectors_tester.py b/software/ros_packages/rover_control/src/effectors_control/effectors_tester.py index 55dad53..0a0fedb 100755 --- a/software/ros_packages/rover_control/src/effectors_control/effectors_tester.py +++ b/software/ros_packages/rover_control/src/effectors_control/effectors_tester.py @@ -2,20 +2,20 @@ import rospy import time -from rover_control.msg import MiningControlMessage +from rover_control.msg import GripperControlMessage -DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC = "/rover_control/mining/control" +TOPIC = "/gripper/control" rospy.init_node("effectors_tester") -publisher = rospy.Publisher(DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC, MiningControlMessage, queue_size=1) +publisher = rospy.Publisher(TOPIC, GripperControlMessage, queue_size=1) time.sleep(2) -message = MiningControlMessage() -message.lift_set = 200 -message.tilt_set = 1023 -message.cal_factor = -1 +message = GripperControlMessage() +message.gripper_mode = 1 +message.gripper_position = 0 +message.should_home = 0 publisher.publish(message) 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 index ff6ba20..a0c5c45 100644 --- 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 @@ -10,7 +10,8 @@ {name: "/rover_control/tower/pan_tilt/control", compress: true, rate: 30.0}, {name: "/rover_control/chassis/pan_tilt/control", compress: true, rate: 30.0}, {name: "/rover_control/mining/control", compress: true, rate: 30.0}, - {name: "/rover_arm/control/relative", compress: true, rate: 30.0}] + {name: "/rover_arm/control/relative", compress: true, rate: 30.0}, + {name: "/rover_control/gripper/control", compress: true, rate: 30.0}] diff --git a/software/ros_packages/rover_science/src/rover_science.py b/software/ros_packages/rover_science/src/rover_science.py index c2562b4..ad49409 100755 --- a/software/ros_packages/rover_science/src/rover_science.py +++ b/software/ros_packages/rover_science/src/rover_science.py @@ -20,8 +20,8 @@ from std_msgs.msg import Float64MultiArray ##################################### NODE_NAME = "science_node" -DEFAULT_PORT = "/dev/rover/ttyIRIS_2_0" -DEFAULT_BAUD = 115200 +DEFAULT_PORT = "/dev/rover/ttyRDF_SoilProbe" +DEFAULT_BAUD = 9600 DEFAULT_INVERT = False @@ -29,13 +29,15 @@ DEFAULT_RDF_PUBLISHER_TOPIC = "rdf/data" PAN_TILT_NODE_ID = 1 -COMMUNICATIONS_TIMEOUT = 0.005 # Seconds +COMMUNICATIONS_TIMEOUT = 0.1 # Seconds RX_DELAY = 0.01 TX_DELAY = 0.01 DEFAULT_HERTZ = 20 +SOIL_PROBE_ADDRESS = "mar" + PAN_TILT_MODBUS_REGISTERS = { "CENTER_ALL": 0, @@ -45,6 +47,16 @@ PAN_TILT_MODBUS_REGISTERS = { "TILT_ADJUST_NEGATIVE": 4 } +SOIL_PROBE_COMMANDS = { + "GET_ADDRESS": "AD=", + "DESCRIPTION": "DS=", + "PROBE_ENABLED": "PE=", + "TAKE_READING": "TR", + "TRANSMIT_READING": "T0", + + "QUERY": "?" +} + NODE_LAST_SEEN_TIMEOUT = 2 # seconds @@ -84,27 +96,57 @@ class RoverScience(object): self.run() def __setup_minimalmodbus_for_485(self): - self.pan_tilt_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT) + self.pan_tilt_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT) # type: serial.Serial self.pan_tilt_node.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0, delay_before_rx=RX_DELAY, delay_before_tx=TX_DELAY) def run(self): # self.send_startup_centering_command() + # self.pan_tilt_node.serial.timeout(500) + + out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["PROBE_ENABLED"] + "1") + self.pan_tilt_node.serial.write(out) + sleep(0.1) + while not rospy.is_shutdown(): + + out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["TAKE_READING"]) + self.pan_tilt_node.serial.write(out) + sleep(0.1) + + out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["TRANSMIT_READING"]) + self.pan_tilt_node.serial.write(out) + start_time = time() - try: - registers = self.pan_tilt_node.read_registers(0, 1) - self.rdf_publisher.publish(Float64MultiArray(data=[registers[0], time()])) - self.modbus_nodes_seen_time = time() - except Exception, Error: - print Error + char = "" + response = "" + while char != '\n' and (time() - start_time) < 2: + # print time() - start_time, (time() - start_time) > 2 + if self.pan_tilt_node.serial.inWaiting(): + char = self.pan_tilt_node.serial.read() + response += char + # print char - if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT: - print "Science not seen for", NODE_LAST_SEEN_TIMEOUT, "seconds. Exiting." - return # Exit so respawn can take over + if response: + print response + else: + print "timeout" + # print "No response" - time_diff = time() - start_time + # start_time = time() + # try: + # registers = self.pan_tilt_node.read_registers(0, 1) + # self.rdf_publisher.publish(Float64MultiArray(data=[registers[0], time()])) + # self.modbus_nodes_seen_time = time() + # except Exception, Error: + # print Error + # + # if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT: + # print "Science not seen for", NODE_LAST_SEEN_TIMEOUT, "seconds. Exiting." + # return # Exit so respawn can take over + + # time_diff = time() - start_time # def run(self): # # self.send_startup_centering_command()