LOCK.acquire()
if all([not data is None for data in [ position ]]):
start = True
#else:
#rp.logwarn('waiting for measurements')
LOCK.release()
RATE.sleep()
while not rp.is_shutdown():
LOCK.acquire()
if stop_publish:
rp.signal_shutdown("agent sensor removed")
#Bearing vector (phi)
bearing = (TARGET_POSITION-position)/np.linalg.norm(TARGET_POSITION-position)
#rp.logwarn(bearing)
distance=np.linalg.norm(TARGET_POSITION-position)
#Distance publishing
distance_msg=gm.PointStamped()
distance_msg.header.seq=0
distance_msg.header.stamp = rp.Time.now()
distance_msg.point.x=distance
distance_msg.point.y=0
distance_msg.point.z=0
distance_pub.publish(distance_msg)
#Bearing vector publishing
bearing_msg = gms.Vector(*bearing)
bearing_pub.publish(bearing_msg)
LOCK.release()
RATE.sleep()
agent_beta = []
#Counterclockwise angle
for name in agent_names:
if not agent_positions[name] is None and np.linalg.norm(
position - agent_positions[name]) <= rho:
agent_beta.append(Angle(position, agent_positions[name]))
# If there is only one agent beta=2*pi
beta = 0
if len(agent_beta) > 0:
beta = min(agent_beta)
#Control law
est_dist = np.linalg.norm(estimate - position)
vel = k_d * bearing_measurement * (
est_dist - DESIRED_DISTANCE) + k_fi * est_dist * phi_bar * (alpha +
beta)
#Velocity message
cmdvel_msg = gms.Vector(x=vel[0], y=vel[1])
#Beta message
beta_msg = gm.PointStamped()
beta_msg.header.seq = 0
beta_msg.header.stamp = rp.Time.now()
beta_msg.point.x = beta
beta_msg.point.y = 0
beta_msg.point.z = 0
LOCK.release()
# Publishing
cmdvel_pub.publish(cmdvel_msg)
beta_pub.publish(beta_msg)
RATE.sleep()
def message(self):
return gms.Vector(self.x, self.y)
def angle_to(self, other, force_positive=False):
result = cm.phase(complex(self.dot(other), self.cross(other)))
if result < 0 and force_positive: result += 2 * cm.pi
return result
def rotate(self, angle):
return self.__class__(self._data * cm.rect(1.0, angle))
def saturate(self, threshold):
if self.norm > threshold: return self * threshold / self.norm
return self
if __name__ == "__main__":
vec = Vector(gms.Vector(x=2, y=3))
print vec
vec2 = Vector(x=1, y=2)
print vec + vec2
print vec - vec2
print vec.dot(vec2)
print 2 * vec
print vec2.angle_to(vec, force_positive=True)
plt.figure()
plt.xlim([-5, 5])
plt.ylim([-5, 5])
vec.draw()
#plt.show()
class Point(Vector):
def serialize(self):
return gms.Vector(self.x, self.y)
import geomtwo.msg as gms print gms.Vector() print gms.Versor() print gms.Point() # print gms.Transform() # print gms.Pose() # print gms.Twist()
#!/usr/bin/env python
import rospy as rp
import geomtwo.msg as gms
import math as m
rp.init_node(name="velocity_publisher")
FREQUENCY = 3e1
RATE = rp.Rate(FREQUENCY)
TIME_STEP = 1 / FREQUENCY
pub = rp.Publisher(name="velocity", data_class=gms.Vector, queue_size=10)
while not rp.is_shutdown():
time = rp.get_time()
pub.publish(gms.Vector(x=m.cos(time), y=m.sin(time)))
RATE.sleep()