def save_dynamic_tf(bag, kitti_type, kitti, initial_time):
print("Exporting time dependent transformations")
if kitti_type.find("raw") != -1:
for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
tf_oxts_msg = TFMessage()
tf_oxts_transform = TransformStamped()
tf_oxts_transform.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
tf_oxts_transform.header.frame_id = 'world'
tf_oxts_transform.child_frame_id = 'base_link'
transform = (oxts.T_w_imu)
T_imu_to_base_link = np.eye(4, 4)
T_imu_to_base_link[0:3, 3] = [2.71/2.0+0.05, -0.32, -0.93]
transform = np.dot(transform,T_imu_to_base_link)
t = transform[0:3, 3]
q = tf.transformations.quaternion_from_matrix(transform)
oxts_tf = Transform()
oxts_tf.translation.x = t[0]
oxts_tf.translation.y = t[1]
oxts_tf.translation.z = t[2]
oxts_tf.rotation.x = q[0]
oxts_tf.rotation.y = q[1]
oxts_tf.rotation.z = q[2]
oxts_tf.rotation.w = q[3]
tf_oxts_transform.transform = oxts_tf
tf_oxts_msg.transforms.append(tf_oxts_transform)
bag.write('/tf', tf_oxts_msg, tf_oxts_msg.transforms[0].header.stamp)
elif kitti_type.find("odom") != -1:
timestamps = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps)
for timestamp, tf_matrix in zip(timestamps, kitti.poses):
tf_msg = TFMessage()
tf_stamped = TransformStamped()
tf_stamped.header.stamp = rospy.Time.from_sec(timestamp)
tf_stamped.header.frame_id = 'camera_init'
tf_stamped.child_frame_id = 'camera_gray_left'
t = tf_matrix[0:3, 3]
q = tf.transformations.quaternion_from_matrix(tf_matrix)
transform = Transform()
transform.translation.x = t[0]
transform.translation.y = t[1]
transform.translation.z = t[2]
transform.rotation.x = q[0]
transform.rotation.y = q[1]
transform.rotation.z = q[2]
transform.rotation.w = q[3]
tf_stamped.transform = transform
tf_msg.transforms.append(tf_stamped)
bag.write('/tf', tf_msg, tf_msg.transforms[0].header.stamp)
def setup_forearm_twenty_two_taxels_transforms(self):
self.link_name_forearm = '/%s_forearm_link'%(self.arm)
n_taxels = 22
self.tar_forearm.data = [Transform()] * n_taxels # [Transform() for i in range(n_taxels)] #[None for i in range(n_taxels)]
idx_list = [14, 10, 16, 8, 9, 12, 0, 1, 4, 19, 21, 15, 7, 13, 2, 3, 6, 5, 20, 17, 11, 18]
x_disp = [.16, .23, .3, .16, .23, .3, .16, .23, .3, .16, .23, .3, .17, .28, .17, .28, .17, .28, .17, .28, .34, .34]
y_disp = [0., 0., 0., -0.06, -0.06, -0.06, 0., 0., 0., 0.06, 0.06, 0.06, -0.05, -0.05, -0.05, -0.05, 0.05, 0.05, 0.05, 0.05 ,-0.05, 0.05]
z_disp = [0.04, 0.02, 0.03, 0., 0., 0., -0.05, -0.05, -0.05, 0., 0., 0., 0.04, 0.02, -0.04, -0.04, -0.04, -0.04, 0.04, 0.02, 0., 0.]
x_ang = [0, 0, 0, -math.pi/2, -math.pi/2, -math.pi/2, math.pi, math.pi, math.pi, math.pi/2, math.pi/2, math.pi/2, -math.pi/4, -math.pi/4, -3*math.pi/4, -3*math.pi/4, 3*math.pi/4, 3*math.pi/4, math.pi/4, math.pi/4, math.radians(-30), math.radians(30)]
y_ang = [-math.pi/4, math.radians(-15), math.radians(20), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, math.radians(-60), math.radians(-60)]
z_ang = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(n_taxels):
t = Transform()
t.translation.x = x_disp[i]
t.translation.y = y_disp[i]
t.translation.z = z_disp[i]
rot_mat = tr.Rz(z_ang[i])*tr.Ry(y_ang[i])*tr.Rx(x_ang[i])
quat = tr.matrix_to_quaternion(rot_mat)
t.rotation.x = quat[0]
t.rotation.y = quat[1]
t.rotation.z = quat[2]
t.rotation.w = quat[3]
self.tar_forearm.data[idx_list[i]] = t
def publish_transforms(self, ts):
ego_t = Transform()
ego_t.translation.x = self.ego_pose[0]
ego_t.translation.y = self.ego_pose[1]
ego_t.translation.z = 0.0
ego_quat = quaternion_from_euler(0.0, 0.0, self.ego_pose[2])
ego_t.rotation.x = ego_quat[0]
ego_t.rotation.y = ego_quat[1]
ego_t.rotation.z = ego_quat[2]
ego_t.rotation.w = ego_quat[3]
ego_ts = TransformStamped()
ego_ts.transform = ego_t
ego_ts.header.stamp = ts
ego_ts.header.frame_id = '/map'
ego_ts.child_frame_id = 'ego_racecar/base_link'
opp_t = Transform()
opp_t.translation.x = self.opp_pose[0]
opp_t.translation.y = self.opp_pose[1]
opp_t.translation.z = 0.0
opp_quat = quaternion_from_euler(0.0, 0.0, self.opp_pose[2])
opp_t.rotation.x = opp_quat[0]
opp_t.rotation.y = opp_quat[1]
opp_t.rotation.z = opp_quat[2]
opp_t.rotation.w = opp_quat[3]
opp_ts = TransformStamped()
opp_ts.transform = opp_t
opp_ts.header.stamp = ts
opp_ts.header.frame_id = '/map'
opp_ts.child_frame_id = 'opp_racecar/base_link'
self.br.sendTransform(ego_ts)
self.br.sendTransform(opp_ts)
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)
tf = TransformStamped(header=Header(frame_id=cmd.header.frame_id,
stamp=cmd.header.stamp),
child_frame_id=cmd.child_frame_id,
transform=Transform(
translation=cmd.pose.pose.position,
rotation=cmd.pose.pose.orientation))
self.tf_pub.sendTransform(tf)
tf = TransformStamped(header=Header(frame_id='map',
stamp=self.last_recieved_stamp),
child_frame_id='rear_right_wheel',
transform=Transform(
translation=self.right_wheel_pose.position,
rotation=self.right_wheel_pose.orientation))
self.tf_pub.sendTransform(tf)
tf = TransformStamped(header=Header(frame_id='map',
stamp=self.last_recieved_stamp),
child_frame_id='rear_left_wheel',
transform=Transform(
translation=self.left_wheel_pose.position,
rotation=self.left_wheel_pose.orientation))
self.tf_pub.sendTransform(tf)
def read_samples(filepath):
samples = []
with open(filepath, 'r') as inputfile:
reader = csv.reader(inputfile)
header = next(reader)
for line in reader:
# transform from robot base to end efector
rob = Transform()
rob.translation.x = float(line[0])
rob.translation.y = float(line[1])
rob.translation.z = float(line[2])
rob.rotation.x = float(line[3])
rob.rotation.y = float(line[4])
rob.rotation.z = float(line[5])
rob.rotation.w = float(line[6])
# transform from camera to marker
opt = Transform()
opt.translation.x = float(line[7])
opt.translation.y = float(line[8])
opt.translation.z = float(line[9])
opt.rotation.x = float(line[10])
opt.rotation.y = float(line[11])
opt.rotation.z = float(line[12])
opt.rotation.w = float(line[13])
samples.append({'robot': rob, 'optical': opt})
return samples
def publish_waypoint(x, y, z, yaw):
"""
Publish a waypoint to
"""
command_publisher = rospy.Publisher('/iris/command/trajectory',
MultiDOFJointTrajectory,
queue_size=10)
# create trajectory msg
traj = MultiDOFJointTrajectory()
traj.header.stamp = rospy.Time.now()
traj.header.frame_id = 'frame'
traj.joint_names.append('base_link')
# create start point for trajectory
transforms = Transform()
transforms.translation.x = 0
transforms.translation.y = 0
transforms.translation.z = 0
quat = tf.transformations.quaternion_from_euler(yaw * np.pi / 180.0,
0,
0,
axes='rzyx')
transforms.rotation.x = quat[0]
transforms.rotation.y = quat[1]
transforms.rotation.z = quat[2]
transforms.rotation.w = quat[3]
velocities = Twist()
accel = Twist()
point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accel],
rospy.Time(2))
traj.points.append(point)
# create end point for trajectory
transforms = Transform()
transforms.translation.x = x
transforms.translation.y = y
transforms.translation.z = z
quat = tf.transformations.quaternion_from_euler((yaw) * np.pi / 180.0,
0,
0,
axes='rzyx')
transforms.rotation.x = quat[0]
transforms.rotation.y = quat[1]
transforms.rotation.z = quat[2]
transforms.rotation.w = quat[3]
velocities = Twist()
accel = Twist()
point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accel],
rospy.Time(2))
traj.points.append(point)
rospy.sleep(1)
command_publisher.publish(traj)
def callback(scan):
try:
t, r = tf_listener.lookupTransform(topics.WORLD_FRAME,
topics.ODOMETRY_FRAME,
rospy.Time(0))
transform = Transform(translation=Vector3(*t),
rotation=Quaternion(*r))
except Exception as e:
rospy.logerr('Failed to lookup transform')
transform = Transform(translation=Vector3(0, 0, 0),
rotation=Quaternion(0, 0, 0, 1))
map_pub.update(
MapUpdate(scan=process_msg(unpickle(scan)), transform=transform))
def test_april_tags_single_interval(self):
#Setup the publisher and subscribers
self.pub_april_tags = rospy.Publisher(
"visual_odometry_april_tags_node/april_tags",
AprilTags,
queue_size=1,
latch=True)
self.pub_wheels_cmd = rospy.Publisher(
"visual_odometry_april_tags_node/wheels_cmd",
WheelsCmdStamped,
queue_size=1,
latch=True)
# Wait for the forward_kinematics_node to finish starting up
timeout = time.time() + 5.0
while (self.pub_wheels_cmd.get_num_connections() < 1 or self.pub_april_tags.get_num_connections() < 1) and \
not rospy.is_shutdown() and not time.time()>timeout:
rospy.sleep(0.1)
# Publish a single wheels cmd, and two simple april tag messages
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.vel_left = 1
msg_wheels_cmd.vel_right = 1
self.pub_wheels_cmd.publish(msg_wheels_cmd)
rospy.sleep(0.2) #Wait so the tags come in the right order
T1 = Transform()
T2 = Transform()
T1.translation.y = 2
T2.translation.y = 2
T2.rotation.x, T2.rotation.y, T2.rotation.z, T2.rotation.w = tr.quaternion_about_axis(
-np.pi / 2, (0, 0, 1))
msg_tag1 = AprilTags()
tag = TagDetection()
tag.transform = T1
msg_tag1.detections.append(tag)
msg_tag1.header.stamp = rospy.Duration(0)
self.pub_april_tags.publish(msg_tag1)
rospy.sleep(0.2) #Wait so the tags come in the right order
msg_tag2 = AprilTags()
msg_tag1.header.stamp = rospy.Duration(1)
tag.transform = T2
msg_tag1.detections.append(tag)
self.pub_april_tags.publish(msg_tag1)
# Wait 1 second for the file to be output
rospy.sleep(3)
res = np.genfromtxt(
rospy.get_param("visual_odometry_april_tags_node/filename", ''))
assert_almost_equal(res, np.array([1, 1, 1, np.pi / 2, 2, 2]))
def save_dynamic_tf(bag, timestamps, tf_matrices, child_frame_id):
print("Exporting time dependent transformations")
for timestamp, tf_matrix in zip(timestamps, tf_matrices):
tf_msg = TFMessage()
tf_stamped = TransformStamped()
tf_stamped.header.stamp = to_rostime(timestamp)
tf_stamped.header.frame_id = 'world'
tf_stamped.child_frame_id = child_frame_id
t = tf_matrix[0:3, 3]
q = quaternion_from_matrix(tf_matrix)
transform = Transform()
transform.translation.x = t[0]
transform.translation.y = t[1]
transform.translation.z = t[2]
transform.rotation.x = q[0]
transform.rotation.y = q[1]
transform.rotation.z = q[2]
transform.rotation.w = q[3]
tf_stamped.transform = transform
tf_msg.transforms.append(tf_stamped)
bag.write('/tf', tf_msg, tf_msg.transforms[0].header.stamp)
def fromRT(rt):
if (rt[0, 0] + rt[1, 1] + rt[2, 2] > 0):
s = math.sqrt(1.0 + rt[0, 0] + rt[1, 1] + rt[2, 2]) * 2 #s=qw*4
qw = s / 4
qx = (rt[2, 1] - rt[1, 2]) / s
qy = (rt[0, 2] - rt[2, 0]) / s
qz = (rt[1, 0] - rt[0, 1]) / s
elif ((rt[0, 0] > rt[1, 1]) and (rt[0, 0] > rt[2, 2])):
s = math.sqrt(1.0 + rt[0, 0] - rt[1, 1] - rt[2, 2]) * 2 #s=qx*4
qw = (rt[2, 1] - rt[1, 2]) / s
qx = s / 4
qy = (rt[0, 1] + rt[1, 0]) / s
qz = (rt[0, 2] + rt[2, 0]) / s
elif (rt[1, 1] > rt[2, 2]):
s = math.sqrt(1.0 - rt[0, 0] + rt[1, 1] - rt[2, 2]) * 2 #s=qy*4
qw = (rt[0, 2] - rt[2, 0]) / s
qx = (rt[0, 1] + rt[1, 0]) / s
qy = s / 4
qz = (rt[1, 2] + rt[2, 1]) / s
else:
s = math.sqrt(1.0 - rt[0, 0] - rt[1, 1] + rt[2, 2]) * 2 #s=qz*4
qw = (rt[1, 0] - rt[0, 1]) / s
qx = (rt[0, 2] + rt[2, 0]) / s
qy = (rt[1, 2] + rt[2, 1]) / s
qz = s / 4
tf = Transform()
tf.rotation.w = qw
tf.rotation.x = qx
tf.rotation.y = qy
tf.rotation.z = qz
tf.translation.x = rt[0, 3]
tf.translation.y = rt[1, 3]
tf.translation.z = rt[2, 3]
return tf
def publish_pose_speed_command(self, x, y, z, rx, ry, rz, speed_vector):
print('Command pose: '+self._namespace, x, y, z, rx, ry, rz, speed_vector )
quaternion = tf.transformations.quaternion_from_euler(rx, ry, rz)
rotation = Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])
location = Point(x, y, z)
transforms = Transform(translation=location,
rotation=rotation)
velocities = Twist()
velocities.linear = speed_vector
accelerations = Twist()
traj = MultiDOFJointTrajectory()
header = std_msgs.msg.Header()
header.stamp = rospy.Time()
header.frame_id = 'frame'
traj.joint_names.append('base_link')
traj.header = header
point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accelerations], rospy.Time(0))
traj.points.append(point)
self.command_pub.publish(traj)
def collisionCallback(self, feedback):
'''
This callback runs on every feedback message received
'''
validity_msg = GetStateValidityRequest(
) # Build message to verify collision
validity_msg.group_name = PLANNING_GROUP
if self.next_pose and (not self.collision):
self.odometry_me.acquire()
x = self.odometry.position.x
y = self.odometry.position.y
z = self.odometry.position.z
# Distance between the robot and the next position
dist = sqrt((self.next_pose.position.x - x)**2 +
(self.next_pose.position.y - y)**2 +
(self.next_pose.position.z - z)**2)
# Pose to verify collision
pose = Transform()
pose.rotation.x = self.odometry.orientation.x
pose.rotation.y = self.odometry.orientation.y
pose.rotation.z = self.odometry.orientation.z
pose.rotation.w = self.odometry.orientation.w
self.odometry_me.release()
#Verify possible collisions on diferent points between the robot and the goal point
# rospy.logerr("\n\n\nCOLLISION CALLBACK: ")
# rospy.logerr(dist)
for d in arange(RESOLUTION, dist + 0.5, RESOLUTION):
pose.translation.x = (self.next_pose.position.x -
x) * (d / dist) + x
pose.translation.y = (self.next_pose.position.y -
y) * (d / dist) + y
pose.translation.z = (self.next_pose.position.z -
z) * (d / dist) + z
self.current_pose.multi_dof_joint_state.transforms[
0] = pose # Insert the correct odometry value
validity_msg.robot_state = self.current_pose
# Call service to verify collision
collision_res = self.validity_srv.call(validity_msg)
# print("\nCollision response:")
# print(collision_res)
# Check if robot is in collision
if not collision_res.valid:
# print(validity_msg)
rospy.logerr('Collision in front [x:{} y:{} z:{}]'.format(
pose.translation.x, pose.translation.y,
pose.translation.z))
rospy.logerr('Current pose [x:{} y:{} z:{}]'.format(
x, y, z))
# print(collision_res)
self.move_client.cancel_goal()
self.collision = True
return
def timer_callback(self, event):
if self.last_recieved_stamp is None:
return
if self.last_recieved_stamp == self.last_stamp:
return
#TODO
self.last_stamp = self.last_recieved_stamp
cmd = Odometry()
cmd.header.stamp = self.last_recieved_stamp
# print(cmd.header.stamp)
cmd.header.frame_id = 'odom'
# this used to be map
# I set this to be 'odom'
cmd.child_frame_id = 'base_footprint'
#I set to be base_link
#BeiYou set to be base_link
#this used to be odom
cmd.pose.pose = self.last_received_pose
cmd.twist.twist = self.last_received_twist
self.pub_odom.publish(cmd)
tf = TransformStamped(header=Header(frame_id=cmd.header.frame_id,
stamp=cmd.header.stamp),
child_frame_id=cmd.child_frame_id,
transform=Transform(
translation=cmd.pose.pose.position,
rotation=cmd.pose.pose.orientation))
self.tf_pub.sendTransform(tf)
def __init__(self,
pose=TransformStamped(
header=Header(frame_id='panda_link8'),
child_frame_id='target',
transform=Transform(rotation=Quaternion(
*tf.quaternion_about_axis(numpy.pi / 4, [0, 0, 1])),
translation=Vector3(0, 0, 0.105)))):
self.robot = RobotModel()
self.robot._add(Joint(pose)) # add a fixed end-effector transform
self.pub = rospy.Publisher('/target_joint_states',
JointState,
queue_size=10)
self.joint_msg = JointState()
self.joint_msg.name = [j.name for j in self.robot.active_joints]
self.joint_msg.position = numpy.asarray([
(j.min + j.max) / 2 + 0.1 * (j.max - j.min) * random.uniform(0, 1)
for j in self.robot.active_joints
])
self.target_link = pose.child_frame_id
self.T, self.J = self.robot.fk(
self.target_link,
dict(zip(self.joint_msg.name, self.joint_msg.position)))
self.im_server = MyInteractiveMarkerServer("controller", self.T)
def inverse_transform(transform):
"""
Get the inverse of a tf transform
Get frame 2 -> frame 1 from frame 1 -> frame 2
:param transform: Transform from frame 1 -> frame 2
:type transform: geometry_msgs/Transform
:return: Transform from frame 2 -> frame 1
:rtype: geometry_msgs/Transform
"""
tmat = translation_matrix(
(transform.translation.x, transform.translation.y,
transform.translation.z))
qmat = quaternion_matrix((transform.rotation.x, transform.rotation.y,
transform.rotation.z, transform.rotation.w))
tf_mat = np.dot(tmat, qmat)
inv_tf_mat = inverse_matrix(tf_mat)
inv_transform = Transform()
inv_transform.translation.x = translation_from_matrix(inv_tf_mat)[0]
inv_transform.translation.y = translation_from_matrix(inv_tf_mat)[1]
inv_transform.translation.z = translation_from_matrix(inv_tf_mat)[2]
inv_transform.rotation = Quaternion(*quaternion_from_matrix(inv_tf_mat))
return inv_transform
def get_current_ros_transform(self):
"""
Override function used to return the current ROS transform of the object
The global map frame has an empty transform.
:return:
"""
return Transform()
def publish_command(position,velocity):
rospy.init_node('goto_poition',anonymous=True)
firefly_command_publisher = rospy.Publisher('/firefly/command/trajectory',MultiDOFJointTrajectory,queue_size=10)
desired_yaw = -10
desired_x = position[0]
desired_y = position[1]
desired_z = position[2]
quaternion = tf.transformations.quaternion_from_euler(0,0,math.radians(desired_yaw))
traj = MultiDOFJointTrajectory()
header = std_msgs.msg.Header()
header.stamp = rospy.Time()
header.frame_id = 'frame'
traj.joint_names.append('base_link')
traj.header = header
transform = Transform(translation=Point(desired_x,desired_y,desired_z),rotation=Quaternion(quaternion[0],quaternion[1],quaternion[2],quaternion[3]))
velocities = Twist()
velocities.linear.x = velocity[0]
velocities.linear.y = velocity[1]
velocities.linear.z = velocity[2]
accelerations = Twist()
point = MultiDOFJointTrajectoryPoint([transform],[velocities],[accelerations],rospy.Time(2))
traj.points.append(point)
time.sleep(3)
firefly_command_publisher.publish(traj)
rospy.loginfo("Have published %s into %s!",position+velocity,'/firefly/command/trajectory')
def __init__(self,
calibration_parameters=None,
transformation=None):
"""
Creates a HandeyeCalibration object.
: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
"""
self.parameters = calibration_parameters
if transformation is None:
transformation = ((0, 0, 0), (0, 0, 0, 1))
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.parameters.eye_on_hand:
self.transformation.header.frame_id = calibration_parameters.robot_effector_frame
else:
self.transformation.header.frame_id = calibration_parameters.robot_base_frame
self.transformation.child_frame_id = calibration_parameters.tracking_base_frame
def __init__(self, name):
"""!
The init function instantiates the class and loads all of the relevant
information from the parameter server.
@param nameThe name of this robot. Also includes how to find information
about it on the parameter server.
"""
# Set the name
self._name = name
# Look up the class number and associated ID
self._class_id = ParameterLookup.lookup(
'~' + self._resolveString('class'))
self._id = ParameterLookup.lookup('~' + self._resolveString("id"))
# Also look up the keypoints used for this robot
self._keypoints = self._initializeKeypoints(
'~' + self._resolveString("keypoints"))
# Look up the bounding shape, which is treated mathematically
# the same as keypoints.
self._bounding_shape = self._initializeKeypoints(
'~' + self._resolveString('bounding_shape'))
# Determine what name to use to find the robot in the TF tree.
self._frame_id = ParameterLookup.lookupWithDefault(
'~' + self._resolveString("frame_id"), "base_link")
# Set the pose to the origin.
transform = Transform()
transform.rotation.w = 1.0
self.recordTransform(transform)
def to_Transform(klampt_se3):
"""From Klampt se3 element to ROS Transform """
ros_pose = Transform()
ros_pose.orientation = to_Quaternion(klampt_se3[0])
ros_pose.translation.x, ros_pose.translation.y, ros_pose.translation.z = klampt_se3[
1]
return ros_pose
def setUp(self):
self.telecentric_projection_client = actionlib.SimpleActionClient(
"project_telecentric", TelecentricProjectionAction)
self.request_data_client = actionlib.SimpleActionClient(
"request_data", RequestDataAction)
for client in [
self.telecentric_projection_client, self.request_data_client
]:
if not client.wait_for_server(rospy.Duration(TIMEOUT)):
self.fail(msg="Request_data action servers timed out!")
self.pc_subscriber = rospy.Subscriber("/projected_point_cloud",
PointCloud2, self.callback)
# Rotation, that is 90 degrees rotated to the original camera in the test
self.trafo = Transform()
self.trafo.rotation.x = 0
self.trafo.rotation.y = -0.7071068
self.trafo.rotation.z = 0
self.trafo.rotation.w = 0.7071068
self.trafo.translation.x = 0
self.trafo.translation.y = 0
self.trafo.translation.z = 0
# Test the projected depth image
self.request_point_cloud()
self.send_goal_depth_image()
self.generate_line_estimate()
# Test the point cloud
self.send_goal_point_cloud()
self.get_projected_point_cloud()
# Test the subscribed cloud
self.send_goal_with_publishing_point_cloud()
def run(self):
"""
The run function for this step
Args:
Yields:
chuck_approach_pose (geometry_msgs/PoseStamped) : approach pose for impedance control with the chuck
.. seealso::
:meth:`task_executor.abstract_step.AbstractStep.run`
"""
rospy.loginfo("Action {}: Detecting schunk.".format(self.name))
self._stopped = False
# Ask for the schunk detector
stub_tf = Transform()
stub_pcl = PointCloud2()
stub = TemplateMatchRequest()
stub.initial_estimate = stub_tf
stub.target_cloud = stub_pcl
chuck_approach_pose = self._detect_schunk_srv(stub).template_pose
self.notify_service_called(
DetectSchunkAction.DETECT_SCHUNK_SERVICE_NAME)
yield self.set_running() # Check the status of the server
if self._stopped:
yield self.set_preempted(action=self.name,
srv=self._detect_schunk_srv.resolved_name,
chuck_approach_pose=chuck_approach_pose)
else:
yield self.set_succeeded(chuck_approach_pose=chuck_approach_pose)
def calculate_transform(self, data):
"""
Takes a marker pose ROS message and returns a robot-base-frame-to-marker transform ROS message.
Parameters:
data - pose ROS message of marker relative to camera
Returns:
Transform ROS message of robot base frame to marker
"""
#calculate the transform
in_x = data.position.x
in_y = data.position.y
in_z = data.position.z
input_translation = [in_x, in_y, in_z]
multiplier = np.array([[ -0.02025737, -0.31392, 0.04627322],
[-0.38235706, 0.04113464, 0.03979437],
[-0.03673691, -0.27182984, -0.36413172 ]], dtype=np.float)
offset = np.array([0.45368236, -0.14424458, 0.8933589], dtype=np.float)
output_translation = np.matmul(multiplier, input_translation)+ offset
#build the transform
output_transform = Transform()
output_transform.translation.x = output_translation[0]
output_transform.translation.y = output_translation[1]
output_transform.translation.z = output_translation[2]
#TODO: Check that the rotation transform is correct.
output_transform.rotation = data.orientation
return output_transform
def ros_publish_waypoint(self, action):
"""Publish single-point ROS trajectory message with given x, y and default z, att."""
# create trajectory msg
traj = MultiDOFJointTrajectory()
traj.header.stamp = rospy.Time.now()
traj.header.frame_id = 'frame'
traj.joint_names.append('base_link')
# create end point for trajectory
transforms = Transform()
transforms.translation.x = action[0]
transforms.translation.y = action[1]
transforms.translation.z = Z
quat = tf.transformations.quaternion_from_euler(0, 0, 0, axes='rzyx')
transforms.rotation.x = quat[0]
transforms.rotation.y = quat[1]
transforms.rotation.z = quat[2]
transforms.rotation.w = quat[3]
velocities = Twist()
accel = Twist()
point = MultiDOFJointTrajectoryPoint([transforms], [velocities],
[accel], rospy.Time())
traj.points.append(point)
self.waypoint_publisher.publish(traj)
def transformFromPose(cls, p):
t = Transform()
t.translation.x = p.position.x
t.translation.y = p.position.y
t.translation.z = p.position.z
t.rotation = deepcopy(p.orientation)
return t
def JointTrajectory2MultiDofTrajectory(self, joint_trajectory):
multi_dof_trajectory = MultiDOFJointTrajectory()
for i in range(0, len(joint_trajectory.points)):
temp_point = MultiDOFJointTrajectoryPoint()
temp_transform = Transform()
temp_transform.translation.x = joint_trajectory.points[
i].positions[0]
temp_transform.translation.y = joint_trajectory.points[
i].positions[1]
temp_transform.translation.z = joint_trajectory.points[
i].positions[2]
temp_transform.rotation.w = 1.0
temp_vel = Twist()
temp_vel.linear.x = joint_trajectory.points[i].velocities[0]
temp_vel.linear.y = joint_trajectory.points[i].velocities[1]
temp_vel.linear.z = joint_trajectory.points[i].velocities[2]
temp_acc = Twist()
temp_acc.linear.x = joint_trajectory.points[i].accelerations[0]
temp_acc.linear.y = joint_trajectory.points[i].accelerations[1]
temp_acc.linear.z = joint_trajectory.points[i].accelerations[2]
temp_point.transforms.append(temp_transform)
temp_point.velocities.append(temp_vel)
temp_point.accelerations.append(temp_acc)
multi_dof_trajectory.points.append(temp_point)
return multi_dof_trajectory
def affineToTransform(affine, summed_transform):
transformation = Transform()
if affine is not None:
scalex = np.linalg.norm(affine[:, 0])
scalez = np.linalg.norm(affine[:, 1])
# offset camera center -> translate affected by scale
t_offsetx = 320 * (1 - scalex) / 2
t_offsetz = 240 * (1 - scalez) / 2
# calc translation
transformation.translation.y = 1 / ((scalex + scalez) / 2)
transformation.translation.x = int(
affine[0, 2] - t_offsetx) #* summed_transform.translation.y
transformation.translation.z = int(
affine[1, 2] - t_offsetz) #* summed_transform.translation.y
# calc rotation
affine[:, 0] /= scalex
affine[:, 1] /= scalez
yaw = math.atan2(affine[1, 0], affine[0, 0])
rotation = tf.transformations.quaternion_from_euler(0, 0, yaw)
transformation.rotation.x = rotation[0]
transformation.rotation.y = rotation[1]
transformation.rotation.z = rotation[2]
transformation.rotation.w = rotation[3]
return transformation
else:
return None
def get_lines(self, fmt):
t = Time()
t.secs = 0
# MESSAGE_TO_TUM_SHORT
line = ROSMsg2CSVLine.to(fmt, Point(), t,
ROSMessageTypes.GEOMETRY_MSGS_VECTOR3)
line = ROSMsg2CSVLine.to(fmt, PointStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_POINTSTAMPED)
line = ROSMsg2CSVLine.to(fmt, Vector3(), t,
ROSMessageTypes.GEOMETRY_MSGS_VECTOR3)
line = ROSMsg2CSVLine.to(fmt, Vector3Stamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_VECTOR3STAMPED)
line = ROSMsg2CSVLine.to(
fmt, PoseWithCovarianceStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED)
line = ROSMsg2CSVLine.to(
fmt, PoseWithCovariance(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCE)
line = ROSMsg2CSVLine.to(fmt, PoseStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSESTAMPED)
line = ROSMsg2CSVLine.to(fmt, Pose(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSE)
line = ROSMsg2CSVLine.to(fmt, Quaternion(), t,
ROSMessageTypes.GEOMETRY_MSGS_QUATERNION)
line = ROSMsg2CSVLine.to(
fmt, QuaternionStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_QUATERNIONSTAMPED)
line = ROSMsg2CSVLine.to(fmt, Transform(), t,
ROSMessageTypes.GEOMETRY_MSGS_TRANSFORM)
line = ROSMsg2CSVLine.to(
fmt, TransformStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_TRANSFORMSTAMPED)
return line
def simulate_mbes(self, mbes_ac):
# Find particle's mbes pose without broadcasting/listening to tf transforms
particle_tf = Transform()
particle_tf.translation = self.pose.position
particle_tf.rotation = self.pose.orientation
mat_part = matrix_from_tf(particle_tf)
trans_mat = np.dot(mat_part, self.mbes_tf_mat)
trans = TransformStamped()
trans.transform.translation.x = translation_from_matrix(trans_mat)[0]
trans.transform.translation.y = translation_from_matrix(trans_mat)[1]
trans.transform.translation.z = translation_from_matrix(trans_mat)[2]
trans.transform.rotation = Quaternion(
*quaternion_from_matrix(trans_mat))
# Build MbesSimGoal to send to action server
mbes_goal = MbesSimGoal()
mbes_goal.mbes_pose.header.frame_id = self.map_frame
# mbes_goal.mbes_pose.child_frame_id = self.mbes_frame_id # The particles will be in a child frame to the map
mbes_goal.mbes_pose.header.stamp = rospy.Time.now()
mbes_goal.mbes_pose.transform = trans.transform
# Get result from action server
mbes_ac.send_goal(mbes_goal)
mbes_ac.wait_for_result()
mbes_res = mbes_ac.get_result()
# Pack result into PointCloud2
mbes_pcloud = PointCloud2()
mbes_pcloud = mbes_res.sim_mbes
mbes_pcloud.header.frame_id = self.map_frame
return mbes_pcloud
def from_dict(in_dict):
"""
Sets values parsed from a given dictionary.
:param in_dict: input dictionary.
:type in_dict: dict[string, string|dict[string,float]]
:rtype: None
"""
global robot_base_frame
global robot_effector_frame
global tracking_base_frame
global eye_on_hand
global transformation
eye_on_hand = in_dict['eye_on_hand']
robot_base_frame = in_dict['robot_base_frame']
tracking_base_frame = in_dict['tracking_base_frame']
transformation = TransformStamped(
child_frame_id=in_dict['tracking_base_frame'],
transform=Transform(
Vector3(in_dict['transformation']['x'],
in_dict['transformation']['y'],
in_dict['transformation']['z']),
Quaternion(in_dict['transformation']['qx'],
in_dict['transformation']['qy'],
in_dict['transformation']['qz'],
in_dict['transformation']['qw'])))
if eye_on_hand:
robot_effector_frame = in_dict['robot_effector_frame']
transformation.header.frame_id = in_dict['robot_effector_frame']
else:
transformation.header.frame_id = in_dict['robot_base_frame']
