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,