Quantcast
Viewing all articles
Browse latest Browse all 130

Use transformation from another node

Hi everyone, I am trying to use frames that have been already created in another node in my node to do a transformation from a plan to another, but it seems that I have missed a step because then my program is running, it doesn't use the frame from the other node. This is the class that I have made to do my transform step. class TransformToGlobal: def __init__(self, tfname): #self.tl = tf.TransformListener() self.mytfname = tfname self.tfBuffer_ = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer_) #rospy.Subscriber("/sometopic", PointStamped, self.some_message_handler) def transformToGlobal(self, x, y , z): transformStamped_ = TransformStamped() br = tf2_ros.TransformBroadcaster() posPoint = Vector3() transformStamped_.header.stamp = rospy.Time.now() transformStamped_.header.frame_id = dronename + "_svo_drone_vision" transformStamped_.child_frame_id = dronename + "_depth_drone_vision" transformStamped_.transform.translation.x = 0.035 transformStamped_.transform.translation.y = 0.05 transformStamped_.transform.translation.z = 0.03 q = tf_conversions.transformations.quaternion_from_euler(-pi/2, 0, 0) transformStamped_.transform.rotation.x = q[0] transformStamped_.transform.rotation.y = q[1] transformStamped_.transform.rotation.z = q[2] transformStamped_.transform.rotation.w = q[3] br.sendTransform(transformStamped_) transformStamped_.header.stamp = rospy.Time.now() transformStamped_.header.frame_id = dronename + "_depth_drone_vision" transformStamped_.child_frame_id = dronename + "_bary_depth_frame" transformStamped_.transform.translation.x = x transformStamped_.transform.translation.y = y transformStamped_.transform.translation.z = z + 0.16 br.sendTransform(transformStamped_) try: transformStamped_ = self.tfBuffer_.lookup_transform(self.mytfname, dronename + "_bary_depth_frame", rospy.Time.now()) posPoint.x = transformStamped_.transform.translation.x posPoint.y = transformStamped_.transform.translation.y posPoint.z = transformStamped_.transform.translation.z except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rate.sleep() rospy.logwarn("Error transform") return posPoint I only get a posPoint filled with 0. This class is called from this part of the code # Récupère les coordonées cartésiennes des points dans le repère de la caméra bottomdist = sensor_msgs.point_cloud2.read_points_list(PC.GetDataPoint(), skip_nans=False, uvs=[bottommost]) topdist = sensor_msgs.point_cloud2.read_points_list(PC.GetDataPoint(), skip_nans=False, uvs=[topmost]) rightdist = sensor_msgs.point_cloud2.read_points_list(PC.GetDataPoint(), skip_nans=False, uvs=[rightmost]) leftdist = sensor_msgs.point_cloud2.read_points_list(PC.GetDataPoint(), skip_nans=False, uvs=[leftmost]) closedist = sensor_msgs.point_cloud2.read_points_list(PC.GetDataPoint(), skip_nans=False, uvs=[min_loc]) # Convertit les coordonnées précemments trouvées dans le repère global # Renseigne le message des coordonnés converties myObs.bottomPoint = tftoglob.transformToGlobal(bottomdist[0][0], bottomdist[0][1], bottomdist[0][2]) myObs.topPoint = tftoglob.transformToGlobal(topdist[0][0], topdist[0][1], topdist[0][2]) myObs.rightPoint = tftoglob.transformToGlobal(rightdist[0][0], rightdist[0][1], rightdist[0][2]) myObs.leftPoint = tftoglob.transformToGlobal(leftdist[0][0], leftdist[0][1], leftdist[0][2]) myObs.closePoint = tftoglob.transformToGlobal(closedist[0][0], closedist[0][1], closedist[0][2]) distanceobstacle.append(myObs) Cheers.

Viewing all articles
Browse latest Browse all 130

Trending Articles



<script src="https://jsc.adskeeper.com/r/s/rssing.com.1596347.js" async> </script>