def callback(msg):
global point
LOCK.acquire()
point = gmi.Point(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,
#!/usr/bin/python
import rospy as rp
import geomtwo.msg as gms
import geomtwo.impl as gmi
import threading as thd
import numpy as np
import circumnavigation_cloud.srv as srvc
TARGET_POSITION = gmi.Point(0.0, 0.0)
publisher = rp.Publisher(name="target_position",
data_class=gms.Point,
queue_size=10)
rp.init_node("target")
RATE = rp.Rate(30.0)
while not rp.is_shutdown():
publisher.publish(TARGET_POSITION.serialize())
RATE.sleep()
#!/usr/bin/env python
import rospy as rp
import geomtwo.msg as gms
import geomtwo.impl as gmi
import threading as thd
rp.init_node(name="point_simulator")
FREQUENCY = 3e1
RATE = rp.Rate(FREQUENCY)
TIME_STEP = 1 / FREQUENCY
LOCK = thd.Lock()
point = gmi.Point(1, 0)
velocity = None
def callback(msg):
global velocity
LOCK.acquire()
velocity = gmi.Vector(msg)
LOCK.release()
rp.Subscriber(name="velocity", data_class=gms.Vector, callback=callback)
pub = rp.Publisher(name="point", data_class=gms.Point, queue_size=10)
read_velocity = None
while not rp.is_shutdown():
LOCK.acquire()
def position_callback(msg):
global position
LOCK.acquire()
position = gmi.Point(msg)
LOCK.release()
def target_position_callback(msg):
global target_position
LOCK.acquire()
target_position = gmi.Point(msg)
LOCK.release()
def estimate_callback(msg, name):
global estimate_positions
LOCK.acquire()
estimate_positions[name] = gmi.Point(msg)
LOCK.release()
def agent_callback(msg, name):
global agent_positions
LOCK.acquire()
agent_positions[name] = gmi.Point(msg)
LOCK.release()