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)