def on_result(self, boxes, classes): rospy.logdebug("on_result") msg = ObjectDetection() msg.header = boxes.header for box, label, prob in zip(boxes.boxes, classes.label_names, classes.label_proba): if not label: continue pose = Object6DPose() pose.pose = box.pose pose.reliability = prob pose.type = label msg.objects.append(pose) if self.publish_tf: t = TransformStamped() t.header = box.header t.child_frame_id = label t.transform.translation.x = box.pose.position.x t.transform.translation.y = box.pose.position.y t.transform.translation.z = box.pose.position.z t.transform.rotation.x = box.pose.orientation.x t.transform.rotation.y = box.pose.orientation.y t.transform.rotation.z = box.pose.orientation.z t.transform.rotation.w = box.pose.orientation.w self.tfb.sendTransform(t) self.pub_detect.publish(msg)
def transformer(data): for marker in data.markers: if marker.subject_name: if marker.subject_name not in crazyflies: crazyflies[marker.subject_name] = {} crazyflie = crazyflies[marker.subject_name] crazyflie[marker.marker_name] = marker.translation crazyflie["frame_id"] = str(data.frame_number) transforms = [] for crazyflie_name in crazyflies: crazyflie = crazyflies[crazyflie_name] trans = TransformStamped() # trans.child_frame_id = crazyflie["frame_id"] trans.child_frame_id = "1" trans.header.frame_id = crazyflie["frame_id"] top = marker_to_vector(crazyflie["top"]) bottom = marker_to_vector(crazyflie["bot"]) left = marker_to_vector(crazyflie["left"]) front = marker_to_vector(crazyflie["front"]) center = get_center(top, left, bottom) rotation = get_rotation(center, front, top, left, bottom) mTrans = trans.transform.translation mTrans.x, mTrans.y, mTrans.z = center mRot = trans.transform.rotation mRot.x, mRot.y, mRot.z, mRot.w = rotation transforms.append(trans) msg.transforms = transforms pubtf.publish(msg)
def publishLatchTransform(self, arm): if arm == 'left': self.pub_tf_left = rospy.Publisher("/tf", tfMessage, queue_size=1, latch=True) else: self.pub_tf_right = rospy.Publisher("/tf", tfMessage, queue_size=1, latch=True) f = open("/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/transform_" + arm + \ ".txt", "r") transform = f.readline().split() f.close() print(transform) # Send static link transforms msg = TransformStamped() msg.header.stamp = rospy.Time.now() msg.transform.rotation.x = float(transform[3]) msg.transform.rotation.y = float(transform[4]) msg.transform.rotation.z = float(transform[5]) msg.transform.rotation.w = float(transform[6]) msg.child_frame_id = "left_BC" msg.transform.translation.x = float(transform[0]) msg.transform.translation.y = float(transform[1]) msg.transform.translation.z = float(transform[2]) if arm == 'left': # msg.header.frame_id = "two_psm_base_link" msg.header.frame_id = "two_remote_center_link" else: msg.header.frame_id = "one_remote_center_link" while not rospy.is_shutdown(): msg.header.stamp = rospy.Time.now() self.pub_tf_right.publish([msg]) rospy.sleep(0.5)
def post_tf(self, component_instance): component_name = component_instance.blender_obj.name frame_id = self._properties[component_name]['frame_id'] child_frame_id = self._properties[component_name]['child_frame_id'] euler = mathutils.Euler((component_instance.local_data['roll'], component_instance.local_data['pitch'], component_instance.local_data['yaw'])) quaternion = euler.to_quaternion() t = TransformStamped() t.header.frame_id = frame_id t.header.stamp = rospy.Time.now() t.child_frame_id = child_frame_id t.transform.translation.x = component_instance.local_data['x'] t.transform.translation.y = component_instance.local_data['y'] t.transform.translation.z = component_instance.local_data['z'] t.transform.rotation = quaternion tfm = tfMessage([t]) for topic in self._topics: # publish the message on the correct topic if str(topic.name) == str("/tf"): topic.publish(tfm)
def pose_to_tf_msg(parent, child, position, orientation): ts = TransformStamped() ts.header.frame_id = parent ts.child_frame_id = child ts.transform.rotation = Quaternion(*orientation) ts.transform.translation = Vector3(*position) return ts
def publish_state(self, t): state_msg = TransformStamped() t_ned_imu = tft.translation_matrix(self.model.get_position()) R_ned_imu = tft.quaternion_matrix(self.model.get_orientation()) T_ned_imu = tft.concatenate_matrices(t_ned_imu, R_ned_imu) #rospy.loginfo("T_ned_imu = \n" + str(T_ned_imu)) if self.T_imu_vicon is None: # grab the static transform from imu to vicon frame from param server: frames = rospy.get_param("frames", None) ypr = frames['vicon_to_imu']['rotation'] q_vicon_imu = tft.quaternion_from_euler(*[radians(x) for x in ypr], axes='rzyx') # xyzw R_vicon_imu = tft.quaternion_matrix(q_vicon_imu) t_vicon_imu = tft.translation_matrix(frames['vicon_to_imu']['translation']) # rospy.loginfo(str(R_vicon_imu)) # rospy.loginfo(str(t_vicon_imu)) self.T_vicon_imu = tft.concatenate_matrices(t_vicon_imu, R_vicon_imu) self.T_imu_vicon = tft.inverse_matrix(self.T_vicon_imu) self.T_enu_ned = tft.euler_matrix(radians(90), 0, radians(180), 'rzyx') rospy.loginfo(str(self.T_enu_ned)) rospy.loginfo(str(self.T_imu_vicon)) #rospy.loginfo(str(T_vicon_imu)) # we have /ned -> /imu, need to output /enu -> /vicon: T_enu_vicon = tft.concatenate_matrices(self.T_enu_ned, T_ned_imu, self.T_imu_vicon ) state_msg.header.stamp = t state_msg.header.frame_id = "/enu" state_msg.child_frame_id = "/simflyer1/flyer_vicon" stt = state_msg.transform.translation stt.x, stt.y, stt.z = T_enu_vicon[:3,3] stro = state_msg.transform.rotation stro.x, stro.y, stro.z, stro.w = tft.quaternion_from_matrix(T_enu_vicon) self.pub_state.publish(state_msg)
def pack_transform(src_frame,dest_frame,trans,rot): transf = TransformStamped() transf.header.frame_id = dest_frame transf.child_frame_id = src_frame transf.transform.translation = trans transf.transform.rotation = rot return transf
def _compute_transform(self, sensor_data, cur_time): parent_frame_id = "base_link" child_frame_id = self.name t = TransformStamped() t.header.stamp = cur_time t.header.frame_id = parent_frame_id t.child_frame_id = child_frame_id # for camera we reorient it to look at the same axis as the opencv projection # in order to get easy depth cloud for RGBD camera t.transform = carla_transform_to_ros_transform( self.carla_object.get_transform()) rotation = t.transform.rotation quat = [rotation.x, rotation.y, rotation.z, rotation.w] quat_swap = tf.transformations.quaternion_from_matrix( [[0, 0, 1, 0], [-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 0, 1]]) quat = tf.transformations.quaternion_multiply(quat, quat_swap) t.transform.rotation.x = quat[0] t.transform.rotation.y = quat[1] t.transform.rotation.z = quat[2] t.transform.rotation.w = quat[3] self.process_msg_fun('tf', t)
def pack_pose(time, child, parent, matrix=None, trans=None, quat=None): if matrix is not None and (trans is None and quat is None): trans = tfs.translation_from_matrix(matrix) quat = tfs.quaternion_from_matrix(matrix) elif matrix is None and trans is not None and quat is not None: matrix = None # nothing else: print 'invalid use' t = TransformStamped() t.header.frame_id = parent t.header.stamp = time t.child_frame_id = child t.transform.translation.x = trans[0] t.transform.translation.y = trans[1] t.transform.translation.z = trans[2] quat = numpy.array(quat) quat = quat / numpy.linalg.norm(quat, ord=2) t.transform.rotation.x = quat[0] t.transform.rotation.y = quat[1] t.transform.rotation.z = quat[2] t.transform.rotation.w = quat[3] return t
def pack_transrot(translation, rotation, time, child, parent): """ :param translation: the translation of the transformtion as a tuple (x, y, z) :param rotation: the rotation of the transformation as a tuple (x, y, z, w) :param time: the time of the transformation, as a rospy.Time() :param child: child frame in tf, string :param parent: parent frame in tf, string Broadcast the transformation from tf frame child to parent on ROS topic ``"/tf"``. """ t = TransformStamped() t.header.frame_id = parent t.header.stamp = time t.child_frame_id = child t.transform.translation.x = translation[0] t.transform.translation.y = translation[1] t.transform.translation.z = translation[2] rotation = np.array(rotation) rotation = rotation / np.linalg.norm(rotation, ord=2) t.transform.rotation.x = rotation[0] t.transform.rotation.y = rotation[1] t.transform.rotation.z = rotation[2] t.transform.rotation.w = rotation[3] return t
def frame_tf(msg): br = tf2_ros.TransformBroadcaster() t = TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "ned" t.child_frame_id = "vicon" t.transform.translation.x=0 t.transform.translation.y=0 t.transform.translation.z=0 q = tf.transformations.quaternion_from_euler(math.pi, 0, -53.0*math.pi/180) t.transform.rotation.x = q[0] t.transform.rotation.y = q[1] t.transform.rotation.z = q[2] t.transform.rotation.w = q[3] br.sendTransform(t) t2 = TransformStamped() t2.header.stamp = rospy.Time.now() t2.header.frame_id = "vicon" t2.child_frame_id = "uav" t2.transform.translation.x = msg.pose.position.x t2.transform.translation.y = msg.pose.position.y t2.transform.translation.z = msg.pose.position.z t2.transform.rotation.x = msg.pose.orientation.x t2.transform.rotation.y = msg.pose.orientation.y t2.transform.rotation.z = msg.pose.orientation.z t2.transform.rotation.w = msg.pose.orientation.w br.sendTransform(t2)
def main(): b = tf2_ros.Buffer() t = TransformStamped() t.transform.translation.x = 1 t.transform.rotation.x = 1 t.header.stamp = rospy.Time(2.0) t.header.frame_id = 'a' t.child_frame_id = 'b' b.set_transform(t, 'eitan_rocks') print b.lookup_transform('a','b', rospy.Time(2.0), rospy.Duration(2.0)) v = PointStamped() v.header.stamp = rospy.Time(2) v.header.frame_id = 'a' v.point.x = 1 v.point.y = 2 v.point.z = 3 print b.transform(v, 'b') v = Vector3Stamped() v.header.stamp = rospy.Time(2) v.header.frame_id = 'a' v.vector.x = 1 v.vector.y = 2 v.vector.z = 3 print b.transform(v, 'b') v = PoseStamped() v.header.stamp = rospy.Time(2) v.header.frame_id = 'a' v.pose.position.x = 1 v.pose.position.y = 2 v.pose.position.z = 3 v.pose.orientation.x = 1 print b.transform(v, 'b')
def tag_callback(self, msg_tag): # Listen for the transform of the tag in the world avg = PoseAverage.PoseAverage() for tag in msg_tag.detections: try: Tt_w = self.tfbuf.lookup_transform(self.world_frame, "tag_{id}".format(id=tag.id), rospy.Time(), rospy.Duration(1)) Mtbase_w=self.transform_to_matrix(Tt_w.transform) Mt_tbase = tr.concatenate_matrices(tr.translation_matrix((0,0,0.17)), tr.euler_matrix(0,0,np.pi)) Mt_w = tr.concatenate_matrices(Mtbase_w,Mt_tbase) Mt_r=self.pose_to_matrix(tag.pose) Mr_t=np.linalg.inv(Mt_r) Mr_w=np.dot(Mt_w,Mr_t) Tr_w = self.matrix_to_transform(Mr_w) avg.add_pose(Tr_w) self.publish_sign_highlight(tag.id) except(tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as ex: rospy.logwarn("Error looking up transform for tag_%s", tag.id) rospy.logwarn(ex.message) Tr_w = avg.get_average() # Average of the opinions # Broadcast the robot transform if Tr_w is not None: # Set the z translation, and x and y rotations to 0 Tr_w.translation.z = 0 rot = Tr_w.rotation rotz=tr.euler_from_quaternion((rot.x, rot.y, rot.z, rot.w))[2] (rot.x, rot.y, rot.z, rot.w) = tr.quaternion_from_euler(0, 0, rotz) T = TransformStamped() T.transform = Tr_w T.header.frame_id = self.world_frame T.header.stamp = rospy.Time.now() T.child_frame_id = self.duckiebot_frame self.pub_tf.publish(TFMessage([T])) self.lifetimer = rospy.Time.now()
def get_tree(frame, depth, step, frame_name, bush=False, idepth=0): branch_factor = get_tree.branch_factor twig_height = get_tree.twig_height tfs = [] F = kdl.Frame() for i in range(branch_factor): F.M = kdl.Rotation.RotZ(2.0*pi*i/branch_factor) F.p = F.M*kdl.Vector(step, 0, twig_height) + bush*frame.p msg = TransformStamped() msg.header.stamp = rospy.Time.now() msg.transform.translation = Vector3(*F.p) msg.transform.rotation = Quaternion(*(F.M.GetQuaternion())) fr_name = frame_name +str(i) msg.child_frame_id = fr_name msg.header.frame_id = fr_name[:-(1 + idepth*bush)] tfs.append(msg) #recurse if depth > 1: tfs.extend(get_tree(F, depth - 1, step / 2.0, fr_name, bush, idepth + 1)) return tfs
def sendTransform(self, translation, rotation, time, child, parent): """ :param translation: the translation of the transformtion as a tuple (x, y, z) :param rotation: the rotation of the transformation as a tuple (x, y, z, w) :param time: the time of the transformation, as a rospy.Time() :param child: child frame in tf, string :param parent: parent frame in tf, string Broadcast the transformation from tf frame child to parent on ROS topic ``"/tf"``. """ t = TransformStamped() t.header.frame_id = parent t.header.stamp = time t.child_frame_id = child t.transform.translation.x = translation[0] t.transform.translation.y = translation[1] t.transform.translation.z = translation[2] t.transform.rotation.x = rotation[0] t.transform.rotation.y = rotation[1] t.transform.rotation.z = rotation[2] t.transform.rotation.w = rotation[3] tfm = tfMessage([t]) self.pub_tf.publish(tfm)
def _toTransform(self, my_x, my_y): transform = TransformStamped() transform.header.stamp = rospy.Time.now() transform.header.frame_id = self.camera_frame transform.child_frame_id = self.blob.name (x, y, z) = self._projectTo3d(my_x, my_y) transform.transform.translation.x = x transform.transform.translation.y = y transform.transform.translation.z = z transform.transform.rotation.w = 1.0 # If our parent frame is not the camera frame then an additional transformation is required. if self.parent_frame != self.camera_frame: point = PointStamped() point.header.frame_id = self.camera_frame point.header.stamp = rospy.Time(0) point.point.x = transform.transform.translation.x point.point.y = transform.transform.translation.y point.point.z = transform.transform.translation.z # Now we've gone from the regular camera frame to the correct parent_frame. point_transformed = self.listener.transformPoint(self.parent_frame, point) transform.header.frame_id = self.parent_frame transform.transform.translation.x = point_transformed.point.x transform.transform.translation.y = point_transformed.point.y transform.transform.translation.z = point_transformed.point.z return transform
def broadcast(self): f = open("/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/camera_frame.p", "rb") self.camera_transform = pickle.load(f) f.close() #SOMETIMES NEED TO INVERT X & Y AXES; just change from 1 to -1 and vice versa offset = tfx.transform([SKETCH_OFFSET, 0, CAP_OFFSET], [[1, 0, 0], [0, 1, 0], [0, 0, 1]]) self.camera_transform = tfx.transform(self.camera_transform).as_transform() * offset transform = tfx.inverse_tf(self.camera_transform) pt = transform.translation rot = transform.rotation msg = TransformStamped() msg.header.stamp = rospy.Time.now() msg.transform.rotation.x = rot.x msg.transform.rotation.y = rot.y msg.transform.rotation.z = rot.z msg.transform.rotation.w = rot.w msg.child_frame_id = "left_BC" msg.transform.translation.x = pt.x msg.transform.translation.y = pt.y msg.transform.translation.z = pt.z msg.header.frame_id = "registration_brick_right" msg.header.stamp = rospy.Time.now() print('boo') while not rospy.is_shutdown(): msg.header.stamp = rospy.Time.now() self.pub.publish([msg]) rospy.sleep(0.1)
def service_handler(self, req): #req is of type PepperCommRequest #prepare message to Pepper pepper_cmd = {} pepper_cmd['speak'] = req.speak + '\n' coord = [req.point_at.point.x, req.point_at.point.y, req.point_at.point.z] pepper_cmd['point_at'] = coord pepper_cmd['point_at_frame'] = req.point_at.header.frame_id pepper_cmd['get_pose'] = req.get_pose pepper_cmd['get_heard'] = req.get_heard #Encode into json pepper_net_string = self.enc.encode(pepper_cmd) #Send over the socket self.sock.sendall(pepper_net_string) timeout = 1.0 #In seconds start_time = time.time() while ((time.time() - start_time) < timeout): received = self.sock.recv(1024) if received != "": data = self.dec.decode(received) break else: time.sleep(0.05) rospy.loginfo('Received:') rospy.loginfo(data) #data['pose'] should be 3 floats: pos_x, pos_y, rot_z (in m and rad) in /map frame pepper_pose = data['robot_pos'] pepper_ts = TransformStamped() pepper_ts.header.frame_id = self.map_frame pepper_ts.child_frame_id = self.pepper_frame pepper_ts.transform.translation.x = pepper_pose[0] pepper_ts.transform.translation.y = pepper_pose[1] pepper_ts.transform.translation.z = 0.0 rotz = pepper_pose[2] quat = kdl.Rotation.RotZ(rotz).GetQuaternion() pepper_ts.transform.rotation.x = quat[0] pepper_ts.transform.rotation.y = quat[1] pepper_ts.transform.rotation.z = quat[2] pepper_ts.transform.rotation.w = quat[3] ans = PepperCommResponse() ans.heard = data['robot_heard'] ans.pepper_pose = pepper_ts return ans
def _publish_tf(self, stamp): # check that we know which frames we need to publish from if self.map is None or self.base_frame is None: rospy.loginfo('Not publishing until map and odometry frames found') return # calculate the mean position x = np.mean([p.x for p in self.particles]) y = np.mean([p.y for p in self.particles]) theta = np.mean([p.theta for p in self.particles]) #TODO - wraparound # map to base_link map2base_frame = PyKDL.Frame( PyKDL.Rotation.Quaternion(*transformations.quaternion_from_euler(0, 0, theta)), PyKDL.Vector(x,y,0) ) odom2base_frame = tf2_kdl.transform_to_kdl(self.tf_buffer.lookup_transform( target_frame=self.odom_frame, source_frame=self.base_frame, time=stamp, timeout=rospy.Duration(4.0) )) # derive frame according to REP105 map2odom = map2base_frame * odom2base_frame.Inverse() br = tf2_ros.TransformBroadcaster() t = TransformStamped() t.header.stamp = stamp t.header.frame_id = self.map.frame t.child_frame_id = self.odom_frame t.transform.translation = Vector3(*map2odom.p) t.transform.rotation = Quaternion(*map2odom.M.GetQuaternion()) br.sendTransform(t) # for Debugging if False: t.header.stamp = stamp t.header.frame_id = self.map.frame t.child_frame_id = self.base_frame+"_old" t.transform.translation = Vector3(*map2base_frame.p) t.transform.rotation = Quaternion(*map2base_frame.M.GetQuaternion()) br.sendTransform(t) if self.publish_cloud: msg = PoseArray( header=Header(stamp=stamp, frame_id=self.map.frame), poses=[ Pose( position=Point(p.x, p.y, 0), orientation=Quaternion(*transformations.quaternion_from_euler(0, 0, p.theta)) ) for p in self.particles ] ) self.pose_array.publish(msg)
def transform_helper(vec3, frame): pos = TransformStamped() pos.child_frame_id = frame pos.header = Header() pos.transform = Transform() pos.transform.translation = vec3 return pos
def timer_tf_pub(self, event): t = TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = self.world_frame t.child_frame_id = self.target_frame t.transform.translation = self.marker_pose.position t.transform.rotation = self.marker_pose.orientation self.pub_tf.sendTransform(t)
def publish(self, data): q = Quaternion() q.x = 0 q.y = 0 q.z = data[6] q.w = data[7] odomMsg = Odometry() odomMsg.header.frame_id = self._odom odomMsg.header.stamp = rospy.get_rostime() odomMsg.child_frame_id = self._baseLink odomMsg.pose.pose.position.x = data[0] odomMsg.pose.pose.position.y = data[1] odomMsg.pose.pose.position.z = 0 odomMsg.pose.pose.orientation = q if self._firstTimePub: self._prevOdom = odomMsg self._firstTimePub = False return velocity = Twist() deltaTime = odomMsg.header.stamp.to_sec() - self._prevOdom.header.stamp.to_sec() yaw, pitch, roll = euler_from_quaternion( [odomMsg.pose.pose.orientation.w, odomMsg.pose.pose.orientation.x, odomMsg.pose.pose.orientation.y, odomMsg.pose.pose.orientation.z]) prevYaw, prevPitch, prevRollprevYaw = euler_from_quaternion( [self._prevOdom.pose.pose.orientation.w, self._prevOdom.pose.pose.orientation.x, self._prevOdom.pose.pose.orientation.y, self._prevOdom.pose.pose.orientation.z]) if deltaTime > 0: velocity.linear.x = (data[8] / deltaTime) deltaYaw = yaw - prevYaw # rospy.loginfo("yaw: %f\t\tpevYaw: %f\t\tdeltaYaw: %f" % (yaw,prevYaw,deltaYaw)) if deltaYaw > math.pi: deltaYaw -= 2 * math.pi elif deltaYaw < -math.pi: deltaYaw += 2 * math.pi if deltaTime > 0: velocity.angular.z = -(deltaYaw / deltaTime) # rospy.loginfo("deltaYaw after check: %f\t\t angular: %f" % (deltaYaw, velocity.angular.z)) odomMsg.twist.twist = velocity self._prevOdom = odomMsg traMsg = TransformStamped() traMsg.header.frame_id = self._odom traMsg.header.stamp = rospy.get_rostime() traMsg.child_frame_id = self._baseLink traMsg.transform.translation.x = data[0] traMsg.transform.translation.y = data[1] traMsg.transform.translation.z = 0 traMsg.transform.rotation = q self._pub.publish(odomMsg) self._broadCase.sendTransformMessage(traMsg)
def read_nav_csv(fname, origin): ''' Reads the nav.csv , converts into a local coordinate frame and returns tf msgs ''' msgs = [] DEG2RAD = m.pi/180.0 with open(fname, 'r') as f: # example data # 1354679075.295000000,6248548.85357,332949.839026,-41.4911136152,-0.00740605127066,-0.0505019649863,1.95254564285 # a Nx7 matrix containing pose [time N E D R P Y] UTM cartesian coordinate system for i, line in enumerate(f): words = line.split(',') time = words[0] [secs, nsecs] = map(int, time.split('.')) x = float(words[1]) y = float(words[2]) z = float(words[3]) R = float(words[4]) P = float(words[5]) Y = float(words[6]) # convert to local coordinate frame, located at the origin x = x - origin[0] y = y - origin[1] z = z - origin[2] # create TF msg tf_msg = tfMessage() geo_msg = TransformStamped() geo_msg.header.stamp = Time(secs,nsecs) geo_msg.header.seq = i geo_msg.header.frame_id = "map" geo_msg.child_frame_id = "body" geo_msg.transform.translation.x = x geo_msg.transform.translation.y = y geo_msg.transform.translation.z = z angles = tf.transformations.quaternion_from_euler(R,P,Y) geo_msg.transform.rotation.x = angles[0] geo_msg.transform.rotation.y = angles[1] geo_msg.transform.rotation.z = angles[2] geo_msg.transform.rotation.w = angles[3] # rviz frame geo_msg2 = TransformStamped() geo_msg2.header.stamp = Time(secs,nsecs) geo_msg2.header.seq = i geo_msg2.header.frame_id = "map_rviz" geo_msg2.child_frame_id = "map" geo_msg2.transform.translation.x = 0 geo_msg2.transform.translation.y = 0 geo_msg2.transform.translation.z = 0 angles = tf.transformations.quaternion_from_euler(DEG2RAD*180, 0, 0) # ax, ay, az geo_msg2.transform.rotation.x = angles[0] geo_msg2.transform.rotation.y = angles[1] geo_msg2.transform.rotation.z = angles[2] geo_msg2.transform.rotation.w = angles[3] # push all transformations tf_msg.transforms.append(geo_msg) tf_msg.transforms.append(geo_msg2) msgs.append(tf_msg) return msgs
def test_fromTf(self): transformer = Transformer(True, rospy.Duration(10.0)) m = TransformStamped() m.header.frame_id = 'wim' m.child_frame_id = 'james' m.transform.translation.x = 2.71828183 m.transform.rotation.w = 1.0 transformer.setTransform(m) b = pm.fromTf(transformer.lookupTransform('wim', 'james', rospy.Time(0)))
def publish_odometry(self): quat = tf.transformations.quaternion_from_euler(0, 0, self.yaw) quaternion = Quaternion() quaternion.x = quat[0] quaternion.y = quat[1] quaternion.z = quat[2] quaternion.w = quat[3] if self.last_time is None: self.last_time = rospy.Time.now() vx = self.vx #(msg->twist.twist.linear.x) *trans_mul_; vy = self.vy #msg->twist.twist.linear.y; vth = self.vyaw #(msg->twist.twist.angular.z) *rot_mul_; current_time = rospy.Time.now() dt = (current_time - self.last_time).to_sec() delta_x = (vx * math.cos(self.yaw) - vy * math.sin(self.yaw)) * dt delta_y = (vx * math.sin(self.yaw) + vy * math.cos(self.yaw)) * dt delta_th = vth * dt self.x += delta_x self.y += delta_y self.yaw += delta_th odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "odom" odom.child_frame_id = "base_link" odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0.0 odom.pose.pose.orientation = quaternion odom.twist.twist.linear.x = vx odom.twist.twist.linear.y = vy odom.twist.twist.angular.z = vth self.odom_pub.publish(odom) transform_stamped = TransformStamped() transform_stamped.header = odom.header transform_stamped.transform.translation.x = self.x transform_stamped.transform.translation.y = self.y transform_stamped.transform.translation.z = 0.0 transform_stamped.transform.rotation = quaternion #self.tfbr.sendTransform(transform_stamped) self.tfbr.sendTransform((self.x, self.y, 0.0), (quat[0], quat[1], quat[2], quat[3]), rospy.Time.now(), "odom", "base_link") self.last_time = current_time
def to_ros_tf(self): t = TransformStamped() t.header.stamp = rospy.Time.from_sec(self.stamp) t.header.frame_id = self.ros_frame_id t.child_frame_id = "" tran = t.transform.translation tran.x, tran.y, tran.z = self.position.as_numpy() rot = t.transform.rotation rot.x, rot.y, rot.z, rot.w = self.quaternion.as_numpy return t
def get_navigation_tf(self, navigation_pose): navigation_tf = TransformStamped() navigation_tf.header.frame_id = "/map" navigation_tf.header.stamp = rospy.Time.now() navigation_tf.child_frame_id = "/odom" navigation_tf.transform.translation .x = navigation_pose.x navigation_tf.transform.translation .y = navigation_pose.y navigation_tf.transform.rotation = self.get_ros_quaternion( almath.Quaternion_fromAngleAndAxisRotation(navigation_pose.theta, 0, 0, 1)) return navigation_tf
def run(self): marker_map = rospy.get_param('~marker_map', {}) args = { 'device': rospy.get_param('~device'), 'marker_map': marker_map, 'callback_global': self.callback_global, 'callback_local': self.callback_local, } parameters = self.get_options() self.fixed_frame_id = rospy.get_param('~fixed_frame_id', 'map') self.robot_frame_id = rospy.get_param('~robot_frame_id', 'base_link') self.stargazer_frame_id = rospy.get_param('~stargazer_frame_id', 'stargazer') self.map_frame_prefix = rospy.get_param('~map_frame_prefix', 'stargazer/map_') self.marker_frame_prefix = rospy.get_param('~marker_frame_prefix', 'stargazer/marker_') self.covariance = rospy.get_param('~covariance', None) if self.covariance is None: raise Exception('The "covariance" parameter is required.') elif len(self.covariance) != 36: raise Exception('The "covariance" parameter must be a 36 element vector.') # Publish static TF frames for the Stargazer map. stamp_now = rospy.Time.now() map_tf_msg = TFMessage() for marker_id, Tmap_marker in marker_map.iteritems(): marker_tf_msg = TransformStamped() marker_tf_msg.header.stamp = stamp_now marker_tf_msg.header.frame_id = self.fixed_frame_id marker_tf_msg.child_frame_id = self.map_frame_prefix + marker_id marker_tf_msg.transform = matrix_to_transform(Tmap_marker) map_tf_msg.transforms.append(marker_tf_msg) self.tf_static_pub = rospy.Publisher('tf_static', TFMessage, latch=True) self.tf_static_pub.publish(map_tf_msg) # Start publishing Stargazer data. with StarGazer(**args) as stargazer: # The StarGazer might be streaming data. Turn off streaming mode. stargazer.stop_streaming() # Set all parameters, possibly to their default values. This is the # safest option because the parameters can be corrupted when the # StarGazer is powered off. for name, value in parameters.iteritems(): stargazer.set_parameter(name, value) # Start streaming. ROS messages will be published in callbacks. stargazer.start_streaming() rospy.spin() # Stop streaming. Try to clean up after ourselves. stargazer.stop_streaming()
def update_tf_tree(self): t = TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "world" t.child_frame_id = self.child_frame t.transform.translation.x = self.point.x t.transform.translation.y = self.point.y t.transform.translation.z = self.depth t.transform.rotation = self.quaternion self.br.sendTransform(t)
def vicon_timer_callback(self, event): msg = TransformStamped() msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_ref_frame_id msg.child_frame_id = self.tf_tracked_frame_id msg.transform.translation.x, msg.transform.translation.y, msg.transform.translation.z = self.dummy_position (msg.transform.rotation.x, msg.transform.rotation.y, msg.transform.rotation.z, msg.transform.rotation.w) = self.dummy_orientation self.pub.publish(msg) if self.enable_tf_broadcast: self.tfb.sendTransform(self.dummy_position, self.dummy_orientation, rospy.Time.now(), self.tf_tracked_frame_id, self.tf_ref_frame_id)
def _reverse_tf(tanslation, euler): ''' # 输入二维码在相机坐标系下的坐标,输出相机base_footprint在二维码坐标系下的坐标 :param tanslation: :param euler: :return: t, euler_angle ''' transform = tf.Transformer(True) m = TransformStamped() m.header.frame_id = 'camera' m.child_frame_id = 'qr_code' m.transform.translation.x = tanslation[0] m.transform.translation.y = tanslation[1] m.transform.translation.z = tanslation[2] quaternion = tf.transformations.quaternion_from_euler(*euler) m.transform.rotation.x = quaternion[0] m.transform.rotation.y = quaternion[1] m.transform.rotation.z = quaternion[2] m.transform.rotation.w = quaternion[3] transform.setTransform(m) m.header.frame_id = 'base_footprint' m.child_frame_id = 'camera' m.transform.translation.x = 0.88 m.transform.translation.y = 0.72 m.transform.translation.z = 0 quaternion = tf.transformations.quaternion_from_euler(0, 0, -1.57) m.transform.rotation.x = quaternion[0] m.transform.rotation.y = quaternion[1] m.transform.rotation.z = quaternion[2] m.transform.rotation.w = quaternion[3] transform.setTransform(m) t, q = transform.lookupTransform('qr_code', 'base_footprint', rospy.Time(0)) euler_angle = tf.transformations.euler_from_quaternion(q) return t, euler_angle
def _broadcastOdom(self): """ INTERNAL METHOD, updates and broadcasts the odometry by broadcasting based on the robot's base tranform """ # Send Transform odom x, y, theta = self.virtual_pepper.getPosition() odom_trans = TransformStamped() odom_trans.header.frame_id = "odom" odom_trans.child_frame_id = "base_link" odom_trans.header.stamp = rospy.get_rostime() odom_trans.transform.translation.x = x odom_trans.transform.translation.y = y odom_trans.transform.translation.z = 0 quaternion = pybullet.getQuaternionFromEuler([0, 0, theta]) odom_trans.transform.rotation.x = quaternion[0] odom_trans.transform.rotation.y = quaternion[1] odom_trans.transform.rotation.z = quaternion[2] odom_trans.transform.rotation.w = quaternion[3] self.transform_broadcaster.sendTransform(odom_trans) # Set up the odometry odom = Odometry() odom.header.stamp = rospy.get_rostime() odom.header.frame_id = "odom" odom.pose.pose.position.x = x odom.pose.pose.position.y = y odom.pose.pose.position.z = 0.0 odom.pose.pose.orientation = odom_trans.transform.rotation odom.child_frame_id = "base_link" [vx, vy, vz], [wx, wy, wz] = pybullet.getBaseVelocity( self.virtual_pepper.robot_model, self.virtual_pepper.getPhysicsClientId()) odom.twist.twist.linear.x = vx odom.twist.twist.linear.y = vy odom.twist.twist.angular.z = wz self.odom_pub.publish(odom)
def __init__(self): #Subscribers self.image_sub = rospy.Subscriber('/ardrone/bottom/image_raw', Image, self.image_callback) self.sub_vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre', TransformStamped, self.update_quadrotor_state) #Global variables self.image = Image self.bridge = CvBridge() self.quad_state = TransformStamped() self.T_body2vicon = np.identity(4) #wait for msg for initiating processing rospy.wait_for_message('/ardrone/bottom/image_raw', Image) #time flags for detecting when bag file is done publishing self.current_time = rospy.Time.now().to_sec() self.last_msg_time = rospy.Time.now().to_sec() #Define landmark locations self.landmarks = np.array([[4.32, 8.05], [0.874, 5.49], [7.68, 4.24], [4.27, 1.23]]) #list of self.landmark_prev_save_rad = [ float('inf'), float('inf'), float('inf'), float('inf') ] self.save_radius = 0.5 #pose graph self.G = {} #load images for classification img1 = cv2.imread('cn_tower.png') img2 = cv2.imread('casa_loma.png') img3 = cv2.imread('nathan_philips_square.png') img4 = cv2.imread('princes_gates.png') self.landmark_ref_images = [img1, img2, img3, img4] self.landmark_names = [ 'cn_tower', 'casa_loma', 'nathan_philips_square', 'princes_gates' ] #Initialize ransac self.ransac = RANSAC()
def timer_callback(self, event): if self.last_recieved_stamp is None: return cmd = Odometry() cmd.header.stamp = self.last_recieved_stamp cmd.header.frame_id = 'map' cmd.child_frame_id = 'base_link' cmd.pose.pose = self.last_received_pose cmd.twist.twist = self.last_received_twist self.pub_odom.publish(cmd) if self.last_x is None: self.last_x = cmd.pose.pose.position.x self.last_y = cmd.pose.pose.position.y self.last_pos = cmd.pose.pose.position p = Pose() p.position.x = cmd.pose.pose.position.x p.position.y = cmd.pose.pose.position.y p.position.z = cmd.pose.pose.position.z # p.x += random.uniform(-0.2,0.2) # p.y += random.uniform(-0.2,0.2) dist = math.sqrt((self.last_pos.x - p.position.x)**2 + (self.last_pos.y - p.position.y)**2) self.last_pos.x = cmd.pose.pose.position.x self.last_pos.y = cmd.pose.pose.position.y roll, pitch, yaw = tf.transformations.euler_from_quaternion([ cmd.pose.pose.orientation.x, cmd.pose.pose.orientation.y, cmd.pose.pose.orientation.z, cmd.pose.pose.orientation.w ]) # dist=dist*1.1 # yaw = yaw*1.05 p.position.x = self.last_x + math.cos(yaw) * dist p.position.y = self.last_y + math.sin(yaw) * dist self.last_x = p.position.x self.last_y = p.position.y _tf = TransformStamped(header=Header(frame_id=cmd.header.frame_id, stamp=cmd.header.stamp), child_frame_id=cmd.child_frame_id, transform=Transform( translation=p.position, rotation=cmd.pose.pose.orientation)) self.tf_pub.sendTransform(_tf)
def __init__(self): # use tf2 buffer to access transforms between existing frames in tf tree self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer) self.tf_br = tf2_ros.TransformBroadcaster() # subscribers and publishers self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.scan_cb, queue_size=1) self.map_pub = rospy.Publisher('/map', OccupancyGrid, queue_size=1) # attributes width = int(MAP_DIM[0] / CELL_SIZE) height = int(MAP_DIM[1] / CELL_SIZE) self.log_odds = np.zeros((width, height)) self.np_map = np.ones( (width, height), dtype=np.uint8) * -1 # -1 for unknown self.map_msg = OccupancyGrid() self.map_msg.info = MapMetaData() self.map_msg.info.resolution = CELL_SIZE self.map_msg.info.width = width self.map_msg.info.height = height # transforms self.base_link_scan_tf = self.tf_buffer.lookup_transform( 'base_link', 'base_scan', rospy.Time(0), rospy.Duration(2.0)) odom_tf = self.tf_buffer.lookup_transform( 'odom', 'base_link', rospy.Time(0), rospy.Duration(2.0)).transform # set origin to center of map rob_to_mid_origin_tf_mat = np.eye(4) rob_to_mid_origin_tf_mat[0, 3] = -width / 2 * CELL_SIZE rob_to_mid_origin_tf_mat[1, 3] = -height / 2 * CELL_SIZE odom_tf_mat = tf_to_tf_mat(odom_tf) self.map_msg.info.origin = convert_tf_to_pose( tf_mat_to_tf(odom_tf_mat.dot(rob_to_mid_origin_tf_mat))) # map to odom broadcaster self.map_odom_timer = rospy.Timer(rospy.Duration(0.1), self.broadcast_map_odom) self.map_odom_tf = TransformStamped() self.map_odom_tf.header.frame_id = 'map' self.map_odom_tf.child_frame_id = 'odom' self.map_odom_tf.transform.rotation.w = 1.0 rospy.spin()
def main(): rospy.init_node("pbvs_object_placement") urdf_xml_string = rospy.get_param("robot_description") srdf_xml_string = rospy.get_param("robot_description_semantic") controller_commander = ControllerCommander() #planner = Planner(controller_commander, urdf_xml_string, srdf_xml_string) transform_fname = sys.argv[1] camera_image_topic = sys.argv[2] camera_trigger_topic = sys.argv[3] camera_info_topic = sys.argv[4] tf_listener = PayloadTransformListener() desired_transform_msg = TransformStamped() with open(transform_fname, 'r') as f: transform_yaml = yaml.load(f) genpy.message.fill_message_args(desired_transform_msg, transform_yaml) desired_transform = rox_msg.msg2transform(desired_transform_msg) markers = get_all_payload_markers() fixed_marker = markers[desired_transform.parent_frame_id] payload_marker = markers[desired_transform.child_frame_id] aruco_dict = get_aruco_dictionary(fixed_marker) camera_info = get_camera_info(camera_info_topic) time.sleep(1) dx = np.array([10000] * 6) while True: img = receive_image(camera_image_topic, camera_trigger_topic) target_pose = compute_step_gripper_target_pose(img, fixed_marker, payload_marker, desired_transform, \ camera_info, aruco_dict, np.array([0.2]*6), tf_listener) plan = planner.trajopt_plan(target_pose, json_config_name="panel_pickup") controller_commander.set_controller_mode( controller_commander.MODE_HALT, 1, [], []) controller_commander.set_controller_mode( controller_commander.MODE_AUTO_TRAJECTORY, 1, [], []) controller_commander.execute_trajectory(plan)
def __init__(self, node_name): """Wheel Encoder Node This implements basic functionality with the wheel encoders. """ # Initialize the DTROS parent class super(EncoderLocalizationNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC) self.veh_name = os.environ['VEHICLE_NAME'] # State variable for the robot self.pose = Pose2D(0.27,0.0,np.pi) # Initial state given arbitrarily # Transform that defines robot state self.current_state = TransformStamped() self.current_state.header.frame_id = 'map' self.current_state.child_frame_id = 'encoder_baselink' self.current_state.transform.translation.z = 0.0 # We operate in a 2D world # Distance traveled by each wheel computed using encoder data self.d_left = 0.0 self.d_right = 0.0 # Variables to account for previous step self.prev_left = 0.0 self.prev_right = 0.0 # To account for first received message self.first_message_left = True self.first_message_right = True self.initial_ticks_left = 0.0 self.initial_ticks_right = 0.0 # Get static parameters self._radius = rospy.get_param('/' + self.veh_name + '/kinematics_node/radius', 0.031) self._baseline = rospy.get_param('/' + self.veh_name + '/kinematics_node/baseline', 0.1) self._resolution = 135.0 # Subscribing to the wheel encoders self.sub_encoder_ticks_left = rospy.Subscriber('/' + self.veh_name + '/left_wheel_encoder_node/tick',WheelEncoderStamped,callback=self.cb_encoder_data,callback_args='left') self.sub_encoder_ticks_right = rospy.Subscriber('/' + self.veh_name + '/right_wheel_encoder_node/tick',WheelEncoderStamped,callback=self.cb_encoder_data,callback_args='right') # Publishers self.pub_robot_pose_tf = rospy.Publisher('~encoder_baselink_transform',TransformStamped,queue_size=1) self.tfBroadcaster = tf.TransformBroadcaster(queue_size=1) # Define timer to publish messages at a 30 Hz frequency self.pub_timer = rospy.Timer(rospy.Duration(1.0/30.0), self.publish_transform) self.log("Initialized")
def __init__(self, dispName="ideal"): super(pureOdometry, self).__init__(displayName=dispName) self.mset = MotionCategorySettings() totalSeconds = 1 fps = 1 / 15.0 nFrames = int(totalSeconds / fps) self.newPoseVertex() motions = genStraightTransform(self.mset["Medium"], nFrames) for f in motions: poseID = self.newPoseVertex() Rtheta = f[0] C = f[1] print(poseID, C) q = quaternion_from_euler(radians(Rtheta[0]), radians(Rtheta[1]), radians(Rtheta[2]), 'szxy') latestPose = TransformStamped() latestPose.transform.translation.x = C[0, 0] latestPose.transform.translation.y = C[1, 0] latestPose.transform.translation.z = C[2, 0] latestPose.transform.rotation.x = q[0] latestPose.transform.rotation.y = q[1] latestPose.transform.rotation.z = q[2] latestPose.transform.rotation.w = q[3] self.nodes[poseID]["msg"].transform = latestPose.transform # self.nodes[poseID][] # Rtheta,C=self.input[m][0],self.input[m][1] # q=quaternion_from_euler(radians(Rtheta[0]), # radians(Rtheta[1]), # radians(Rtheta[2]), # 'szxy') # latestPose=TransformStamped() # latestPose.header.frame_id=self.frameNames[m] # latestPose.child_frame_id=self.frameNames[m+1] # latestPose.transform.translation.x=C[0,0] # latestPose.transform.translation.y=C[1,0] # latestPose.transform.translation.z=C[2,0] # latestPose.transform.rotation.x=q[0] # latestPose.transform.rotation.y=q[1] # latestPose.transform.rotation.z=q[2] # latestPose.transform.rotation.w=q[3] print(len(self.nodes()))
def __init__(self): rospy.init_node("Odomer") self.odom_publisher = rospy.Publisher("odom",Odometry,queue_size = 1) self.odom_broadcast = TransformBroadcaster() self.can_listener = rospy.Subscriber("raw_odom", Vector3, callback=self.handle) self.odom_tf = TransformStamped() self.odom_msg = Odometry() #last_rec -> en son alinan ilerleme miktari self.last_rec = 0 #hesaplanan x, y ve theta degerleri self.x = 0 self.y = 0 self.th = 0
def get_relative_coordinate(parent, child): tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) trans = TransformStamped() while not rospy.is_shutdown(): try: trans = tfBuffer.lookup_transform(parent, child, rospy.Time().now(), rospy.Duration(4.0)) break except (tf2_ros.ExtrapolationException): pass return trans.transform
def __init__(self): self.pub_land = rospy.Publisher('/ardrone/land', Empty, queue_size=1) self.pub_takeoff = rospy.Publisher('/ardrone/takeoff', Empty, queue_size=1) self.pub_vel = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32) self.vicon_topic = '/vicon/ARDroneCarre/ARDroneCarre' self.bottom_image_topic = '/ardrone/bottom/image_raw' self._vicon_msg = TransformStamped() self._bottom_image_msg = Image() rospy.Subscriber(self.vicon_topic, TransformStamped, callback=self._vicon_callback) rospy.Subscriber(self.bottom_image_topic, Image, callback=self._bottom_image_callback)
def transform_pose(pose, trans, rot): t = TransformStamped() t.transform.translation.x = trans[0] t.transform.translation.y = trans[1] t.transform.translation.z = trans[2] t.transform.rotation.x = rot[0] t.transform.rotation.y = rot[1] t.transform.rotation.z = rot[2] t.transform.rotation.w = rot[3] pose_stamped = PoseStamped() pose_stamped.pose = pose pose_transformed = tf2_geometry_msgs.do_transform_pose(pose_stamped, t) return pose_transformed.pose
def odom_callback(data): odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = 'odom' # 'map' odom.child_frame_id = 'base_link' # 'odom' odom.pose = data.pose odom.pose.pose.position.y = odom.pose.pose.position.y + 5.0 odom.twist = data.twist tf = TransformStamped(header=Header(frame_id=odom.header.frame_id, stamp=odom.header.stamp), child_frame_id=odom.child_frame_id, transform=Transform( translation=odom.pose.pose.position, rotation=odom.pose.pose.orientation)) odom_pub.publish(odom) tf_pub.sendTransform(tf)
def sim_pose_callback(pose_msg): orientation_msg = OrientationAnglesStamped() orientation_msg.header.stamp = pose_msg.header.stamp orientation = pose_msg.pose.orientation roll, pitch, yaw = tf.transformations.euler_from_quaternion((orientation.x, orientation.y, orientation.z, orientation.w)) orientation_msg.data.roll = roll orientation_msg.data.pitch = -pitch orientation_msg.data.yaw = (2*math.pi - yaw) % (2*math.pi) orientation_pub.publish(orientation_msg) global last_drone_position last_drone_position = pose_msg.pose.position if publish_ground_truth_orientation: transform_msg = TransformStamped() transform_msg.header.stamp = pose_msg.header.stamp transform_msg.header.frame_id = 'level_quad' transform_msg.child_frame_id = 'quad' transform_msg.transform.rotation = pose_msg.pose.orientation tf2_broadcaster.sendTransform(transform_msg) if publish_ground_truth_localization: transform_msg = TransformStamped() transform_msg.header.stamp = pose_msg.header.stamp transform_msg.header.frame_id = 'map' transform_msg.child_frame_id = 'level_quad' transform_msg.transform.translation = pose_msg.pose.position transform_msg.transform.rotation.w = 1 tf2_broadcaster.sendTransform(transform_msg) if publish_ground_truth_camera_localization: camera_pose_msg = PoseWithCovarianceStamped() camera_pose_msg.header.stamp = pose_msg.header.stamp camera_pose_msg.header.frame_id = 'map' camera_pose_msg.pose.pose.position = pose_msg.pose.position camera_pose_msg.pose.covariance[0*6+0] = 0.000001 camera_pose_msg.pose.covariance[1*6+1] = 0.000001 camera_pose_msg.pose.covariance[2*6+2] = 100.0 camera_pose_pub.publish(camera_pose_msg) if publish_ground_truth_altitude: # TODO: Make this also publish the altimeter_reading topic # Publish altimeter_pose altimeter_pose = PoseWithCovarianceStamped() altimeter_pose.header.stamp = pose_msg.header.stamp altimeter_pose.pose.pose.position.z = pose_msg.pose.position.z altimeter_pose_pub.publish(altimeter_pose) short_range_altimeter_pose_pub.publish(altimeter_pose)
def __init__(self, turning_radius, path=""): self.path = path self.odom_msg = Odometry() self.odom_trans = TransformStamped() self.odom_trans.header.frame_id = 'odom' self.odom_trans.child_frame_id = 'base_footprint' self.odom_pub = rospy.Publisher('odom', Odometry, queue_size=5) self.tfb = tf.TransformBroadcaster() self.tfl = tf.TransformListener() self.yaw = 0.0 self.odom_frame_yaw = 0.0 self.odom_pos = (0.0, 0.0) self.om_transform = ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]) self.weights = self._load_weights(turning_radius) self.acc_delta = np.zeros((1, 2))
def __init__(self): # Atributos # Para realizar un broadcast self.broadcts = tf2_ros.TransformBroadcaster() self.transform = TransformStamped() # Para realizar la escucha "listener" self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) # Subscribers rospy.Subscriber("/ar_pose_marker", numpy_msg(AlvarMarkers), self.Marker_Callback, queue_size=10)
def __init__(self, root_frame="mocap", tf_topic="tf"): rospy.init_node("csv_tf") self.tfpublisher = rospy.Publisher(tf_topic, tf.msg.tfMessage) self.pose_msg = TransformStamped() self.pose_msg.header.frame_id = root_frame self.pose_msg.child_frame_id = '' self.pose_msg.transform.translation.x = 0.0 self.pose_msg.transform.translation.y = 0.0 self.pose_msg.transform.translation.z = 0.0 self.pose_msg.transform.rotation.x = 0.0 self.pose_msg.transform.rotation.y = 0.0 self.pose_msg.transform.rotation.z = 0.0 self.pose_msg.transform.rotation.w = 1.0 self.csvLayout = [] self.loop = False self.line_range = [-1, -1]
def from_bag(self, reader: Rosbag1Reader, topic: str = "/tf", static_topic: str = "/tf_static") -> None: """ Loads the TF topics from a bagfile into the buffer, if it's not already cached. :param reader: opened bag reader (rosbags.rosbag1) :param topic: TF topic """ tf_topics = [topic] if topic not in reader.topics: raise TfCacheException( "no messages for topic {} in bag".format(topic)) # Implicitly add static TFs to buffer if present. if static_topic in reader.topics: tf_topics.append(static_topic) # Add TF data to buffer if this bag/topic pair is not already cached. for tf_topic in tf_topics: if tf_topic in self.topics and reader.path.name in self.bags: logger.debug("Using cache for topic {} from {}".format( tf_topic, reader.path.name)) continue logger.debug("Caching TF topic {} from {} ...".format( tf_topic, reader.path.name)) connections = [ c for c in reader.connections.values() if c.topic == tf_topic ] for connection, _, rawdata in reader.messages( connections=connections): msg = deserialize_cdr(ros1_to_cdr(rawdata, connection.msgtype), connection.msgtype) for tf in msg.transforms: # Convert from rosbags.typesys.types to native ROS. # Related: https://gitlab.com/ternaris/rosbags/-/issues/13 stamp = rospy.Time() stamp.secs = tf.header.stamp.sec stamp.nsecs = tf.header.stamp.nanosec tf = TransformStamped(Header(0, stamp, tf.header.frame_id), tf.child_frame_id, tf.transform) if tf_topic == static_topic: self.buffer.set_transform_static(tf, __name__) else: self.buffer.set_transform(tf, __name__) self.topics.append(tf_topic) self.bags.append(reader.path.name)
def __init__(self): """Initialize the ROSLabInterface class.""" # Publishers self.pub_vel_prop = rospy.Publisher('/aer1217_ardrone/vel_prop', MotorCommands, queue_size=300) self.model_name = 'ARDroneCarre' self.pub_vicon_data = rospy.Publisher('/vicon/{0}/{0}'.format( self.model_name), TransformStamped, queue_size=30) self.pub_estimated_state_rviz = rospy.Publisher( '/aer1217_ardrone/estimated_state_rviz', PoseStamped, queue_size=30) # Subscribers self.sub_gazebo_pose = rospy.Subscriber( '/aer1217_ardrone/gazebo_state', GazeboState, self.update_quadrotor_state) self.sub_cmd_vel = rospy.Subscriber('cmd_vel', Twist, self.update_offboard_command) # Initialize messages for publishing self.vel_prop_msg = MotorCommands() self.quadrotor_state = TransformStamped() # Run the onboard controller at 200 Hz self.onboard_loop_frequency = 200. # Create an onboard controller for calculation of the motor commands self.onboard_controller = ARDroneOnboardController() # Run this ROS node at the onboard loop frequency self.pub_prop_vel = rospy.Timer( rospy.Duration(1. / self.onboard_loop_frequency), self.update_motor_speeds) # Keep time for differentiation and integration within the controller self.old_time = rospy.get_time() self.seq = 0
def __init__(self, eye_on_hand=False, robot_base_frame=None, robot_effector_frame=None, tracking_base_frame=None, transformation=None): """ Creates a HandeyeCalibration object. :param eye_on_hand: if false, it is a eye-on-base calibration :type eye_on_hand: bool :param robot_base_frame: needed only for eye-on-base calibrations: robot base link tf name :type robot_base_frame: string :param robot_effector_frame: needed only for eye-on-hand calibrations: robot tool tf name :type robot_effector_frame: string :param tracking_base_frame: tracking system tf name :type tracking_base_frame: string :param transformation: transformation between optical origin and base/tool robot frame as tf tuple :type transformation: ((float, float, float), (float, float, float, float)) :return: a HandeyeCalibration object :rtype: easy_handeye.handeye_calibration.HandeyeCalibration """ if transformation is None: transformation = ((0, 0, 0), (0, 0, 0, 1)) self.eye_on_hand = eye_on_hand """ if false, it is a eye-on-base calibration :type: bool """ self.transformation = TransformStamped(transform=Transform(Vector3(*transformation[0]), Quaternion(*transformation[1]))) """ transformation between optical origin and base/tool robot frame :type: geometry_msgs.msg.TransformedStamped """ # tf names if self.eye_on_hand: self.transformation.header.frame_id = robot_effector_frame else: self.transformation.header.frame_id = robot_base_frame self.transformation.child_frame_id = tracking_base_frame
def copy_transform(self, transform, child_frame_id=None): copied_tf = TransformStamped() copied_tf.header = transform.header if child_frame_id: copied_tf.child_frame_id = child_frame_id else: copied_tf.child_frame_id = transform.child_frame_id copied_tf.transform = transform.transform copied_tf.header.stamp = rospy.Time.now() return copied_tf
def __init__(self): super(NeatoNode, self).__init__('neato_botvac') self.bot = BotvacDriver('/dev/ttyACM0', callbacks=BotvacDriverCallbacks( encoders=self.encoders_cb, battery=self.battery_cb, accel=self.accel_cb, scan=self.scan_cb)) self.scan_pub = self.create_publisher(LaserScan, 'scan', 10) self.battery_pub = self.create_publisher(BatteryState, 'battery', 2) self.odom_pub = self.create_publisher(Odometry, 'odom', 10) self.tf_pub = self.create_publisher(TFMessage, 'tf', 10) self.imu_pub = self.create_publisher(Imu, 'imu', 10) self.cmd_vel = (0, 0) self.last_cmd_vel = self.get_clock().now() self.cmd_vel_timeout = Duration(seconds=0.2) self.cmd_sub = self.create_subscription(Twist, 'cmd_vel', self.cmd_vel_cb, 10) self.scan_msg = LaserScan(angle_min=0.0, angle_max=deg_to_rad(359), angle_increment=deg_to_rad(1.0), time_increment=0.0, scan_time=0., range_min=0.2, range_max=5.0) self.scan_msg.header.frame_id = 'laser_link' self.battery_msg = BatteryState() self.battery_msg.header.frame_id = 'base_link' self.imu_msg = Imu( orientation_covariance=[0, 0, 0, 0, 0, 0, 0, 0, 0], angular_velocity_covariance=[-1, 0, 0, 0, 0, 0, 0, 0, 0], linear_acceleration_covariance=[0, 0, 0, 0, 0, 0, 0, 0, 0], ) self.imu_msg.header.frame_id = 'base_link' self.odom_to_base_tf = TransformStamped(header=Header(frame_id='odom'), child_frame_id='base_link') self.odometer = Odometer(self.get_clock(), self.bot.base_width) self.tf_timer = self.create_timer(0.025, self.pub_tf) self.scan_timer = self.create_timer(0.2, self.bot.requestScan) self.battery_timer = self.create_timer(1, self.bot.requestBattery) self.encoder_timer = self.create_timer(0.1, self.bot.requestEncoders) self.accel_timer = self.create_timer(0.1, self.bot.requestAccel) self.send_cmd_timer = self.create_timer(0.1, self.send_cmd_vel)
def __init__(self): rospy.init_node('ZYang2_3002_map') self.rate = rospy.Rate(10) self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) self.tf_broadcaster = tf2_ros.TransformBroadcaster() self.nav_subscriber = rospy.Subscriber('/map', OccupancyGrid, self.map_callback) self.mod_map = rospy.Publisher('/mm', GridCells, queue_size=10) self.map = None self.map_dummy = GridCells() self.map_dummy.header.frame_id = 'odom' self.map_dummy.cell_height = 0.0 self.map_dummy.cell_width = 0.0 self.map_dummy.cells = [] self.map_trans = TransformStamped() self.map_trans.transform.rotation.w = 1.0 self.now_grid = [0, 0] self.goal_grid = [0, 0] self.h_matrix = None self.g_matrix = None while not rospy.is_shutdown(): if self.map is not None: try: self.map_trans.child_frame_id = 'odom' self.map_trans.header.frame_id = 'map' self.map_trans.header.stamp = rospy.Time.now() self.tf_broadcaster.sendTransform(self.map_trans) self.mod_map.publish(self.map_dummy) except Exception as e: print e self.rate.sleep() continue try: self.update_now_grid() self.update_goal_grid() except Exception as e: print e self.rate.sleep()
def _handle_trigger(self, trigg_conf): trigg_conf['robot_ns'] trig_type = trigg_conf['trig_callback'] save_data = [] if trig_type == "joint_save": rospy.logdebug('joint_save') save_data = [trigg_conf['joint_data']] elif trig_type == "joint_pose_save": joint_data = trigg_conf['joint_data'] rospy.logdebug('joint_pose_save') save_data = [joint_data, TransformStamped()] # , tf_data] elif trig_type == "joint_dmp_save": rospy.logdebug('joint_dmp_save') traj_dur = traj_duration_sec(self._joint_traj) if traj_dur < 1: rospy.logwarn("Trajectory too short to store") else: dmp_request = EncodeTrajectoryToDMPRequest() dmp_request.demonstratedTrajectory = self._joint_traj dmp_request.dmpParameters.N = trigg_conf['joint_dmp_N'] result_dmp = self._dmp_ecoder.call(dmp_request).encodedDMP save_data = [result_dmp] self._joint_traj = [] else: rospy.logerr("Unknown saving type !!") rospy.logerr("trig_type = {}".format(trig_type)) # self.status_report['status'] = "Error. See terminal" for data in save_data: now = datetime.now() entry_identifier = now.strftime("%H%M%S%f") current_time = now.strftime("%H:%M:%S") self._data_buffer[entry_identifier] = { 'data': data, 'time': current_time, 'trig_name': trigg_conf['trig_name'], 'trig_type': trigg_conf['trig_type'] } self._refresh_data_table_contents_sig.emit()
def __init__(self): super().__init__('static_steer') self.broadcaster = self.create_publisher(TFMessage, '/tf', 10) self.transform = TransformStamped() self.transform.header.frame_id = 'odom' self.transform.child_frame_id = 'base_link' self.transform.transform.translation.x = 0.05 self.transform.transform.translation.y = 0.0 self.transform.transform.translation.z = 0.0 self.transform.transform.rotation.x = 0.0 self.transform.transform.rotation.y = 0.0 self.transform.transform.rotation.z = 0.0 self.transform.transform.rotation.w = 1.0 self.timer_period = 0.1 self.tmr = self.create_timer(self.timer_period, self.timer_callback)
def __init__(self, logger, config): self.config = config self.logger = logger self.odometry_list = [] # Get the environment variables self.ACQ_DEVICE_NAME = self.config['ACQ_DEVICE_NAME'] self.ACQ_TOPIC_WHEEL_COMMAND = self.config["ACQ_TOPIC_WHEEL_COMMAND"] self.ACQ_ODOMETRY_TOPIC = self.config['ACQ_ODOMETRY_TOPIC'] self.last_call_back_time = rospy.get_time() self.subscriber = rospy.Subscriber('/' + self.ACQ_DEVICE_NAME + '/' + self.ACQ_TOPIC_WHEEL_COMMAND, WheelsCmdStamped, self.wheels_cmd_callback, queue_size=150) self.wheel_radius = 0.065 self.duckiebot_width = 0.10 self.speed_factor = 0.65 # full speed is one, that is 0.65m/s self.turn_factor = 0.5 self.linear_velocity_factor = self.speed_factor / 2.0 self.angular_velocity_factor = self.turn_factor * \ self.speed_factor / self.duckiebot_width self.last_message_time = -1.0 self.last_linear_velocity = 0.0 self.last_angular_velocity = 0.0 self.odometry_processed = False self.last_header_stamp = None self.odometry_topic = str("/poses_acquisition/" + self.ACQ_ODOMETRY_TOPIC) self.end_reached = False self.odometry_msg = TransformStamped() self.odometry_msg.header.frame_id = self.ACQ_DEVICE_NAME self.odometry_msg.child_frame_id = self.ACQ_DEVICE_NAME self.odometry_msg.transform.translation.z = 0.0 self.odometry_msg.transform.rotation.x = 0.0 self.odometry_msg.transform.rotation.y = 0.0 self.logger.info('Acquisition processor is set up.')
def __init__ (self): #Node initialisation #rospy.init_node('final_assignment_node_adb') #Setting the frequency of robot to 10MHz self.rate = rospy.Rate(10) self.start_time = rospy.get_rostime() #Subscribers #self.sub_chatter = rospy.Subscriber('/chatter', String, self.callback_chatter) #self.sub_odomA = rospy.Subscriber('/odomA', Int64, self.callback_odomA) #self.sub_odomB = rospy.Subscriber('/odomB', Int64, self.callback_odomB) #self.sub_cmd_vel = rospy.Subscriber('/cmd_vel', Twist, self.callback_cmd_vel) #self.sub_cmd_vel = rospy.Subscriber('/cmd_vel', Int32, self.callback_cmd_vel) #Publishers self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1) #self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Int32, queue_size=1) #self.odom_pub = rospy.Publisher("/odom", Odometry, queue_size=1) self.v_r_pub = rospy.Publisher("/v_r", Int16, queue_size=1) self.v_l_pub = rospy.Publisher("/v_l", Int16, queue_size=1) self.command_string = String() self.command_int32 = Int32() self.command_twist = Twist() #Variables related to the odometry self.wheelDistance = 0.30 self.odomA = 0 self.odomA_previous = 0 self.odomB = 0 self.odomB_previous = 0 self.odomCenter = 0 self.distancePerCount = 0.0185 self.wheelDiameter = 0.065 self.odomX = 0 self.odomY = 0 self.odomTh = 0 self.odomQuat = Quaternion() self.odomTransform = TransformStamped() self.odom = Odometry() self.odomLastTime = rospy.get_rostime() #Variables related to cmd_vel self.v_r = Int16() self.v_l = Int16()
def look_up_transform(self, target_frame, source_frame): transform = TransformStamped() try: (trans, rot) = self.listener.lookupTransform(target_frame, source_frame, rospy.Duration(0)) transform.transform.rotation = Quaternion(x=rot[0], y=rot[1], z=rot[2], w=rot[3]) transform.transform.translation = Vector3(x=trans[0], y=trans[1], z=trans[2]) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print('Error when looking up for transform!') return transform
def matToTransform(self, mat): trans = TransformStamped() # go back to quaternion and 3x1 arrays rot_array = transformations.quaternion_from_matrix(mat) trans_array = transformations.translation_from_matrix(mat) trans.transform.translation.x = trans_array[0] trans.transform.translation.y = trans_array[1] trans.transform.translation.z = trans_array[2] trans.transform.rotation.x = rot_array[0] trans.transform.rotation.y = rot_array[1] trans.transform.rotation.z = rot_array[2] trans.transform.rotation.w = rot_array[3] return trans
def __init__(self): rospy.init_node('odom_filters') print "Node 'odom_filters' has initialized." rospy.Subscriber('/mavros/local_position/odom', Odometry, self.filter_z) #rospy.Subscriber('/mavros/wheel_odometry/odom', Odometry, self.filter_z) self.counter = 0 self.pub_odom = rospy.Publisher('/odom', Odometry, queue_size=1) self.odom_topic = Odometry() self.odom_topic.header.seq = self.counter self.odom_topic.header.stamp = rospy.Time.now() self.odom_topic.header.frame_id = 'odom' self.odom_topic.child_frame_id = 'base_footprint' self.odom_topic.pose.pose.position.x = 0 self.odom_topic.pose.pose.position.y = 0 self.odom_topic.pose.pose.position.z = 0 self.odom_topic.pose.pose.orientation.x = 0 self.odom_topic.pose.pose.orientation.y = 0 self.odom_topic.pose.pose.orientation.z = 0 self.odom_topic.pose.pose.orientation.w = 1 self.odom_topic.twist.twist.linear.x = 0 self.odom_topic.twist.twist.linear.y = 0 self.odom_topic.twist.twist.linear.z = 0 self.odom_topic.twist.twist.angular.x = 0 self.odom_topic.twist.twist.angular.y = 0 self.odom_topic.twist.twist.angular.z = 0 self.tf_bc = tf2_ros.TransformBroadcaster() self.tf = TransformStamped() self.tf.header.seq = self.counter self.tf.header.stamp = rospy.Time.now() self.tf.header.frame_id = 'odom' self.tf.child_frame_id = 'base_footprint' self.tf.transform.translation.x = 0 self.tf.transform.translation.y = 0 self.tf.transform.translation.z = 0 self.tf.transform.rotation.x = 0 self.tf.transform.rotation.y = 0 self.tf.transform.rotation.z = 0 self.tf.transform.rotation.w = 1