def get_ros_transform(self, pose, timestamp): if self.synchronous_mode: if not self.relative_spawn_pose: self.node.logwarn("{}: No relative spawn pose defined".format( self.get_prefix())) return pose = self.relative_spawn_pose child_frame_id = self.get_prefix() if self.parent is not None: frame_id = self.parent.get_prefix() else: frame_id = "map" else: child_frame_id = self.get_prefix() frame_id = "map" transform = tf2_ros.TransformStamped() transform.header.stamp = ros_timestamp(sec=timestamp, from_sec=True) transform.header.frame_id = frame_id transform.child_frame_id = child_frame_id transform.transform.translation.x = pose.position.x transform.transform.translation.y = pose.position.y transform.transform.translation.z = pose.position.z transform.transform.rotation.x = pose.orientation.x transform.transform.rotation.y = pose.orientation.y transform.transform.rotation.z = pose.orientation.z transform.transform.rotation.w = pose.orientation.w return transform
def main(): argv = rospy.myargv(argv=sys.argv)[1:] rospy.init_node("state_publisher") log('State publisher started') send_map_transform() log('Sent map frame.') global broadcaster broadcaster = tf2_ros.TransformBroadcaster() for turtle in argv: log(f'Started publishing transform for {TURTLE_FRAME(turtle)}.') t = tf2_ros.TransformStamped() t.header.frame_id = BASE_FRAME t.child_frame_id = TURTLE_FRAME(turtle) t.transform.translation.z = 0 t.transform.rotation.x = 0 t.transform.rotation.y = 0 rospy.Subscriber(f'/{turtle}/pose', Pose, publish_transform, t) rospy.spin()
def get_transform_msgs(transforms): # List to store messages transform_msgs = [] # Create a tf message from each tf in input list for transform in transforms: # Create new tf message tf_msg = tf2_ros.TransformStamped() tf_msg.header.stamp = rospy.Time.now() # Extract parent and child frame ids tf_msg.header.frame_id = transform["frame_id"] tf_msg.child_frame_id = transform["child_id"] # Extract translation tf_msg.transform.translation.x = transform["trans_x"] tf_msg.transform.translation.y = transform["trans_y"] tf_msg.transform.translation.z = transform["trans_z"] # Extract rotation tf_msg.transform.rotation.x = transform["rot_x"] tf_msg.transform.rotation.y = transform["rot_y"] tf_msg.transform.rotation.z = transform["rot_z"] tf_msg.transform.rotation.w = transform["rot_w"] # Add to the list transform_msgs.append(tf_msg) # Return the list of tf msgs return transform_msgs
def translationPoseFromEnd(self, pose, direction, distance): ''' # 以末端坐标系为基坐标系,沿着设定方向(x/y/z)移动一定距离 # pose: 机械臂末端当前pose # direction: 沿着机械臂当前末端坐标系移动的方向 # distance: 移动的距离 ''' transform2 = tf2.TransformStamped() transform2.transform.translation.x = pose.pose.position.x transform2.transform.translation.y = pose.pose.position.y transform2.transform.translation.z = pose.pose.position.z transform2.transform.rotation.x = pose.pose.orientation.x transform2.transform.rotation.y = pose.pose.orientation.y transform2.transform.rotation.z = pose.pose.orientation.z transform2.transform.rotation.w = pose.pose.orientation.w pose1 = tf2_gm.PoseStamped() if (direction == OilApp.X): pose1.pose.position.x = distance elif (direction == OilApp.Y): pose1.pose.position.y = distance elif (direction == OilApp.Z): pose1.pose.position.z = distance pose1.pose.orientation.w = 1 pose2 = tf2_gm.do_transform_pose(pose1, transform2) pose2.header.frame_id = "world" return pose2
def rotationFrameAxis(self, pose, axis, angle): transform2 = tf2.TransformStamped() transform2.transform.translation.x = pose.pose.position.x transform2.transform.translation.y = pose.pose.position.y transform2.transform.translation.z = pose.pose.position.z transform2.transform.rotation.x = pose.pose.orientation.x transform2.transform.rotation.y = pose.pose.orientation.y transform2.transform.rotation.z = pose.pose.orientation.z transform2.transform.rotation.w = pose.pose.orientation.w quaternion = [0] * 4 if (axis == OilApp.X): quaternion = tf_trans.quaternion_from_euler(angle, 0, 0) elif (axis == OilApp.Y): quaternion = tf_trans.quaternion_from_euler(0, angle, 0) elif (axis == OilApp.Z): quaternion = tf_trans.quaternion_from_euler(0, 0, angle) pose1 = tf2_gm.PoseStamped() pose1.pose.orientation.x = quaternion[0] pose1.pose.orientation.y = quaternion[1] pose1.pose.orientation.z = quaternion[2] pose1.pose.orientation.w = quaternion[3] pose2 = tf2_gm.do_transform_pose(pose1, transform2) pose2.header.frame_id = "world" return pose2
def aruco_pose_callback(fiducial_transform_msg): if (fiducial_transform_msg.transforms != []): # Create broadcaster b = tf2_ros.TransformBroadcaster() # Create transform message tf_msg = tf2_ros.TransformStamped() tf_msg.header.stamp = rospy.Time.now() tf_msg.header.frame_id = "camera" tf_msg.child_frame_id = "aruco_marker" tf_msg.transform.translation.x = fiducial_transform_msg.transforms[ 0].transform.translation.x tf_msg.transform.translation.y = fiducial_transform_msg.transforms[ 0].transform.translation.y tf_msg.transform.translation.z = fiducial_transform_msg.transforms[ 0].transform.translation.z tf_msg.transform.rotation.x = fiducial_transform_msg.transforms[ 0].transform.rotation.x tf_msg.transform.rotation.y = fiducial_transform_msg.transforms[ 0].transform.rotation.y tf_msg.transform.rotation.z = fiducial_transform_msg.transforms[ 0].transform.rotation.z tf_msg.transform.rotation.w = fiducial_transform_msg.transforms[ 0].transform.rotation.w # Publish transform b.sendTransform(tf_msg)
def get_tf(self, pose_msg): tf_msg = tf2_ros.TransformStamped() tf_msg.header.stamp = rospy.Time.now() tf_msg.header.frame_id = self.frame_id tf_msg.child_frame_id = self.child_id tf_msg.transform.translation = pose_msg.position tf_msg.transform.rotation = pose_msg.orientation self.broadcaster.sendTransform(tf_msg)
def broadcast_tf(odom_msg, pub_time): # broadcast tf for noisy_odom w.r.t odom noisy_odom_transform = tf2_ros.TransformStamped() noisy_odom_transform.header.stamp = pub_time noisy_odom_transform.header.frame_id = "wheel_odom" noisy_odom_transform.child_frame_id = "base_footprint" noisy_odom_transform.transform.translation = odom_msg.pose.pose.position noisy_odom_transform.transform.rotation = odom_msg.pose.pose.orientation tf_broadcaster.sendTransform(noisy_odom_transform)
def publish_odom(current_velocities=None): """ publishes current odometry estimate to tf and odom topics. Args: current_velocities (List[float],optional): updated velocity, if applicable. If not given, will use the last cached velocity estimate """ global current_time global last_time global totalOdom global totalAngle global last_velocities global odom_publisher global tf_broadcast current_time = rospy.Time.now() dt = (current_time - last_time).to_sec() dist = last_velocities * float(dt) #totalOdom[2]+=dist[2]#angular movement doesn't need to be changed tf frame transform = numpy.matrix([[cos(totalAngle), -sin(totalAngle)], [sin(totalAngle), cos(totalAngle)]]) linear = numpy.matrix(dist[:2]).transpose() linear = transform * linear totalOdom[0] += linear[0, 0] totalOdom[1] += linear[1, 0] totalAngle += dist[2] #tf2 library's odometry message quat = Quaternion(*quaternion_from_euler(0, 0, totalAngle)) t = tf2_ros.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = 'odom' t.child_frame_id = 'base_link' t.transform.translation.x = totalOdom[0] t.transform.translation.y = totalOdom[1] t.transform.translation.z = 0.0 t.transform.rotation = quat tf_broadcast.sendTransform(t) #Odom topic's message odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = "world" odom.pose.pose = Pose(Point(totalOdom[0], totalOdom[1], 0.), quat) odom.child_frame_id = "base_link" odom.twist.twist = Twist( Vector3(last_velocities[0], last_velocities[1], 0.), Vector3(0., 0., last_velocities[2])) odom_publisher.publish(odom) last_time = current_time if current_velocities is not None: last_velocities = current_velocities
def frame_from_pose(self, pose, child_frame_id, parent_frame_id='world'): frame = tf2_ros.TransformStamped() frame.header.frame_id = parent_frame_id frame.child_frame_id = child_frame_id frame.header.stamp = rospy.Time.now() frame.transform.translation.x = pose.position.x frame.transform.translation.y = pose.position.y frame.transform.translation.z = pose.position.z frame.transform.rotation.x = pose.orientation.x frame.transform.rotation.y = pose.orientation.y frame.transform.rotation.z = pose.orientation.z frame.transform.rotation.w = pose.orientation.w return frame
def __init__(self): rospy.init_node('quad_node', anonymous=True) self.p = PoseStamped() # To publish self.br = tf2_ros.TransformBroadcaster() self.world_n = tf2_ros.TransformStamped( ) # new frame specified by config.yaml # self.quadT = tf2_ros.TransformStamped() # position relative to world_n frame ''' SETTING 'world_n' ''' self.world_n.header.frame_id = 'world' self.world_n.child_frame_id = 'world_new' self.world_n.transform.translation.x = rospy.get_param('~x') self.world_n.transform.translation.y = rospy.get_param('~y') self.world_n.transform.translation.z = rospy.get_param('~z') self.transVector = np.array([ self.world_n.transform.translation.x, self.world_n.transform.translation.y, self.world_n.transform.translation.z ]) self.theta = rospy.get_param('~roll') self.phi = rospy.get_param('~pitch') self.psi = rospy.get_param('~yaw') self.q = tf.transformations.quaternion_from_euler( self.theta, self.phi, self.psi) self.world_n.transform.rotation.x = self.q[0] self.world_n.transform.rotation.y = self.q[1] self.world_n.transform.rotation.z = self.q[2] self.world_n.transform.rotation.w = self.q[3] ''' SETTING PUBLISHER AND SUBSCRIBER + LISTENER FOR TF ''' self.p.header.frame_id = 'world_new' self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) self.pubRb1 = rospy.Publisher('RB1/pose', PoseStamped, queue_size=10) rospy.Subscriber('vrpn_client_node/RigidBody01/pose', PoseStamped, self.handle_pose_CB)
def __init__(self, pose_topic, child_id, frame_id, invert): # Subscriber to pose topic self.pose_sub = rospy.Subscriber(pose_topic, Pose, self.tf_from_pose) # TF broadcaster self.broadcaster = tf2_ros.TransformBroadcaster() # Create base transform message self.tf_msg = tf2_ros.TransformStamped() self.tf_msg.header.frame_id = frame_id self.tf_msg.child_frame_id = child_id # Does this transform need to be inverted self.invert = invert
def send_map_transform(x=5.5, y=5.5, z=0, w=1): # rename args assert z * z + w * w == 1 static_br = tf2_ros.StaticTransformBroadcaster() map_t = tf2_ros.TransformStamped() map_t.header.stamp = rospy.Time.now() map_t.header.frame_id = BASE_FRAME map_t.child_frame_id = MAP_FRAME map_t.transform.translation.x = x map_t.transform.translation.y = y map_t.transform.rotation.z = z map_t.transform.rotation.w = w static_br.sendTransform(map_t)
def aruco_pose_callback(pose_msg): # Create broadcaster b = tf2_ros.TransformBroadcaster() # Create transform message tf_msg = tf2_ros.TransformStamped() tf_msg.header.stamp = rospy.Time.now() tf_msg.header.frame_id = "aruco_board_origin" tf_msg.child_frame_id = "aruco_camera" tf_msg.transform.translation.x = pose_msg.pose.position.x tf_msg.transform.translation.y = pose_msg.pose.position.y tf_msg.transform.translation.z = pose_msg.pose.position.z tf_msg.transform.rotation.x = pose_msg.pose.orientation.x tf_msg.transform.rotation.y = pose_msg.pose.orientation.y tf_msg.transform.rotation.z = pose_msg.pose.orientation.z tf_msg.transform.rotation.w = pose_msg.pose.orientation.w # Publish transform b.sendTransform(tf_msg)
def camera_angle_callback(pose_msg): # Create broadcaster b = tf2_ros.TransformBroadcaster() # Create transform message tf_msg = tf2_ros.TransformStamped() tf_msg.header.stamp = rospy.Time.now() tf_msg.header.frame_id = "robot_frame" tf_msg.child_frame_id = "aruco_camera" tf_msg.transform.translation.x = 0 #define these with CAD files tf_msg.transform.translation.y = 0 #define these with CAD files tf_msg.transform.translation.z = 0 #define these with CAD files tf_msg.transform.rotation.x = pose_msg.pose.orientation.x tf_msg.transform.rotation.y = pose_msg.pose.orientation.y tf_msg.transform.rotation.z = pose_msg.pose.orientation.z tf_msg.transform.rotation.w = pose_msg.pose.orientation.w # Publish transform b.sendTransform(tf_msg)
def main(): rospy.init_node('Orbit') br = tf2_ros.TransformBroadcaster() t = tf2_ros.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = rospy.get_param('~parent') vel = rospy.get_param('~velocity') * 0.001 raio = rospy.get_param('~radius') vel_rot = rospy.get_param('~vel_rot') angulo = 0 t.child_frame_id = rospy.get_param('~child') rot = 0 while not rospy.is_shutdown(): # Polar coordinates: t.transform.translation.x = raio * cos(angulo) t.transform.translation.y = raio * sin(angulo) t.transform.translation.z = 0 q = tf_conversions.transformations.quaternion_from_euler(0, 0, rot) t.transform.rotation.x = q[0] t.transform.rotation.y = q[1] t.transform.rotation.z = q[2] t.transform.rotation.w = q[3] t.header.stamp = rospy.Time.now() br.sendTransform(t) angulo += vel rot += vel_rot rospy.sleep(0.05) if angulo > 2 * pi: angulo = 0 if rot > 2 * pi: rot = 0 rospy.spin()
def publish_tf(self, pose, timestamp): if self.synchronous_mode: if not self.relative_spawn_pose: self.node.logwarn("{}: No relative spawn pose defined".format( self.get_prefix())) return pose = self.relative_spawn_pose child_frame_id = self.get_prefix() if self.parent is not None: frame_id = self.parent.get_prefix() else: frame_id = "map" else: child_frame_id = self.get_prefix() frame_id = "map" transform = tf2_ros.TransformStamped() transform.header.stamp = ros_timestamp(sec=timestamp, from_sec=True) transform.header.frame_id = frame_id transform.child_frame_id = child_frame_id transform.transform.translation.x = pose.position.x transform.transform.translation.y = pose.position.y transform.transform.translation.z = pose.position.z transform.transform.rotation.x = pose.orientation.x transform.transform.rotation.y = pose.orientation.y transform.transform.rotation.z = pose.orientation.z transform.transform.rotation.w = pose.orientation.w try: self._tf_broadcaster.sendTransform(transform) except ROSException: if ros_ok(): self.node.logwarn("Sensor {} failed to send transform.".format( self.uid))
# Publish transform b.sendTransform(tf_msg) if __name__ == "__main__": # Initialize ROS node rospy.init_node("transform_publisher") # 'world' frame assumed to be the base of the collection bin (side by the outer wall, # closest to the corner of the competition area) # Marker board transform (relative to world - FIXED) broadcaster = tf2_ros.StaticTransformBroadcaster() board_tf = tf2_ros.TransformStamped() board_tf.header.stamp = rospy.Time.now() board_tf.header.frame_id = "aruco_marker" board_tf.child_frame_id = "map" board_tf.transform.translation.x = 0 board_tf.transform.translation.y = 0 board_tf.transform.translation.z = 0 board_tf.transform.rotation.x = -0.7070727 board_tf.transform.rotation.w = 0.7070727 broadcaster.sendTransform(board_tf) # Aruco marker camera transform (relative to marker board) rospy.Subscriber("/fiducial_transforms", FiducialTransformArray, aruco_pose_callback) # (OTHER KINECT FRAMES AUTOMATICALLY PUBLISHED RELATIVE TO KINECT BASE)
def update(self): ############################################################################# now = rospy.Time.now() if now > self.t_next: elapsed = now - self.then self.then = now elapsed = elapsed.to_sec() # calculate odometry if self.enc_left == None: d_left = 0 d_right = 0 else: d_left = (self.left - self.enc_left) / self.ticks_meter d_right = (self.right - self.enc_right) / self.ticks_meter self.enc_left = self.left self.enc_right = self.right # distance traveled is the average of the two wheels d = (d_left + d_right) / 2 # this approximation works (in radians) for small angles th = (d_right - d_left) / self.base_width # calculate velocities self.dx = d / elapsed self.dr = th / elapsed if (d != 0): # calculate distance traveled in x and y x = cos(th) * d y = -sin(th) * d # calculate the final position of the robot self.x = self.x + (cos(self.th) * x - sin(self.th) * y) self.y = self.y + (sin(self.th) * x + cos(self.th) * y) if (th != 0): self.th = self.th + th # publish the odom information quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin(self.th / 2) quaternion.w = cos(self.th / 2) ts = tf2_ros.TransformStamped() ts.header.stamp = rospy.Time.now() ts.header.frame_id = 'odom' ts.child_frame_id = 'base_link' ts.transform.translation.x = self.x ts.transform.translation.y = self.y ts.transform.translation.z = 0 ts.transform.rotation.x = 0.0 ts.transform.rotation.y = 0.0 ts.transform.rotation.z = sin(self.th / 2) ts.transform.rotation.w = cos(self.th / 2) self.odomBroadcaster.sendTransform(ts) odom = Odometry() odom.header.stamp = now odom.header.frame_id = self.odom_frame_id odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.child_frame_id = self.base_frame_id odom.twist.twist.linear.x = self.dx odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = self.dr self.odomPub.publish(odom)
# Publish transform b.sendTransform(tf_msg) if __name__ == "__main__": # Initialize ROS node rospy.init_node("transform_publisher") # 'world' frame assumed to be the base of the collection bin (side by the outer wall, # closest to the corner of the competition area) # Marker board transform (relative to world - FIXED) broadcaster = tf2_ros.StaticTransformBroadcaster() board_tf = tf2_ros.TransformStamped() board_tf.header.stamp = rospy.Time.now() board_tf.header.frame_id = "world" board_tf.child_frame_id = "aruco_board_origin" board_tf.transform.translation.x = 0.2 board_tf.transform.translation.y = 0.1 board_tf.transform.translation.z = 0.75 board_tf.transform.rotation.w = 1.0 broadcaster.sendTransform(board_tf) # Aruco marker camera transform (relative to marker board) rospy.Subscriber("ekf/pose_filtered", Pose, aruco_pose_callback) # Aruco marker camera base transform (relative to marker camera) rospy.Subscriber("camera_angle", Pose, camera_angle_callback)