From 628efbb1c58b18031fbb05be29f762057e767f25 Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Sat, 3 Mar 2018 20:36:24 -0800 Subject: [PATCH] Added status data from rover_drive nodes. --- .../ros_packages/rover_control/CMakeLists.txt | 1 + .../rover_control/msg/DriveStatusMessage.msg | 11 +++ .../control_coordinators/drive_coordinator.py | 2 +- .../src/drive_control/drive_control.py | 86 ++++++++++++++----- .../topic_transport_senders.launch | 2 +- 5 files changed, 80 insertions(+), 22 deletions(-) create mode 100644 software/ros_packages/rover_control/msg/DriveStatusMessage.msg diff --git a/software/ros_packages/rover_control/CMakeLists.txt b/software/ros_packages/rover_control/CMakeLists.txt index 61cd569..7d43cdb 100644 --- a/software/ros_packages/rover_control/CMakeLists.txt +++ b/software/ros_packages/rover_control/CMakeLists.txt @@ -52,6 +52,7 @@ find_package(catkin REQUIRED COMPONENTS FILES DriveCommandMessage.msg DriveControlMessage.msg + DriveStatusMessage.msg ) ## Generate services in the 'srv' folder diff --git a/software/ros_packages/rover_control/msg/DriveStatusMessage.msg b/software/ros_packages/rover_control/msg/DriveStatusMessage.msg new file mode 100644 index 0000000..4dff729 --- /dev/null +++ b/software/ros_packages/rover_control/msg/DriveStatusMessage.msg @@ -0,0 +1,11 @@ +bool first_motor_connected +bool second_motor_connected + +float32 first_motor_current +float32 second_motor_current + +bool first_motor_fault +bool second_motor_fault + +float32 first_motor_temp +float32 second_motor_temp \ No newline at end of file diff --git a/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py b/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py index 036677a..44587f3 100755 --- a/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py +++ b/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py @@ -23,7 +23,7 @@ DEFAULT_RIGHT_BOGIE_TOPIC = "drive_control/right" UINT16_MAX = 65535 -DEFAULT_HERTZ = 15 +DEFAULT_HERTZ = 20 WATCHDOG_TIMEOUT = 0.3 diff --git a/software/ros_packages/rover_control/src/drive_control/drive_control.py b/software/ros_packages/rover_control/src/drive_control/drive_control.py index e8e0011..47415fb 100755 --- a/software/ros_packages/rover_control/src/drive_control/drive_control.py +++ b/software/ros_packages/rover_control/src/drive_control/drive_control.py @@ -11,7 +11,7 @@ import serial.rs485 import minimalmodbus # Custom Imports -from rover_control.msg import DriveControlMessage +from rover_control.msg import DriveControlMessage, DriveStatusMessage ##################################### # Global Variables @@ -24,6 +24,7 @@ DEFAULT_BAUD = 115200 DEFAULT_INVERT = False DEFAULT_DRIVE_CONTROL_TOPIC = "drive_control/rear" +DEFAULT_DRIVE_CONTROL_STATUS_TOPIC = "drive_status/rear" FIRST_MOTOR_ID = 1 SECOND_MOTOR_ID = 2 @@ -68,6 +69,8 @@ class DriveControl(object): self.drive_control_subscriber_topic = rospy.get_param("~drive_control_topic", DEFAULT_DRIVE_CONTROL_TOPIC) + self.drive_control_status_topic = rospy.get_param("~drive_control_status_topic", DEFAULT_DRIVE_CONTROL_STATUS_TOPIC) + self.first_motor = minimalmodbus.Instrument(self.port, FIRST_MOTOR_ID) self.second_motor = minimalmodbus.Instrument(self.port, SECOND_MOTOR_ID) self.__setup_minimalmodbus_for_485() @@ -75,6 +78,11 @@ class DriveControl(object): self.drive_control_subscriber = \ rospy.Subscriber(self.drive_control_subscriber_topic, DriveControlMessage, self.drive_control_callback) + self.drive_control_status_publisher = rospy.Publisher(self.drive_control_status_topic, DriveStatusMessage, queue_size=1) + + self.drive_control_message = DriveControlMessage() + self.new_control_message = False + self.run() def __setup_minimalmodbus_for_485(self): @@ -90,27 +98,65 @@ class DriveControl(object): def run(self): while not rospy.is_shutdown(): - sleep(0.25) + self.send_drive_control_message() + self.get_drive_status() + sleep(0.005) + + def send_drive_control_message(self): + if self.new_control_message: + drive_control = self.drive_control_message + try: + first_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE) + first_direction = \ + not drive_control.first_motor_direction if self.first_motor_inverted else drive_control.first_motor_direction + first_motor_register_data[MODBUS_REGISTERS["DIRECTION"]] = first_direction + first_motor_register_data[MODBUS_REGISTERS["SPEED"]] = min(drive_control.first_motor_speed, UINT16_MAX) + + second_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE) + second_direction = not drive_control.second_motor_direction if self.second_motor_inverted else drive_control.second_motor_direction + second_motor_register_data[MODBUS_REGISTERS["DIRECTION"]] = second_direction + second_motor_register_data[MODBUS_REGISTERS["SPEED"]] = min(drive_control.second_motor_speed, UINT16_MAX) + + self.first_motor.write_registers(0, first_motor_register_data) + self.second_motor.write_registers(0, second_motor_register_data) + + except Exception, error: + print "Error occurred:", error + + self.new_control_message = False + + def get_drive_status(self): + status = DriveStatusMessage() + + first_motor_status = [0, 0, 0] + second_motor_status = [0, 0, 0] + + try: + first_motor_status = self.first_motor.read_registers(3, 3) + status.first_motor_connected = True + except Exception: + status.first_motor_connected = False + + try: + second_motor_status = self.second_motor.read_registers(3, 3) + status.second_motor_connected = True + except Exception: + status.second_motor_connected = False + + if status.first_motor_connected: + status.first_motor_current = first_motor_status[0] / 1000.0 + status.first_motor_fault = first_motor_status[1] + status.first_motor_temp = first_motor_status[2] / 1000.0 + + if status.second_motor_connected: + status.second_motor_current = second_motor_status[0] / 1000.0 + status.second_motor_fault = second_motor_status[1] + status.second_motor_temp = second_motor_status[2] / 1000.0 def drive_control_callback(self, drive_control): - try: - first_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE) - first_direction = \ - not drive_control.first_motor_direction if self.first_motor_inverted else drive_control.first_motor_direction - first_motor_register_data[MODBUS_REGISTERS["DIRECTION"]] = first_direction - first_motor_register_data[MODBUS_REGISTERS["SPEED"]] = min(drive_control.first_motor_speed, UINT16_MAX) - - second_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE) - second_direction = not drive_control.second_motor_direction if self.second_motor_inverted else drive_control.second_motor_direction - second_motor_register_data[MODBUS_REGISTERS["DIRECTION"]] = second_direction - second_motor_register_data[MODBUS_REGISTERS["SPEED"]] = min(drive_control.second_motor_speed, UINT16_MAX) - - self.first_motor.write_registers(0, first_motor_register_data) - self.second_motor.write_registers(0, second_motor_register_data) - - except Exception, error: - print "Error occurred:", error + self.drive_control_message = drive_control + self.new_control_message = True if __name__ == "__main__": - DriveControl() + DriveControl() \ No newline at end of file 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 5de3a5a..0c1dea6 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 @@ -2,7 +2,7 @@ - +