def run():
rospy.init_node('boundary_tracker')
print '--Scarab tracker starting--'
## Listen for the localization
rospy.Subscriber('/vicon/Scarab_DCT_S45', Subject, localization_callback, queue_size=1)
# Topic to control the robot velocity
velPub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# initial time
init_time = time.time()
# Load boundaries
boundaries = pickle.load(open("boundary.p", "rb"))
log = []
# Desired angular speed
Omeg = .3
# attarction
Attrac = .6
drawer = RobotDrawer()
vel = Twist()
# ####### Control Loop ###########
while not rospy.is_shutdown():
rospy.sleep(0.1)
# Compute boundary time
t = time.time()
boundary_time = int(t - init_time) + 30
if boundary_time == len(boundaries):
pickle.dump(log, open("experiment%d.p" % t, "wb"))
# Stop the robot
vel.linear.x, vel.angular.z = 0., 0
velPub.publish(vel)
break
boundary = np.array(boundaries[boundary_time])
boundary[:, 0] -= 1
boundary[:, 1] -= .7
boundary[:] *= 1.5
drawer.draw_polygons([boundary])
if pose is None:
print 'Error: no localization.1'
continue
##### Robot pose
rx, ry = pose.position.x, pose.position.y
quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
rth = euler_from_quaternion(quat)[2]
drawer.update_robots([(rx, ry, rth)])
#### Control
clst = point_to_follow(Polygon(boundary), Point((rx, ry)), .2)
# vector to approach to the desired point
vr = np.array([clst[0] - rx, clst[1] - ry])
# Total vector
vtotal = 2 * vr
# Feedback linearization
vel.linear.x = vtotal[0] * cos(rth) + vtotal[1] * sin(rth)
vel.angular.z = -vtotal[0] * sin(rth) / .1 + vtotal[1] * cos(rth) / .1
## LOG
log.append((boundary_time, t, rx, ry, rth))
# vel.linear.x, vel.angular.z = 0., 0
# print "v=%f w=%f" % (vel.linear.x, vel.angular.z)
print boundary_time
velPub.publish(vel)
def run():
rospy.init_node('boundary_tracker')
print '--Scarab tracker starting--'
## Listen for the localization
rospy.Subscriber('/vicon/Scarab_DCT_S45',
Subject,
localization_callback,
queue_size=1)
# Topic to control the robot velocity
velPub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# Desired angular speed
Omeg = .5
# attarction
Attrac = 1.5
drawer = RobotDrawer()
# ####### Control Loop ###########
while not rospy.is_shutdown():
rospy.sleep(0.1)
if pose is None:
print 'Error: no localization.1'
continue
theta = np.linspace(0, 2 * pi, 50)
boundary = [(cos(th1), sin(th1)) for th1 in theta]
drawer.draw_polygons([boundary])
# Robot pose
rx, ry = pose.position.x, pose.position.y
quat = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
rth = euler_from_quaternion(quat)[2]
drawer.update_robots([(rx, ry, rth)])
# Circle
map_theta = atan2(ry, rx)
# Tangent
vt = np.array([-sin(map_theta), cos(map_theta)])
# Atractor to the unitary radious
vr = np.array([cos(map_theta), sin(map_theta)]) - np.array([rx, ry])
# Total vector
vtotal = Attrac * vr + Omeg * vt
vel = Twist()
# Feedback linearization
vel.linear.x = vtotal[0] * cos(rth) + vtotal[1] * sin(rth)
vel.angular.z = -vtotal[0] * sin(rth) / .1 + vtotal[1] * cos(rth) / .1
# The angle is in the contrary direction
# vel.angular.z = .2 * (atan2(vtotal[1], vtotal[0]) - rth)
# vel.angular.z = 0.2 * (map_theta - rth + pi)
# vel.angular.z = 0.2 * (0- rth)
# print degrees(rth), degrees(atan2(ry, rx)), vel.angular.z
# vel.linear.x, vel.angular.z = 0., 0
print "v=%f w=%f" % (vel.linear.x, vel.angular.z)
velPub.publish(vel)
def run():
rospy.init_node('boundary_tracker')
print '--Scarab tracker starting--'
## Listen for the localization
rospy.Subscriber('/vicon/Scarab_DCT_S45', Subject, localization_callback, queue_size=1)
# Topic to control the robot velocity
velPub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# Desired angular speed
Omeg = .5
# attarction
Attrac = 1.5
drawer = RobotDrawer()
# ####### Control Loop ###########
while not rospy.is_shutdown():
rospy.sleep(0.1)
if pose is None:
print 'Error: no localization.1'
continue
theta = np.linspace(0, 2 * pi, 50)
boundary = [(cos(th1), sin(th1)) for th1 in theta]
drawer.draw_polygons([boundary])
# Robot pose
rx, ry = pose.position.x, pose.position.y
quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
rth = euler_from_quaternion(quat)[2]
drawer.update_robots([(rx, ry, rth)])
# Circle
map_theta = atan2(ry, rx)
# Tangent
vt = np.array([-sin(map_theta), cos(map_theta)])
# Atractor to the unitary radious
vr = np.array([cos(map_theta), sin(map_theta)]) - np.array([rx, ry])
# Total vector
vtotal = Attrac * vr + Omeg * vt
vel = Twist()
# Feedback linearization
vel.linear.x = vtotal[0] * cos(rth) + vtotal[1] * sin(rth)
vel.angular.z = -vtotal[0] * sin(rth) / .1 + vtotal[1] * cos(rth) / .1
# The angle is in the contrary direction
# vel.angular.z = .2 * (atan2(vtotal[1], vtotal[0]) - rth)
# vel.angular.z = 0.2 * (map_theta - rth + pi)
# vel.angular.z = 0.2 * (0- rth)
# print degrees(rth), degrees(atan2(ry, rx)), vel.angular.z
# vel.linear.x, vel.angular.z = 0., 0
print "v=%f w=%f" % (vel.linear.x, vel.angular.z)
velPub.publish(vel)
def run():
rospy.init_node('boundary_tracker')
print '--Scarab tracker starting--'
## Listen for the localization
rospy.Subscriber('/vicon/Scarab_DCT_S45', Subject, localization_callback, queue_size=1)
# Topic to control the robot velocity
velPub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# initial time
init_time = time.time()
# Load boundaries
boundaries = pickle.load(open("boundary.p", "rb"))
log = []
# Desired angular speed
Omeg = .3
# attarction
Attrac = .6
drawer = RobotDrawer()
vel = Twist()
# ####### Control Loop ###########
while not rospy.is_shutdown():
rospy.sleep(0.1)
# Compute boundary time
t = time.time()
boundary_time = int(t - init_time) + 30
if boundary_time == len(boundaries):
pickle.dump(log, open("experiment%d.p" % t, "wb"))
# Stop the robot
vel.linear.x, vel.angular.z = 0., 0
velPub.publish(vel)
break
boundary = np.array(boundaries[boundary_time])
boundary[:, 0] -= 1
boundary[:, 1] -= .7
boundary[:] *= 1.5
drawer.draw_polygons([boundary])
if pose is None:
print 'Error: no localization.1'
continue
##### Robot pose
rx, ry = pose.position.x, pose.position.y
quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
rth = euler_from_quaternion(quat)[2]
drawer.update_robots([(rx, ry, rth)])
#### Control
# Tangent
# vt = np.array([-sin(map_theta), cos(map_theta)])
clst = closest_on_polygon(Polygon(boundary), Point((rx, ry)))
vt = tanget_dir(clst, boundary)
# Attractor to the unitary radius
# vector to approach
vr = np.array([clst[0] - rx, clst[1] - ry])
# Total vector
vtotal = Attrac * vr + Omeg * vt
# Feedback linearization
vel.linear.x = vtotal[0] * cos(rth) + vtotal[1] * sin(rth)
vel.angular.z = -vtotal[0] * sin(rth) / .1 + vtotal[1] * cos(rth) / .1
## LOG
log.append((boundary_time, t, rx, ry, rth))
# vel.linear.x, vel.angular.z = 0., 0
# print "v=%f w=%f" % (vel.linear.x, vel.angular.z)
print boundary_time
velPub.publish(vel)