def cloud_handler(req):
tcorr = rp.get_time()
bearing_measurement[req.ID] = np.array([req.bear_msrm.x, req.bear_msrm.y])
control_values[req.ID] = req.control_signal
instant_access[req.ID] = req.time_access
while len(bearing_measurement) < len(AGENT_NAMES):
rp.sleep(0.1)
betas = [Counterclockwise_angle(bearing_measurement[req.ID], meas, agent) for agent, meas in bearing_measurement.items() if agent != req.ID]
beta_neigh = min(betas, key=lambda x: x[0])
#return srvc.CloudServiceResponse(beta_neigh[0])
resp = srvc.CloudServiceResponse()
resp.beta = beta_neigh[0]
bear_arr = np.array(bearing_measurement[beta_neigh[1]])
resp.bear_msrm_neigh = gmi.Vector(bear_arr[0], bear_arr[1])
resp.control_signal_neigh = control_values[beta_neigh[1]]
resp.t_neigh = instant_access[beta_neigh[1]]
return resp
def cmdvel_callback(msg):
global velocity
LOCK.acquire()
velocity = gmi.Vector(msg)
LOCK.release()
#!/usr/bin/python
import rospy as rp
import geomtwo.msg as gms
import geomtwo.impl as gmi
import std_msgs.msg as sms
import threading as thd
rp.init_node('vehicle_simulator')
LOCK = thd.Lock()
position = gmi.Point(rp.get_param("initial_position"))
position_timestamp = rp.get_time()
velocity = gmi.Vector()
FREQUENCY = 60.0
RATE = rp.Rate(FREQUENCY)
TIME_STEP = 1.0 / FREQUENCY
def cmdvel_callback(msg):
global velocity
LOCK.acquire()
velocity = gmi.Vector(msg)
LOCK.release()
rp.Subscriber(name='cmdvel',
data_class=gms.Vector,
# while not rp.is_shutdown():
# ti = rp.get_time()
# resp = sensor_proxy()
# bearing_measurement = resp.bear_msrm
# angle = Counterclockwise_angle(np.array([bear_ref.x,bear_ref.y]),([bearing_measurement.x,bearing_measurement.y]))
# if angle>=beta:
# cloud_resp = cloud_proxy(str(agent_name), bearing_measurement, vel)
# bear_ref = bearing_measurement
# beta = cloud_resp.beta
tprec = 0.0
while not rp.is_shutdown():
tcorr = rp.get_time()
resp = sensor_proxy()
bearing_measurement = gmi.Vector(resp.bear_msrm)
angle = Counterclockwise_angle(np.array(
[bear_ref.x,
bear_ref.y]), ([bearing_measurement.x, bearing_measurement.y]))
if angle >= beta:
req = srvc.CloudServiceRequest(str(agent_name), bearing_measurement,
vel, tcorr)
cloud_resp = cloud_proxy(req)
bear_ref = gmi.Vector(bearing_measurement)
estimate_neigh_pos = gmi.Vector(cloud_resp.bear_msrm_neigh).rotate(
cloud_resp.control_signal_neigh *
(tcorr - float(cloud_resp.t_neigh)))
beta = Counterclockwise_angle(
np.array([bearing_measurement.x, bearing_measurement.y]),
np.array([estimate_neigh_pos.x, estimate_neigh_pos.y]))
#!/usr/bin/env python
import rospy as rp
import geomtwo.msg as gms
import geomtwo.impl as gmi
import math as m
rp.init_node(name="twist_publisher")
FREQUENCY = 3e1
RATE = rp.Rate(FREQUENCY)
TIME_STEP = 1 / FREQUENCY
pub = rp.Publisher(name="twist", data_class=gms.Twist, queue_size=10)
while not rp.is_shutdown():
time = rp.get_time()
pub.publish(gmi.Vector(magnitude=1.0, angle=time).message, m.cos(time))
#pub.publish(gms.Vector(), 1.0)
RATE.sleep()
def sensor_srvc_handler(req):
LOCK.acquire()
#Bearing vector (phi)
bearing = (TARGET_POSITION-position)/np.linalg.norm(TARGET_POSITION-position)
LOCK.release()
return srvc.SensorServiceResponse(gmi.Vector(bearing[0], bearing[1]))