diff --git a/sphero_bringup/launch/sphero.launch b/sphero_bringup/launch/sphero.launch index 860c71f..48bd5ae 100644 --- a/sphero_bringup/launch/sphero.launch +++ b/sphero_bringup/launch/sphero.launch @@ -3,14 +3,20 @@ + - + + - - \ No newline at end of file + + + + + + diff --git a/sphero_description/urdf/sphero.urdf b/sphero_description/urdf/sphero.urdf index 85fc9a1..9dbb360 100644 --- a/sphero_description/urdf/sphero.urdf +++ b/sphero_description/urdf/sphero.urdf @@ -9,21 +9,32 @@ + + + - - - - - - - - - + - + - + + + + + + + + + + + + + + + + + + @@ -34,21 +45,11 @@ - - - - - - - + diff --git a/sphero_driver/src/sphero_driver/sphero_driver.py b/sphero_driver/src/sphero_driver/sphero_driver.py index e64a908..17c7ffb 100755 --- a/sphero_driver/src/sphero_driver/sphero_driver.py +++ b/sphero_driver/src/sphero_driver/sphero_driver.py @@ -115,7 +115,8 @@ CMD_INIT_MACRO_EXECUTIVE = [0x02, 0x54], CMD_ABORT_MACRO = [0x02, 0x55], CMD_GET_MACRO_STATUS = [0x02, 0x56], - CMD_SET_MACRO_STATUS = [0x02, 0x57]) + CMD_SET_MACRO_STATUS = [0x02, 0x57], + CMD_CONFIG_LOCATOR = [0x02, 0x13]) STRM_MASK1 = dict( GYRO_H_FILTERED = 0x00000001, @@ -158,743 +159,776 @@ VELOCITY_X = 0x01000000, VELOCITY_Y = 0x00800000) +# STRM_MASK2 = dict( +# QUATERNION_Q0 = 0x00000001, +# QUATERNION_Q1 = 0x00000002, +# QUATERNION_Q2 = 0x00000004, +# QUATERNION_Q3 = 0x00000008, +# ODOM_X = 0x00000010, +# ODOM_Y = 0x00000020, +# ACCELONE = 0x00000040, +# VELOCITY_X = 0x00000080, +# VELOCITY_Y = 0x00000100) class BTInterface(object): - def __init__(self, target_name = 'Sphero', port = 1): - self.target_name = target_name - self.port = port - self.found_device = False - self.tries = 0 - self.target_address = None - self.sock = None - - def connect(self): - sys.stdout.write("Searching for devices....") - sys.stdout.flush() - - for i in range(10): - sys.stdout.write("....") - sys.stdout.flush() - nearby_devices = bluetooth.discover_devices(lookup_names = True) - - if len(nearby_devices)>0: - for bdaddr, name in nearby_devices: - #look for a device name that starts with Sphero - if name.startswith(self.target_name): - self.found_device = True - self.target_address = bdaddr - break - if self.found_device: - break - - - if self.target_address is not None: - sys.stdout.write("\nFound Sphero device with address: %s\n" % (self.target_address)) - sys.stdout.flush() - else: - sys.stdout.write("\nNo Sphero devices found.\n" ) - sys.stdout.flush() - sys.exit(1) - - try: - self.sock=bluetooth.BluetoothSocket(bluetooth.RFCOMM) - self.sock.connect((bdaddr,self.port)) - except bluetooth.btcommon.BluetoothError as error: - sys.stdout.write(error) - sys.stdout.flush() - time.sleep(5.0) - sys.exit(1) - sys.stdout.write("Paired with Sphero.\n") - sys.stdout.flush() - return True - - def send(self, data): - self.sock.send(data) - - def recv(self, num_bytes): - return self.sock.recv(num_bytes) - - def close(self): - self.sock.close() + def __init__(self, target_name = 'Sphero', port = 1): + self.target_name = target_name + self.port = port + self.found_device = False + self.tries = 0 + self.target_address = None + self.sock = None + + def connect(self, bt_address): + + if bt_address is None or bt_address == '': +# if True: + sys.stdout.write("Searching for devices....") + sys.stdout.flush() + + for i in range(10): + sys.stdout.write("....") + sys.stdout.flush() + nearby_devices = bluetooth.discover_devices(lookup_names = True) + + if len(nearby_devices)>0: + for bdaddr, name in nearby_devices: + #look for a device name that starts with Sphero + if name.startswith(self.target_name): + self.found_device = True + self.target_address = bdaddr + break + if self.found_device: + break + + if self.target_address is not None: + sys.stdout.write("\nFound Sphero device with address: %s\n" % (self.target_address)) + sys.stdout.flush() + else: + sys.stdout.write("\nNo Sphero devices found.\n" ) + sys.stdout.flush() + sys.exit(1) + + else: + self.target_address = bt_address + +# try: + self.sock=bluetooth.BluetoothSocket(bluetooth.RFCOMM) + self.sock.connect((self.target_address,self.port)) +# except bluetooth.btcommon.BluetoothError as error: +# sys.stdout.write(error) +# sys.stdout.flush() +# time.sleep(5.0) +# sys.exit(1) + + sys.stdout.write("Paired with Sphero.\n") + sys.stdout.flush() + return True + + def send(self, data): + self.sock.send(data) + + def recv(self, num_bytes): + return self.sock.recv(num_bytes) + + def close(self): + self.sock.close() class Sphero(threading.Thread): - def __init__(self, target_name = 'Sphero'): - threading.Thread.__init__(self) - self.target_name = target_name - self.bt = None - self.shutdown = False - self.is_connected = False - self.mask_list = None - self.stream_mask1 = None - self.stream_mask2 = None - self.seq = 0 - self.raw_data_buf = [] - self._communication_lock = threading.Lock() - self._async_callback_dict = dict() - self._sync_callback_dict = dict() - self._sync_callback_queue = [] - - def connect(self): - self.bt = BTInterface(self.target_name) - self.is_connected = self.bt.connect() - return True - - def inc_seq(self): - self.seq = self.seq + 1 - if self.seq > 0xff: - self.seq = 0 - - def pack_cmd(self, req ,cmd): - self.inc_seq() - # print req + [self.seq] + [len(cmd)+1] + cmd - return req + [self.seq] + [len(cmd)+1] + cmd - - def data2hexstr(self, data): - return ' '.join([ ("%02x"%ord(d)) for d in data]) - - def create_mask_list(self, mask1, mask2): - #save the mask - sorted_STRM1 = sorted(STRM_MASK1.iteritems(), key=operator.itemgetter(1), reverse=True) - #create a list containing the keys that are part of the mask - self.mask_list1 = [key for key, value in sorted_STRM1 if value & mask1] - - sorted_STRM2 = sorted(STRM_MASK2.iteritems(), key=operator.itemgetter(1), reverse=True) - #create a list containing the keys that are part of the mask - self.mask_list2 = [key for key, value in sorted_STRM2 if value & mask2] - self.mask_list = self.mask_list1 + self.mask_list2 - - - def add_async_callback(self, callback_type, callback): - self._async_callback_dict[callback_type] = callback - - def remove_async_callback(self, callback_type): - del self._async_callback_dict[callback_type] - - def add_sync_callback(self, callback_type, callback): - self._sync_callback_dict[callback_type] = callback - - def remove_sync_callback(self, callback_type): - del self._sync_callback_dict[callback_type] - - def clamp(self, n, minn, maxn): - return max(min(maxn, n), minn) - - def ping(self, response): - """ - The Ping command is used to verify both a solid data link with the - Client and that Sphero is awake and dispatching commands. - - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_PING'],[]), response) - - def get_version(self, response): - """ - The Get Versioning command returns a whole slew of software and - hardware information. - - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_VERSION'],[]), response) - - def set_device_name(self, name, response): - """ - This assigned name is held internally and produced as part of the - Get Bluetooth Info service below. Names are clipped at 48 - characters in length to support UTF-8 sequences; you can send - something longer but the extra will be discarded. This field - defaults to the Bluetooth advertising name. - - :param name: 48 character name. - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_SET_BT_NAME'],[name]), response) - - def get_bt_name(self, response): - """ - This returns the textual name (in ASCII) that the Bluetooth module - advertises. It also returns the BTA Bluetooth Address or MAC ID - for this device. Both values are returned in ASCII and have field - widths of 16 characters, with unused trailing characters set to - 00h. - - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_GET_BT_NAME'],[]), response) - - def set_auto_reconnect(self, enable, time, response): - """ - This configures the control of the Bluetooth module in its attempt - to automatically reconnect with the last iPhone device. This is a - courtesy behavior since the iPhone Bluetooth stack doesn't initiate - automatic reconnection on its own. The two parameters are simple: - flag = 00h to disable or 01h to enable, and time = the number of - seconds after power-up in which to enable auto reconnect mode. For - example, if time = 30 then the module will be attempt reconnecting 30 - seconds after waking up. - - :param enable: 00h to disable or 01h to enable auto reconnecting. - :param time: the number of seconds after power-up in which to\ - enable auto reconnect mode - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_SET_AUTO_RECONNECT'],[enable,time]), response) - - def get_auto_reconnect(self, response): - """ - This returns the Bluetooth auto reconnect values as defined in the - Set Auto Reconnect command. - - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_GET_AUTO_RECONNECT'],[]), reponse) - - def get_power_state(self, response): - """ - This returns the current power state and some additional - parameters to the Client. - - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_GET_PWR_STATE'],[]), response) - - def set_power_notify(self, enable, response): - """ - This enables Sphero to asynchronously notify the Client - periodically with the power state or immediately when the power - manager detects a state change. Timed notifications arrive every 10 - seconds until they're explicitly disabled or Sphero is unpaired. The - flag is as you would expect, 00h to disable and 01h to enable. - - :param enable: 00h to disable and 01h to enable power notifications. - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_SET_PWR_NOTIFY'],[enable]), response) - - def go_to_sleep(self, time, macro, response): - """ - This puts Sphero to sleep immediately with two parameters: the - first is the number of seconds after which it will automatically - re-awaken. If set to zero, this feature is disabled. If non-zero then - the MACRO parameter allows an optional system macro to be run upon - wakeup. If this is set to zero, no macro is executed. - - :param time: number of seconds wait before auto re-awake. - :param macro: macro number to run when re-awakened. - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_SLEEP'],[(time>>8), (time & 0xff), macro]), response) - - def run_l1_diags(self, response): - """ - This is a developer-level command to help diagnose aberrant - behavior. Most system counters, process flags, and system states - are decoded into human readable ASCII. There are two responses to - this command: a Simple Response followed by a large async message - containing the results of the diagnostic tests. - - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_RUN_L1_DIAGS'],[]), response) - - def run_l2_diags(self, response): - """ - This is a developers-only command to help diagnose aberrant - behavior. It is much less impactful than the Level 1 command as it - doesn't interfere with the current operation it simply returns a - current set of statistics to the client. - - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_RUN_L2_DIAGS'],[]), response) - - def clear_counters(self, response): - """ - This is a developers-only command to clear the various system - counters described in the level 2 diag. - - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_CLEAR_COUNTERS'],[]), response) - - def assign_counter_value(self, counter, response): - """ - Sphero contains a 32-bit counter that increments every millisecond - when it's not in the Idle state. It has no absolute meaning and is - reset for various reasons. This command assigns the counter a - specific value for subsequent sampling. - - :param counter: value to set the counter to. - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_ASSIGN_COUNTER'],[((counter>>24) & 0xff), ((counter>>16) & 0xff), ((counter>>8) & 0xff) ,(counter & 0xff)]), response) - - def poll_packet_times(self, time, response): - """ - This command helps the Client application profile the transmission - and processing latencies in Sphero so that a relative - synchronization of timebases can be performed. It allows the - Client to reconcile time stamped messages from Sphero to its own - time stamped events. - - The scheme is as follows: the Client sends the command with the - Client Tx time (:math:`T_{1}`) filled in. Upon receipt of the packet, the - command processor in Sphero copies that time into the response - packet and places the current value of the millisecond counter - into the Sphero Rx time field (:math:`T_{2}`). Just before the transmit - engine streams it into the Bluetooth module, the Sphero Tx time - value (:math:`T_{3}`) is filled in. If the Client then records the time at - which the response is received (:math:`T_{4}`) the relevant time segments can - be computed from the four time stamps (:math:`T_{1}, T_{2}, T_{3}, T_{4}`): - - * The value offset represents the maximum-likelihood time offset\ - of the Client clock to Sphero's system clock. - * offset :math:`= 1/2*[(T_{2} - T_{1}) + (T_{3} - T_{4})]` - - * The value delay represents the round-trip delay between the\ - Client and Sphero: - * delay :math:`= (T_{4} - T_{1}) - (T_{3} - T_{2})` - - :param time: client Tx time. - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_POLL_TIME'],[((time>>24) & 0xff), ((time>>16) & 0xff), ((time>>8) & 0xff), (time & 0xff)]), response) - - def set_heading(self, heading, response): - """ - This allows the client to adjust the orientation of Sphero by - commanding a new reference heading in degrees, which ranges from 0 - to 359. You will see the ball respond immediately to this command - if stabilization is enabled. - - :param heading: heading in degrees from 0 to 359 (motion will be\ - shortest angular distance to heading command) - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_SET_HEADING'],[(heading>>8),(heading & 0xff)]), response) - - def set_stablization(self, enable, response): - """ - This turns on or off the internal stabilization of Sphero, in - which the IMU is used to match the ball's orientation to its - various set points. The flag value is as you would expect, 00h for - off and 01h for on. - - :param enable: 00h for off and 01h for on (on by default). - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_SET_STABILIZ'],[enable]), response) - - def set_rotation_rate(self, rate, response): - """ - This allows you to control the rotation rate that Sphero will use - to meet new heading commands. The commanded value is in units of - 0.784 degrees/sec. So, setting a value of c8h will set the - rotation rate to 157 degrees/sec. A value of 255 jumps to the - maximum and a value of 1 is the minimum. - - :param rate: rotation rate in units of 0.784degrees/sec (setting\ - this value will not cause the device to move only set the rate it\ - will move in other funcation calls). - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_SET_ROTATION_RATE'],[self.clamp(rate, 0, 255)]), response) - - def set_app_config_blk(self, app_data, response): - """ - This allows you to write a 32 byte block of data from the - configuration block that is set aside for exclusive use by - applications. - - :param app_data: block set aside for application. - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_SET_APP_CONFIG_BLK'],[((app_data>>24) & 0xff), ((app_data>>16) & 0xff), ((app_data>>8) & 0xff), (app_data & 0xff)]), response) - - def get_app_config_blk(self, response): - """ - This allows you to retrieve the application configuration block\ - that is set aside for exclusive use by applications. - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_GET_APP_CONFIG_BLK'], []), response) - - def set_data_strm(self, sample_div, sample_frames, sample_mask1, pcnt, sample_mask2, response): - """ - Currently the control system runs at 400Hz and because it's pretty - unlikely you will want to see data at that rate, N allows you to - divide that down. sample_div = 2 yields data samples at 200Hz, - sample_div = 10, 40Hz, etc. Every data sample consists of a - "frame" made up of the individual sensor values as defined by the - sample_mask. The sample_frames value defines how many frames to - collect in memory before the packet is emitted. In this sense, it - controls the latency of the data you receive. Increasing - sample_div and the number of bits set in sample_mask drive the - required throughput. You should experiment with different values - of sample_div, sample_frames and sample_mask to see what works - best for you. - - :param sample_div: divisor of the maximum sensor sampling rate. - :param sample_frames: number of sample frames emitted per packet. - :param sample_mask1: bitwise selector of data sources to stream. - :param pcnt: packet count (set to 0 for unlimited streaming). - :param response: request response back from Sphero. - """ - data = self.pack_cmd(REQ['CMD_SET_DATA_STRM'], \ - [(sample_div>>8), (sample_div & 0xff), (sample_frames>>8), (sample_frames & 0xff), ((sample_mask1>>24) & 0xff), \ - ((sample_mask1>>16) & 0xff),((sample_mask1>>8) & 0xff), (sample_mask1 & 0xff), pcnt, ((sample_mask2>>24) & 0xff), \ - ((sample_mask2>>16) & 0xff),((sample_mask2>>8) & 0xff), (sample_mask2 & 0xff)]) - self.create_mask_list(sample_mask1, sample_mask2) - self.stream_mask1 = sample_mask1 - self.stream_mask2 = sample_mask2 - #print data - self.send(data, response) - - def set_filtered_data_strm(self, sample_div, sample_frames, pcnt, response): - """ - Helper function to add all the filtered data to the data strm - mask, so that the user doesn't have to set the data strm manually. - - :param sample_div: divisor of the maximum sensor sampling rate. - :param sample_frames: number of sample frames emitted per packet. - :param pcnt: packet count (set to 0 for unlimited streaming). - :param response: request response back from Sphero. - """ - mask1 = 0 - mask2 = 0 - for key,value in STRM_MASK1.iteritems(): - if 'FILTERED' in key: - mask1 = mask1|value - for value in STRM_MASK2.itervalues(): - mask2 = mask2|value - self.set_data_strm(sample_div, sample_frames, mask1, pcnt, mask2, response) - - def set_raw_data_strm(self, sample_div, sample_frames, pcnt, response): - """ - Helper function to add all the raw data to the data strm mask, so - that the user doesn't have to set the data strm manually. - - :param sample_div: divisor of the maximum sensor sampling rate. - :param sample_frames: number of sample frames emitted per packet. - :param pcnt: packet count (set to 0 for unlimited streaming). - :param response: request response back from Sphero. - """ - mask1 = 0 - mask2 = 0 - for key,value in STRM_MASK1.iteritems(): - if 'RAW' in key: - mask1 = mask1|value - for value in STRM_MASK2.itervalues(): - mask2 = mask2|value - self.set_data_strm(sample_div, sample_frames, mask1, pcnt, mask2, response) - - - def set_all_data_strm(self, sample_div, sample_frames, pcnt, response): - """ - Helper function to add all the data to the data strm mask, so - that the user doesn't have to set the data strm manually. - - :param sample_div: divisor of the maximum sensor sampling rate. - :param sample_frames: number of sample frames emitted per packet. - :param pcnt: packet count (set to 0 for unlimited streaming). - :param response: request response back from Sphero. - """ - mask1 = 0 - mask2 = 0 - for value in STRM_MASK1.itervalues(): - mask1 = mask1|value - for value in STRM_MASK2.itervalues(): - mask2 = mask2|value - self.set_data_strm(sample_div, sample_frames, mask1, pcnt, mask2, response) - - def config_collision_detect(self, method, Xt, Xspd, Yt, Yspd, ignore_time, response): - """ - This command either enables or disables asynchronous message - generation when a collision is detected.The Ignore Time parameter - disables collision detection for a period of time after the async - message is generated, preventing message overload to the - client. The value is in 10 millisecond increments. - - :param method: Detection method type to use. Currently the only\ - method supported is 01h. Use 00h to completely disable this\ - service. - :param Xt, Yt: An 8-bit settable threshold for the X (left/right)\ - and Y (front/back) axes of Sphero. A value of 00h disables the\ - contribution of that axis. - :param Xspd,Yspd: An 8-bit settable speed value for the X and Y\ - axes. This setting is ranged by the speed, then added to Xt, Yt to\ - generate the final threshold value. - :param ignore_time: An 8-bit post-collision dead time to prevent\ - retriggering; specified in 10ms increments. - """ - self.send(self.pack_cmd(REQ['CMD_CFG_COL_DET'],[method, Xt, Xspd, Yt, Yspd, ignore_time]), response) - - def set_rgb_led(self, red, green, blue, save, response): - """ - This allows you to set the RGB LED color. The composite value is - stored as the "application LED color" and immediately driven to - the LED (if not overridden by a macro or orbBasic operation). If - FLAG is true, the value is also saved as the "user LED color" - which persists across power cycles and is rendered in the gap - between an application connecting and sending this command. - - :param red: red color value. - :param green: green color value. - :param blue: blue color value. - :param save: 01h for save (color is saved as "user LED color"). - """ - self.send(self.pack_cmd(REQ['CMD_SET_RGB_LED'],[self.clamp(red,0,255), self.clamp(green,0,255), self.clamp(blue,0,255), save]), response) - - def set_back_led(self, brightness, response): - """ - This allows you to control the brightness of the back LED. The - value does not persist across power cycles. - - :param brightness: 0-255, off-on (the blue LED on hemisphere of the Sphero). - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_SET_BACK_LED'],[self.clamp(brightness,0,255)]), response) - - def get_rgb_led(self, response): - """ - This retrieves the "user LED color" which is stored in the config - block (which may or may not be actively driven to the RGB LED). - - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_GET_RGB_LED'],[]), response) - - def roll(self, speed, heading, state, response): - """ - This commands Sphero to roll along the provided vector. Both a - speed and a heading are required; the latter is considered - relative to the last calibrated direction. A state Boolean is also - provided (on or off). The client convention for heading follows the 360 - degrees on a circle, relative to the ball: 0 is straight ahead, 90 - is to the right, 180 is back and 270 is to the left. The valid - range is 0..359. - - :param speed: 0-255 value representing 0-max speed of the sphero. - :param heading: heading in degrees from 0 to 359. - :param state: 00h for off (braking) and 01h for on (driving). - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_ROLL'],[self.clamp(speed,0,255), (heading>>8), (heading & 0xff), state]), response) - - def boost(self, time, heading, response): - """ - This commands Sphero to meet the provided heading, disable - stabilization and ramp the motors up to full-speed for a period of - time. The Time parameter is the duration in tenths of a - second. Setting it to zero enables constant boost until a Set - Stabilization command is received. - - :param time: duration of boost in tenths of seconds. - :param heading: the heading to travel while boosting. - :param response: request response back from Sphero. - """ - self.send(self.pack_cmd(REQ['CMD_BOOST'], [time, (heading>>8), (heading & 0xff)]), response) - - def set_raw_motor_values(self, l_mode, l_power, r_mode, r_power, response): - """ - This allows you to take over one or both of the motor output - values, instead of having the stabilization system control - them. Each motor (left and right) requires a mode (see below) and - a power value from 0- 255. This command will disable stabilization - if both modes aren't "ignore" so you'll need to re-enable it via - CID 02h once you're done. - - :param mode: 0x00 - off, 0x01 - forward, 0x02 - reverse, 0x03 -\ - brake, 0x04 - ignored. - :param power: 0-255 scalar value (units?). - """ - self.send(self.pack_cmd(REQ['CMD_RAW_MOTORS'], [l_mode, l_power, r_mode, r_power]), response) - - def send(self, data, response): - """ - Packets are sent from Client -> Sphero in the following byte format:: - - ------------------------------------------------------- - | SOP1 | SOP2 | DID | CID | SEQ | DLEN | | CHK | - ------------------------------------------------------- - - * SOP1 - start packet 1 - Always 0xff. - * SOP2 - start packet 2 - Set to 0xff when an acknowledgement is\ - expected, 0xfe otherwise. - * DID - Device ID - * CID - Command ID - * SEQ - Sequence Number - This client field is echoed in the\ - response for all synchronous commands (and ignored by Sphero\ - when SOP2 = 0xfe) - * DLEN - Data - * Length - Number of bytes through the end of the packet. - * - * CHK - Checksum - The modulo 256 sum of all the bytes from the\ - DID through the end of the data payload, bit inverted (1's\ - complement). - """ - #compute the checksum - #modulo 256 sum of data bit inverted - checksum =~ sum(data) % 256 - #if expecting response back from the sphero - if response: - output = REQ['WITH_RESPONSE'] + data + [checksum] - else: - output = REQ['WITHOUT_RESPONSE'] + data + [checksum] - #pack the msg - msg = ''.join(struct.pack('B',x) for x in output) - #send the msg - with self._communication_lock: - self.bt.send(msg) - - def run(self): - # this is larger than any single packet - self.recv(1024) - - def recv(self, num_bytes): - ''' - Commands are acknowledged from the Sphero -> Client in the - following format:: - - ------------------------------------------------- - |SOP1 | SOP2 | MSRP | SEQ | DLEN | | CHK | - ------------------------------------------------- - - * SOP1 - Start Packet 1 - Always 0xff. - * SOP2 - Start Packet 2 - Set to 0xff when this is an\ - acknowledgement, 0xfe otherwise. - * MSRP - Message Response - This is generated by the message\ - decoder of the virtual device. - * SEQ - Sequence Number - Echoed to the client when this is a\ - direct message response (set to 00h when SOP2 = FEh) - * DLEN - Data Length - The number of bytes following through the\ - end of the packet - * - * CHK - Checksum - - The modulo 256 sum of all the bytes from the\ - DID through the end of the data payload, bit inverted (1's\ - complement) - - Asynchronous Packets are sent from Sphero -> Client in the - following byte format:: - - ------------------------------------------------------------- - |SOP1 | SOP2 | ID CODE | DLEN-MSB | DLEN-LSB | | CHK | - ------------------------------------------------------------- - - * SOP1 - Start Packet 1 - Always 0xff. - * SOP2 - Start Packet 2 - Set to 0xff when this is an - acknowledgement, 0xfe otherwise. - * ID CODE - ID Code - See the IDCODE dict - * DLEN-MSB - Data Length MSB - The MSB number of bytes following\ - through the end of the packet - * DLEN-LSB - Data Length LSB - The LSB number of bytes following\ - through the end of the packet - * - * CHK - Checksum - - The modulo 256 sum of all the bytes from the\ - DID through the end of the data payload, bit inverted (1's\ - complement) - - ''' - - while self.is_connected and not self.shutdown: - with self._communication_lock: - self.raw_data_buf += self.bt.recv(num_bytes) - data = self.raw_data_buf - while len(data)>5: - if data[:2] == RECV['SYNC']: - #print "got response packet" - # response packet - data_length = ord(data[4]) - if data_length+5 <= len(data): - data_packet = data[:(5+data_length)] - data = data[(5+data_length):] - else: - break - #print "Response packet", self.data2hexstr(data_packet) - - elif data[:2] == RECV['ASYNC']: - data_length = (ord(data[3])<<8)+ord(data[4]) - if data_length+5 <= len(data): - data_packet = data[:(5+data_length)] - data = data[(5+data_length):] - else: - # the remainder of the packet isn't long enough - break - if data_packet[2]==IDCODE['DATA_STRM'] and self._async_callback_dict.has_key(IDCODE['DATA_STRM']): - self._async_callback_dict[IDCODE['DATA_STRM']](self.parse_data_strm(data_packet, data_length)) - elif data_packet[2]==IDCODE['COLLISION'] and self._async_callback_dict.has_key(IDCODE['COLLISION']): - self._async_callback_dict[IDCODE['COLLISION']](self.parse_collision_detect(data_packet, data_length)) - elif data_packet[2]==IDCODE['PWR_NOTIFY'] and self._async_callback_dict.has_key(IDCODE['PWR_NOTIFY']): - self._async_callback_dict[IDCODE['PWR_NOTIFY']](self.parse_pwr_notify(data_packet, data_length)) - else: - print "got a packet that isn't streaming" - else: - raise RuntimeError("Bad SOF : " + self.data2hexstr(data)) - self.raw_data_buf=data - - def parse_pwr_notify(self, data, data_length): - ''' - The data payload of the async message is 1h bytes long and - formatted as follows:: - - -------- - |State | - -------- - - The power state byte: - * 01h = Battery Charging, - * 02h = Battery OK, - * 03h = Battery Low, - * 04h = Battery Critical - ''' - return struct.unpack_from('B', ''.join(data[5:]))[0] - - def parse_collision_detect(self, data, data_length): - ''' - The data payload of the async message is 10h bytes long and - formatted as follows:: - - ----------------------------------------------------------------- - |X | Y | Z | AXIS | xMagnitude | yMagnitude | Speed | Timestamp | - ----------------------------------------------------------------- - - * X, Y, Z - Impact components normalized as a signed 16-bit\ - value. Use these to determine the direction of collision event. If\ - you don't require this level of fidelity, the two Magnitude fields\ - encapsulate the same data in pre-processed format. - * Axis - This bitfield specifies which axes had their trigger\ - thresholds exceeded to generate the event. Bit 0 (01h) signifies\ - the X axis and bit 1 (02h) the Y axis. - * xMagnitude - This is the power that crossed the programming\ - threshold Xt + Xs. - * yMagnitude - This is the power that crossed the programming\ - threshold Yt + Ys. - * Speed - The speed of Sphero when the impact was detected. - * Timestamp - The millisecond timer value at the time of impact;\ - refer to the documentation of CID 50h and 51h to make sense of\ - this value. - ''' - output={} - - output['X'], output['Y'], output['Z'], output['Axis'], output['xMagnitude'], output['yMagnitude'], output['Speed'], output['Timestamp'] = struct.unpack_from('>hhhbhhbI', ''.join(data[5:])) - return output - - def parse_data_strm(self, data, data_length): - output={} - for i in range((data_length-1)/2): - unpack = struct.unpack_from('>h', ''.join(data[5+2*i:])) - output[self.mask_list[i]] = unpack[0] - #print self.mask_list - #print output - return output - - - - def disconnect(self): - self.is_connected = False - self.bt.close() - return self.is_connected - + def __init__(self, target_name = 'Sphero'): + threading.Thread.__init__(self) + self.target_name = target_name + self.bt = None + self.shutdown = False + self.is_connected = False + self.mask_list = None + self.stream_mask1 = None + self.stream_mask2 = None + self.seq = 0 + self.raw_data_buf = [] + self._communication_lock = threading.Lock() + self._async_callback_dict = dict() + self._sync_callback_dict = dict() + self._sync_callback_queue = [] + + def connect(self, bt_address): + self.bt = BTInterface(self.target_name) + self.is_connected = self.bt.connect(bt_address) + return True + + def inc_seq(self): + self.seq = self.seq + 1 + if self.seq > 0xff: + self.seq = 0 + + def pack_cmd(self, req ,cmd): + self.inc_seq() + # print req + [self.seq] + [len(cmd)+1] + cmd + return req + [self.seq] + [len(cmd)+1] + cmd + + def data2hexstr(self, data): + return ' '.join([ ("%02x"%ord(d)) for d in data]) + + def create_mask_list(self, mask1, mask2): + #save the mask + sorted_STRM1 = sorted(STRM_MASK1.iteritems(), key=operator.itemgetter(1), reverse=True) + #create a list containing the keys that are part of the mask + self.mask_list1 = [key for key, value in sorted_STRM1 if value & mask1] + + sorted_STRM2 = sorted(STRM_MASK2.iteritems(), key=operator.itemgetter(1), reverse=True) + #create a list containing the keys that are part of the mask + self.mask_list2 = [key for key, value in sorted_STRM2 if value & mask2] + self.mask_list = self.mask_list1 + self.mask_list2 + + + def add_async_callback(self, callback_type, callback): + self._async_callback_dict[callback_type] = callback + + def remove_async_callback(self, callback_type): + del self._async_callback_dict[callback_type] + + def add_sync_callback(self, callback_type, callback): + self._sync_callback_dict[callback_type] = callback + + def remove_sync_callback(self, callback_type): + del self._sync_callback_dict[callback_type] + + def clamp(self, n, minn, maxn): + return max(min(maxn, n), minn) + + def ping(self, response): + """ + The Ping command is used to verify both a solid data link with the + Client and that Sphero is awake and dispatching commands. + + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_PING'],[]), response) + + def get_version(self, response): + """ + The Get Versioning command returns a whole slew of software and + hardware information. + + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_VERSION'],[]), response) + + def set_device_name(self, name, response): + """ + This assigned name is held internally and produced as part of the + Get Bluetooth Info service below. Names are clipped at 48 + characters in length to support UTF-8 sequences; you can send + something longer but the extra will be discarded. This field + defaults to the Bluetooth advertising name. + + :param name: 48 character name. + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_SET_BT_NAME'],[name]), response) + + def get_bt_name(self, response): + """ + This returns the textual name (in ASCII) that the Bluetooth module + advertises. It also returns the BTA Bluetooth Address or MAC ID + for this device. Both values are returned in ASCII and have field + widths of 16 characters, with unused trailing characters set to + 00h. + + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_GET_BT_NAME'],[]), response) + + def set_auto_reconnect(self, enable, time, response): + """ + This configures the control of the Bluetooth module in its attempt + to automatically reconnect with the last iPhone device. This is a + courtesy behavior since the iPhone Bluetooth stack doesn't initiate + automatic reconnection on its own. The two parameters are simple: + flag = 00h to disable or 01h to enable, and time = the number of + seconds after power-up in which to enable auto reconnect mode. For + example, if time = 30 then the module will be attempt reconnecting 30 + seconds after waking up. + + :param enable: 00h to disable or 01h to enable auto reconnecting. + :param time: the number of seconds after power-up in which to\ + enable auto reconnect mode + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_SET_AUTO_RECONNECT'],[enable,time]), response) + + def get_auto_reconnect(self, response): + """ + This returns the Bluetooth auto reconnect values as defined in the + Set Auto Reconnect command. + + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_GET_AUTO_RECONNECT'],[]), reponse) + + def get_power_state(self, response): + """ + This returns the current power state and some additional + parameters to the Client. + + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_GET_PWR_STATE'],[]), response) + + def set_power_notify(self, enable, response): + """ + This enables Sphero to asynchronously notify the Client + periodically with the power state or immediately when the power + manager detects a state change. Timed notifications arrive every 10 + seconds until they're explicitly disabled or Sphero is unpaired. The + flag is as you would expect, 00h to disable and 01h to enable. + + :param enable: 00h to disable and 01h to enable power notifications. + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_SET_PWR_NOTIFY'],[enable]), response) + + def go_to_sleep(self, time, macro, response): + """ + This puts Sphero to sleep immediately with two parameters: the + first is the number of seconds after which it will automatically + re-awaken. If set to zero, this feature is disabled. If non-zero then + the MACRO parameter allows an optional system macro to be run upon + wakeup. If this is set to zero, no macro is executed. + + :param time: number of seconds wait before auto re-awake. + :param macro: macro number to run when re-awakened. + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_SLEEP'],[(time>>8), (time & 0xff), macro]), response) + + def run_l1_diags(self, response): + """ + This is a developer-level command to help diagnose aberrant + behavior. Most system counters, process flags, and system states + are decoded into human readable ASCII. There are two responses to + this command: a Simple Response followed by a large async message + containing the results of the diagnostic tests. + + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_RUN_L1_DIAGS'],[]), response) + + def run_l2_diags(self, response): + """ + This is a developers-only command to help diagnose aberrant + behavior. It is much less impactful than the Level 1 command as it + doesn't interfere with the current operation it simply returns a + current set of statistics to the client. + + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_RUN_L2_DIAGS'],[]), response) + + def clear_counters(self, response): + """ + This is a developers-only command to clear the various system + counters described in the level 2 diag. + + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_CLEAR_COUNTERS'],[]), response) + + def assign_counter_value(self, counter, response): + """ + Sphero contains a 32-bit counter that increments every millisecond + when it's not in the Idle state. It has no absolute meaning and is + reset for various reasons. This command assigns the counter a + specific value for subsequent sampling. + + :param counter: value to set the counter to. + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_ASSIGN_COUNTER'],[((counter>>24) & 0xff), ((counter>>16) & 0xff), ((counter>>8) & 0xff) ,(counter & 0xff)]), response) + + def poll_packet_times(self, time, response): + """ + This command helps the Client application profile the transmission + and processing latencies in Sphero so that a relative + synchronization of timebases can be performed. It allows the + Client to reconcile time stamped messages from Sphero to its own + time stamped events. + + The scheme is as follows: the Client sends the command with the + Client Tx time (:math:`T_{1}`) filled in. Upon receipt of the packet, the + command processor in Sphero copies that time into the response + packet and places the current value of the millisecond counter + into the Sphero Rx time field (:math:`T_{2}`). Just before the transmit + engine streams it into the Bluetooth module, the Sphero Tx time + value (:math:`T_{3}`) is filled in. If the Client then records the time at + which the response is received (:math:`T_{4}`) the relevant time segments can + be computed from the four time stamps (:math:`T_{1}, T_{2}, T_{3}, T_{4}`): + + * The value offset represents the maximum-likelihood time offset\ + of the Client clock to Sphero's system clock. + * offset :math:`= 1/2*[(T_{2} - T_{1}) + (T_{3} - T_{4})]` + + * The value delay represents the round-trip delay between the\ + Client and Sphero: + * delay :math:`= (T_{4} - T_{1}) - (T_{3} - T_{2})` + + :param time: client Tx time. + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_POLL_TIME'],[((time>>24) & 0xff), ((time>>16) & 0xff), ((time>>8) & 0xff), (time & 0xff)]), response) + + def set_heading(self, heading, response): + """ + This allows the client to adjust the orientation of Sphero by + commanding a new reference heading in degrees, which ranges from 0 + to 359. You will see the ball respond immediately to this command + if stabilization is enabled. + + :param heading: heading in degrees from 0 to 359 (motion will be\ + shortest angular distance to heading command) + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_SET_HEADING'],[(heading>>8),(heading & 0xff)]), response) + + def set_stablization(self, enable, response): + """ + This turns on or off the internal stabilization of Sphero, in + which the IMU is used to match the ball's orientation to its + various set points. The flag value is as you would expect, 00h for + off and 01h for on. + + :param enable: 00h for off and 01h for on (on by default). + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_SET_STABILIZ'],[enable]), response) + + def set_rotation_rate(self, rate, response): + """ + This allows you to control the rotation rate that Sphero will use + to meet new heading commands. The commanded value is in units of + 0.784 degrees/sec. So, setting a value of c8h will set the + rotation rate to 157 degrees/sec. A value of 255 jumps to the + maximum and a value of 1 is the minimum. + + :param rate: rotation rate in units of 0.784degrees/sec (setting\ + this value will not cause the device to move only set the rate it\ + will move in other funcation calls). + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_SET_ROTATION_RATE'],[self.clamp(rate, 0, 255)]), response) + + def set_app_config_blk(self, app_data, response): + """ + This allows you to write a 32 byte block of data from the + configuration block that is set aside for exclusive use by + applications. + + :param app_data: block set aside for application. + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_SET_APP_CONFIG_BLK'],[((app_data>>24) & 0xff), ((app_data>>16) & 0xff), ((app_data>>8) & 0xff), (app_data & 0xff)]), response) + + def get_app_config_blk(self, response): + """ + This allows you to retrieve the application configuration block\ + that is set aside for exclusive use by applications. + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_GET_APP_CONFIG_BLK'], []), response) + + def set_data_strm(self, sample_div, sample_frames, sample_mask1, pcnt, sample_mask2, response): + """ + Currently the control system runs at 400Hz and because it's pretty + unlikely you will want to see data at that rate, N allows you to + divide that down. sample_div = 2 yields data samples at 200Hz, + sample_div = 10, 40Hz, etc. Every data sample consists of a + "frame" made up of the individual sensor values as defined by the + sample_mask. The sample_frames value defines how many frames to + collect in memory before the packet is emitted. In this sense, it + controls the latency of the data you receive. Increasing + sample_div and the number of bits set in sample_mask drive the + required throughput. You should experiment with different values + of sample_div, sample_frames and sample_mask to see what works + best for you. + + :param sample_div: divisor of the maximum sensor sampling rate. + :param sample_frames: number of sample frames emitted per packet. + :param sample_mask1: bitwise selector of data sources to stream. + :param pcnt: packet count (set to 0 for unlimited streaming). + :param response: request response back from Sphero. + """ + data = self.pack_cmd(REQ['CMD_SET_DATA_STRM'], \ + [(sample_div>>8), (sample_div & 0xff), (sample_frames>>8), (sample_frames & 0xff), ((sample_mask1>>24) & 0xff), \ + ((sample_mask1>>16) & 0xff),((sample_mask1>>8) & 0xff), (sample_mask1 & 0xff), pcnt, ((sample_mask2>>24) & 0xff), \ + ((sample_mask2>>16) & 0xff),((sample_mask2>>8) & 0xff), (sample_mask2 & 0xff)]) + self.create_mask_list(sample_mask1, sample_mask2) + self.stream_mask1 = sample_mask1 + self.stream_mask2 = sample_mask2 + #print data + self.send(data, response) + + def disable_data_strm(self, response): + data = self.pack_cmd(REQ['CMD_SET_DATA_STRM'], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) + self.send(data, response) + + def set_filtered_data_strm(self, sample_div, sample_frames, pcnt, response): + """ + Helper function to add all the filtered data to the data strm + mask, so that the user doesn't have to set the data strm manually. + + :param sample_div: divisor of the maximum sensor sampling rate. + :param sample_frames: number of sample frames emitted per packet. + :param pcnt: packet count (set to 0 for unlimited streaming). + :param response: request response back from Sphero. + """ + mask1 = 0 + mask2 = 0 + for key,value in STRM_MASK1.iteritems(): + if 'FILTERED' in key: + mask1 = mask1|value + for value in STRM_MASK2.itervalues(): + mask2 = mask2|value + self.set_data_strm(sample_div, sample_frames, mask1, pcnt, mask2, response) + + def set_raw_data_strm(self, sample_div, sample_frames, pcnt, response): + """ + Helper function to add all the raw data to the data strm mask, so + that the user doesn't have to set the data strm manually. + + :param sample_div: divisor of the maximum sensor sampling rate. + :param sample_frames: number of sample frames emitted per packet. + :param pcnt: packet count (set to 0 for unlimited streaming). + :param response: request response back from Sphero. + """ + mask1 = 0 + mask2 = 0 + for key,value in STRM_MASK1.iteritems(): + if 'RAW' in key: + mask1 = mask1|value + for value in STRM_MASK2.itervalues(): + mask2 = mask2|value + self.set_data_strm(sample_div, sample_frames, mask1, pcnt, mask2, response) + + + def set_all_data_strm(self, sample_div, sample_frames, pcnt, response): + """ + Helper function to add all the data to the data strm mask, so + that the user doesn't have to set the data strm manually. + + :param sample_div: divisor of the maximum sensor sampling rate. + :param sample_frames: number of sample frames emitted per packet. + :param pcnt: packet count (set to 0 for unlimited streaming). + :param response: request response back from Sphero. + """ + mask1 = 0 + mask2 = 0 + for value in STRM_MASK1.itervalues(): + mask1 = mask1|value + for value in STRM_MASK2.itervalues(): + mask2 = mask2|value + self.set_data_strm(sample_div, sample_frames, mask1, pcnt, mask2, response) + + def config_locator(self, enable, x, y, yaw_tare, response): + + if enable: + flags = 0x1 + else: + flags = 0x0 + + data = self.pack_cmd(REQ['CMD_CONFIG_LOCATOR'], \ + [(flags & 0xff), (x>>8), (x & 0xff), (y>>8), (y & 0xff), (yaw_tare>>8), (yaw_tare & 0xff)]) + + self.send(data, response) + + def config_collision_detect(self, method, Xt, Xspd, Yt, Yspd, ignore_time, response): + """ + This command either enables or disables asynchronous message + generation when a collision is detected.The Ignore Time parameter + disables collision detection for a period of time after the async + message is generated, preventing message overload to the + client. The value is in 10 millisecond increments. + + :param method: Detection method type to use. Currently the only\ + method supported is 01h. Use 00h to completely disable this\ + service. + :param Xt, Yt: An 8-bit settable threshold for the X (left/right)\ + and Y (front/back) axes of Sphero. A value of 00h disables the\ + contribution of that axis. + :param Xspd,Yspd: An 8-bit settable speed value for the X and Y\ + axes. This setting is ranged by the speed, then added to Xt, Yt to\ + generate the final threshold value. + :param ignore_time: An 8-bit post-collision dead time to prevent\ + retriggering; specified in 10ms increments. + """ + self.send(self.pack_cmd(REQ['CMD_CFG_COL_DET'],[method, Xt, Xspd, Yt, Yspd, ignore_time]), response) + + def set_rgb_led(self, red, green, blue, save, response): + """ + This allows you to set the RGB LED color. The composite value is + stored as the "application LED color" and immediately driven to + the LED (if not overridden by a macro or orbBasic operation). If + FLAG is true, the value is also saved as the "user LED color" + which persists across power cycles and is rendered in the gap + between an application connecting and sending this command. + + :param red: red color value. + :param green: green color value. + :param blue: blue color value. + :param save: 01h for save (color is saved as "user LED color"). + """ + self.send(self.pack_cmd(REQ['CMD_SET_RGB_LED'],[self.clamp(red,0,255), self.clamp(green,0,255), self.clamp(blue,0,255), save]), response) + + def set_back_led(self, brightness, response): + """ + This allows you to control the brightness of the back LED. The + value does not persist across power cycles. + + :param brightness: 0-255, off-on (the blue LED on hemisphere of the Sphero). + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_SET_BACK_LED'],[self.clamp(brightness,0,255)]), response) + + def get_rgb_led(self, response): + """ + This retrieves the "user LED color" which is stored in the config + block (which may or may not be actively driven to the RGB LED). + + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_GET_RGB_LED'],[]), response) + + def roll(self, speed, heading, state, response): + """ + This commands Sphero to roll along the provided vector. Both a + speed and a heading are required; the latter is considered + relative to the last calibrated direction. A state Boolean is also + provided (on or off). The client convention for heading follows the 360 + degrees on a circle, relative to the ball: 0 is straight ahead, 90 + is to the right, 180 is back and 270 is to the left. The valid + range is 0..359. + + :param speed: 0-255 value representing 0-max speed of the sphero. + :param heading: heading in degrees from 0 to 359. + :param state: 00h for off (braking) and 01h for on (driving). + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_ROLL'],[self.clamp(speed,0,255), (heading>>8), (heading & 0xff), state]), response) + + def boost(self, time, heading, response): + """ + This commands Sphero to meet the provided heading, disable + stabilization and ramp the motors up to full-speed for a period of + time. The Time parameter is the duration in tenths of a + second. Setting it to zero enables constant boost until a Set + Stabilization command is received. + + :param time: duration of boost in tenths of seconds. + :param heading: the heading to travel while boosting. + :param response: request response back from Sphero. + """ + self.send(self.pack_cmd(REQ['CMD_BOOST'], [time, (heading>>8), (heading & 0xff)]), response) + + def set_raw_motor_values(self, l_mode, l_power, r_mode, r_power, response): + """ + This allows you to take over one or both of the motor output + values, instead of having the stabilization system control + them. Each motor (left and right) requires a mode (see below) and + a power value from 0- 255. This command will disable stabilization + if both modes aren't "ignore" so you'll need to re-enable it via + CID 02h once you're done. + + :param mode: 0x00 - off, 0x01 - forward, 0x02 - reverse, 0x03 -\ + brake, 0x04 - ignored. + :param power: 0-255 scalar value (units?). + """ + self.send(self.pack_cmd(REQ['CMD_RAW_MOTORS'], [l_mode, l_power, r_mode, r_power]), response) + + def send(self, data, response): + """ + Packets are sent from Client -> Sphero in the following byte format:: + + ------------------------------------------------------- + | SOP1 | SOP2 | DID | CID | SEQ | DLEN | | CHK | + ------------------------------------------------------- + + * SOP1 - start packet 1 - Always 0xff. + * SOP2 - start packet 2 - Set to 0xff when an acknowledgement is\ + expected, 0xfe otherwise. + * DID - Device ID + * CID - Command ID + * SEQ - Sequence Number - This client field is echoed in the\ + response for all synchronous commands (and ignored by Sphero\ + when SOP2 = 0xfe) + * DLEN - Data + * Length - Number of bytes through the end of the packet. + * + * CHK - Checksum - The modulo 256 sum of all the bytes from the\ + DID through the end of the data payload, bit inverted (1's\ + complement). + """ + #compute the checksum + #modulo 256 sum of data bit inverted + checksum =~ sum(data) % 256 + #if expecting response back from the sphero + if response: + output = REQ['WITH_RESPONSE'] + data + [checksum] + else: + output = REQ['WITHOUT_RESPONSE'] + data + [checksum] + #pack the msg + msg = ''.join(struct.pack('B',x) for x in output) + #send the msg + with self._communication_lock: + self.bt.send(msg) + + def run(self): + # this is larger than any single packet + self.recv(1024) + + def recv(self, num_bytes): + ''' + Commands are acknowledged from the Sphero -> Client in the + following format:: + + ------------------------------------------------- + |SOP1 | SOP2 | MSRP | SEQ | DLEN | | CHK | + ------------------------------------------------- + + * SOP1 - Start Packet 1 - Always 0xff. + * SOP2 - Start Packet 2 - Set to 0xff when this is an\ + acknowledgement, 0xfe otherwise. + * MSRP - Message Response - This is generated by the message\ + decoder of the virtual device. + * SEQ - Sequence Number - Echoed to the client when this is a\ + direct message response (set to 00h when SOP2 = FEh) + * DLEN - Data Length - The number of bytes following through the\ + end of the packet + * + * CHK - Checksum - - The modulo 256 sum of all the bytes from the\ + DID through the end of the data payload, bit inverted (1's\ + complement) + + Asynchronous Packets are sent from Sphero -> Client in the + following byte format:: + + ------------------------------------------------------------- + |SOP1 | SOP2 | ID CODE | DLEN-MSB | DLEN-LSB | | CHK | + ------------------------------------------------------------- + + * SOP1 - Start Packet 1 - Always 0xff. + * SOP2 - Start Packet 2 - Set to 0xff when this is an + acknowledgement, 0xfe otherwise. + * ID CODE - ID Code - See the IDCODE dict + * DLEN-MSB - Data Length MSB - The MSB number of bytes following\ + through the end of the packet + * DLEN-LSB - Data Length LSB - The LSB number of bytes following\ + through the end of the packet + * + * CHK - Checksum - - The modulo 256 sum of all the bytes from the\ + DID through the end of the data payload, bit inverted (1's\ + complement) + + ''' + + while self.is_connected and not self.shutdown: + with self._communication_lock: + self.raw_data_buf += self.bt.recv(num_bytes) + data = self.raw_data_buf + while len(data)>5: + if data[:2] == RECV['SYNC']: + #print "got response packet" + # response packet + data_length = ord(data[4]) + if data_length+5 <= len(data): + data_packet = data[:(5+data_length)] + data = data[(5+data_length):] + else: + break + + elif data[:2] == RECV['ASYNC']: + data_length = (ord(data[3])<<8)+ord(data[4]) + if data_length+5 <= len(data): + data_packet = data[:(5+data_length)] + data = data[(5+data_length):] + else: + # the remainder of the packet isn't long enough + break + if data_packet[2]==IDCODE['DATA_STRM'] and self._async_callback_dict.has_key(IDCODE['DATA_STRM']): + self._async_callback_dict[IDCODE['DATA_STRM']](self.parse_data_strm(data_packet, data_length)) + elif data_packet[2]==IDCODE['COLLISION'] and self._async_callback_dict.has_key(IDCODE['COLLISION']): + self._async_callback_dict[IDCODE['COLLISION']](self.parse_collision_detect(data_packet, data_length)) + elif data_packet[2]==IDCODE['PWR_NOTIFY'] and self._async_callback_dict.has_key(IDCODE['PWR_NOTIFY']): + self._async_callback_dict[IDCODE['PWR_NOTIFY']](self.parse_pwr_notify(data_packet, data_length)) + else: + print "got a packet that isn't streaming" + else: +# sys.stdout.write("Bad SOF : %s\n" %self.data2hexstr(data)) +# sys.stdout.flush() + raise RuntimeError("Bad SOF : " + self.data2hexstr(data)) + + self.raw_data_buf=data + + def parse_pwr_notify(self, data, data_length): + ''' + The data payload of the async message is 1h bytes long and + formatted as follows:: + + -------- + |State | + -------- + + The power state byte: + * 01h = Battery Charging, + * 02h = Battery OK, + * 03h = Battery Low, + * 04h = Battery Critical + ''' + return struct.unpack_from('B', ''.join(data[5:]))[0] + + def parse_collision_detect(self, data, data_length): + ''' + The data payload of the async message is 10h bytes long and + formatted as follows:: + + ----------------------------------------------------------------- + |X | Y | Z | AXIS | xMagnitude | yMagnitude | Speed | Timestamp | + ----------------------------------------------------------------- + + * X, Y, Z - Impact components normalized as a signed 16-bit\ + value. Use these to determine the direction of collision event. If\ + you don't require this level of fidelity, the two Magnitude fields\ + encapsulate the same data in pre-processed format. + * Axis - This bitfield specifies which axes had their trigger\ + thresholds exceeded to generate the event. Bit 0 (01h) signifies\ + the X axis and bit 1 (02h) the Y axis. + * xMagnitude - This is the power that crossed the programming\ + threshold Xt + Xs. + * yMagnitude - This is the power that crossed the programming\ + threshold Yt + Ys. + * Speed - The speed of Sphero when the impact was detected. + * Timestamp - The millisecond timer value at the time of impact;\ + refer to the documentation of CID 50h and 51h to make sense of\ + this value. + ''' + output={} + + output['X'], output['Y'], output['Z'], output['Axis'], output['xMagnitude'], output['yMagnitude'], output['Speed'], output['Timestamp'] = struct.unpack_from('>hhhbhhbI', ''.join(data[5:])) + return output + + def parse_data_strm(self, data, data_length): + output={} + for i in range((data_length-1)/2): + unpack = struct.unpack_from('>h', ''.join(data[5+2*i:])) + output[self.mask_list[i]] = unpack[0] + #print self.mask_list + #print output + return output + + def disconnect(self): + sys.stdout.write("disconnecting from robot...") + sys.stdout.flush() + self.is_connected = False + self.bt.close() + return self.is_connected diff --git a/sphero_node/CMakeLists.txt b/sphero_node/CMakeLists.txt index 7350774..19abdde 100644 --- a/sphero_node/CMakeLists.txt +++ b/sphero_node/CMakeLists.txt @@ -6,6 +6,15 @@ find_package(catkin REQUIRED message_generation dynamic_reconfigure std_msgs) add_message_files(FILES SpheroCollision.msg ) + +add_service_files( + DIRECTORY + srv + FILES + StabilizationSrv.srv + SetFloatSrv.srv + SetIntSrv.srv +) generate_messages(DEPENDENCIES std_msgs) generate_dynamic_reconfigure_options(cfg/Reconfig.cfg) diff --git a/sphero_node/nodes/sphero.py b/sphero_node/nodes/sphero.py index ad6243e..545c7c1 100755 --- a/sphero_node/nodes/sphero.py +++ b/sphero_node/nodes/sphero.py @@ -37,6 +37,7 @@ import sys import tf import PyKDL +import time from sphero_driver import sphero_driver import dynamic_reconfigure.server @@ -46,9 +47,13 @@ from geometry_msgs.msg import Point, Pose, Quaternion, Twist, TwistWithCovariance, Vector3 from sphero_node.msg import SpheroCollision from std_msgs.msg import ColorRGBA, Float32, Bool +from sphero_node.srv import StabilizationSrv, StabilizationSrvResponse, SetFloatSrv, SetFloatSrvResponse, SetIntSrv, SetIntSrvResponse from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue from sphero_node.cfg import ReconfigConfig +from numpy import roll +from roslib import msgs +MAX_SPEED = 2 # m/s class SpheroNode(object): battery_state = {1:"Battery Charging", @@ -88,25 +93,33 @@ def __init__(self, default_update_rate=50.0): self.last_cmd_vel_time = rospy.Time.now() self.last_diagnostics_time = rospy.Time.now() self.cmd_heading = 0 + self.last_cmd_heading = 0 self.cmd_speed = 0 self.power_state_msg = "No Battery Info" self.power_state = 0 def _init_pubsub(self): - self.odom_pub = rospy.Publisher('odom', Odometry) - self.imu_pub = rospy.Publisher('imu', Imu) - self.collision_pub = rospy.Publisher('collision', SpheroCollision) - self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray) + self.odom_pub = rospy.Publisher('odom', Odometry, queue_size = 1) + self.imu_pub = rospy.Publisher('imu', Imu, queue_size = 1) + self.collision_pub = rospy.Publisher('collision', SpheroCollision, queue_size = 1) + self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size = 1) self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Twist, self.cmd_vel, queue_size = 1) self.color_sub = rospy.Subscriber('set_color', ColorRGBA, self.set_color, queue_size = 1) - self.back_led_sub = rospy.Subscriber('set_back_led', Float32, self.set_back_led, queue_size = 1) - self.stabilization_sub = rospy.Subscriber('disable_stabilization', Bool, self.set_stabilization, queue_size = 1) - self.heading_sub = rospy.Subscriber('set_heading', Float32, self.set_heading, queue_size = 1) +# self.back_led_sub = rospy.Subscriber('set_back_led', Float32, self.set_back_led, queue_size = 1) + # self.stabilization_sub = rospy.Subscriber('disable_stabilization', Bool, self.set_stabilization, queue_size = 1) +# self.heading_sub = rospy.Subscriber('set_heading', Float32, self.set_heading, queue_size = 1) self.angular_velocity_sub = rospy.Subscriber('set_angular_velocity', Float32, self.set_angular_velocity, queue_size = 1) self.reconfigure_srv = dynamic_reconfigure.server.Server(ReconfigConfig, self.reconfigure) self.transform_broadcaster = tf.TransformBroadcaster() + self.heading_sub = rospy.Subscriber("heading", Twist, self.update_heading, queue_size = 1) + + self.stabilization_srv = rospy.Service('~set_stabilization', StabilizationSrv, self.set_stabilization) + self.heading_srv = rospy.Service('~set_heading', SetFloatSrv, self.set_heading) + self.back_led_srv = rospy.Service('~set_back_led', SetIntSrv, self.set_back_led) + def _init_params(self): + self.bt_address = rospy.get_param('~address', None) self.connect_color_red = rospy.get_param('~connect_red',0) self.connect_color_blue = rospy.get_param('~connect_blue',0) self.connect_color_green = rospy.get_param('~connect_green',255) @@ -117,12 +130,28 @@ def normalize_angle_positive(self, angle): return math.fmod(math.fmod(angle, 2.0*math.pi) + 2.0*math.pi, 2.0*math.pi); def start(self): - try: - self.is_connected = self.robot.connect() - rospy.loginfo("Connect to Sphero with address: %s" % self.robot.bt.target_address) - except: - rospy.logerr("Failed to connect to Sphero.") - sys.exit(1) + retries = 0 + + while not rospy.is_shutdown(): + try: + rospy.loginfo("Connecting to Sphero...") + self.is_connected = self.robot.connect(self.bt_address) + rospy.loginfo("Connected to %s" % self.robot.bt.target_address) + break; + except Exception as e: + if retries < 5: + retries += 1 + rospy.logwarn("Retry connection (%d)..." %retries) + else: + rospy.logerr("Failed to connect to Sphero. [%s]" %e) + sys.exit(1) + + # reset the x and y position values of the sphero's locater to (0,0) and + # set the yaw tare to 270 degrees so that its movement corresponds to + # fwd in +x axis and left in +y axis (ROS conform), otherwise odometry + # will be reported in a wrong coordinate system + self.robot.config_locator(True, 0, 0, 270, False) + #setup streaming self.robot.set_filtered_data_strm(self.sampling_divisor, 1 , 0, True) self.robot.add_async_callback(sphero_driver.IDCODE['DATA_STRM'], self.parse_data_strm) @@ -135,6 +164,7 @@ def start(self): #set the ball to connection color self.robot.set_rgb_led(self.connect_color_red,self.connect_color_green,self.connect_color_blue,0,False) #now start receiving packets + self.robot.start() def spin(self): @@ -145,6 +175,7 @@ def spin(self): if self.cmd_heading != 0 or self.cmd_speed != 0: self.cmd_heading = 0 self.cmd_speed = 0 + self.last_cmd_heading = 0 self.robot.roll(int(self.cmd_speed), int(self.cmd_heading), 1, False) if (now - self.last_diagnostics_time) > self.diag_update_rate: self.last_diagnostics_time = now @@ -152,6 +183,7 @@ def spin(self): r.sleep() def stop(self): + rospy.loginfo('stopping....') #tell the ball to stop moving before quiting self.robot.roll(int(0), int(0), 1, False) self.robot.shutdown = True @@ -199,61 +231,115 @@ def parse_power_notify(self, data): def parse_data_strm(self, data): if self.is_connected: now = rospy.Time.now() - imu = Imu(header=rospy.Header(frame_id="imu_link")) + imu = Imu(header=rospy.Header(frame_id="base_link")) imu.header.stamp = now - imu.orientation.x = data["QUATERNION_Q0"] - imu.orientation.y = data["QUATERNION_Q1"] - imu.orientation.z = data["QUATERNION_Q2"] - imu.orientation.w = data["QUATERNION_Q3"] - imu.linear_acceleration.x = data["ACCEL_X_FILTERED"]/4096.0*9.8 - imu.linear_acceleration.y = data["ACCEL_Y_FILTERED"]/4096.0*9.8 + + ### SPHERO: + ### pitch: -90 when front of sphero is pointing straight down, +90 when straight up + ### roll: -90 when sphero lying on the left, +90 when lying on the right + ### yaw: + when rotating left (ccw), - when turning right (cw) (seen from top) + ### ROS: follows the right hand rule, rotating ccw increases the value + ### pitch: +90 when down, +90 when up + ### roll: -90 on the left, +90 on the right + ### yaw: + when rotating left (ccw), - when turning right (cw) (seen from top) + ### CONCLUSION: x axis (front/back) is inverted, so we need to invert pitch values + roll = data["IMU_ROLL_FILTERED"]*math.pi/180 + pitch = -data["IMU_PITCH_FILTERED"]*math.pi/180 + yaw = data["IMU_YAW_FILTERED"]*math.pi/180 + quat = PyKDL.Rotation.RPY(roll, pitch, yaw).GetQuaternion() + imu.orientation = Quaternion(*quat) + + ### SPHERO: X is the pitch axis, Y is roll and Z is yaw + ### ROS: X is the roll axis, Y is pitch and Z is yaw + ### CONCLUSION: we need to switch x and y values, also because of the + ### conclusion above (inverted x axis) we need to invert the values + ### corresponding to the x axis + imu.linear_acceleration.x = -data["ACCEL_Y_FILTERED"]/4096.0*9.8 + imu.linear_acceleration.y = data["ACCEL_X_FILTERED"]/4096.0*9.8 imu.linear_acceleration.z = data["ACCEL_Z_FILTERED"]/4096.0*9.8 - imu.angular_velocity.x = data["GYRO_X_FILTERED"]*10*math.pi/180 - imu.angular_velocity.y = data["GYRO_Y_FILTERED"]*10*math.pi/180 + imu.angular_velocity.x = -data["GYRO_Y_FILTERED"]*10*math.pi/180 + imu.angular_velocity.y = data["GYRO_X_FILTERED"]*10*math.pi/180 imu.angular_velocity.z = data["GYRO_Z_FILTERED"]*10*math.pi/180 self.imu = imu self.imu_pub.publish(self.imu) - + odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_footprint') odom.header.stamp = now - odom.pose.pose = Pose(Point(data["ODOM_X"]/100.0,data["ODOM_Y"]/100.0,0.0), Quaternion(0.0,0.0,0.0,1.0)) - odom.twist.twist = Twist(Vector3(data["VELOCITY_X"]/1000.0, 0, 0), Vector3(0, 0, data["GYRO_Z_FILTERED"]*10.0*math.pi/180.0)) + # for odometry we are only interested in rotation about the z axis (yaw) + odom_quat = PyKDL.Rotation.RotZ(yaw).GetQuaternion(); + odom.pose.pose = Pose(Point(data["ODOM_X"]/100.0,data["ODOM_Y"]/100.0,0.0), Quaternion(*odom_quat)) + odom.twist.twist = Twist(Vector3(data["VELOCITY_X"]/1000.0, data["VELOCITY_Y"]/1000.0, 0), Vector3(0, 0, data["GYRO_Z_FILTERED"]*10.0*math.pi/180.0)) odom.pose.covariance =self.ODOM_POSE_COVARIANCE odom.twist.covariance =self.ODOM_TWIST_COVARIANCE self.odom_pub.publish(odom) - #need to publish this trasform to show the roll, pitch, and yaw properly + # need to publish this transform to show the roll and pitch properly + # we don't use the yaw here, because the base_footprint frame is already + # updated with the yaw value, so we only update the base_link with the + # roll and pitch values and set the yaw value to 0, so the base_link will + # always have the same z orientation as the base_footprint + quat_transform = PyKDL.Rotation.RPY(roll, pitch, 0.0).GetQuaternion() self.transform_broadcaster.sendTransform((0.0, 0.0, 0.038 ), - (data["QUATERNION_Q0"], data["QUATERNION_Q1"], data["QUATERNION_Q2"], data["QUATERNION_Q3"]), + (quat_transform[0], quat_transform[1], quat_transform[2], quat_transform[3]), odom.header.stamp, "base_link", "base_footprint") def cmd_vel(self, msg): if self.is_connected: self.last_cmd_vel_time = rospy.Time.now() - self.cmd_heading = self.normalize_angle_positive(math.atan2(msg.linear.x,msg.linear.y))*180/math.pi - self.cmd_speed = math.sqrt(math.pow(msg.linear.x,2)+math.pow(msg.linear.y,2)) - self.robot.roll(int(self.cmd_speed), int(self.cmd_heading), 1, False) + if ((msg.linear.x == 0) and (msg.linear.y == 0)): +# rospy.loginfo("roll(%d, %d)" %(0, int(self.last_cmd_heading))) + self.robot.roll(0, int(self.last_cmd_heading), 1, False) + else: + self.cmd_heading = self.normalize_angle_positive(math.atan2(msg.linear.y, msg.linear.x))*180/math.pi + # print msg + t1 = math.atan2(msg.linear.x,msg.linear.y) + t2 = self.normalize_angle_positive(math.atan2(msg.linear.x,msg.linear.y)) +# rospy.loginfo("math.atan2(msg.linear.x,msg.linear.y): %f, self.normalize_angle_positive(math.atan2(msg.linear.x,msg.linear.y)): %f" %(t1, t2)) + self.cmd_speed = math.sqrt(math.pow(msg.linear.x,2)+math.pow(msg.linear.y,2)) / MAX_SPEED * 255 + +# rospy.loginfo("roll(%d, %d)" %(self.cmd_speed, self.cmd_heading)) + self.robot.roll(int(self.cmd_speed), int(self.cmd_heading), 1, False) + self.last_cmd_heading = self.cmd_heading + + def update_heading(self, msg): + if self.is_connected: + self.cmd_heading = self.normalize_angle_positive(math.atan2(msg.linear.y, msg.linear.x))*180/math.pi + self.robot.roll(0, int(self.cmd_heading), 1, False) + def set_color(self, msg): if self.is_connected: self.robot.set_rgb_led(int(msg.r*255),int(msg.g*255),int(msg.b*255),0,False) - def set_back_led(self, msg): + def set_back_led(self, req): if self.is_connected: - self.robot.set_back(msg.data, False) + rospy.loginfo("set back led: %d" %req.value) + self.robot.set_back_led(req.value, False) + return SetIntSrvResponse(True) + else: + return SetIntSrvResponse(False) - def set_stabilization(self, msg): + def set_stabilization(self, req): if self.is_connected: - if not msg.data: + rospy.loginfo("set stabilization: %s" %req.value) + if req.value: self.robot.set_stablization(1, False) else: self.robot.set_stablization(0, False) + return StabilizationSrvResponse(True) + else: + return StabilizationSrvResponse(False) - def set_heading(self, msg): + def set_heading(self, req): if self.is_connected: - heading_deg = int(self.normalize_angle_positive(msg.data)*180.0/math.pi) + heading_deg = int(self.normalize_angle_positive(req.value)*180.0/math.pi) + rospy.loginfo("set heading raw: %f" %req.value) + rospy.loginfo("set heading: %d" %heading_deg) self.robot.set_heading(heading_deg, False) + return SetFloatSrvResponse(True) + else: + return SetFloatSrvResponse(False) def set_angular_velocity(self, msg): if self.is_connected: @@ -274,4 +360,3 @@ def reconfigure(self, config, level): s.start() s.spin() s.stop() - diff --git a/sphero_node/srv/SetFloatSrv.srv b/sphero_node/srv/SetFloatSrv.srv new file mode 100644 index 0000000..e94ba3e --- /dev/null +++ b/sphero_node/srv/SetFloatSrv.srv @@ -0,0 +1,3 @@ +float32 value +--- +bool success \ No newline at end of file diff --git a/sphero_node/srv/SetIntSrv.srv b/sphero_node/srv/SetIntSrv.srv new file mode 100644 index 0000000..230ad84 --- /dev/null +++ b/sphero_node/srv/SetIntSrv.srv @@ -0,0 +1,3 @@ +uint32 value +--- +bool success \ No newline at end of file diff --git a/sphero_node/srv/StabilizationSrv.srv b/sphero_node/srv/StabilizationSrv.srv new file mode 100644 index 0000000..44f34b1 --- /dev/null +++ b/sphero_node/srv/StabilizationSrv.srv @@ -0,0 +1,3 @@ +bool value +--- +bool success \ No newline at end of file diff --git a/sphero_teleop/CMakeLists.txt b/sphero_teleop/CMakeLists.txt new file mode 100644 index 0000000..9ca9276 --- /dev/null +++ b/sphero_teleop/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.8.3) +project(sphero_teleop) + +find_package(catkin REQUIRED geometry_msgs roscpp) + +catkin_package(DEPENDS geometry_msgs roscpp) + +include_directories(${catkin_INCLUDE_DIRS}) + +add_executable(sphero_teleop_joy + src/sphero_joy.cpp +) +target_link_libraries(sphero_teleop_joy + ${catkin_LIBRARIES} +) + +install(TARGETS sphero_teleop_joy + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/sphero_teleop/launch/ps3_teleop.launch b/sphero_teleop/launch/ps3_teleop.launch new file mode 100644 index 0000000..56e8765 --- /dev/null +++ b/sphero_teleop/launch/ps3_teleop.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/sphero_teleop/package.xml b/sphero_teleop/package.xml new file mode 100644 index 0000000..6647ad5 --- /dev/null +++ b/sphero_teleop/package.xml @@ -0,0 +1,21 @@ + + sphero_teleop + 0.1.2 + + Provides teleoperation using joystick. Adapted from on turtlebot_teleop + + Dominik Egger + Dominik Egger + BSD + http://ros.org/wiki/sphero_teleop + + catkin + + geometry_msgs + roscpp + + joy + geometry_msgs + roscpp + + diff --git a/sphero_teleop/src/sphero_joy.cpp b/sphero_teleop/src/sphero_joy.cpp new file mode 100644 index 0000000..996eef0 --- /dev/null +++ b/sphero_teleop/src/sphero_joy.cpp @@ -0,0 +1,129 @@ +/* + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include "boost/thread/mutex.hpp" +#include "boost/thread/thread.hpp" +#include "ros/console.h" + +class SpheroTeleop +{ +public: + SpheroTeleop(); + +private: + void joyCallback(const sensor_msgs::Joy::ConstPtr& joy); + void publish(); + void updateMotors(); + + ros::NodeHandle ph_, nh_; + + int x_axis_, y_axis_, deadman_axis_, heading_axis_; + bool x_inverted_, y_inverted_; + + double l_scale_, a_scale_; + ros::Publisher vel_pub_, heading_pub_; + ros::Subscriber joy_sub_; + + geometry_msgs::Twist last_published_; + boost::mutex publish_mutex_; + bool deadman_pressed_, heading_pressed_; + ros::Timer timer_; + +}; + +SpheroTeleop::SpheroTeleop(): + ph_("~"), + x_axis_(1), + y_axis_(0), + x_inverted_(false), + y_inverted_(false), + deadman_axis_(4), + heading_axis_(5), + l_scale_(0.3), + a_scale_(0.9) +{ + ph_.param("x_axis", x_axis_, x_axis_); + ph_.param("x_inverted", x_inverted_, x_inverted_); + ph_.param("y_axis", y_axis_, y_axis_); + ph_.param("y_inverted", y_inverted_, y_inverted_); + ph_.param("axis_deadman", deadman_axis_, deadman_axis_); + ph_.param("axis_heading", heading_axis_, heading_axis_); + ph_.param("scale_angular", a_scale_, a_scale_); + ph_.param("scale_linear", l_scale_, l_scale_); + + vel_pub_ = ph_.advertise("cmd_vel", 1); + heading_pub_ = ph_.advertise("heading", 1); + joy_sub_ = nh_.subscribe("joy", 10, &SpheroTeleop::joyCallback, this); + + timer_ = nh_.createTimer(ros::Duration(0.1), boost::bind(&SpheroTeleop::publish, this)); +} + +void SpheroTeleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy) +{ + geometry_msgs::Twist vel; + if (!y_inverted_) { + vel.linear.y = l_scale_*joy->axes[y_axis_]; + } else { + vel.linear.y = -l_scale_*joy->axes[y_axis_]; + } + if (!x_inverted_) { + vel.linear.x = l_scale_*joy->axes[x_axis_]; + } else { + vel.linear.x = -l_scale_*joy->axes[x_axis_]; + } + last_published_ = vel; + deadman_pressed_ = joy->buttons[deadman_axis_]; + heading_pressed_ = joy->buttons[heading_axis_]; +} + +void SpheroTeleop::publish() +{ + boost::mutex::scoped_lock lock(publish_mutex_); + + if (deadman_pressed_) + { + vel_pub_.publish(last_published_); + } + else if (heading_pressed_) + { + heading_pub_.publish(last_published_); + } + +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "Sphero_teleop"); + SpheroTeleop Sphero_teleop; + + ros::spin(); +}