def execute(self, userdata):
rospy.loginfo('Executing state AssessScrape')
tf = UnifiedTFClient.get_unified_tf_client()
(trans, rot) = tf.lookupTransform('torso_lift_link',
'r_wrist_roll_link', rospy.Time(0))
starty = trans[1]
for i in range(self.MAX_MIXES):
rospy.loginfo('Executing state Scrape')
xoff = 0
zstartoff1 = 0
if i == 1:
xoff = .08
elif i == 2:
xoff = -.08
elif i == 3:
zstartoff1 = .1
(trans, rot) = tf.lookupTransform('torso_lift_link',
'r_wrist_roll_link',
rospy.Time(0))
yoffset = -1 * (starty - trans[0])
csc = CleaningClient.get_cleaning_client()
#csc.scrape(xmove = xoff, zstartoff = zstartoff1, yoff=yoffset)
#TODO: something is very wrong here (1/26/11). the hand backs out further and further for every scrape action
# when there is no spoon. maybe have to fix this.
csc.scrape(xmove=xoff, zstartoff=zstartoff1)
#print 'now going to plunge spoon'
#mc = MixingClient.get_mixing_client()
#x = userdata.cookie_sheet.pose.pose.position.x
#z = .90 #TODO: will want to change this to be less fragile
#tfl = UnifiedTFClient.get_unified_tf_client()
#try:
# (trans, rot) = tfl.lookupTransform('/base_link', '/torso_lift_link', rospy.Time(0))
#except Exception as e:
# print e
# print 'burning off a bad tf reading and waiting'
# time.sleep(1)
# (trans, rot) = tfl.lookupTransform('/base_link', '/torso_lift_link', rospy.Time(0))
#if rot is not (0.0,0.0,0.0,1.0):
# rospy.logerr('nonzero rotation between base link and torso lift link!')
# print rot
#x = x - trans[0]
#print 'nx',x
#status = mc.plunge_spoon(bowl_x = x, bowl_z = z, ismix = False)
return 'success'
def moving_goal(self):
obstacle = ModelState()
ptFinal, oriFinal = tf.lookupTransform("base_link", "ar_marker_0",
rospy.Time(0))
obstacle.model_name = self.model_name
obstacle.pose = model.pose[i]
obstacle.twist = Twist()
obstacle.twist.linear.y = 1.3
obstacle.twist.angular.z = 0
self.pub_model.publish(obstacle)
def get_time_and_pose(tf, f1, f2):
try:
t = tf.getLatestCommonTime(f1, f2)
pos, q = tf.lookupTransform(f1, f2, t)
except:
traceback.print_exc(file=sys.stdout)
return None, None
rpy = euler_from_quaternion(q)
return t, Pose2D(pos[0], pos[1], rpy[2])
def get_time_and_pose(tf, f1, f2):
try:
t = tf.getLatestCommonTime(f1, f2)
pos, q = tf.lookupTransform(f1, f2, t)
except:
traceback.print_exc(file=sys.stdout)
return None, None
rpy = euler_from_quaternion(q)
return t, Pose2D(pos[0], pos[1], rpy[2])
def update_assembly(self, objs):
for obj in objs:
if obj.object_class == 'node_uniform':
# add to nodes with position
frame = kdl.Frame()
self.nodes[obj.id] = (frame, 0)
pass
for obj in objs:
if obj.object_class == 'link_uniform':
# check above in z direction
(trans, rot) = tf.lookupTransform(self.world, obj.id,
rospy.Time.now())
trans_above = trans
trans[2] += (0.15 / 2)
node_above = self.search_position(pm.toKDL(trans_above, rot),
self.err)
# check below in z direction
trans_below = trans
trans[2] -= (0.15 / 2)
node_below = self.search_position(pm.toKDL(trans_below, rot),
self.err)
# add to links with position
connected_above = 0
connected_below = 0
if node_above is not None:
connected_above += self.nodes[node_above][1]
self.nodes[node_above][1] += 1
if node_below is not None:
connected_below += self.nodes[node_above][1]
self.nodes[node_below][1] += 1
if node_above is not None and node_below is not None:
self.nodes[node_above][1] += connected_below
self.nodes[node_below][1] += connected_above
connected = 1 + connected_above + connected_below
links[obj.id] = (pm.toKDL(trans, rot), connected)
# make sure list of predicates is correct
if not obj.object_class in self.valid_msg.predicates:
self.valid_msg.predicates.append(obj.object_class)
if not obj.object_class in self.valid_msg.assignments:
self.valid_msg.assignments.append(obj.object_class)
if not obj.id in self.valid_msg.assignments:
self.valid_msg.assignments.append(obj.id)
if not is_det_pred in self.valid_msg.predicates:
self.valid_msg.predicates.append(is_det_pred)
def update_assembly(self, objs):
for obj in objs:
if obj.object_class == 'node_uniform':
# add to nodes with position
frame = kdl.Frame()
self.nodes[obj.id] = (frame, 0)
pass
for obj in objs:
if obj.object_class == 'link_uniform':
# check above in z direction
(trans, rot) = tf.lookupTransform(self.world,
obj.id,
rospy.Time.now())
trans_above = trans
trans[2] += (0.15 / 2)
node_above = self.search_position(pm.toKDL(trans_above, rot), self.err)
# check below in z direction
trans_below = trans
trans[2] -= (0.15 / 2)
node_below = self.search_position(pm.toKDL(trans_below, rot), self.err)
# add to links with position
connected_above = 0;
connected_below = 0;
if node_above is not None:
connected_above += self.nodes[node_above][1]
self.nodes[node_above][1] += 1
if node_below is not None:
connected_below += self.nodes[node_above][1]
self.nodes[node_below][1] += 1
if node_above is not None and node_below is not None:
self.nodes[node_above][1] += connected_below
self.nodes[node_below][1] += connected_above
connected = 1 + connected_above + connected_below;
links[obj.id] = (pm.toKDL(trans,rot), connected)
# make sure list of predicates is correct
if not obj.object_class in self.valid_msg.predicates:
self.valid_msg.predicates.append(obj.object_class)
if not obj.object_class in self.valid_msg.assignments:
self.valid_msg.assignments.append(obj.object_class)
if not obj.id in self.valid_msg.assignments:
self.valid_msg.assignments.append(obj.id)
if not is_det_pred in self.valid_msg.predicates:
self.valid_msg.predicates.append(is_det_pred)
def tfLookup(tf, target_frame, source_frame, time, timeout=rospy.Duration(1)):
tf.waitForTransform(target_frame, source_frame, time, timeout)
return tfPoseToGeometry(
tf.lookupTransform(target_frame, source_frame, time))
import numpy as np
import tf
from tf.transformations import *
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2
from std_msgs import msg
import rospy
import cv2
from cv_bridge import CvBridge, CvBridgeError
import matplotlib.pyplot as plt
"""The steps for collecting depth image and then converting to a merged
point cloud in Baxter's base frame:
1. Collect an averaged depth image from each Kinect. Call these dimg1 and dimg2
for Kinect1 and Kinect2 respectively. (This should be done in a separate method).
2. Compute the tfs for each Kinect to the base, i.e. FROM KinectX TO base -->
tf.lookupTransform('base', /KinectX_link, rospy.time(0)). Store these tfs as
transX, rotX for the respective KinectX.
3. Call merge_dimg_to_pcs(dimg1, dimg2, trans1, rot1, trans2, rot2) to get
a merged pointcloud in Baxter's base frame. The merged pointcloud
will be an nX3 numpy array.
4. (Optional) Visualize the pointcloud in RVIZ using rviz_pc_visualizer() to double
check correctness.
5. Run the merged pointcloud through the bounding box by calling boxer() with
the desired arguments.
import navigator_tools
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):
H = 0
cf_v8 = np.array([0.0, 0.0, 0.0])
swarm = Crazyswarm()
timeHelper = swarm.timeHelper
allcfs = swarm.allcfs
cf8 = allcfs.crazyfliesById[8]
allcfs.takeoff(targetHeight=Z, duration=1.0 + 0.5)
tf = tf.TransformListener()
tf.waitForTransform("/world", "/vicon/cftarget_2/cftarget_2",
rospy.Time(0), rospy.Duration(0.1))
timeHelper.sleep(2)
t_now = rospy.Time.now()
t_now2 = rospy.Time.now()
dt = 0
position_last, quaternion = tf.lookupTransform(
"/world", "/vicon/cftarget_2/cftarget_2", rospy.Time(0))
Op_point_8 = np.array([position_last[0], position_last[1], Z])
Op_point_group = {8: Op_point_8}
##def init(self,ID,target_ID,offset,master_point,a,b,h,phi_factor)
Agent8 = cfAct(8, [8], np.array([0, 0, 0]), False, 1, 0.6,
0.5 * np.array([[0, 0, 0]]), 0.05)
while (True):
t_last = rospy.Time.now()
dt = (t_last - t_now2).to_sec()
if dt > 0.03:
t_now2 = rospy.Time.now()
position, quaternion = tf.lookupTransform(
"/world", "/vicon/cftarget_2/cftarget_2", rospy.Time(0))
v_tar = (np.array(position) - np.array(position_last)) / dt
topic = rospy.resolve_name('pose_state')
publish = rospy.Publisher(topic, PoseStamped, queue_size=1).publish
# Note that frame tip in root = transform root to tip
target_frame = rospy.get_param('~root_frame', 'base_link')
source_frame = rospy.get_param('~tip_frame', 'ee_link')
rospy.loginfo('Publishing {}->{} on {}'.format(source_frame, target_frame,
topic))
seq = 0
while not rospy.is_shutdown():
try:
if tf.frameExists(source_frame) and tf.frameExists(target_frame):
t = tf.getLatestCommonTime(source_frame, target_frame)
p, q = tf.lookupTransform(target_frame, source_frame, t)
point = Point()
point.x = p[0]
point.y = p[1]
point.z = p[2]
quaternion = Quaternion()
quaternion.x = q[0]
quaternion.y = q[1]
quaternion.z = q[2]
quaternion.w = q[3]
pose = PoseStamped()
pose.header.seq = seq
pose.header.frame_id = source_frame
rospy.Time(0), rospy.Duration(0.1))
tf.waitForTransform("/world", "/vicon/cftarget_2/cftarget_2",
rospy.Time(0), rospy.Duration(0.1))
tf.waitForTransform("/world", "/vicon/cftarget_3/cftarget_3",
rospy.Time(0), rospy.Duration(0.1))
tf.waitForTransform("/world", "/vicon/cftarget_4/cftarget_4",
rospy.Time(0), rospy.Duration(0.1))
tf.waitForTransform("/world", "/vicon/cftarget_5/cftarget_5",
rospy.Time(0), rospy.Duration(0.1))
tf.waitForTransform("/world", "/vicon/cftarget_6/cftarget_6",
rospy.Time(0), rospy.Duration(0.1))
timeHelper.sleep(2)
t_now = rospy.Time.now()
t_now2 = rospy.Time.now()
dt = 0
position_last_1, quaternion = tf.lookupTransform(
"/world", "/vicon/cftarget_1/cftarget_1", rospy.Time(0))
position_last_2, quaternion = tf.lookupTransform(
"/world", "/vicon/cftarget_2/cftarget_2", rospy.Time(0))
position_last_3, quaternion = tf.lookupTransform(
"/world", "/vicon/cftarget_3/cftarget_3", rospy.Time(0))
position_last_4, quaternion = tf.lookupTransform(
"/world", "/vicon/cftarget_4/cftarget_4", rospy.Time(0))
position_last_5, quaternion = tf.lookupTransform(
"/world", "/vicon/cftarget_5/cftarget_5", rospy.Time(0))
position_last_6, quaternion = tf.lookupTransform(
"/world", "/vicon/cftarget_6/cftarget_6", rospy.Time(0))
Op_point_2 = np.array([position_last_1[0], position_last_1[1], Z])
Op_point_4 = np.array([position_last_2[0], position_last_2[1], Z])
Op_point_5 = np.array([position_last_3[0], position_last_3[1], Z])
Op_point_7 = np.array([position_last_4[0], position_last_4[1], Z])
Op_point_1 = np.array([position_last_5[0], position_last_5[1], Z])
