4 Copyright (c) 2016, Allgeyer Tobias, Aumann Florian, Borella Jocelyn, Karrenbauer Oliver, Marek Felix, Meissner Pascal, Stroh Daniel, Trautmann Jeremias 7 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 9 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 11 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 13 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 21 import subprocess, os, signal
29 from std_msgs.msg
import ColorRGBA
35 if len(matrix_1[0]) != len(matrix_2):
36 print "Invalid Matrix" 38 for i
in range(len(matrix_1)):
40 for j
in range(len(matrix_2[0])):
42 for k
in range(len(matrix_1[0])):
43 element = element + matrix_1[i][k] * matrix_2[k][j]
45 result_matrix.append(line)
51 result_vect.append(mat[0]*vector[0]+mat[1]*vector[1]+mat[2]*vector[2])
56 rotation_matrix_x = []
57 rotation_matrix_x.append([1,0,0])
58 rotation_matrix_x.append([0,math.cos(angle_x),-math.sin(angle_x)])
59 rotation_matrix_x.append([0,math.sin(angle_x),math.cos(angle_x)])
61 rotation_matrix_y = []
62 rotation_matrix_y.append([math.cos(angle_y),0,math.sin(angle_y)])
63 rotation_matrix_y.append([0,1,0])
64 rotation_matrix_y.append([-math.sin(angle_y),0,math.cos(angle_y)])
66 rotation_matrix_z = []
67 rotation_matrix_z.append([math.cos(angle_z),-math.sin(angle_z),0])
68 rotation_matrix_z.append([math.sin(angle_z),math.cos(angle_z),0])
69 rotation_matrix_z.append([0,0,1])
75 returns a sphere marker 77 color = ColorRGBA(*[r,g,b,1])
79 marker.header.frame_id =
"map" 80 marker.header.stamp = rospy.get_rostime()
85 marker.pose.position = pose
86 marker.pose.orientation = orientation
91 marker.lifetime = rospy.Duration()
96 returns a sphere marker 98 color = ColorRGBA(*[0,0,0,0])
100 marker.header.frame_id =
"map" 101 marker.header.stamp = rospy.get_rostime()
106 marker.pose.position = pose
107 marker.pose.orientation = orientation
108 marker.scale.x = 0.001
109 marker.scale.y = 0.001
110 marker.scale.z = 0.001
112 marker.mesh_resource = meshpath
113 marker.mesh_use_embedded_materials =
True 114 marker.lifetime = rospy.Duration()
119 returns an arrow marker 122 color = ColorRGBA(*[r,g,b,1])
123 marker.header.frame_id =
"map" 124 marker.header.stamp = rospy.get_rostime()
129 marker.pose.position=point
130 marker.pose.orientation = orientation
132 marker.scale.y = 0.02
133 marker.scale.z = 0.02
137 if __name__ ==
"__main__":
139 rospy.init_node(
'transformation_tool')
140 while not rospy.is_shutdown():
142 angle_x = 0*math.pi/180
143 angle_y = 0*math.pi/180
144 angle_z = -150*math.pi/180
146 translation = [-0.2,-0.6,0]
149 point_constellation = []
151 pose1.position = Point(*[-0.0831926,0.787348,0.705691])
152 pose1.orientation = Quaternion(*[-0.569348,-0.42073,0.411762,0.57383])
153 path1 =
'package://asr_object_database/rsc/databases/textured_objects/Knaeckebrot/Knaeckebrot.dae' 154 object1 = [pose1,path1]
156 pose2.position = Point(*[-0.58582,0.495014,0.786433])
157 pose2.orientation = Quaternion(*[-0.579757,0.402269,-0.378432,0.599042])
158 path2 =
'package://asr_object_database/rsc/databases/textured_objects/Smacks/Smacks.dae' 159 object2 = [pose2,path2]
161 pose3.position = Point(*[-0.188009,0.682787,0.74663])
162 pose3.orientation = Quaternion(*[-0.600667,-0.393664,0.370855,0.588808])
163 path3 =
'package://asr_object_database/rsc/databases/textured_objects/VitalisSchoko/VitalisSchoko.dae' 164 object3 = [pose3,path3]
166 pose4.position = Point(*[-0.399521,0.648461,0.687763])
167 pose4.orientation = Quaternion(*[-0.708298,-0.0518195,0.0561499,0.701766])
168 path4 =
'package://asr_object_database/rsc/databases/textured_objects/CoffeeBox/CoffeeBox.dae' 169 object4 = [pose4,path4]
172 point_constellation.append(object1)
173 point_constellation.append(object2)
174 point_constellation.append(object3)
175 point_constellation.append(object4)
176 new_constellation = []
179 publisher = rospy.Publisher(
"transformation_tool", Marker, queue_size=10)
182 for poses
in point_constellation:
184 marker =
getMeshMarker(poses[0].position,id,
'transformation_orig',poses[0].orientation,poses[1])
186 publisher.publish(marker)
188 marker =
getMarkerArrow(poses[0].position,id,
'transformation_orig',1,0,0,poses[0].orientation)
190 publisher.publish(marker)
193 for poses
in point_constellation:
194 base_vec = [poses[0].position.x,poses[0].position.y,poses[0].position.z]
196 new_position = Point(*[new_base_vec[0],new_base_vec[1],new_base_vec[2]])
198 new_position.x += translation[0]
199 new_position.y += translation[1]
200 new_position.z += translation[2]
203 poses[0].orientation.x,
204 poses[0].orientation.y,
205 poses[0].orientation.z,
206 poses[0].orientation.w])
207 euler_vec = tf.transformations.euler_from_quaternion(q)
208 euler_vec = list(euler_vec)
209 euler_vec[0] += angle_x
210 euler_vec[1] += angle_y
211 euler_vec[2] += angle_z
212 new_quat = tf.transformations.quaternion_from_euler(euler_vec[0],euler_vec[1],euler_vec[2])
215 new_pose.position = new_position
216 new_pose.orientation = Quaternion(*new_quat)
217 object = [new_pose,poses[1]]
218 new_constellation.append(object)
221 for poses
in new_constellation:
223 marker2 =
getMeshMarker(poses[0].position,id,
'transformation_new',poses[0].orientation,poses[1])
224 publisher.publish(marker2)
227 marker2 =
getMarkerArrow(poses[0].position,id,
'transformation_new',0,1,0,poses[0].orientation)
229 publisher.publish(marker2)
230 rospy.loginfo(
'Object: ' +poses[1])
231 rospy.loginfo(
'posestring: ' + str(poses[0].position.x) +
','+ str(poses[0].position.y) +
','+ str(poses[0].position.z) +
',' 232 + str(poses[0].orientation.w) +
','+ str(poses[0].orientation.x) +
','+ str(poses[0].orientation.y) +
','+ str(poses[0].orientation.z))