diff --git a/.travis.yml b/.travis.yml index ae8c9c2..16e545d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -89,7 +89,7 @@ install: - cd ~/catkin_ws/src - ln -s $CI_SOURCE_PATH . -# Install all dependencies, using wstool and rosdep. +# Install all dependencies, using wstool, rosdep, and pip. # wstool looks for a ROSINSTALL_FILE defined in the environment variables. before_script: # source dependencies: install using wstool. @@ -100,6 +100,7 @@ before_script: # package depdencies: install using rosdep. - cd ~/catkin_ws - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO + - sudo pip install pyusb --pre # Compile and test (fail if any step fails). If the CATKIN_OPTIONS file exists, # use it as an argument to catkin_make, for example to blacklist certain diff --git a/cr17/CMakeLists.txt b/cr17/CMakeLists.txt index 4c6312c..2274c50 100644 --- a/cr17/CMakeLists.txt +++ b/cr17/CMakeLists.txt @@ -190,10 +190,15 @@ include_directories( # endif() ## Add folders to be run by python nosetests -#catkin_add_nosetests(test) +catkin_add_nosetests(test) if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) - add_rostest(test/test_navigator.test) + foreach(T + test/test_navigator.test + test/test_mbed_interface.test + ) + add_rostest(${T}) + endforeach() endif() # Build transform inverter diff --git a/cr17/launch/Demo_Teleop.launch b/cr17/launch/Demo_Teleop.launch new file mode 100644 index 0000000..466f207 --- /dev/null +++ b/cr17/launch/Demo_Teleop.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/cr17/launch/control_station.launch b/cr17/launch/control_station.launch index 31e829b..98ef5ac 100644 --- a/cr17/launch/control_station.launch +++ b/cr17/launch/control_station.launch @@ -2,4 +2,12 @@ + + + + + + + + diff --git a/cr17/launch/mbed_interface_launch.launch b/cr17/launch/mbed_interface_launch.launch new file mode 100644 index 0000000..16428fa --- /dev/null +++ b/cr17/launch/mbed_interface_launch.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/cr17/launch/robot_teleop.launch b/cr17/launch/robot_teleop.launch index 01f5680..8f6bdfd 100644 --- a/cr17/launch/robot_teleop.launch +++ b/cr17/launch/robot_teleop.launch @@ -1,13 +1,12 @@ - - - - - - + + + + + diff --git a/cr17/launch/robot_teleop_logitech.launch b/cr17/launch/robot_teleop_logitech.launch new file mode 100644 index 0000000..e41da0e --- /dev/null +++ b/cr17/launch/robot_teleop_logitech.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/cr17/launch/teleop_tank_logitech.launch b/cr17/launch/teleop_tank_logitech.launch new file mode 100644 index 0000000..1931f7f --- /dev/null +++ b/cr17/launch/teleop_tank_logitech.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/cr17/launch/usb_cam.launch b/cr17/launch/usb_cam.launch new file mode 100644 index 0000000..3d25fe9 --- /dev/null +++ b/cr17/launch/usb_cam.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + diff --git a/cr17/nodes/cr17/joystick_controller.py b/cr17/nodes/cr17/joystick_controller.py index c93934b..60506d8 100755 --- a/cr17/nodes/cr17/joystick_controller.py +++ b/cr17/nodes/cr17/joystick_controller.py @@ -4,6 +4,8 @@ from sensor_msgs.msg import Joy from std_msgs.msg import Int16, String from cr17.srv import autonomousActive +from cr17.msg import scoopControl + ''' This module is used to convert joystick messages into appropriate affector commands. @@ -12,13 +14,15 @@ ################################### # Constants CONTROLLER_BUTTONS = {"A": 0, "B":1, "X": 2, "Y": 3, "R1": 5, "L1": 4, "BACK": 6, "START": 7} # TODO: FINISH THIS, USE BELOW -CONTROLLER_AXES = {"LSTICKV": 1, "LSTICKH": 0} +CONTROLLER_AXES = {"LSTICKV": 1, "LSTICKH": 0, "RTRIGGER":5, "LTRIGGER":2} # TODO: make these ROS parameters -MAX_MAG = 7 +MAX_MAG = 1 SLOP_THRESH = 0.15 +ARM_MOVE_MAG = 10 +HAND_MOVE_MAG = 12 # Axes to use for drive twists -JOY_LINEAR_AXIS = CONTROLLER_AXES["LSTICKV"] -JOY_ANGULAR_AXIS = CONTROLLER_AXES["LSTICKH"] +JOY_LINEAR_AXIS = rospy.get_param("joy/linear", CONTROLLER_AXES["LSTICKV"]) +JOY_ANGULAR_AXIS = rospy.get_param("joy/angular", CONTROLLER_AXES["LSTICKH"]) ARM_DOWN_AXIS = rospy.get_param("joy/arm_down", 2) ARM_UP_AXIS = rospy.get_param("joy/arm_up", 5) @@ -45,13 +49,16 @@ def __init__(self): ''' rospy.init_node("joystick_controller") - self.joy_received = False + self.joy_received = False + self.arm_state = scoopControl() + self.arm_state.armAngle = 50 + self.arm_state.scoopAngle = 50 global DRIVE_SPEED try: # Constants for drive speeds - DRIVE_SPEED = int(rospy.get_param("drive_settings/drive_speed")) - except: + DRIVE_SPEED = int(rospy.get_param("drive_settings/drive_speed", 1)) + except: rospy.logerr("Failed to load motor parameters.") self.controller_state = Joy() @@ -66,18 +73,24 @@ def __init__(self): # Load topic names self.joystick_topic = rospy.get_param("topics/joystick", "joy") drive_topic = rospy.get_param("topics/drive_cmds", "cmd_vel") + self.arm_state_topic = rospy.get_param('topics/scoop_state_cmds', "cmd_scoop") + self.arm_state_in = rospy.get_param("topics/scoop_state_current", "scoop_out") + # Setup publishers self.drive_pub = rospy.Publisher(drive_topic, Twist, queue_size = 10) + self.scoop_pub = rospy.Publisher(self.arm_state_topic, scoopControl, queue_size=10) + # Setup subscribers rospy.Subscriber(self.joystick_topic, Joy, self.joy_callback) + rospy.Subscriber(self.arm_state_in, scoopControl, self.arm_callback) def start_teleop(self): self.teleopEnabled = True - try: - autonomousScoopService = rospy.ServiceProxy("/autonomousScoop", autonomousActive) - autonomousScoopService(False) - except rospy.ServiceException, e: - rospy.logerr("/autonomousScoop service call failed: %s",e) + # try: + # autonomousScoopService = rospy.ServiceProxy("/autonomousScoop", autonomousActive) + # autonomousScoopService(False) + # except rospy.ServiceException, e: + # rospy.logerr("/autonomousScoop service call failed: %s",e) try: autonomousNavigatorService = rospy.ServiceProxy("/autonomousNavigator", autonomousActive) @@ -125,6 +138,11 @@ def joy_callback(self, data): self.start_teleop() self.teleopButton_prev = data.buttons[TELEOP_BUTTON] + def arm_callback(self, data): + self.arm_state = data + + def combineCleanArm(self, armDown, armUp): + return (-1*(armUp-1))+(armDown-1) def run(self): ''' @@ -138,6 +156,7 @@ def run(self): while not rospy.is_shutdown(): # Grab most recent controller state current_state = self.controller_state + self.joy_received = False ###### # Build Twist message ###### @@ -152,14 +171,14 @@ def run(self): if (lin_val <= SLOP_THRESH) and (lin_val >= -SLOP_THRESH): # Within 0 point slop lin_vel = 0 - else: - lin_vel = (lin_val / mag) * MAX_MAG + else: + lin_vel = lin_val * MAX_MAG#(lin_val / mag) * MAX_MAG if (ang_val <= SLOP_THRESH) and (ang_val >= -SLOP_THRESH): # Within 0 point slop ang_vel = 0 else: - ang_vel = (ang_val / mag) * MAX_MAG + ang_vel = ang_val * MAX_MAG #(ang_val / mag) * MAX_MAG lin_history.pop(0) vel_history.pop(0) @@ -174,7 +193,27 @@ def run(self): if self.teleopEnabled: self.drive_pub.publish(twister) - self.joy_received = False + arm_up = current_state.axes[ARM_UP_AXIS] + arm_down = current_state.axes[ARM_DOWN_AXIS] + + hand_up = current_state.buttons[HAND_UP_BUTTON] + hand_down = current_state.buttons[HAND_DOWN_BUTTON] + + arm_change = self.combineCleanArm(arm_down, arm_up) + hand_change = hand_up - hand_down + + try: + arm_change = arm_change/abs(arm_change) #this is temporary and just for the video + except Exception: + arm_change = 0 + + desired_scoop_state = scoopControl() + + desired_scoop_state.armAngle = self.arm_state.armAngle + (ARM_MOVE_MAG*arm_change) + desired_scoop_state.scoopAngle = self.arm_state.scoopAngle + (HAND_MOVE_MAG*hand_change) + + self.scoop_pub.publish(desired_scoop_state) + rate.sleep() if __name__ == "__main__": diff --git a/cr17/nodes/cr17/joystick_controller_tank.py b/cr17/nodes/cr17/joystick_controller_tank.py new file mode 100755 index 0000000..9f252a9 --- /dev/null +++ b/cr17/nodes/cr17/joystick_controller_tank.py @@ -0,0 +1,208 @@ +#!/usr/bin/env python +import rospy, math +from geometry_msgs.msg import Twist +from sensor_msgs.msg import Joy +from std_msgs.msg import Int16, String +from cr17.srv import autonomousActive +from cr17.msg import scoopControl + + +''' +This module is used to convert joystick messages into appropriate affector commands. +''' + +################################### +# Constants +CONTROLLER_BUTTONS = {"A": 0, "B":1, "X": 2, "Y": 3, "R1": 5, "L1": 4, "BACK": 6, "START": 7} # TODO: FINISH THIS, USE BELOW +CONTROLLER_AXES = {"LSTICKV": 1, "LSTICKH": 0, "RTRIGGER":5, "LTRIGGER":2} +# TODO: make these ROS parameters +MAX_MAG = 1 +SLOP_THRESH = 0.15 +ARM_MOVE_MAG = 10 +HAND_MOVE_MAG = 12 +# Axes to use for drive twists +JOY_LINEAR_AXIS = rospy.get_param("joy/linear", CONTROLLER_AXES["LSTICKV"]) +JOY_ANGULAR_AXIS = rospy.get_param("joy/angular", CONTROLLER_AXES["LSTICKH"]) + +ARM_DOWN_AXIS = rospy.get_param("joy/arm_down", 2) +ARM_UP_AXIS = rospy.get_param("joy/arm_up", 5) +HAND_DOWN_BUTTON = rospy.get_param("joy/hand_down", 4) +HAND_UP_BUTTON = rospy.get_param("joy/hand_up", 5) +TELEOP_BUTTON = rospy.get_param("joy/teleop", 8) #the center button on a wired xbox 360 controller + +################################### +# Global Constants +# Drive constants +DRIVE_SPEED = 7 +################################### + +START_IN_TELEOP = rospy.get_param("init_in_teleop", True) + + +class Joystick_Controller(object): + ''' + Drive: Twist messages + ''' + def __init__(self): + ''' + Joystick Controller constructor + ''' + rospy.init_node("joystick_controller") + + self.joy_received = False + self.arm_state = scoopControl() + self.arm_state.armAngle = 50 + self.arm_state.scoopAngle = 50 + + global DRIVE_SPEED + try: + # Constants for drive speeds + DRIVE_SPEED = int(rospy.get_param("drive_settings/drive_speed", 1)) + except: + rospy.logerr("Failed to load motor parameters.") + + self.controller_state = Joy() + + self.teleopEnabled = None + self.teleopButton_prev = 0 + if START_IN_TELEOP: + self.start_teleop() + else: + self.end_teleop() + + # Load topic names + self.joystick_topic = rospy.get_param("topics/joystick", "joy") + drive_topic = rospy.get_param("topics/drive_cmds", "cmd_vel") + self.arm_state_topic = rospy.get_param('topics/scoop_state_cmds', "cmd_scoop") + self.arm_state_in = rospy.get_param("topics/scoop_state_current", "scoop_out") + + # Setup publishers + self.drive_pub = rospy.Publisher(drive_topic, Twist, queue_size = 10) + self.scoop_pub = rospy.Publisher(self.arm_state_topic, scoopControl, queue_size=10) + + # Setup subscribers + rospy.Subscriber(self.joystick_topic, Joy, self.joy_callback) + rospy.Subscriber(self.arm_state_in, scoopControl, self.arm_callback) + + def start_teleop(self): + self.teleopEnabled = True + # try: + # autonomousScoopService = rospy.ServiceProxy("/autonomousScoop", autonomousActive) + # autonomousScoopService(False) + # except rospy.ServiceException, e: + # rospy.logerr("/autonomousScoop service call failed: %s",e) + + try: + autonomousNavigatorService = rospy.ServiceProxy("/autonomousNavigator", autonomousActive) + autonomousNavigatorService(False) + except rospy.ServiceException, e: + rospy.logerr("/autonomousNavigator service call failed: %s",e) + + try: + autonomousHLSCService = rospy.ServiceProxy("/autonomousHLSC", autonomousActive) + autonomousHLSCService(False) + except rospy.ServiceException, e: + rospy.logerr("/autonomousHLSC service call failed: %s",e) + + def end_teleop(self): + self.teleopEnabled = False + try: + autonomousScoopService = rospy.ServiceProxy("/autonomousScoop", autonomousActive) + autonomousScoopService(True) + except rospy.ServiceException, e: + rospy.logerr("/autonomousScoop service call failed: %s",e) + + try: + autonomousNavigatorService = rospy.ServiceProxy("/autonomousNavigator", autonomousActive) + autonomousNavigatorService(True) + except rospy.ServiceException, e: + rospy.logerr("/autonomousNavigator service call failed: %s",e) + + try: + autonomousHLSCService = rospy.ServiceProxy("/autonomousHLSC", autonomousActive) + autonomousHLSCService(True) + except rospy.ServiceException, e: + rospy.logerr("/autonomousHLSC service call failed: %s",e) + + def joy_callback(self, data): + ''' + Joy topic callback + ''' + self.controller_state = data + self.joy_received = True + if(data.buttons[TELEOP_BUTTON] == 1): + if(self.teleopButton_prev == 0): + if self.teleopEnabled: + self.end_teleop() + else: + self.start_teleop() + self.teleopButton_prev = data.buttons[TELEOP_BUTTON] + + def arm_callback(self, data): + self.arm_state = data + + def combineCleanArm(self, armDown, armUp): + return (-1*(armUp-1))+(armDown-1) + + def run(self): + ''' + This function is the processing function for this module. + ''' + rospy.wait_for_message(self.joystick_topic, Joy) # Wait for messege on joy topic + rate = rospy.Rate(10) + lin_history = [0 for i in xrange(0, 5)] + vel_history = [0 for i in xrange(0, 5)] + + while not rospy.is_shutdown(): + # Grab most recent controller state + current_state = self.controller_state + self.joy_received = False + ###### + # Build Twist message + ###### + twister = Twist() + ##### + # Get drive command + ##### + lin_val = current_state.axes[JOY_LINEAR_AXIS] + ang_val = current_state.axes[JOY_ANGULAR_AXIS] + + lin_history.pop(0) + vel_history.pop(0) + lin_history.append(lin_val) + vel_history.append(ang_val) + + lin_hist_avg = sum(lin_history) / len(lin_history) + vel_hist_avg = sum(vel_history) / len(vel_history) + + twister.linear.x = lin_hist_avg #reduce sudden changes + twister.angular.z = vel_hist_avg #reduce sudden changes + if self.teleopEnabled: + self.drive_pub.publish(twister) + + arm_up = current_state.axes[ARM_UP_AXIS] + arm_down = current_state.axes[ARM_DOWN_AXIS] + + hand_up = current_state.buttons[HAND_UP_BUTTON] + hand_down = current_state.buttons[HAND_DOWN_BUTTON] + + arm_change = self.combineCleanArm(arm_down, arm_up) + hand_change = hand_up - hand_down + + try: + arm_change = arm_change/abs(arm_change) #this is temporary and just for the video + except Exception: + arm_change = 0 + + desired_scoop_state = scoopControl() + + desired_scoop_state.armAngle = self.arm_state.armAngle + (ARM_MOVE_MAG*arm_change) + desired_scoop_state.scoopAngle = self.arm_state.scoopAngle + (HAND_MOVE_MAG*hand_change) + + self.scoop_pub.publish(desired_scoop_state) + + rate.sleep() + +if __name__ == "__main__": + controller = Joystick_Controller() + controller.run() diff --git a/cr17/nodes/cr17/mbed_interface.py b/cr17/nodes/cr17/mbed_interface.py index 66f721b..88a7cbd 100755 --- a/cr17/nodes/cr17/mbed_interface.py +++ b/cr17/nodes/cr17/mbed_interface.py @@ -1,15 +1,302 @@ #!/usr/bin/env python2 import rospy +from geometry_msgs.msg import Twist +from cr17.msg import wheelData, scoopControl + +import os +import sys + + +import usb.core +import usb.util + + +from time import sleep +import random + +''' +Sends low-level commands(scoop, arm, linear velocity, angular velocity) to Mbed over USB. +Also accepts speed values from Mbed for each wheel, and publishes to the wheel_speed topic. + +This node must be run under su privelages. I've been doing it using sudo su, then the command to run the node. +''' + +##Send to MBed Package Description(8 bytes) +#0 CMD_Vel: Z-Angular +#1 CMD_Vel: X-inear +#2 Scoop : Arm-Angle-MSB +#3 Scoop : Arm-ANgle-LSB +#4 Unused : Scoop-Angle-MSB +#5 Unused : Scooop-Angle-LSB +#6 Unused : +#7 Unused : + +##Receive from MBed Package Description(8 bytes) +#0 wheelData : FrntLVel +#1 wheelData : FrntRVel +#2 wheelData : BackLVel +#3 wheelData : BackRVel +#4 Unused : +#5 Unused : +#6 Unused : +#7 Unused : + +DATA_ARRAY_SIZE = 8 + +ROS_SLEEP_RATE = 10 + +MBED_VENDOR_ID = 0x1234 +MBED_PRODUCT_ID = 0x0006 + + +###Single Byte Implementation Constants## +# 4 bits for fraction + 3 bits for integer + 1 bit for sign = 8 bits +FRAC_BIT_WIDTH = 4 + +SIGN_BYTE = 0b10000000 #Used to designae the sign of the float(the MSb decides whether it is pos or neg) + +#Float upper and lowewr limits +SINGLE_BYTE_FLOAT_UPPER_LIMIT = 7.9 +SINGLE_BYTE_FLOAT_LOWER_LIMIT = -7.9 + +#Signle Byte upper and lower limts +BYTE_UPPER_LIMIT = 255 +BYTE_LOWER_LIMIT = 0 + +#Used with a bitwise AND operation to seperate the bits designating the integer portion of the float and the bits representing the fraction +INT_BITS_AND = 0b01110000 +FRAC_BITS_AND = 0b00001111 + +#Used to make room/take away for the fractional bits +INT_BITS_SHIFT = 4 + +###Two Byte Implementation Constants### + +#TWO Byte float upper/lower limits +TWO_BYTE_FLOAT_UPPER_LIMIT = 511.9 # 2^13 * 2^FRAC_BIT_WIDTH +TWO_BYTE_FLOAT_LOWER_LIMIT = 0.0 + +#The upper limts of the MSB and the LSB +MSB_UPPER_LIMIT = 255 +LSB_UPPER_LIMIT = 31 + +#The lower limts of the MSB and the LSB +MSB_LOWER_LIMIT = 0 +LSB_LOWER_LIMIT = 0 + +#USed to shift MSB around +MSB_SHIFT = 5 + +#Used to isolate the LSB +LSB_AND = 0b0000000011111 + +###################################### +#Topic Variables +###################################### +DRIVE_TOPIC = "/cmd_vel" +SCOOP_TOPIC = "/cmd_scoop" +WHEEL_SPEED_TOPIC = "/wheel_speed" +#SCOOP_OUT = "scoop_out" topic to eventually send scoop values over + +###################################### +# Needed Functions +###################################### + +#Converts two ints used into a floating point value. The implementation is described in the wiki. +def fixed_to_float_2(int_MSB, int_LSB): + + #Makes sure both fixed val inputs are ints + if ((type(int_MSB) != int) | (type(int_LSB) != int)): + raise TypeError + + #Makes sure the fixed value uses only 13 bits, and sticks to the correct scheme + if ((int_MSB > MSB_UPPER_LIMIT) | ( int_LSB > LSB_UPPER_LIMIT)): + raise ValueError("One of the two bytes is too large a value.") + + if ((int_MSB < MSB_LOWER_LIMIT) | (int_LSB < LSB_LOWER_LIMIT)): + raise ValueError("One of the two bytes is too small a value.") + + fixed_val = (int_MSB << MSB_SHIFT) | int_LSB #Shifts int_MSB by MSB_RIGHT_SHIFT in order to make room for the LSB portion + + float_val = fixed_val* 2.0**-FRAC_BIT_WIDTH + + return float_val + +#Converts float to fixed point(represented in 2 bytes) based of the scheme described in the wiki. +def float_to_fixed_2(float_val): + + if(type(float_val) != float): + raise TypeError + + if (float_val > TWO_BYTE_FLOAT_UPPER_LIMIT) | (float_val < TWO_BYTE_FLOAT_LOWER_LIMIT): + raise ValueError("Out of range for unsigned two byte implementation: > 511.9 or < 0.0") + + fixed_val = fixed_val = int(round(float_val * 2**FRAC_BIT_WIDTH)) + int_MSB = fixed_val >> MSB_SHIFT + int_LSB = fixed_val & LSB_AND + + return int_MSB, int_LSB + + +#Converts float to fixed point(represented as an 8 byte int) based of the scheme described in the wiki. +def float_to_fixed(float_val): + #Test to make sure input type is float + if(type(float_val) != float): + raise TypeError + + #Raises an error if values are out of the limts of what this unibyte encoding can handle + if (float_val > SINGLE_BYTE_FLOAT_UPPER_LIMIT) | (float_val < SINGLE_BYTE_FLOAT_LOWER_LIMIT): + raise ValueError("Out of range for signed single byte implementation: > 7.9 or < -7.9") + + if float_val < 0.0: + fixed_prime = int(round(abs(float_val) * 2**FRAC_BIT_WIDTH)) + fixed_val = fixed_prime | SIGN_BYTE #Sets the MSb to one indicatng a negative val + else: + fixed_val = int(round(abs(float_val) * 2**FRAC_BIT_WIDTH)) + + return fixed_val + + +#Converts fixed point(represented as an 8 byte int) based of the scheme described in the wiki. +def fixed_to_float(fixed_val): + + #Test to make sure input is an int + if(type(fixed_val) != int): + raise TypeError + + #If a value greater than 255 is received then this single byte encoding scheme won't work. + if (fixed_val > BYTE_UPPER_LIMIT) | (fixed_val < BYTE_LOWER_LIMIT): + raise ValueError("Value is higher/lower than the single byte limits(0 or 255)") + + #The MSb determines if it is positive or negative, hence being greater than 128 tells the sign(pos = 0, neg = 1) + if fixed_val > 128: + is_neg = True + else: + is_neg = False + + int_val = (fixed_val & INT_BITS_AND) >> INT_BITS_SHIFT + decimal_val = (fixed_val & FRAC_BITS_AND) * 2.0**-4 + + float_val = int_val + decimal_val + + if is_neg: + float_val = -float_val + + return float_val + class MbedInterface(object): def __init__(self): rospy.init_node('mbed_interface') + #These will be updated as new data is received for it. + self.__cmd_vel = Twist() + self.__cmd_scoop = scoopControl() + self.__wheel_data = wheelData() + + #Raw data list(byte array?) to/from the Mbed + self.__data = [0x0] * DATA_ARRAY_SIZE #Initializes bytes to be set to zero + self.__bytes = None + + self.arm_state_out = rospy.get_param("topics/scoop_state_current", "scoop_out") + + ###################################### + # Setup ROS Subscribers for this node + ###################################### + rospy.Subscriber(DRIVE_TOPIC, Twist, self.cmd_vel_callback) + rospy.Subscriber(SCOOP_TOPIC, scoopControl, self.cmd_scoop_callback) + + + ###################################### + # Setup ROS Publishers for this node + ###################################### + self.wheel_speed_pub = rospy.Publisher(WHEEL_SPEED_TOPIC, wheelData, queue_size = 10) + self.scoop_pub = rospy.Publisher(self.arm_state_out, scoopControl, queue_size=10) + + #This sets up the code for the USB. I'm not 100% what it all does, but it works and is entirely based off the example. + # Find device + self.__hid_device = usb.core.find(idVendor=MBED_VENDOR_ID,idProduct=MBED_PRODUCT_ID) + + if not self.__hid_device: + print "No device connected" + else: + sys.stdout.write('mbed found\n') + if self.__hid_device.is_kernel_driver_active(0): + try: + self.__hid_device.detach_kernel_driver(0) + except usb.core.USBError as e: + sys.exit("Could not detatch kernel driver: %s" % str(e)) + try: + self.__hid_device.set_configuration() + self.__hid_device.reset() + except usb.core.USBError as e: + sys.exit("Could not set configuration: %s" % str(e)) + + self.__endpoint = self.__hid_device[0][(0,0)][0] + + + + ###################################### + # ROS Callbacks + ###################################### + + #Add new cmd_vel to USB message(x-linear/z-angular) + def cmd_vel_callback(self, twist_msg): + #print "Received Twist from ", DRIVE_TOPIC + self.__data[0] = float_to_fixed(twist_msg.angular.z) + self.__data[1] = float_to_fixed(twist_msg.linear.x) + + #Add new scoop cmd to USB message(arm velocity/scoop velocity) + def cmd_scoop_callback(self, scoop_msg): + #print "Received scoopControl from ", SCOOP_TOPIC + self.__data[2], self.__data[3] = float_to_fixed_2(scoop_msg.armAngle) + self.__data[4], self.__data[5] = float_to_fixed_2(scoop_msg.scoopAngle) + + #Sends data recieved from Mbed over a topic(wheelData) + def mbed_recieve_handler(self, data): + + #publish out all the received MBED data + + #Sets up wheelData message + self.__wheel_data.frontLeftVel = fixed_to_float(data[0]) + self.__wheel_data.frontRightVel = fixed_to_float(data[1]) + self.__wheel_data.backLeftVel = fixed_to_float(data[2]) + self.__wheel_data.backRightVel = fixed_to_float(data[3]) + + #print self.__wheel_data.frontRightVel + #print self.__wheel_data.frontLeftVel + + #publishes wheelData message to topic + self.wheel_speed_pub.publish(self.__wheel_data) + + #TODO: Send Values Scoop Position Value to TF Tree + #print fixed_to_float_2(data[4], data[5]) + #print fixed_to_float_2(data[6], data[7]) + + scoop_msg = scoopControl() + scoop_msg.scoopAngle = fixed_to_float_2(data[4], data[5]) + scoop_msg.armAngle = fixed_to_float_2(data[6], data[7]) + + self.scoop_pub.publish(scoop_msg) + + + def run(self): - rospy.spin() + while not rospy.is_shutdown(): + rate = rospy.Rate(ROS_SLEEP_RATE) + + #read the data + self.__bytes = self.__hid_device.read(self.__endpoint.bEndpointAddress, 8, timeout=5000) + self.mbed_recieve_handler(self.__bytes) + + self.__hid_device.write(1, self.__data) + + rate.sleep() + + if __name__ == "__main__": - mbedInterface = MbedInterface() - mbedInterface.run() + mbedInterface = MbedInterface() + print mbedInterface.run() diff --git a/cr17/nodes/cr17/usbcam_receiver.py b/cr17/nodes/cr17/usbcam_receiver.py new file mode 100755 index 0000000..21c53cb --- /dev/null +++ b/cr17/nodes/cr17/usbcam_receiver.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python2 + +import rospy +import socket +import threading +import json +import subprocess +import roslib.message +from rospy_message_converter import message_converter, json_message_converter +from sensor_msgs.msg import CompressedImage, CameraInfo + +PINOIR_IMAGE_TOPIC = "usb_cam/image_raw/compressed" +PINOIR_CAMERA_INFO_TOPIC = "usb_cam/camera_info" + +MCAST_GRP = '224.1.1.1' +IMAGEPORT = 1026 +INFOPORT = 1036 + +try: + MY_IP_ADDR = subprocess.check_output(["ifconfig", "wlan0"]).split("inet addr:")[1].split(" ")[0] +except: + print("wlan0 failed, trying eth0") + try: + MY_IP_ADDR = subprocess.check_output(["ifconfig", "eth0"]).split("inet addr:")[1].split(" ")[0] + except: + print("eth0 failed, trying eth1") + MY_IP_ADDR = subprocess.check_output(["ifconfig", "eth1"]).split("inet addr:")[1].split(" ")[0] + + +class usbcam_receiver(object): + def __init__(self): + rospy.init_node("usbcam_receiver") + self.joy_pub = rospy.Publisher(PINOIR_IMAGE_TOPIC, CompressedImage, queue_size = 10) + self.camInfo_pub = rospy.Publisher(PINOIR_CAMERA_INFO_TOPIC, CameraInfo, queue_size = 10) + print("starting socket") + + self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 3) + self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.sock.bind((MCAST_GRP, IMAGEPORT)) + self.host = MY_IP_ADDR + self.sock.setsockopt(socket.SOL_IP, socket.IP_MULTICAST_IF, socket.inet_aton(self.host)) + self.sock.setsockopt(socket.SOL_IP, socket.IP_ADD_MEMBERSHIP, socket.inet_aton(MCAST_GRP) + socket.inet_aton(self.host)) + self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 0) + self.sock.setblocking(False) + + print("opened socket") + + + self.sockInfo = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + self.sockInfo.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 3) + self.sockInfo.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.sockInfo.bind((MCAST_GRP, INFOPORT)) + self.sockInfo.setsockopt(socket.SOL_IP, socket.IP_MULTICAST_IF, socket.inet_aton(self.host)) + self.sockInfo.setsockopt(socket.SOL_IP, socket.IP_ADD_MEMBERSHIP, socket.inet_aton(MCAST_GRP) + socket.inet_aton(self.host)) + self.sockInfo.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 0) + self.sockInfo.setblocking(False) + + + def run(self): + rate = rospy.Rate(50) + #print("here1") + maxsize = 65535 + print("about to loop") + while not rospy.is_shutdown(): + #print("here2") + try: + data2, addr = self.sock.recvfrom(maxsize) + data = CompressedImage() + data.deserialize(data2) + except socket.error, e: + #print 'Exception' + continue + + try: + data2, addr = self.sockInfo.recvfrom(maxsize) + + dataInfo = CameraInfo() + dataInfo.deserialize(data2) + except socket.error, e: + #print 'Exception' + continue + + self.joy_pub.publish(data) + self.camInfo_pub.publish(dataInfo) + rate.sleep() + + +if __name__ == "__main__": + piCamRx = usbcam_receiver() + piCamRx.run() diff --git a/cr17/nodes/cr17/usbcam_sender.py b/cr17/nodes/cr17/usbcam_sender.py new file mode 100755 index 0000000..8ab4db3 --- /dev/null +++ b/cr17/nodes/cr17/usbcam_sender.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python2 + +import rospy +import socket +import threading +import json +import subprocess +import roslib.message +import StringIO, struct +import roslib +from rospy_message_converter import message_converter, json_message_converter +from sensor_msgs.msg import CompressedImage, CameraInfo + +#PICAM_IMAGE_TOPIC = "rosberrypi_cam/image_raw" +PICAM_IMAGE_TOPIC = "/usb_cam/image_raw/compressed" +PICAM_CAMERA_INFO_TOPIC = "/usb_cam/camera_info" + +MCAST_GRP = '224.1.1.1' +IMAGEPORT = 1026 +INFOPORT = 1036 + +class usbcam_sender(object): + def __init__(self): + rospy.init_node("usbcam_sender") + + rospy.Subscriber(PICAM_IMAGE_TOPIC, CompressedImage, self.image_sub) + rospy.Subscriber(PICAM_CAMERA_INFO_TOPIC, CameraInfo, self.camInfo_sub) + print("in init") + self.message = CompressedImage() + self.newImage = False + self.newInfo = False + + self.socketSetup = False + self.socket_setup() + + + def socket_setup(self): + if self.socketSetup: + return 0 + try: + MY_IP_ADDR = subprocess.check_output(["ifconfig", "wlan0"]).split("inet addr:")[1].split(" ")[0] + except: + print("wlan0 failed, trying eth1") + try: + MY_IP_ADDR = subprocess.check_output(["ifconfig", "eth1"]).split("inet addr:")[1].split(" ")[0] + except: + print("eth1 failed, trying eth0") + try: + MY_IP_ADDR = subprocess.check_output(["ifconfig", "eth0"]).split("inet addr:")[1].split(" ")[0] + except: + self.socketSetup = False + return -1 + + self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + + self.host = MY_IP_ADDR + self.sock.setsockopt(socket.SOL_IP, socket.IP_MULTICAST_IF, socket.inet_aton(self.host)) + self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 0) + self.sock.setsockopt(socket.SOL_IP, socket.IP_ADD_MEMBERSHIP, socket.inet_aton(MCAST_GRP) + socket.inet_aton(self.host)) + self.socketSetup = True + return 1 + + + def image_sub(self, data): + self.message = data + self.newImage = True + + def camInfo_sub(self, data): + self.camInfo = data + self.newInfo = True + + def run(self): + rate = rospy.Rate(7) + while not rospy.is_shutdown(): + rate.sleep() + if not self.socketSetup: + self.socket_setup() + if notself.socketSetup: + continue + if self.newImage: + serializedData = StringIO.StringIO() + self.message.serialize(serializedData) + self.sock.sendto(serializedData.getvalue(), (MCAST_GRP, int(IMAGEPORT))) + if self.newInfo: + serializedData = StringIO.StringIO() + self.camInfo.serialize(serializedData) + self.sock.sendto(serializedData.getvalue(), (MCAST_GRP, int(INFOPORT))) + +if __name__ == "__main__": + piCamTx = usbcam_sender() + piCamTx.run() diff --git a/cr17/package.xml b/cr17/package.xml index ae4f8df..ab0450d 100644 --- a/cr17/package.xml +++ b/cr17/package.xml @@ -4,7 +4,7 @@ 0.0.0 The cr17 package - + billy @@ -49,6 +49,10 @@ rospy std_msgs joy + libusb-1.0 + + + rosunit diff --git a/cr17/param/logitech.yaml b/cr17/param/logitech.yaml new file mode 100644 index 0000000..70ff984 --- /dev/null +++ b/cr17/param/logitech.yaml @@ -0,0 +1,8 @@ +joy: + linear: 7 + angular: 6 + arm_down: 2 + arm_up: 5 + hand_down: 4 + hand_up: 5 + teleop: 8 \ No newline at end of file diff --git a/cr17/param/logitech_tank.yaml b/cr17/param/logitech_tank.yaml new file mode 100644 index 0000000..3625882 --- /dev/null +++ b/cr17/param/logitech_tank.yaml @@ -0,0 +1,8 @@ +joy: + linear: 7 #1 + angular: 4 + arm_down: 2 + arm_up: 5 + hand_down: 4 + hand_up: 5 + teleop: 8 \ No newline at end of file diff --git a/cr17/test/test_mbed_interface.py b/cr17/test/test_mbed_interface.py new file mode 100755 index 0000000..87f1fc1 --- /dev/null +++ b/cr17/test/test_mbed_interface.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python + +PKG = 'cr17' +NAME = 'test_mbed_interface' + +import unittest +import rostest + +from cr17 import mbed_interface + +SINGLE_BYTE_UPPER_LIMIT = 7.9 +SINGLE_BYTE_LOWER_LIMIT = -7.9 + + +class TestMbedInterface(unittest.TestCase): + +######Test methods###### + + #=============================# + # Test for fixed_to_float_2 # + #=============================# + #Makes sure it rejects non int values + def test_fixed_only_input(self): + self.assertRaises(TypeError, mbed_interface.fixed_to_float_2(1.0, 1.0)) + self.assertRaises(TypeError, mbed_interface.fixed_to_float_2(100, 1.0)) + self.assertRaises(TypeError, mbed_interface.fixed_to_float_2(1.0, 100)) + + + #Makes sure values greater than the limits are rejected + def test_fixed_input_upper_limit(self): + self.assertRaises(ValueError, mbed_interface.fixed_to_float_2, 256, 31) + self.assertRaises(ValueError, mbed_interface.fixed_to_float_2, 255, 32) + self.assertRaises(ValueError, mbed_interface.fixed_to_float_2, 256, 32) + + #Makes sure values less than zero are rejected + def test_fixed_input_lower_limit(self): + self.assertRaises(ValueError, mbed_interface.fixed_to_float_2, -140, 5) + self.assertRaises(ValueError, mbed_interface.fixed_to_float_2, 140, -5) + self.assertRaises(ValueError, mbed_interface.fixed_to_float_2, -140, -5) + + #Test different input output pairs + def test_fixed_input_float_output_pairs_2(self): + input_output_pairs = ( + (0,0,0.0), + (0,10,0.625), + (0,31,1.9375), + (100,0,200.0), + (100,31, 201.9375), + (255, 31, 511.9375) + ) + for MSB, LSB, output_float in input_output_pairs: + self.assertEqual(mbed_interface.fixed_to_float_2(MSB,LSB), output_float) + + + #=============================# + # Test for float_to_fixed_2 # + #=============================# + + def test_float_input_2(self): + self.assertRaises(TypeError, mbed_interface.float_to_fixed_2, 100) + + def test_float_input_upper_limit(self): + self.assertRaises(ValueError, mbed_interface.float_to_fixed_2, 512.0) + + def test_float_input_lower_limit(self): + self.assertRaises(ValueError, mbed_interface.float_to_fixed_2, -1.0) + + def test_float_input_fixed_output_pairs_2(self): + input_output_pairs = ( + (0.0, 0,0), + (0.625, 0, 10), + (1.9375, 0, 31), + (200.0, 100, 0), + (201.9375, 100, 31), + (511.9, 255, 31) + ) + for float_val, MSB, LSB in input_output_pairs: + return_MSB, return_LSB = mbed_interface.float_to_fixed_2(float_val) + self.assertAlmostEqual(return_LSB, LSB, delta = 1) + self.assertAlmostEqual(return_MSB, MSB, delta = 1) + + #===========================# + # Test for fixed_to_float # + #===========================# + + #Test for TypeError if an fixed(int) is not passed in + def test_fixed_only_input(self): + self.assertRaises(TypeError, mbed_interface.fixed_to_float, 1.0) + + #Test if value error is thrown for being greater than 255 + def test_fixed_input_upper_limit_rejection(self): + self.assertRaises(ValueError, mbed_interface.fixed_to_float, 256) + + def test_float_output_is_negative(self): + for s in (129, 180, 240, 255): + self.assertLess(mbed_interface.fixed_to_float(s), 0.0) + + def test_float_output_is_positive(self): + for s in (1, 20, 100, 127): + self.assertGreater(mbed_interface.fixed_to_float(s), 0.0) + + def test_fixed_input_float_output_pairs(self): + input_output_pairs = ( + (0, 0.0), + (2, 0.1), + (130, -0.1), + (16, 1.0), + (144, -1.0), + (40, 2.5), + (168, -2.5), + (126, 7.9), + (254, -7.9)) + for input_int, output_float in input_output_pairs: + self.assertAlmostEqual(mbed_interface.fixed_to_float(input_int), output_float, delta=0.0625) + + + #===========================# + # Test for float_to_fixed # + #===========================# + + #Makes sure a float is passed in + def test_float_only_input(self): + for s in (240, 40, 0): + self.assertRaises(TypeError, mbed_interface.float_to_fixed, s) + + #Test for upper/lower out of range error + def test_float_input_upper_limit_rejection(self): + self.assertRaises(ValueError, mbed_interface.float_to_fixed, 8.0) + + def test_float_input_lower_limit_rejection(self): + self.assertRaises(ValueError, mbed_interface.float_to_fixed, -8.0) + + #Test for making sure negative #'s are greater than 128' + def test_fixed_output_is_negative(self): + for s in (-7.0, -5.0, -2.5, -0.1): + self.assertGreater(mbed_interface.float_to_fixed(s), 128) + + #Test for making sure pos. #'s are less than 128' + def test_fixed_output_is_positive(self): + for s in (7.0, 5.0, 2.5, 0.1, 0.001): + self.assertLess(mbed_interface.float_to_fixed(s), 128) + + def test_float_input_fixed_output_pairs(self): + input_output_pairs = ( + (0.0, 0), + (-0.0, 0), + (0.1, 2), + (-0.1, 130), + (1.0, 16), + (-1.0, 144), + (2.5, 40), + (-2.5, 168), + (7.9, 126), + (-7.9, 254)) + for input_float, output_int in input_output_pairs: + self.assertEqual(mbed_interface.float_to_fixed(input_float), output_int) + + + +if __name__ == '__main__': + rostest.unitrun(PKG, NAME, TestMbedInterface) + diff --git a/cr17/test/test_mbed_interface.test b/cr17/test/test_mbed_interface.test new file mode 100644 index 0000000..c96e2e5 --- /dev/null +++ b/cr17/test/test_mbed_interface.test @@ -0,0 +1,3 @@ + + + \ No newline at end of file