From 31c2235f338b259aa0664a348bfdd6646856b0b4 Mon Sep 17 00:00:00 2001 From: Jacob Panikulam Date: Sat, 12 Sep 2015 13:31:51 -0400 Subject: [PATCH 1/7] DRIVERS: Add thruster serial communication --- .../sub8_videoray_m5_thruster/CMakeLists.txt | 1 + drivers/sub8_videoray_m5_thruster/setup.py | 11 ++ .../sub8_thruster_comm/__init__.py | 13 ++ .../sub8_thruster_comm/protocol.py | 21 +++ .../sub8_thruster_comm/thruster_comm.py | 152 ++++++++++++++++++ .../sub8_thruster_comm/thruster_fake.py | 77 +++++++++ 6 files changed, 275 insertions(+) create mode 100644 drivers/sub8_videoray_m5_thruster/setup.py create mode 100644 drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/__init__.py create mode 100644 drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/protocol.py create mode 100644 drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/thruster_comm.py create mode 100644 drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/thruster_fake.py diff --git a/drivers/sub8_videoray_m5_thruster/CMakeLists.txt b/drivers/sub8_videoray_m5_thruster/CMakeLists.txt index 69e5925..49c7b14 100644 --- a/drivers/sub8_videoray_m5_thruster/CMakeLists.txt +++ b/drivers/sub8_videoray_m5_thruster/CMakeLists.txt @@ -3,6 +3,7 @@ project(sub8_videoray_m5_thruster) find_package(catkin REQUIRED COMPONENTS rospy ) +catkin_python_setup() catkin_package() include_directories( ${catkin_INCLUDE_DIRS} diff --git a/drivers/sub8_videoray_m5_thruster/setup.py b/drivers/sub8_videoray_m5_thruster/setup.py new file mode 100644 index 0000000..c08b4dc --- /dev/null +++ b/drivers/sub8_videoray_m5_thruster/setup.py @@ -0,0 +1,11 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['sub8_thruster_comm'], + package_dir={'sub8_thruster_comm': '.'}, +) +setup(**setup_args) \ No newline at end of file diff --git a/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/__init__.py b/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/__init__.py new file mode 100644 index 0000000..2906f9b --- /dev/null +++ b/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/__init__.py @@ -0,0 +1,13 @@ +from .thruster_comm import ThrusterPort +from .thruster_fake import FakeThrusterPort + + +def thruster_comm_factory(port_info, fake=False): + '''Return the appropriate thruster communication class + Purpose: + - Use the same code to run both simulated thrusters and real thrusters + ''' + if fake: + return FakeThrusterPort(port_info) + else: + return ThrusterPort(port_info) \ No newline at end of file diff --git a/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/protocol.py b/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/protocol.py new file mode 100644 index 0000000..f83b0a8 --- /dev/null +++ b/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/protocol.py @@ -0,0 +1,21 @@ +class Const(object): + ''' + Bibliography: + [1] VideoRay Example Code [Online] + Available: https://github.com/videoray/Thruster/blob/master/thruster.py + ''' + # VRCSR protocol defines + sync_request = 0x5ff5 + sync_response = 0x0ff0 + protocol_vrcsr_header_size = 6 + protocol_vrcsr_xsum_size = 4 + + # CSR address for sending an application specific custom command + addr_custom_command = 0xf0 + propulsion_command = 0xaa + + # Flag for the standard thruster response which contains + response_thruster_standard = 0x2 + + # Standard response is the device type followed by 4 32-bit floats and 1 byte + response_thruster_standard_length = 1 + 4 * 4 + 1 \ No newline at end of file diff --git a/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/thruster_comm.py b/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/thruster_comm.py new file mode 100644 index 0000000..c242253 --- /dev/null +++ b/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/thruster_comm.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python +import numpy as np +import struct +import binascii +from .protocol import Const +import serial + + +class ThrusterPort(object): + _baud_rate = 115200 + + def __init__(self, port_info): + '''Communicate on a single port with some thrusters + port_info should be a dictionary, drawn from a .yaml of the form... + + - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A403IMC9-if00-port0 + thrusters: + FLV: { + node_id: 10, + } + FLL: { + node_id: 11, + } + Note: + - The thrusters automatically shut down if they do not recieve a + command for ~2 seconds + ''' + self.port_name = port_info['port'] + self.thruster_dict = {} + for thruster_name, thruster_info in port_info['thrusters'].items(): + self.load_thruster_config(thruster_name, thruster_info) + + self.port = self.connect_port(self.port_name) + + def connect_port(self, port_name): + '''Connect to and return a serial port''' + try: + serial_port = serial.Serial(port_name, self._baud_rate) + except IOError, e: + rospy.logerr("Could not connect to thruster port {}".format(port_name)) + raise(e) + return serial_port + + def load_thruster_config(self, thruster_name, thruster_info): + self.thruster_dict[thruster_name] = thruster_info['node_id'] + + def checksum_struct(self, _struct): + '''Take a struct, convert it to a bytearray, and append its crc32 checksum''' + struct_bytearray = bytearray(_struct) + struct_checksum = bytearray(struct.pack('i', binascii.crc32(struct_bytearray))) + return struct_bytearray + struct_checksum + + def make_header(self, node_id, msg_size): + '''Construct a header''' + length = 2 + (msg_size * 4) + flag = Const.response_thruster_standard + header = self.checksum_struct( + struct.pack('HBBBB', + Const.sync_request, + node_id, + flag, + Const.addr_custom_command, + length + ) + ) + return header + + def make_thrust_payload(self, motor_id, thrust): + '''Construct a payload that commands a thrust''' + send_thrust = np.clip(thrust, -1, 1) + payload = self.checksum_struct( + struct.pack(' Date: Sat, 12 Sep 2015 13:33:49 -0400 Subject: [PATCH 2/7] DRIVERS: Add thruster communication ros node --- .../config/calibration.json | 91 +++++++++++++ .../nodes/thruster_driver.py | 125 ++++++++++++++++++ drivers/sub8_videoray_m5_thruster/readme.md | 18 +++ 3 files changed, 234 insertions(+) create mode 100644 drivers/sub8_videoray_m5_thruster/config/calibration.json create mode 100755 drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py create mode 100644 drivers/sub8_videoray_m5_thruster/readme.md diff --git a/drivers/sub8_videoray_m5_thruster/config/calibration.json b/drivers/sub8_videoray_m5_thruster/config/calibration.json new file mode 100644 index 0000000..b89d210 --- /dev/null +++ b/drivers/sub8_videoray_m5_thruster/config/calibration.json @@ -0,0 +1,91 @@ +{ + "note": "Force calibration data for the VideoRay m5 Thrusters, gifted to UF by VideoRay", + "calibration_data": { + "newtons": [ + -85.16798603621542, + -83.11245965680112, + -80.72780555898515, + -77.26080420472348, + -74.24265026914527, + -67.98354200661714, + -64.73528274880252, + -58.839594796790784, + -54.214964815228186, + -49.31660284098751, + -43.84480379387167, + -39.157067326228415, + -33.524153970597816, + -27.99452422080355, + -21.62868280175304, + -16.710638097126996, + -11.252028508516814, + -6.915131637123009, + -3.325975605624676, + -0.6694157478467332, + -0.0, + 1.2820153667461807, + 4.5426524240814805, + 8.881375528191455, + 13.348137837179078, + 18.081935952218934, + 23.798044353808, + 28.566743805201153, + 34.71221980983474, + 41.44351068686406, + 45.79359701676346, + 53.66790665936434, + 58.868611605503126, + 64.13079971975266, + 70.43881043612473, + 75.2278013621419, + 79.71972509966326, + 83.98438432139565, + 87.39923658587354, + 92.2549864634037, + 93.17094362793253 + ], + "thruster_input": [ + -1.0, + -0.95, + -0.9, + -0.85, + -0.8, + -0.75, + -0.7, + -0.65, + -0.6, + -0.55, + -0.5, + -0.45, + -0.4, + -0.35, + -0.3, + -0.25, + -0.2, + -0.15, + -0.1, + -0.05, + 0.0, + 0.05, + 0.1, + 0.15, + 0.2, + 0.25, + 0.3, + 0.35, + 0.4, + 0.45, + 0.5, + 0.55, + 0.6, + 0.65, + 0.7, + 0.75, + 0.8, + 0.85, + 0.9, + 0.95, + 1.0 + ] + } +} diff --git a/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py b/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py new file mode 100755 index 0000000..cfc1bf6 --- /dev/null +++ b/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python +import numpy as np +import json +import rospy +import scipy.interpolate +import threading +import argparse +from sub8_msgs.msg import Thrust, ThrusterCmd +from sub8_ros_tools import wait_for_param, thread_lock +from sub8_msgs.srv import ThrusterInfo, ThrusterInfoResponse +from sub8_thruster_comm import thruster_comm_factory + + +lock = threading.Lock() +class ThrusterDriver(object): + def __init__(self, config_path, bus_layout): + '''Thruster driver, an object for commanding all of the sub's thrusters + - Gather configuration data and make it available to other nodes + - Instantiate ThrusterPorts, (Either simulated or real), for communicating with thrusters + - Track a thrust_dict, which maps thruster names to the appropriate port + - Given a command message, route that command to the appropriate port/thruster + + TODO: + - Publish thruster status messages + + ''' + self.make_fake = rospy.get_param('simulate', False) + if self.make_fake: + rospy.logwarn("Running fake thrusters for simulation, based on parameter '/simulate'") + + # Individual thruster configuration data + newtons, thruster_input = self.load_config(config_path) + self.interpolate = scipy.interpolate.interp1d(newtons, thruster_input) + + # Bus configuration + self.port_dict = self.load_bus_layout(bus_layout) + + thrust_service = rospy.Service('thruster_range', ThrusterInfo, self.get_thruster_info) + self.thrust_sub = rospy.Subscriber('/thrust', Thrust, self.thrust_cb, queue_size=1) + + def load_config(self, path): + '''Load the configuration data: + - Map force inputs from Newtons to [-1, 1] required by the thruster + ''' + try: + _file = file(path) + except IOError, e: + rospy.logerr("Could not find thruster configuration file at {}".format(path)) + raise(e) + + json_data = json.load(_file) + newtons = json_data['calibration_data']['newtons'] + thruster_input = json_data['calibration_data']['thruster_input'] + return newtons, thruster_input + + def get_thruster_info(self, srv): + '''Get the thruster info for a particular thruster ID + Right now, this is only the min and max thrust data + ''' + # Unused right now + query_id = srv.thruster_id + + min_thrust = min(self.interpolate.x) + max_thrust = max(self.interpolate.x) + thruster_info = ThrusterInfoResponse( + min_force=min_thrust, + max_force=max_thrust + ) + return thruster_info + + @thread_lock(lock) + def load_bus_layout(self, layout): + '''Load and handle the thruster bus layout''' + port_dict = {} + for port in layout: + thruster_port = thruster_comm_factory(port, fake=self.make_fake) + + # Add the thrusters to the thruster dict + for thruster_name, thruster_info in port['thrusters'].items(): + port_dict[thruster_name] = thruster_port + + return port_dict + + @thread_lock(lock) + def command_thruster(self, name, force): + '''Issue a a force command (in Newtons) to a named thruster + Example names are BLR, FLL, etc + ''' + target_port = self.port_dict[name] + clipped_force = np.clip(force, min(self.interpolate.x), max(self.interpolate.x)) + normalized_force = self.interpolate(clipped_force) + + # We immediately get thruster_status back + thruster_status = target_port.command_thruster(name, force) + + def thrust_cb(self, msg): + '''Callback for recieving thrust commands + These messages contain a list of instructions, one for each thruster + ''' + for thrust_cmd in msg.thruster_commands: + self.command_thruster(thrust_cmd.name, thrust_cmd.thrust) + + +if __name__ == '__main__': + ''' + --> Simulation capabilities (Return thruster ranges) + ''' + usage_msg = "Interface to Sub8's VideoRay M5 thrusters" + desc_msg = "Specify a path to the configuration.json file containing the thrust calibration data" + parser = argparse.ArgumentParser(usage=usage_msg, description=desc_msg) + parser.add_argument('--configuration_path', dest='config_path', + help='Designate the absolute path of the calibration/configuration json file') + args = parser.parse_args(rospy.myargv()[1:]) + config_path = args.config_path + + rospy.init_node('videoray_m5_thruster_driver') + + layout_parameter = '/busses' + rospy.loginfo("Thruster Driver waiting for parameter, {}".format(layout_parameter)) + busses = wait_for_param(layout_parameter) + if busses is None: + raise(rospy.exceptions.ROSException("Failed to find parameter '{}'".format(layout_parameter))) + + thruster_driver = ThrusterDriver(config_path, busses) + rospy.spin() \ No newline at end of file diff --git a/drivers/sub8_videoray_m5_thruster/readme.md b/drivers/sub8_videoray_m5_thruster/readme.md new file mode 100644 index 0000000..f9b7223 --- /dev/null +++ b/drivers/sub8_videoray_m5_thruster/readme.md @@ -0,0 +1,18 @@ +VideoRay M5 Thruster Driver +=========================== + +# Authors + +This code was originally authored by Forrest Voight (forrestv) and Matthew Griessler. + +# Purpose + +This node handles the interface between the sub's control computer and the thrusters. It communicates via serial to each thruster, individually issuing commands. + +# Configuration +The thrusters take an input on [-1, 1]. The configuration.json file provided relates an maps a desired thrust in Newtons to the thruster's acceptable input range. + +Run with the argument `--configuration_path=$(find sub8_videoray_m5_thruster)/config/calibration.json`, or the path to the appropriate calibration file + + +# ROS From bc71e4beaafb9792dc86d8a924af339c04bb5eaf Mon Sep 17 00:00:00 2001 From: Jacob Panikulam Date: Sat, 12 Sep 2015 13:35:08 -0400 Subject: [PATCH 3/7] DRIVERS: Add thruster driver example roslaunch --- .../sub8_videoray_m5_thruster/launch/thruster_driver.launch | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 drivers/sub8_videoray_m5_thruster/launch/thruster_driver.launch diff --git a/drivers/sub8_videoray_m5_thruster/launch/thruster_driver.launch b/drivers/sub8_videoray_m5_thruster/launch/thruster_driver.launch new file mode 100644 index 0000000..bd80105 --- /dev/null +++ b/drivers/sub8_videoray_m5_thruster/launch/thruster_driver.launch @@ -0,0 +1,4 @@ + + + \ No newline at end of file From c83d0bf3070780add3c8dd942d76f9168625a747 Mon Sep 17 00:00:00 2001 From: Jacob Panikulam Date: Sat, 12 Sep 2015 13:35:59 -0400 Subject: [PATCH 4/7] MAPPER: Add querying thrusters for force min/max --- .../launch/thruster_mapper.launch | 2 +- gnc/sub8_thruster_mapper/nodes/mapper.py | 26 ++++++++++--------- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/gnc/sub8_thruster_mapper/launch/thruster_mapper.launch b/gnc/sub8_thruster_mapper/launch/thruster_mapper.launch index 2499135..94652df 100644 --- a/gnc/sub8_thruster_mapper/launch/thruster_mapper.launch +++ b/gnc/sub8_thruster_mapper/launch/thruster_mapper.launch @@ -1,6 +1,6 @@ - busses: + busses: - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A403IMC9-if00-port0 thrusters: FLV: { diff --git a/gnc/sub8_thruster_mapper/nodes/mapper.py b/gnc/sub8_thruster_mapper/nodes/mapper.py index 372d0c0..1df25a6 100755 --- a/gnc/sub8_thruster_mapper/nodes/mapper.py +++ b/gnc/sub8_thruster_mapper/nodes/mapper.py @@ -5,6 +5,7 @@ from sub8_ros_tools import wait_for_param, thread_lock, rosmsg_to_numpy import threading from sub8_msgs.msg import Thrust, ThrusterCmd +from sub8_msgs.srv import ThrusterInfo from geometry_msgs.msg import WrenchStamped @@ -35,8 +36,10 @@ def __init__(self, layout): ''' self.min_thrust, self.max_thrust = self.get_ranges() + # Make thruster layout self.B = self.generate_B(layout) + self.wrench_sub = rospy.Subscriber('/wrench', WrenchStamped, self.request_wrench_cb, queue_size=1) self.thruster_pub = rospy.Publisher('/thrust', Thrust, queue_size=1) @@ -51,15 +54,21 @@ def update_layout(self, srv): def get_ranges(self): '''Get upper and lower thrust limits for each thruster - Todo: Actually do something - --> Wait for a service in the thruster driver to say something - - We have this information in the form of calibration.json + --> Add range service proxy using thruster names + --> This is not necessary, since they are all the same thruster ''' - rospy.logwarn("Thruster mapper not querying thrusters for range data") - return np.array([-90] * 8), np.array([90] * 8) + range_service = 'thruster_range' + rospy.logwarn("Waiting for service {}".format(range_service)) + rospy.wait_for_service(range_service) + rospy.logwarn("Got {}".format(range_service)) + range_service_proxy = rospy.ServiceProxy(range_service, ThrusterInfo) + thruster_range = range_service_proxy(0) + + return np.array([thruster_range.min_force] * 8), np.array([thruster_range.max_force] * 8) def get_thruster_wrench(self, position, direction): '''Compute a single column of B, or the wrench created by a particular thruster''' + assert np.close(1.0, np.linalg.norm(direction), atol=1e-3), "Direction must be a unit vector" forces = direction torques = np.cross(position, forces) wrench_column = np.hstack([forces, torques]) @@ -153,13 +162,6 @@ def request_wrench_cb(self, msg): if __name__ == '__main__': - ''' - --> Deal with ranges - --> Deal with getting B - --> Figure out how to communicate with the thrusters - --> Do these things in TF - - Decided not to do this to avoid needless traffic for a lot of static transforms - ''' rospy.init_node('thruster_mapper') busses = wait_for_param('busses') mapper = ThrusterMapper(busses) From 986e6ceda78ad126b4667f6f66216c318ff59e90 Mon Sep 17 00:00:00 2001 From: Jacob Panikulam Date: Sat, 12 Sep 2015 14:36:37 -0400 Subject: [PATCH 5/7] DRIVERS: Add thruster status publishing - over thrusters/thruster_status --- .../nodes/thruster_driver.py | 33 +++++++++++++------ drivers/sub8_videoray_m5_thruster/readme.md | 1 + .../sub8_thruster_comm/thruster_comm.py | 7 ++-- .../sub8_thruster_comm/thruster_fake.py | 12 +++---- utils/sub8_msgs/CMakeLists.txt | 2 ++ utils/sub8_msgs/msg/ThrusterStatus.msg | 8 +++++ 6 files changed, 44 insertions(+), 19 deletions(-) create mode 100644 utils/sub8_msgs/msg/ThrusterStatus.msg diff --git a/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py b/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py index cfc1bf6..6fc8baa 100755 --- a/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py +++ b/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py @@ -5,7 +5,8 @@ import scipy.interpolate import threading import argparse -from sub8_msgs.msg import Thrust, ThrusterCmd +from std_msgs.msg import Header +from sub8_msgs.msg import Thrust, ThrusterCmd, ThrusterStatus from sub8_ros_tools import wait_for_param, thread_lock from sub8_msgs.srv import ThrusterInfo, ThrusterInfoResponse from sub8_thruster_comm import thruster_comm_factory @@ -19,10 +20,7 @@ def __init__(self, config_path, bus_layout): - Instantiate ThrusterPorts, (Either simulated or real), for communicating with thrusters - Track a thrust_dict, which maps thruster names to the appropriate port - Given a command message, route that command to the appropriate port/thruster - - TODO: - - Publish thruster status messages - + - Send a thruster status message describing the status of the particular thruster ''' self.make_fake = rospy.get_param('simulate', False) if self.make_fake: @@ -35,8 +33,9 @@ def __init__(self, config_path, bus_layout): # Bus configuration self.port_dict = self.load_bus_layout(bus_layout) - thrust_service = rospy.Service('thruster_range', ThrusterInfo, self.get_thruster_info) - self.thrust_sub = rospy.Subscriber('/thrust', Thrust, self.thrust_cb, queue_size=1) + thrust_service = rospy.Service('thrusters/thruster_range', ThrusterInfo, self.get_thruster_info) + self.thrust_sub = rospy.Subscriber('thrusters/thrust', Thrust, self.thrust_cb, queue_size=1) + self.status_pub = rospy.Publisher('thrusters/thruster_status', ThrusterStatus, queue_size=8) def load_config(self, path): '''Load the configuration data: @@ -92,6 +91,23 @@ def command_thruster(self, name, force): # We immediately get thruster_status back thruster_status = target_port.command_thruster(name, force) + message_contents = [ + 'rpm', + 'bus_voltage', + 'bus_current', + 'temperature', + 'fault', + 'response_node_id', + ] + message_keyword_args = {key: thruster_status[key] for key in message_contents} + + self.status_pub.publish( + ThrusterStatus( + header=Header(stamp=rospy.Time.now()), + name=name, + **message_keyword_args + ) + ) def thrust_cb(self, msg): '''Callback for recieving thrust commands @@ -102,9 +118,6 @@ def thrust_cb(self, msg): if __name__ == '__main__': - ''' - --> Simulation capabilities (Return thruster ranges) - ''' usage_msg = "Interface to Sub8's VideoRay M5 thrusters" desc_msg = "Specify a path to the configuration.json file containing the thrust calibration data" parser = argparse.ArgumentParser(usage=usage_msg, description=desc_msg) diff --git a/drivers/sub8_videoray_m5_thruster/readme.md b/drivers/sub8_videoray_m5_thruster/readme.md index f9b7223..4c33768 100644 --- a/drivers/sub8_videoray_m5_thruster/readme.md +++ b/drivers/sub8_videoray_m5_thruster/readme.md @@ -16,3 +16,4 @@ Run with the argument `--configuration_path=$(find sub8_videoray_m5_thruster)/co # ROS +The thruster driver takes commands over the topic `thrusters/thrust` and publishes status over `thrusters/thruster_status` \ No newline at end of file diff --git a/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/thruster_comm.py b/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/thruster_comm.py index c242253..0b01a6e 100644 --- a/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/thruster_comm.py +++ b/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/thruster_comm.py @@ -4,6 +4,7 @@ import binascii from .protocol import Const import serial +import rospy class ThrusterPort(object): @@ -109,9 +110,9 @@ def read_status(self): 'header_checksum', 'device_type', 'rpm', - 'bus_v', - 'bus_i', - 'temp', + 'bus_voltage', + 'bus_current', + 'temperature', 'fault', 'payload_checksum', ] diff --git a/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/thruster_fake.py b/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/thruster_fake.py index efe68bb..6c5c8f0 100644 --- a/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/thruster_fake.py +++ b/drivers/sub8_videoray_m5_thruster/sub8_thruster_comm/thruster_fake.py @@ -17,8 +17,8 @@ def load_thruster_config(self, thruster_name, thruster_info): self.status_dict[thruster_name] = { 'fault': 0, 'rpm': 0, - 'temp': 30, - 'bus_v': 48, + 'temperature': 30, + 'bus_voltage': 48, } def read_status(self, thruster_name): @@ -35,13 +35,13 @@ def read_status(self, thruster_name): 'header_checksum', 'device_type', 'rpm', - 'bus_v', - 'bus_i', - 'temp', + 'bus_voltage', + 'bus_current', + 'temperature', 'fault', 'payload_checksum', ] - response_dict = {key: self.status_dict.get(key, 0) for key in response_keys} + response_dict = {key: self.status_dict[thruster_name].get(key, 0) for key in response_keys} return response_dict def command_thruster(self, thruster_name, normalized_thrust): diff --git a/utils/sub8_msgs/CMakeLists.txt b/utils/sub8_msgs/CMakeLists.txt index 41ebf26..f9e88c6 100644 --- a/utils/sub8_msgs/CMakeLists.txt +++ b/utils/sub8_msgs/CMakeLists.txt @@ -14,6 +14,7 @@ add_message_files( Alarm.msg Thrust.msg ThrusterCmd.msg + ThrusterStatus.msg ) add_service_files( FILES @@ -24,6 +25,7 @@ add_service_files( # FILES # Action1.action # ) + generate_messages( DEPENDENCIES geometry_msgs diff --git a/utils/sub8_msgs/msg/ThrusterStatus.msg b/utils/sub8_msgs/msg/ThrusterStatus.msg new file mode 100644 index 0000000..045cd0b --- /dev/null +++ b/utils/sub8_msgs/msg/ThrusterStatus.msg @@ -0,0 +1,8 @@ +std_msgs/Header header +string name +float32 rpm +float32 bus_voltage +float32 bus_current +float32 temperature +uint8 fault +uint8 response_node_id \ No newline at end of file From b1180d725d134024423e5964d7cfdd16b1ecec68 Mon Sep 17 00:00:00 2001 From: Jacob Panikulam Date: Sat, 12 Sep 2015 14:43:30 -0400 Subject: [PATCH 6/7] MAPPER: Fix publisher namespaces --- gnc/sub8_thruster_mapper/nodes/mapper.py | 8 ++++---- .../test/test_sub8_mapper.py | 18 +++++++++++++++++- .../test/test_sub8_mapper.test | 1 - 3 files changed, 21 insertions(+), 6 deletions(-) diff --git a/gnc/sub8_thruster_mapper/nodes/mapper.py b/gnc/sub8_thruster_mapper/nodes/mapper.py index 1df25a6..4b4e008 100755 --- a/gnc/sub8_thruster_mapper/nodes/mapper.py +++ b/gnc/sub8_thruster_mapper/nodes/mapper.py @@ -40,8 +40,8 @@ def __init__(self, layout): # Make thruster layout self.B = self.generate_B(layout) - self.wrench_sub = rospy.Subscriber('/wrench', WrenchStamped, self.request_wrench_cb, queue_size=1) - self.thruster_pub = rospy.Publisher('/thrust', Thrust, queue_size=1) + self.wrench_sub = rospy.Subscriber('wrench', WrenchStamped, self.request_wrench_cb, queue_size=1) + self.thruster_pub = rospy.Publisher('thrusters/thrust', Thrust, queue_size=1) @thread_lock(lock) def update_layout(self, srv): @@ -57,7 +57,7 @@ def get_ranges(self): --> Add range service proxy using thruster names --> This is not necessary, since they are all the same thruster ''' - range_service = 'thruster_range' + range_service = 'thrusters/thruster_range' rospy.logwarn("Waiting for service {}".format(range_service)) rospy.wait_for_service(range_service) rospy.logwarn("Got {}".format(range_service)) @@ -68,7 +68,7 @@ def get_ranges(self): def get_thruster_wrench(self, position, direction): '''Compute a single column of B, or the wrench created by a particular thruster''' - assert np.close(1.0, np.linalg.norm(direction), atol=1e-3), "Direction must be a unit vector" + assert np.isclose(1.0, np.linalg.norm(direction), atol=1e-3), "Direction must be a unit vector" forces = direction torques = np.cross(position, forces) wrench_column = np.hstack([forces, torques]) diff --git a/gnc/sub8_thruster_mapper/test/test_sub8_mapper.py b/gnc/sub8_thruster_mapper/test/test_sub8_mapper.py index cdbc175..b5a4b00 100755 --- a/gnc/sub8_thruster_mapper/test/test_sub8_mapper.py +++ b/gnc/sub8_thruster_mapper/test/test_sub8_mapper.py @@ -7,6 +7,7 @@ import unittest import numpy as np from sub8_msgs.msg import Thrust, ThrusterCmd +from sub8_msgs.srv import ThrusterInfo, ThrusterInfoResponse from geometry_msgs.msg import WrenchStamped, Wrench, Vector3 import rospy import rostest @@ -22,11 +23,23 @@ def setUp(self, *args): ''' self.got_msg = False self.test_data = [] + thrust_service = rospy.Service('thrusters/thruster_range', ThrusterInfo, self.get_thruster_info) + def thrust_callback(self, msg): self.got_msg = True self.test_data.append(msg) + def get_thruster_info(self, srv): + query_id = srv.thruster_id + min_thrust = -100 + max_thrust = 90 + thruster_info = ThrusterInfoResponse( + min_force=min_thrust, + max_force=max_thrust + ) + return thruster_info + def test_map_good(self): '''Test desired wrenches that are known to be achievable ''' @@ -52,8 +65,11 @@ def test_map_good(self): np.arange(6)[::-1] * 5, np.arange(6) * -5, np.arange(6)[::-1] * -5, + np.arange(6)[::-1] * -100, + np.arange(6)[::-1] * 100, ] - rospy.Subscriber("/thrust", Thrust, self.thrust_callback) + + rospy.Subscriber("/thrusters/thrust", Thrust, self.thrust_callback) for num, wrench in enumerate(wrenches): wrench_msg = WrenchStamped( diff --git a/gnc/sub8_thruster_mapper/test/test_sub8_mapper.test b/gnc/sub8_thruster_mapper/test/test_sub8_mapper.test index d6f1deb..937f432 100644 --- a/gnc/sub8_thruster_mapper/test/test_sub8_mapper.test +++ b/gnc/sub8_thruster_mapper/test/test_sub8_mapper.test @@ -1,5 +1,4 @@ - \ No newline at end of file From 8991fb2de66ca28725311fada49e70baacfd2a8c Mon Sep 17 00:00:00 2001 From: Jacob Panikulam Date: Sat, 12 Sep 2015 14:44:07 -0400 Subject: [PATCH 7/7] DRIVERS: Add thruster communication unit tests --- .../sub8_videoray_m5_thruster/CMakeLists.txt | 8 +++- .../test/test_thruster_comm.py | 43 +++++++++++++++++++ 2 files changed, 50 insertions(+), 1 deletion(-) create mode 100644 drivers/sub8_videoray_m5_thruster/test/test_thruster_comm.py diff --git a/drivers/sub8_videoray_m5_thruster/CMakeLists.txt b/drivers/sub8_videoray_m5_thruster/CMakeLists.txt index 49c7b14..7536482 100644 --- a/drivers/sub8_videoray_m5_thruster/CMakeLists.txt +++ b/drivers/sub8_videoray_m5_thruster/CMakeLists.txt @@ -7,4 +7,10 @@ catkin_python_setup() catkin_package() include_directories( ${catkin_INCLUDE_DIRS} -) \ No newline at end of file +) + +# Add folders to be run by python nosetests +if(CATKIN_ENABLE_TESTING) + catkin_add_nosetests(test) +endif() + diff --git a/drivers/sub8_videoray_m5_thruster/test/test_thruster_comm.py b/drivers/sub8_videoray_m5_thruster/test/test_thruster_comm.py new file mode 100644 index 0000000..4fb6362 --- /dev/null +++ b/drivers/sub8_videoray_m5_thruster/test/test_thruster_comm.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python +import unittest +import numpy as np +from sub8_thruster_comm import thruster_comm_factory, FakeThrusterPort + +class TestThrusterComm(unittest.TestCase): + def setUp(self): + port = '/dev/fake_port' + node_id = 17 + thruster_name = 'BRL' + + self.port_info = { + 'port': port, + 'thrusters': { + 'BRV': { + 'node_id': 16, + }, + thruster_name: { + 'node_id': node_id, + } + } + } + + def test_thruster_comm_factory_fake(self): + '''Test that the thruster factory returns a proper simulated FakeThrusterPort''' + # This should succeed + thrust_comm = thruster_comm_factory(self.port_info, fake=True) + + def test_fake_thruster_status(self): + '''Test that the fake thruster status published is what we expect''' + thrust_comm = FakeThrusterPort(self.port_info) + fake_status = thrust_comm.command_thruster('BRV', 0.2) + self.assertEqual(fake_status['bus_voltage'], 48) + + def test_thruster_comm_factory_real_fail(self): + '''Test that the comm factory fails to create a ThrusterPort on a port that does not exist''' + # This should fail + with self.assertRaises(IOError): + thrust_comm = thruster_comm_factory(self.port_info, fake=False) + + +if __name__ == '__main__': + unittest.main() \ No newline at end of file