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