def panTilt(panValue, tiltValue): bthere_log.i('panTilt input. Pan: ' + str(panValue) + ' Tilt: ' + str(tiltValue)) global currentPan global currentTilt newPan = currentPan + panValue newTilt = currentTilt + tiltValue pan(newPan) tilt(newTilt)
def get_config_or_default(key, default): if key in args: bthere_log.i("Found desired config value for " + key + " so returning " + str(args[key])) return args[key] else: bthere_log.i("Didn't find desired config value for " + key + " so returning default: " + str(default)) return default
def callback(twist): rospy.loginfo(rospy.get_caller_id() + " Received: " + str(twist)) pan = int(twist.angular.z * increment_scalar) tilt = int(twist.angular.y * increment_scalar) if (pan == 0 and tilt == 0): return bthere_log.i('Pan incrementing by ' + str(pan) + ' Tilt incrementing by ' + str(tilt)) maestro_servos.panTilt(pan, tilt)
def tilt(amount): amount = clamp(amount, 0, 100) global currentTilt if amount == currentTilt: return currentTilt = amount data = get_servo_data_value(amount) control_servo(tilt_servo_channel, data) bthere_log.i("Tilting to " + str(amount))
def pan(amount): amount = clamp(amount, 0, 100) global currentPan if amount == currentPan: return currentPan = amount data = get_servo_data_value(amount) control_servo(pan_servo_channel, data) bthere_log.i("Panning to " + str(amount))
def setup(): global ser global is_setup if not is_setup: # spin looking to make sure the ttyUSB exists if config.args['mock_servos']: bthere_log.i( "Using mock serial, so not gonna check if the serial port device exists" ) else: if os.path.exists(command_serial_port) is False: bthere_log.i("Serial port not available yet. Waiting " + str(time_to_wait_for_usb) + " milliseconds in hopes that it'll show up") time.sleep(time_to_wait_for_usb / 1000.0) if os.path.exists(command_serial_port) is False: bthere_log.e( "Serial port still isn't available. Giving up") raise Exception("Serial port not available: " + command_serial_port) ser = serial.Serial(port=command_serial_port, baudrate=9600, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS, timeout=0, write_timeout=0) is_setup = True bthere_log.i("connected to: " + ser.portstr)
def cleanup(): global ser if ser is not None: ser.close() ser = None bthere_log.i("serial cleaned up")
def pack_command_to_channel(channel, command, data): packed = struct.pack("BBBB", command, channel, data & 0x7F, (data >> 7) & 0x7F) if verbose: bthere_log.i("Packed data: " + binascii.hexlify(packed)) return packed
def pack(number): packed = struct.pack("B", number) bthere_log.i(binascii.hexlify(packed)) if verbose: bthere_log.i("Packed data: " + binascii.hexlify(packed)) return packed
def pan(amount): bthere_log.i("servo mock panning to " + str(amount))
json_values = json.load(f) f.close() os.remove(tmp_filename) return json_values for arg in raw_args: arg_parts = arg.split('=') if len(arg_parts) > 1: if arg_parts[1] == 'True': args[arg_parts[0]] = True elif arg_parts[1] == 'False': args[arg_parts[0]] = False else: args[arg_parts[0]] = arg_parts[1] bthere_log.i("From command line setting " + arg_parts[0] + " to " + arg_parts[1]) else: args[arg_parts[0]] = True bthere_log.i("From command line setting " + arg_parts[0] + " to True") bthere_log.i("Command line args: " + str(args)) config_file = get_config_or_default('config_file', DEFAULT_CONFIG_FILE) if config_file is not None: bthere_log.i("Looking for config file: " + config_file) try: config_values = json_stripped(config_file) bthere_log.i("Config file values: " + str(config_values)) args.update(config_values) except IOError: bthere_log.w("Could not find configuration file: " + config_file)
# This file assumes communication is in USB dual mode. # There's also an assumption that there are two servos attached to the controller, on channels 1 and 2. import config import bthere_log import os.path import binascii import math import string import struct import time print("maestro_servos") if config.args['mock_servos']: bthere_log.i("Using mock serial") import serial_mock as serial else: bthere_log.i("Using real serial") import serial as serial # mac ports are not valid - just guesses right now command_port_mac = "/dev/cu.usbmodem00654321" ttl_serial_port_mac = "/dev/cu.usbmodem00654322" command_port_linux = "/dev/ttyACM0" ttl_serial_port_linux = "/dev/ttyACM0" time_to_wait_for_usb = 1000 if config.args['platform'] == "linux": command_serial_port = config.get_config_or_default(
def flushOutput(self): bthere_log.i("Serial mock flushOutput called")
def write(self, data): bthere_log.i("Serial mock write called")
def close(self): bthere_log.i("Serial mock close called")
def __init__(self, port, baudrate, parity, stopbits, bytesize, timeout, write_timeout): self.portstr = port bthere_log.i("mock Serial.__init__")
# This file defines a couple base constants import rospy import bthere_log import xml.etree.ElementTree as ET import os # General system constants dir_path = os.path.dirname(os.path.realpath(__file__)) DEFAULT_CONFIG_FILE = dir_path + '/bthere.cfg' VERSION = 'unknown' NAME = 'maestro_servo_controller' try: bthere_log.i("Looking for " + dir_path + "/../package.xml") tree = ET.parse(dir_path + "/../package.xml") bthere_log.i("Parsed package.xml") root = tree.getroot() version_node = root.find("version") if version_node is not None: VERSION = version_node.text bthere_log.i("Found version in package.xml") name_node = root.find("name") if name_node is not None: NAME = name_node.text bthere_log.i("Found name in package.xml") except: bthere_log.w("Can't read package.xml") bthere_log.i("VERSION: " + VERSION) bthere_log.i("NAME: " + NAME)
def main(): bthere_log.i("bthere_servos running...") topic = rospy.get_param("~topic_name", "/camera_servo1/teleop") maestro_servos.setup() listener(topic)
def setup(): bthere_log.i("mock servos setup")
def tilt(amount): bthere_log.i("servo mock tilting to " + str(amount))
def cleanup(): bthere_log.i("servo mock cleaned up")