transformation_tool.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 '''
4 Copyright (c) 2016, Allgeyer Tobias, Aumann Florian, Borella Jocelyn, Karrenbauer Oliver, Marek Felix, Meissner Pascal, Stroh Daniel, Trautmann Jeremias
5 All rights reserved.
6 
7 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
8 
9 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
10 
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.
12 
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.
14 
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.
16 '''
17 
18 import sys
19 import rospy
20 import roslib
21 import subprocess, os, signal
22 import math
23 import rospkg
24 import datetime
25 import tf
26 import numpy
27 
28 from geometry_msgs.msg import Point,Quaternion,Pose
29 from std_msgs.msg import ColorRGBA
30 from visualization_msgs.msg import Marker, MarkerArray
31 
32 
33 def multiply_matrix(matrix_1,matrix_2):
34  result_matrix = []
35  if len(matrix_1[0]) != len(matrix_2):
36  print "Invalid Matrix"
37  return False
38  for i in range(len(matrix_1)):
39  line = []
40  for j in range(len(matrix_2[0])):
41  element = 0
42  for k in range(len(matrix_1[0])):
43  element = element + matrix_1[i][k] * matrix_2[k][j]
44  line.append(element)
45  result_matrix.append(line)
46  return result_matrix
47 
48 def matrix_vector_mult(matrix,vector):
49  result_vect = []
50  for mat in matrix:
51  result_vect.append(mat[0]*vector[0]+mat[1]*vector[1]+mat[2]*vector[2])
52  return result_vect
53 
54 
55 def get_rotation_matrix(angle_x, angle_y, angle_z):
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)])
60 
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)])
65 
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])
70 
71  return multiply_matrix(rotation_matrix_x,multiply_matrix(rotation_matrix_y,rotation_matrix_z))
72 
73 def getCubeMarker(pose,k,n_s,r,g,b,orientation):
74  """
75  returns a sphere marker
76  """
77  color = ColorRGBA(*[r,g,b,1])
78  marker = Marker()
79  marker.header.frame_id = "map"
80  marker.header.stamp = rospy.get_rostime()
81  marker.ns = n_s
82  marker.id = k
83  marker.type = 1
84  marker.action = 0
85  marker.pose.position = pose
86  marker.pose.orientation = orientation
87  marker.scale.x = 0.1
88  marker.scale.y = 0.1
89  marker.scale.z = 0.1
90  marker.color = color
91  marker.lifetime = rospy.Duration()
92  return marker
93 
94 def getMeshMarker(pose,k,n_s,orientation,meshpath):
95  """
96  returns a sphere marker
97  """
98  color = ColorRGBA(*[0,0,0,0])
99  marker = Marker()
100  marker.header.frame_id = "map"
101  marker.header.stamp = rospy.get_rostime()
102  marker.ns = n_s
103  marker.id = k
104  marker.type = 10
105  marker.action = 0
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
111  marker.color = color
112  marker.mesh_resource = meshpath
113  marker.mesh_use_embedded_materials = True
114  marker.lifetime = rospy.Duration()
115  return marker
116 
117 def getMarkerArrow(point,k,n_s,r,g,b,orientation):
118  """
119  returns an arrow marker
120  """
121  marker = Marker()
122  color = ColorRGBA(*[r,g,b,1])
123  marker.header.frame_id = "map"
124  marker.header.stamp = rospy.get_rostime()
125  marker.ns = n_s
126  marker.id = k
127  marker.type = 0
128  marker.action = 0
129  marker.pose.position=point
130  marker.pose.orientation = orientation
131  marker.scale.x = 0.2
132  marker.scale.y = 0.02
133  marker.scale.z = 0.02
134  marker.color = color
135  return marker
136 
137 if __name__ == "__main__":
138 
139  rospy.init_node('transformation_tool')
140  while not rospy.is_shutdown():
141  # transformation
142  angle_x = 0*math.pi/180
143  angle_y = 0*math.pi/180
144  angle_z = -150*math.pi/180
145 
146  translation = [-0.2,-0.6,0]
147 
148  # constellation
149  point_constellation = []
150  pose1 = Pose()
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]
155  pose2 = Pose()
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]
160  pose3 = Pose()
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]
165  pose4 = Pose()
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]
170 
171 
172  point_constellation.append(object1)
173  point_constellation.append(object2)
174  point_constellation.append(object3)
175  point_constellation.append(object4)
176  new_constellation = []
177  id = 0
178 
179  publisher = rospy.Publisher("transformation_tool", Marker, queue_size=10)
180 
181  # publish orig
182  for poses in point_constellation:
183  marker = Marker()
184  marker = getMeshMarker(poses[0].position,id,'transformation_orig',poses[0].orientation,poses[1])
185  id += 1
186  publisher.publish(marker)
187 
188  marker = getMarkerArrow(poses[0].position,id,'transformation_orig',1,0,0,poses[0].orientation)
189  id += 1
190  publisher.publish(marker)
191 
192  # transform constellation
193  for poses in point_constellation:
194  base_vec = [poses[0].position.x,poses[0].position.y,poses[0].position.z]
195  new_base_vec = matrix_vector_mult(get_rotation_matrix(angle_x, angle_y, angle_z),base_vec)
196  new_position = Point(*[new_base_vec[0],new_base_vec[1],new_base_vec[2]])
197 
198  new_position.x += translation[0]
199  new_position.y += translation[1]
200  new_position.z += translation[2]
201 
202  q = numpy.array([
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])
213 
214  new_pose = Pose()
215  new_pose.position = new_position
216  new_pose.orientation = Quaternion(*new_quat)
217  object = [new_pose,poses[1]]
218  new_constellation.append(object)
219 
220  # publish new/transformed constellation
221  for poses in new_constellation:
222  marker2 = Marker()
223  marker2 = getMeshMarker(poses[0].position,id,'transformation_new',poses[0].orientation,poses[1])
224  publisher.publish(marker2)
225  id += 1
226 
227  marker2 = getMarkerArrow(poses[0].position,id,'transformation_new',0,1,0,poses[0].orientation)
228  id += 1
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))
233 
234 
235  rospy.spin()
236 
237 
238 
239 
def getMarkerArrow(point, k, n_s, r, g, b, orientation)
def getMeshMarker(pose, k, n_s, orientation, meshpath)
def get_rotation_matrix(angle_x, angle_y, angle_z)
def matrix_vector_mult(matrix, vector)
def multiply_matrix(matrix_1, matrix_2)
def getCubeMarker(pose, k, n_s, r, g, b, orientation)


asr_resources_for_active_scene_recognition
Author(s): Allgeyer Tobias, Aumann Florian, Borella Jocelyn, Karrenbauer Oliver, Marek Felix, Mei├čner Pascal, Stroh Daniel, Trautmann Jeremias
autogenerated on Tue Feb 18 2020 03:14:15