def dot(self, other):
raise TypeError
def cross(self, other):
raise TypeError
def angle_to(self, other):
raise TypeError
def saturate(self, other):
raise TypeError
if __name__ == "__main__":
pt = Point(gms.Point())
pt.draw()
pt2 = Point(2, 3)
(pt - pt2).draw(color="red")
#plt.show()
print pt2 + vec
class Versor(Vector):
def __init__(self, *args, **kwargs):
if len(args) is 2:
norm = cm.sqrt(args[0]**2 + args[1]**2)
Vector.__init__(self, args[0] / norm, args[1] / norm)
return
if len(args) is 1:
if isinstance(args[0], (gms.Vector, gms.Versor, Vector, Versor)):
def message(self):
return gms.Point(self.x, self.y)
#Publisher
estimate_pub = rp.Publisher(name='estimate',
data_class=gms.Point,
queue_size=10)
start = False
while not rp.is_shutdown() and not start:
LOCK.acquire()
if all([not data is None for data in [position, bearing_measurement]]):
start = True
#else:
#rp.logwarn('waiting for position and measurements')
LOCK.release()
#Initial estimate publishing
estimate_pub.publish(gms.Point(x=estimate[0], y=estimate[1]))
RATE.sleep()
while not rp.is_shutdown():
LOCK.acquire()
#Estimate algorithm
d_estimate = -estimate_gain * (np.eye(2) - np.outer(
bearing_measurement, bearing_measurement)).dot(estimate - position)
#rp.logwarn(d_estimate)
#Integration
estimate = estimate + d_estimate * TIME_STEP
LOCK.release()
#Estimate publishing
#rp.logwarn(estimate)
estimate_pub.publish(gms.Point(x=estimate[0], y=estimate[1]))
RATE.sleep()
LOCK=thd.Lock()
#Target initial position
target_position = np.array(rp.get_param('target_initial_position'))
rp.init_node('target')
#Frequency
FREQUENCY = 15e1
RATE = rp.Rate(FREQUENCY)
TIME_STEP = 1.0/FREQUENCY
#Publisher
target_pub = rp.Publisher('target_position', gms.Point, queue_size=10)
start = False
while not rp.is_shutdown():
LOCK.acquire()
#Target motion
t=rp.get_time()
d_target_position=np.array([0.05*math.sin(0.05*t),0.05*math.cos(0.05*t)])
#Integration
target_position = target_position+d_target_position*TIME_STEP
LOCK.release()
#Position publishing
target_pub.publish(gms.Point(x=target_position[0], y=target_position[1]))
RATE.sleep()
global velocity
LOCK.acquire()
velocity = np.array([msg.x, msg.y])
LOCK.release()
rp.Subscriber(name='cmdvel',
data_class=gms.Vector,
callback=cmdvel_callback,
queue_size=10)
start = False
while not rp.is_shutdown() and not start:
LOCK.acquire()
if not velocity is None:
start = True
#else:
#rp.logwarn('waiting for cmdvel')
LOCK.release()
#Initial position publishing
pub.publish(gms.Point(x=position[0], y=position[1]))
RATE.sleep()
while not rp.is_shutdown():
LOCK.acquire()
#Integration
position = position + velocity * TIME_STEP
LOCK.release()
#Position publishing
pub.publish(gms.Point(x=position[0], y=position[1]))
RATE.sleep()
def serialize(self):
return gms.Point(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()
