def run(self):
counter = 0;
# set default Q and mu values to eye(6) and zero
Q = np.eye(6)
mu = np.zeros(6)
# Initial sensor calibration
Q, mu = self.calibrateSensors()
# Initialize Kalman filter
self.P_lin = np.diag([1.,0.,1.,0.])*pow(self.initial_position_uncertainty,2)
self.Q_lin = self.G_lin.dot(Q[:2,:2]).dot(self.G_lin.T)
self.P_ang = pow(self.initial_orientation_uncertainty,2)
self.Q_ang = self.G_ang*Q[5,5]*self.G_ang
self.R_lin = np.eye(2)*self.camera_position_error
self.R_ang = self.camera_orientation_error
self.acc_bias = mu[:2]
self.gyro_bias = mu[5]
self.x_lin = np.array([self.z.translation.x, self.z.translation.y])
self.v_lin = np.array([0.,0.])
self.psi = 2*np.arccos(self.z.rotation.w)*np.sign(self.z.rotation.z) # Assume we have a quaternion with vertical axis
# Run Kalman filter
while not rospy.is_shutdown():
# Obtain IMU measurement
rospy.wait_for_service('last_imu')
try:
get_imu = rospy.ServiceProxy('last_imu', ImuSrv)
self.u = get_imu()
except rospy.ServiceException, e:
#print "Service call to IMU Server failed: %s"%e
print "No IMU update this time step"
# Assume previous measured u (Zero-Order Hold)
# Perform time and measurement updates as appropriate
self.timeUpdate(self.u)
if self.updateFlag:
self.measurementUpdate(self.z)
# Publish state estimate in topic
state = Transform()
state.translation.x = self.x_lin[0]
state.translation.y = self.x_lin[1]
state.rotation.z = 1
state.rotation.w = self.psi # Quaternion form
self.state_pub.publish(state)
state_tf = TFMessage()
state_tf.transforms = [TransformStamped()]
state_tf.transforms[0].header.seq = counter
state_tf.transforms[0].header.frame_id = self.origin_tag
state_tf.transforms[0].child_frame_id = self.mname
state_tf.transforms[0].transform = state
self.state_pub_tf.publish(state_tf)
counter = counter + 1
# Finish cycle and loop
self.rate.sleep()
def tf_listener():
global all_static_transforms
rospy.init_node('static_tf2_listener', anonymous=True)
sub = rospy.Subscriber("tf_static", TFMessage, tf_msg_callback)
pub = rospy.Publisher('tf_static', TFMessage, latch=True, queue_size=1)
# check what transforms we've heart
while not rospy.is_shutdown():
print("Num. of TFs received so far: " +
str(len(all_static_transforms)))
control_char = raw_input(
"c: clear, p: publish latched (and un-suscribe), q: quit: ")
if control_char == "c":
all_static_transforms = []
elif control_char == "p":
sub.unregister()
time.sleep(2)
newTFMessage = TFMessage()
newTFMessage.transforms = all_static_transforms
pub.publish(newTFMessage)
print(
"Publishing the latched tf_static thing and waiting for the final CTRL+C..."
)
break
elif control_char == "q":
return
else:
print("Input c, p, or q. Not " + control_char + ". Facepalm.")
rospy.spin()
def drone_att_callback(self, rcv):
print("POSE: " + str(rcv))
self.lastPose = rcv
rcv.x *= math.pi / 180.0
rcv.y *= math.pi / 180.0
rcv.z *= math.pi / 180.0
# Publish pose for rviz
msg = TransformStamped()
msg.header.frame_id = "/map"
msg.header.stamp = Clock().now().to_msg()
msg.child_frame_id = "/base_footprint_drone_attitude"
quat = self.euler_to_quaternion(rcv.y, rcv.x, rcv.z)
orientation = Quaternion()
orientation.x = quat[0]
orientation.y = quat[1]
orientation.z = quat[2]
orientation.w = quat[3]
msg.transform.rotation = orientation
tfmsg = TFMessage()
tfmsg.transforms = [msg]
self.pub_tf.publish(tfmsg)
# Publish velocity
msg = Twist()
msg.linear.x = rcv.x
msg.angular.z = rcv.y
self.pub_vel.publish(msg)
def save_known_poses(self, req):
print self.known_poses.values()
with rosbag.Bag('bundle.bag','w') as bag:
tf_msg = TFMessage()
tf_msg.transforms = self.known_poses.values()
bag.write('/tf',tf_msg,rospy.Time.now())
bag.close()
return SaveKnownPosesResponse()
def tf_timer_callback(self):
msg = TransformStamped()
msg.header.frame_id = "/map"
msg.header.stamp = Clock().now().to_msg()
msg.child_frame_id = "/base_footprint"
msg.transform.translation = self.last_position
msg.transform.rotation = self.last_orientation
tfmsg = TFMessage()
tfmsg.transforms = [msg]
self.pub_tf.publish(tfmsg)
def get_tfmessage(self, sample, current_pose=None, next_pose=None):
# get transforms for the current sample
tf_array = TFMessage()
tf_array.transforms = self.get_tfs(sample, current_pose)
# add transforms from the next sample to enable interpolation
next_sample = self.nusc.get(
'sample', sample['next']) if sample.get('next') != '' else None
if next_sample is not None:
tf_array.transforms += self.get_tfs(next_sample, next_pose)
return tf_array
def callback(pub, odom, robot, data):
da_tf = TFMessage()
base_transform = TransformStamped()
base_transform.header.stamp = rospy.get_rostime()
base_transform.header.frame_id = odom
base_transform.child_frame_id = robot
base_transform.transform.translation.x = data.pose.position.x
base_transform.transform.translation.y = data.pose.position.y
base_transform.transform.translation.z = data.pose.position.z
# Some weird things happening with rotations in Unity, so do some rearrangement
base_transform.transform.rotation.x = data.pose.orientation.y
base_transform.transform.rotation.y = data.pose.orientation.x
base_transform.transform.rotation.z = data.pose.orientation.z
base_transform.transform.rotation.w = -data.pose.orientation.w
da_tf.transforms = [base_transform]
pub.publish(da_tf)
def drone_odom_callback(self, rcv):
print("ODOM: " + str(rcv))
# Publish pose for rviz
msg = TransformStamped()
msg.header.frame_id = "/map"
msg.header.stamp = Clock().now().to_msg()
msg.child_frame_id = "/base_footprint_drone"
quat = self.euler_to_quaternion(self.lastPose.x, self.lastPose.y,
self.lastPose.z)
orientation = Quaternion()
orientation.x = quat[0]
orientation.y = quat[1]
orientation.z = quat[2]
orientation.w = quat[3]
msg.transform.rotation = orientation
position = Vector3()
position.x = rcv.x
position.y = rcv.y
position.z = rcv.z
msg.transform.translation = position
tfmsg = TFMessage()
tfmsg.transforms = [msg]
self.pub_tf.publish(tfmsg)
# Publish Rviz Path
msgpath = Path()
msgpath.header.frame_id = "/map"
msgpath.header.stamp = Clock().now().to_msg()
pose = PoseStamped()
pose.header.frame_id = "/map"
pose.header.stamp = Clock().now().to_msg()
pose.pose.position.x = rcv.x
pose.pose.position.y = rcv.y
pose.pose.position.z = rcv.z
self.posearray.append(pose)
if len(self.posearray) > 50:
self.posearray = self.posearray[1:]
msgpath.poses = self.posearray
self.pub_posearray.publish(msgpath)
def base_info_cb(self, msg):
self._odom.header.stamp = msg.stamp
self._odom.pose.pose.position.x = msg.x
self._odom.pose.pose.position.y = msg.y
self._odom.pose.pose.orientation.z = math.sin(msg.orientation / 2.0)
self._odom.pose.pose.orientation.w = math.cos(msg.orientation / 2.0)
self._odom.twist.twist.linear.x = msg.forward_velocity
self._odom.twist.twist.angular.z = msg.rotational_velocity
self.pub_odom.publish(self._odom)
self._transform.header.stamp = msg.stamp
pos = self._odom.pose.pose.position
self._transform.transform.translation = Vector3(x=pos.x,
y=pos.y,
z=pos.z)
self._transform.transform.rotation = self._odom.pose.pose.orientation
tfmsg = TFMessage()
tfmsg.transforms = [self._transform]
self.pub_tf.publish(tfmsg)
def tf_static_cb(self, tf_msg):
"""Callback for messages from /tf_static.
Add all transforms to the cache and then publish a TF message with all the transforms.
:param TFMessage tf_msg: A /tf_static message.
"""
# Protect against reacting to this node's own published messages, otherwise it'd end up in an infinite loop.
callerid = tf_msg._connection_header['callerid']
if callerid == rospy.get_name() or \
(self._forbidden_callerid_prefix is not None and callerid.startswith(self._forbidden_callerid_prefix)):
return
# Process the incoming transforms, merge them with our cache.
for transform in tf_msg.transforms:
key = transform.child_frame_id
if key not in self._transforms or not self._update_only_with_newer or \
self._transforms[key].header.stamp < transform.header.stamp:
rospy.logdebug("Updated transform %s->%s. Old timestamp was %s, new timestamp is %s." % (
transform.header.frame_id, transform.child_frame_id,
str(self._transforms[key].header.stamp if key in self._transforms else "None"),
str(transform.header.stamp)))
self._transforms[key] = transform
# Publish the cached messages.
msg = TFMessage()
msg.transforms = self._transforms.values()
self._tf_publisher.publish(msg)
rospy.logdebug("Sent %i transforms: %s" % (len(self._transforms), str(self._transforms.keys())))
# After publishing the first message, send also this ~/ready message, so that nodes that rely on this node's
# operation know that it has been brought up.
if not self._ready:
self._ready = True
ready_msg = Bool()
ready_msg.data = True
self._ready_publisher.publish(ready_msg)
rospy.logdebug("Sent ready message")
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node("tf2_example_pub")
pub = node.create_publisher(TFMessage, "/tf")
t1 = TransformStamped()
t1.header.frame_id = "base_link"
t1.child_frame_id = "arm1"
t2 = TransformStamped()
t2.header.frame_id = "base_link"
t2.child_frame_id = "arm2"
msg = TFMessage()
msg.transforms = [t1, t2]
while rclpy.ok():
pub.publish(msg)
sleep(0.9)
node.destroy_node()
rclpy.shutdown()
q = euler_to_quaternion(euler)
rate = rospy.Rate(10)
for i in range(len(path)):
tf_msg = TFMessage()
tf_stamp = TransformStamped()
tf_stamp.header.stamp = rospy.Time.now()
tf_stamp.header.frame_id = 'map'
tf_stamp.child_frame_id = 'load_base_link'
tf_stamp.transform.rotation.w = 1
tf_stamp.transform.translation = path[i].pose.position
tf_stamp.transform.rotation.x = q[i,0]
tf_stamp.transform.rotation.y = q[i,1]
tf_stamp.transform.rotation.z = q[i,2]
tf_stamp.transform.rotation.w = q[i,3]
tf_msg.transforms = [tf_stamp]
tf_pub.publish(tf_msg)
rate.sleep()
avg_tf = TransformStamped()
avg_tf.header.frame_id = tf_group[0].header.frame_id
avg_tf.child_frame_id = tf_group[0].child_frame_id
avg_tf.transform = numpy_to_tf_msg(avg_trans, avg_rot)
avgs.append(avg_tf)
return avgs
# Pass bagfile name as first argument
if __name__ == '__main__':
tf_groups = get_tf_groups(sys.argv[1])
avgs = avg_transforms(tf_groups)
"""
for tfg, avg in zip(tf_groups,avgs):
print '%d transforms from \'%s\' to \'%s\'' % (
len(tfg), tfg[0].header.frame_id, tfg[0].child_frame_id
)
print 'Average Transform:'
print avg.transform
"""
if len(sys.argv) > 2:
with rosbag.Bag(sys.argv[2],'w') as outbag:
tf_msg = TFMessage()
tf_msg.transforms = avgs
outbag.write("/tf", tf_msg, rospy.Time.from_sec(time.time()))
outbag.close()
def drone_tf_callback(self, tf):
# Wrap and publish TF
tfmsg = TFMessage()
tfmsg.transforms = [tf]
self.pub_tf.publish(tfmsg)
def run(self):
counter = 0;
# set default Q and mu values to eye(6) and zero
Q = np.eye(6)
mu = np.zeros(6)
# Initial sensor calibration
Q, mu = self.calibrateSensors()
# Initialize Kalman filter
self.P_lin = np.diag([1.,0.,1.,0.])*pow(self.initial_position_uncertainty,2)
self.Q_lin = self.G_lin.dot(Q[:2,:2]).dot(self.G_lin.T)
self.P_ang = pow(self.initial_orientation_uncertainty,2)
self.Q_ang = self.G_ang*Q[5,5]*self.G_ang
self.R_lin = np.eye(2)*pow(self.camera_position_error,2)
self.R_ang = pow(self.camera_orientation_error,2)
self.acc_bias = mu[:2]
self.gyro_bias = mu[5]
self.x_lin = np.array([self.z.translation.x, self.z.translation.y])
self.z_position = 0.
self.v_lin = np.array([0.,0.])
self.psi = 2*np.arccos(self.z.rotation.w)*np.sign(self.z.rotation.z) # Assume we have a quaternion with vertical axis
# Run Kalman filter
while not rospy.is_shutdown() and not self.needs_to_calibrate:
# Obtain IMU measurement
rospy.wait_for_service('last_imu')
try:
get_imu = rospy.ServiceProxy('last_imu', ImuSrv)
self.u = get_imu()
# update bias estimate for accelerometer IMU update this time step: 1, trying aga
self.updateAccelBias()
except rospy.ServiceException, e:
#print "Service call to IMU Server failed: %s"%e
print "No IMU update this time step"
# Only runs if self.u is unitialized
while not self.u:
try:
self.u = get_imu()
except:
print 'Trying to initialize'
# Assume previous measured u (Zero-Order Hold)
# Perform time and measurement updates as appropriate
self.timeUpdate(self.u)
if self.updateFlag:
self.measurementUpdate(self.z)
# Publish state estimate in topic
state = Transform()
state.translation.x = self.x_lin[0]
state.translation.y = self.x_lin[1]
state.translation.z = self.z_position
state.rotation.z = np.sin(self.psi/2)
state.rotation.w = np.cos(self.psi/2) # Quaternion form
self.state_pub.publish(state)
self.psi_pub.publish(self.psi)
state_tf = TFMessage()
state_tf.transforms = [TransformStamped()]
state_tf.transforms[0].header.seq = counter
state_tf.transforms[0].header.frame_id = self.origin_tag
state_tf.transforms[0].child_frame_id = self.mname
state_tf.transforms[0].transform = state
self.state_pub_tf.publish(state_tf)
counter = counter + 1
# Finish cycle and loop
self.rate.sleep()
"""
out = TransformStamped()
out.header.frame_id = parent_frame
out.child_frame_id = child_frame
# some random data
out.transform.rotation.w = 1
out.transform.translation.x = 0.1
return out
if __name__ == '__main__':
rospy.init_node("publisher")
# setup some static transforms
pub = rospy.Publisher("/tf", TFMessage, queue_size=10)
msg = TFMessage()
delay = rospy.Duration(rospy.get_param("delay", 0.0))
source = rospy.get_param("source", 3)
msg.transforms = [
make_transform(str(ii), str(ii + 1)) for ii in range(source)
]
while not rospy.is_shutdown():
rospy.sleep(rospy.Duration(0.05))
for m in msg.transforms:
m.header.stamp = rospy.Time.now() - delay
pub.publish(msg)
