def cloud_handler(req):
time = rp.get_time()
LOCK.acquire()
access_times[req.name] = time
bearings[req.name] = gmi.Versor(req.bearing)
current_bearings = dict()
for name in access_times:
if name != req.name:
current_bearings[name] = bearings[name].rotate(
control_values[name] * (time - access_times[name]))
if len(current_bearings) > 0:
beta = min((bearings[req.name].angle_to(current_bearing,
force_positive=True)
for current_bearing in current_bearings.values()))
control_values[req.name] = GAIN * (ALPHA + beta)
next_access = time + beta / control_values[req.name]
else:
beta = 0.0
control_values[req.name] = GAIN * (ALPHA + beta)
next_access = time + 1.0
scheduled_accesses[req.name] = next_access
LOCK.release()
return ccs.CloudServiceResponse(beta, next_access)
def sensor_service_handler(req):
LOCK.acquire()
bearing = gmi.Versor(target_position - position)
LOCK.release()
return ccs.SensorServiceResponse(bearing.serialize())
callback=position_callback,
queue_size=10)
rp.wait_for_message('vehicle_position', gms.Point)
rp.wait_for_message('/target_position', gms.Point)
#Handler for the service SensorService
def sensor_service_handler(req):
LOCK.acquire()
bearing = gmi.Versor(target_position - position)
LOCK.release()
return ccs.SensorServiceResponse(bearing.serialize())
rp.Service(name='sensor_service',
service_class=ccs.SensorService,
handler=sensor_service_handler)
bearing_pub = rp.Publisher(name='bearing',
data_class=gms.Versor,
queue_size=10)
RATE = rp.Rate(10.0)
while not rp.is_shutdown():
LOCK.acquire()
bearing = gmi.Versor(target_position - position)
bearing_pub.publish(bearing.serialize())
LOCK.release()
RATE.sleep()
#!/usr/bin/python
import rospy as rp
import geomtwo.msg as gms
import geomtwo.impl as gmi
import threading as thd
from std_msgs.msg import Float32
#Initial position
position = rp.get_param('initial_position')[0]
pos = gmi.Versor(position)
agent_name = rp.get_param('agentID')
LOCK = thd.Lock()
#Velocity
velocity = None
rp.init_node('simulator')
FREQUENCY = 10e1
RATE = rp.Rate(FREQUENCY)
TIME_STEP = 2.0/FREQUENCY
def cmdvel_callback(msg):
global velocity
LOCK.acquire()
velocity = msg
LOCK.release()
rp.Subscriber(
def bearing_callback(msg, name):
LOCK.acquire()
bearings[name] = gmi.Versor(msg)
LOCK.release()
def position_callback(msg):
global vehicle_position
LOCK.acquire()
vehicle_position = gmi.Point(msg)
LOCK.release()
rp.Subscriber(
name = 'vehicle_position',
data_class = gms.Point,
callback = position_callback,
queue_size = 10)
rp.wait_for_service('sensor_service')
sensor_proxy = rp.ServiceProxy('sensor_service', ccs.SensorService)
measured_bearing = sensor_proxy.call().bearing_measurement
estimated_bearing = gmi.Versor(measured_bearing)
rp.wait_for_message('vehicle_position', gms.Point)
estimated_target_position = vehicle_position + DESIRED_DISTANCE*estimated_bearing
rp.wait_for_service('/cloud_service')
cloud_service_proxy = rp.ServiceProxy('/cloud_service', ccs.CloudService)
#Publisher
cmdvel_pub = rp.Publisher(
name = 'cmdvel',
data_class = gms.Vector,