def __init__(self, name="sphero", color=None, addr=None):
self._name = name
self._addr = addr
self._color = color
self._orb = Sphero(addr)
self._connected = False
if (self._color is None):
self._color = ColorRGBA(0, 0, 128, 0)
elif (self._color == "blue"):
self._color = ColorRGBA(0, 0, 128, 0)
elif (self._color == "red"):
self._color = ColorRGBA(128, 0, 0, 0)
def __init__(self, default_update_rate=50.0):
self.update_rate = default_update_rate
self.sampling_divisor = int(400 / self.update_rate)
self.is_connected = False
self._node_name = rospy.get_name()
self._namespace = rospy.get_namespace()
self._address = rospy.get_param("~adresa")
self.robot = Sphero(self._address)
self._init_pubsub()
self._init_params()
self.cmd_heading = 0
self.cmd_speed = 0
self.last_cmd_vel_time = rospy.Time.now()
self.last_diagnostics_time = rospy.Time.now()
self.power_state_msg = "No Battery Info"
self.power_state = 0
from sphero_sprk import Sphero
from communitysdk import list_connected_devices, MotionSensorKit
devices = list_connected_devices()
msk_filter = filter(lambda device: isinstance(device, MotionSensorKit), devices)
msk = next(msk_filter, None) # Get first Motion Sensor Kit
orb = Sphero()
orb.connect()
orb.set_rgb_led(0, 0, 0)
if msk != None:
previous_value = 0
def on_gesture(gestureValue):
print('Gesture detected:', gestureValue)
if gestureValue == "right":
orb.set_rgb_led(255, 102, 26)
orb.roll(20, 90)
elif gestureValue == "left":
orb.set_rgb_led(0, 255, 0)
orb.roll(20, 270)
elif gestureValue == "down":
orb.set_rgb_led(102, 140, 255)
orb.roll(20, 180)
elif gestureValue == "up":
orb.roll(20, 0)
orb.set_rgb_led(255, 255, 0)
def get_goal(event, x, y, flags, param):
global goal, goal_set
if event == cv2.EVENT_LBUTTONDOWN:
buff = map_to_xy(x, y)
goal = [int(buff[0]), int(buff[1])]
goal_set = True
print("mouse click event")
cv2.namedWindow("Frame")
cv2.setMouseCallback("Frame", get_goal)
goal_set = False
#orb = Sphero("C9:93:8F:F1:91:E2")
orb = Sphero("DA:06:00:55:3A:8D")
orb.connect()
print("Connection 1 Established")
print(orb.ping())
time.sleep(2)
while True:
(grabbed, frame) = camera.read()
frame = imutils.resize(frame, width=1200)
height = np.size(frame, 0)
width = np.size(frame, 1)
frame = frame[int(1.3 * height / 10):int(9.1 * height / 10),
int(1.75 * width / 10):int(7.6 * width / 10)]
blurred = cv2.GaussianBlur(frame, (15, 15), 0)
class SpheroNode(object):
def __init__(self, default_update_rate=50.0):
self.update_rate = default_update_rate
self.sampling_divisor = int(400 / self.update_rate)
self.is_connected = False
self._node_name = rospy.get_name()
self._namespace = rospy.get_namespace()
self._address = rospy.get_param("~adresa")
self.robot = Sphero(self._address)
self._init_pubsub()
self._init_params()
self.cmd_heading = 0
self.cmd_speed = 0
self.last_cmd_vel_time = rospy.Time.now()
self.last_diagnostics_time = rospy.Time.now()
self.power_state_msg = "No Battery Info"
self.power_state = 0
def _init_pubsub(self):
self.heading_sub = rospy.Subscriber('set_heading_',
Float32,
self.set_heading,
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.stabilization_sub = rospy.Subscriber('disable_stabilization_',
Bool,
self.set_stabilization,
queue_size=1)
self.angular_velocity_sub = rospy.Subscriber('set_angular_velocity_',
Float32,
self.set_angular_velocity,
queue_size=1)
def _init_params(self):
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)
self.cmd_vel_timeout = rospy.Duration(
rospy.get_param('~cmd_vel_timeout', 0.6))
# program
def start(self):
try:
self.is_connected = self.robot.connect()
rospy.loginfo("Connected to Sphero %s" % self._namespace +
"\n with address: %s" % self._address)
except:
rospy.logerr("Failed to connect to Sphero.")
sys.exit(1)
self.robot.set_rgb_led(self.connect_color_red,
self.connect_color_green,
self.connect_color_blue, 0, False)
self.robot.start()
def spin(self):
r = rospy.Rate(10.0)
while not rospy.is_shutdown():
now = rospy.Time.now()
if (now - self.last_cmd_vel_time) > self.cmd_vel_timeout:
if self.cmd_heading != 0 or self.cmd_speed != 0:
self.cmd_heading = 0
self.cmd_speed = 0
self.robot.roll(int(self.cmd_speed), int(self.cmd_heading),
1, False)
r.sleep()
def stop(self):
# tell the ball to stop moving before quiting
self.robot.roll(int(0), int(0), 1, False)
self.robot.shutdown = True
rospy.sleep(1.0)
self.is_connected = self.robot.disconnect()
self.robot.join()
# commands
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
#print(self.cmd_heading)
self.cmd_speed = math.sqrt(
math.pow(msg.linear.x, 2) + math.pow(msg.linear.y, 2))
#print(self.cmd_speed)
self.robot.roll(int(self.cmd_speed), 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_stabilization(self, msg):
if self.is_connected:
if not msg.data:
self.robot.set_stabilization(1, False)
print('Stabilization on')
else:
self.robot.set_stabilization(0, False)
print('Stabilization off')
def set_heading(self, msg):
if self.is_connected:
heading_deg = int(
self.normalize_angle_positive(msg.data) * 180.0 / math.pi)
print heading_deg
self.robot.set_heading(heading_deg, False)
def set_angular_velocity(self, msg):
if self.is_connected:
rate = int((msg.data * 180 / math.pi) / 0.784)
self.robot.set_rotation_rate(rate, False)
def normalize_angle_positive(self, angle):
return math.fmod(
math.fmod(angle, 2.0 * math.pi) + 2.0 * math.pi, 2.0 * math.pi)
class SpheroNode(object):
VERSION = "1.0"
def __init__(self, name="sphero", color=None, addr=None):
self._name = name
self._addr = addr
self._color = color
self._orb = Sphero(addr)
self._connected = False
if (self._color is None):
self._color = ColorRGBA(0, 0, 128, 0)
elif (self._color == "blue"):
self._color = ColorRGBA(0, 0, 128, 0)
elif (self._color == "red"):
self._color = ColorRGBA(128, 0, 0, 0)
def connect(self):
self._orb.connect()
orb = self._orb.get_device_name()
print("Connected to: " + orb['name'])
rospy.loginfo("Connected to: " + orb['name'])
version = self._orb.version()
print("Model: " + str(version['MDL']) + " HW Version: " +
str(version['HW']))
print("App Version: " + str(version['MSA-ver']) + "." +
str(version['MSA-rev']))
print("Firmware Version: " + str(version['BL']))
rospy.loginfo("Model: " + str(version['MDL']) + " HW Version: " +
str(version['HW']))
rospy.loginfo("App Version: " + str(version['MSA-ver']) + "." +
str(version['MSA-rev']))
rospy.loginfo("Firmware Version: " + str(version['BL']))
self._connected = True
def cmd_vel(self, twist):
if (not self._connected):
return
self._orb.roll(int(twist.linear.x), int(twist.angular.z))
def set_tail(self, brightness):
if (not self._connected):
return
self._orb.set_tail_light(brightness.data)
def set_stab(self, flag):
if (not self._connected):
return
goal_state = flag.data
print("Setting it to: " + str(goal_state))
if (goal_state):
print("Turning ON Stabilization")
else:
print("Turning OFF Stabilization")
self._orb.set_stabilization(goal_state)
def set_color(self, color):
if (not self._connected):
return
self._orb.set_rgb_led(int(color.r), int(color.g), int(color.b))
def reset_heading(self, heading):
if (not self._connected):
print(self._name + " is not connected)")
return
print("Reset Heading")
self._orb.set_heading(0)
def set_position(self, pt):
'''
Set Position of Sphero, in mm (automatically convert to cm here)
:param pt:
:return:
'''
if (not self._connected):
print(self._name + " is not connected)")
return
print("Setting Position to: \n" + str(pt))
x = int(pt.x / 10)
y = int(pt.y / 10)
self._orb.config_locator(x, y, 0)
def set_heading(self, heading):
if (not self._connected):
print(self._name + " is not connected)")
return
print("Set Heading to: " + str(heading.data))
self._orb.set_stabilization(False)
self._orb.set_tail_light(255)
rospy.sleep(2)
self._orb.roll(1, heading.data)
rospy.sleep(2)
self._orb.set_heading(0)
rospy.sleep(2)
self._orb.set_tail_light(0)
self._orb.set_stabilization(True)
def init_ros(self):
rospy.init_node(self._name, anonymous=True)
topic_root = '/' + self._name
# Subscriptions
sub_cmd_vel = rospy.Subscriber(topic_root + '/cmd_vel',
Twist,
self.cmd_vel,
queue_size=1)
sub_color = rospy.Subscriber(topic_root + '/set_color',
ColorRGBA,
self.set_color,
queue_size=1)
sub_tail = rospy.Subscriber(topic_root + '/set_tail',
Int16,
self.set_tail,
queue_size=1)
sub_stab = rospy.Subscriber(topic_root + '/set_stabilization',
Bool,
self.set_stab,
queue_size=1)
sub_heading = rospy.Subscriber(topic_root + '/set_heading',
Int16,
self.set_heading,
queue_size=1)
sub_heading = rospy.Subscriber(topic_root + '/reset_heading',
Int16,
self.reset_heading,
queue_size=1)
sub_set_pos = rospy.Subscriber(topic_root + '/set_position',
Point,
self.set_position,
queue_size=1)
# Publishables
self.pub_odometry = rospy.Publisher(topic_root + '/odometry',
PointStamped,
queue_size=1)
self.pub_velocity = rospy.Publisher(topic_root + '/velocity',
PointStamped,
queue_size=1)
self.pub_accel = rospy.Publisher(topic_root + '/accel',
Int16,
queue_size=1)
#self.pub_accel_raw = rospy.Publisher(topic_root + '/accel', PointStamped, queue_size=1)
#self.pub_imu = rospy.Publisher(topic_root + '/imu', PointStamped, queue_size=1)
#self.pub_gyro = rospy.Publisher(topic_root + '/gyro', PointStamped, queue_size=1)
def pub_accelone_data(self, data):
val = struct.unpack('>h', data)
self.pub_accel.publish(val[0])
def pub_accel_raw_data(self, data):
val = struct.unpack('>hhh', data)
ps = PointStamped()
ps.header.stamp = rospy.Time.now()
ps.point.x = val[0] / 4096 # assuming filtered values
ps.point.y = val[1] / 4096
ps.point.z = val[2] / 4096
self.pub_accel.publish(ps)
def pub_imu_data(self, data):
ps = PointStamped()
ps.header.stamp = rospy.Time.now()
ps.point.x = data['x']
ps.point.y = data['y']
ps.point.z = data['z']
self.pub_imu.publish(ps)
def pub_gyro_data(self, data):
ps = PointStamped()
ps.header.stamp = rospy.Time.now()
ps.point.x = data['x']
ps.point.y = data['y']
ps.point.z = data['z']
self.pub_gyro.publish(ps)
def pub_odom_data(self, data):
'''
Publish Odometry data in mm
:param data:
:return:
'''
if (len(data) == 0):
return
val = struct.unpack('>hh', data)
ps = PointStamped()
ps.header.stamp = rospy.Time.now()
ps.point.x = val[0] * 10 # assuming filtered values
ps.point.y = val[1] * 10
self.pub_odometry.publish(ps)
def pub_vel_data(self, data):
if (len(data) == 0):
return
val = struct.unpack('>hh', data)
ps = PointStamped()
ps.header.stamp = rospy.Time.now()
ps.point.x = val[0] # assuming filtered values
ps.point.y = val[1]
self.pub_velocity.publish(ps)
def setup_publishables(self):
#self._orb.start_accel_callback(rate=10, callback=self.pub_accel_raw_data)
#self._orb.start_IMU_callback(rate=10, callback=self.pub_imu_data)
#self._orb.start_gyro_callback(rate=10, callback=self.pub_gyro_data)
self._orb.set_stream_callback('odometer',
callback=self.pub_odom_data,
mask_id=2)
self._orb.set_stream_callback('accelone',
callback=self.pub_accelone_data,
mask_id=2)
self._orb.set_stream_callback('velocity',
callback=self.pub_vel_data,
mask_id=2)
self._orb.update_streaming(rate=10)
def my_ping(self, last_call):
self._orb.ping()
def my_streaming(self, last_call):
self._orb.update_streaming(rate=5)
def info(self):
print("Sphero Node v" + SpheroNode.VERSION)
print("Node: " + self._name + ", Color: (" + str(self._color.r) + "," +
str(self._color.g) + "," + str(self._color.b) + "," +
str(self._color.a) + ")")
def main(self):
self.info()
self.connect()
self.init_ros()
rospy.sleep(2)
self.setup_publishables()
heartbeat = rospy.Timer(rospy.Duration(.05), self.my_ping)
streaming = rospy.Timer(rospy.Duration(30), self.my_streaming)
self.set_color(self._color)
rate = rospy.Rate(40) # Hz
while not rospy.is_shutdown():
rate.sleep()
#!/usr/bin/env python
from sphero_sprk import Sphero
import time
import socket
def successful_connection_led():
robot.set_rgb_led(200, 0, 0)
for i in range(3):
time.sleep(0.3)
robot.set_rgb_led(0, 255 - 100 * i, 0)
print("If connection was successful, sphero should now be green")
print('Waiting for connection to Sphero...')
robot = Sphero("F3:8D:AC:BE:FB:83")
robot.connect()
successful_connection_led()
class Server():
def __init__(self, Adress=('', 5000), MaxClient=1):
self.s = socket.socket()
self.s.bind(Adress)
self.s.listen(MaxClient)
def WaitForConnection(self):
print('Waiting for connection from client...')
self.Client, self.Adr = (self.s.accept())
print('Got a connection from: ' + str(self.Client) + '.')
import _thread
import pickle
import matplotlib.pyplot as plt
robots = []
blobs = []
goals= [[20,80],[50,60],[80,80]]
counter = 0
costMatrix = np.empty([len(goals),len(robots)])
motionEnabled = True
speed_min = 20
speed_max = 35
angle_step_max = 18
s1 = Sphero("C9:93:8F:F1:91:E2")
s2 = Sphero("DA:06:00:55:3A:8D")
s3 = Sphero("DC:F3:09:7B:4D:BB")
#s4 = Sphero("F5:8F:A1:93:56:40")
class balls:
def __init__(self,identifier,xy=[0,0],available = False, isMoving = False, currentGoal = [0,0], old_xy = [0,0], speed = 0, heading = 0):
self.identifier = identifier
self.xy = xy
self.available = available
self.isMoving = isMoving
self.currentGoal = currentGoal
self.old_xy = old_xy
self.speed = speed
self.heading = heading