def apply_tf_transform(self, tf, point):
x = point[0][0]
y = point[1][0]
z = point[2][0]
m2 = geometry_msgs.msg.PointStamped()
m2.header.frame_id = 'camera_frame'
m2.point.x = x
m2.point.y = y
m2.point.z = z
return tf.transformPoint('baxter_frame', m2).point
Quaternion(goal[1][0], goal[1][1], goal[1][2], goal[1][3]))))
if __name__ == '__main__':
rospy.init_node('willow_tour_node')
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
rospy.loginfo("Waiting for move_base")
client.wait_for_server()
rospy.loginfo("Sending goals")
tf = tf.TransformListener()
rospy.sleep(2)
p = PointStamped()
p.header.frame_id = 'base_footprint'
p.header.stamp = rospy.Time()
pos = tf.transformPoint('map', p)
best_dist = 1000000
current = 0
for (i, g) in enumerate(goals):
if squared_dist(pos, g[0]) < best_dist:
best_dist = squared_dist(pos, g[0])
current = i
rospy.loginfo('Closest waypoint is %s: %s', current, goals[current])
for c in range(5):
while current < len(goals):
mb = mb_goal(goals[current])
rospy.loginfo("Sending goal %s" % goals[current])
client.send_goal(mb)
client.wait_for_result()
import tf
from tf import transformations as trans
ping_sub = yield navigator.nh.subscribe("/hydrophones/processed", ProcessedPing)
yield ping_sub.get_next_message()
target_freq = 35000
while True:
processed_ping = yield ping_sub.get_next_message()
print processed_ping
if isinstance(processed_ping, ProcessedPing):
print "Got processed ping message:\n{}".format(processed_ping)
if processed_ping.freq > 35000 and processed_ping.freq < 36000:
freq_dev = abs(target_freq - processed_ping.freq)
print "Trustworthy pinger heading"
hydrophones_enu_p, hydrophones_enu_q = tf.lookupTransform("/hydrophones", "/enu", processed_ping.header.stamp)
pinger_enu_p = navigator_tools.point_to_numpy(tf.transformPoint())
dir_ = navigator_tools.point_to_numpy(processed_ping.position)
mv_mag = 2
mv_hyd_frame = dir_ / np.linalg.norm(dir_)
pinger_move = navigator.move.set_position(navigator_tools.point_to_numpy(processed_ping.position)).go()
print "Heading towards pinger"
else:
print "Untrustworthy pinger heading. Freq = {} kHZ".format(processed_ping.freq)
else:
print "Expected ProcessedPing, got {}".format(type(processed_ping))
# Hydrophone locate mission
@txros.util.cancellableInlineCallbacks
def main(navigator):
kill_alarm_broadcaster, kill_alarm = single_alarm('kill', action_required=True, problem_description="Killing wamv to listen to pinger")