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")