diff --git a/software/ros_packages/rover_control/CMakeLists.txt b/software/ros_packages/rover_control/CMakeLists.txt index 7d43cdb..0156d23 100644 --- a/software/ros_packages/rover_control/CMakeLists.txt +++ b/software/ros_packages/rover_control/CMakeLists.txt @@ -53,6 +53,7 @@ find_package(catkin REQUIRED COMPONENTS DriveCommandMessage.msg DriveControlMessage.msg DriveStatusMessage.msg + IrisStatusMessage.msg ) ## Generate services in the 'srv' folder diff --git a/software/ros_packages/rover_control/msg/IrisStatusMessage.msg b/software/ros_packages/rover_control/msg/IrisStatusMessage.msg new file mode 100644 index 0000000..97ef7c9 --- /dev/null +++ b/software/ros_packages/rover_control/msg/IrisStatusMessage.msg @@ -0,0 +1,2 @@ +bool iris_connected +uint16 voltage_24v \ No newline at end of file diff --git a/software/ros_packages/rover_control/src/iris_controller/iris_controller.py b/software/ros_packages/rover_control/src/iris_controller/iris_controller.py index 69ab713..73c579b 100755 --- a/software/ros_packages/rover_control/src/iris_controller/iris_controller.py +++ b/software/ros_packages/rover_control/src/iris_controller/iris_controller.py @@ -11,7 +11,7 @@ import serial.rs485 import minimalmodbus # Custom Imports -from rover_control.msg import DriveCommandMessage +from rover_control.msg import DriveCommandMessage, IrisStatusMessage ##################################### # Global Variables @@ -22,6 +22,7 @@ DEFAULT_PORT = "/dev/rover/ttyIRIS" DEFAULT_BAUD = 115200 DEFAULT_DRIVE_COMMAND_TOPIC = "command_control/iris_drive" +DEFAULT_IRIS_STATUS_TOPIC = "iris_status" DEFAULT_HERTZ = 10 COMMUNICATIONS_TIMEOUT = 0.15 # Seconds @@ -81,15 +82,17 @@ class IrisController(object): self.baud = rospy.get_param("~baud", DEFAULT_BAUD) self.drive_command_publisher_topic = rospy.get_param("~drive_command_topic", DEFAULT_DRIVE_COMMAND_TOPIC) + self.iris_status_publisher_topic = rospy.get_param("~iris_status_topic", DEFAULT_IRIS_STATUS_TOPIC) self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ) - # print self.wait_time self.iris = minimalmodbus.Instrument(self.port, MODBUS_ID) self.__setup_minimalmodbus_for_485() self.drive_command_publisher = rospy.Publisher(self.drive_command_publisher_topic, DriveCommandMessage, queue_size=1) + self.iris + self.registers = [] @@ -108,6 +111,7 @@ class IrisController(object): self.read_registers() self.broadcast_drive_if_current_mode() self.broadcast_arm_if_current_mode() + self.broadcast_iris_status() except Exception, error: print "Error occurred:", error @@ -156,6 +160,9 @@ class IrisController(object): SBUS_VALUES["SBUS_MIN"] + SBUS_VALUES["SBUS_DEADZONE"]: print "Arm" + def broadcast_iris_status(self): + print self.registers[MODBUS_REGISTERS["VOLTAGE_24V"]] + if __name__ == "__main__": IrisController() diff --git a/software/testing/nav_testing/nav.launch b/software/testing/nav_testing/nav.launch index c0ffc28..5f48637 100644 --- a/software/testing/nav_testing/nav.launch +++ b/software/testing/nav_testing/nav.launch @@ -2,7 +2,7 @@ - +