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])