def get_current_pose(self): """Helper method to get the robot's current pose.""" pose_base = tf2_geometry_msgs.PoseStamped() pose_base.header.frame_id = 'base_link' pose_base.header.stamp = rospy.get_rostime() pose_base.pose.orientation.w = 1.0 return util.pose_transform(self.tf_buffer, pose_base, 'map')
def translationPoseFromEnd(self, pose, direction, distance): ''' # 以末端坐标系为基坐标系,沿着设定方向(x/y/z)移动一定距离 # pose: 机械臂末端当前pose # direction: 沿着机械臂当前末端坐标系移动的方向 # distance: 移动的距离 ''' transform2 = tf2.TransformStamped() transform2.transform.translation.x = pose.pose.position.x transform2.transform.translation.y = pose.pose.position.y transform2.transform.translation.z = pose.pose.position.z transform2.transform.rotation.x = pose.pose.orientation.x transform2.transform.rotation.y = pose.pose.orientation.y transform2.transform.rotation.z = pose.pose.orientation.z transform2.transform.rotation.w = pose.pose.orientation.w pose1 = tf2_gm.PoseStamped() if (direction == OilApp.X): pose1.pose.position.x = distance elif (direction == OilApp.Y): pose1.pose.position.y = distance elif (direction == OilApp.Z): pose1.pose.position.z = distance pose1.pose.orientation.w = 1 pose2 = tf2_gm.do_transform_pose(pose1, transform2) pose2.header.frame_id = "world" return pose2
def transform_to_new_frame(self, parent_old, parent_new): """ Transform frame from one fixed pose to another. :param parent_old: current fixed frame :param parent_new: desired fixed frame :returns: pose in new fixed frame """ # build old pose pose_old = tf2_geometry_msgs.PoseStamped() pose_old.pose.position = self.tf_wrt_cam.transform.translation pose_old.pose.orientation = self.tf_wrt_cam.transform.rotation pose_old.header.frame_id = parent_old pose_old.header.stamp = rospy.Time.now() try: # transform pose to frame parent_new pose_new = self.tf_buffer.transform( pose_old, parent_new, rospy.Duration(1)) # set/update tf wrt map return pose_new # self.set_tf_wrt_map(pose_new) # TODO except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): raise
def findArTag(self): # (is_find, position, orientation) = self.ar_tag_track_.find() pose_ar_tag = tf2_gm.PoseStamped() # 测试使用 is_find = True # orientation = tf_trans.quaternion_from_euler(0, -math.pi / 3 * 2, 0) pose_ar_tag.pose.position.x = -0.128719220233 pose_ar_tag.pose.position.y = 0.364502367234 pose_ar_tag.pose.position.z = 1.1390057802 pose_ar_tag.pose.orientation.x = 0.743059355513 pose_ar_tag.pose.orientation.y = -0.493321566965 pose_ar_tag.pose.orientation.z = 0.364886823662 pose_ar_tag.pose.orientation.w = 0.267121815451 # 实际使用 # pose_ar_tag.pose.position.x = position[0] # pose_ar_tag.pose.position.y = position[1] # pose_ar_tag.pose.position.z = position[2] # pose_ar_tag.pose.orientation.x = orientation[0] # pose_ar_tag.pose.orientation.y = orientation[1] # pose_ar_tag.pose.orientation.z = orientation[2] # pose_ar_tag.pose.orientation.w = orientation[3] pose_ar_tag.header.frame_id = "base_link" return is_find, pose_ar_tag
def rotationFrameAxis(self, pose, axis, angle): transform2 = tf2.TransformStamped() transform2.transform.translation.x = pose.pose.position.x transform2.transform.translation.y = pose.pose.position.y transform2.transform.translation.z = pose.pose.position.z transform2.transform.rotation.x = pose.pose.orientation.x transform2.transform.rotation.y = pose.pose.orientation.y transform2.transform.rotation.z = pose.pose.orientation.z transform2.transform.rotation.w = pose.pose.orientation.w quaternion = [0] * 4 if (axis == OilApp.X): quaternion = tf_trans.quaternion_from_euler(angle, 0, 0) elif (axis == OilApp.Y): quaternion = tf_trans.quaternion_from_euler(0, angle, 0) elif (axis == OilApp.Z): quaternion = tf_trans.quaternion_from_euler(0, 0, angle) pose1 = tf2_gm.PoseStamped() pose1.pose.orientation.x = quaternion[0] pose1.pose.orientation.y = quaternion[1] pose1.pose.orientation.z = quaternion[2] pose1.pose.orientation.w = quaternion[3] pose2 = tf2_gm.do_transform_pose(pose1, transform2) pose2.header.frame_id = "world" return pose2
def pose_to_tf2(pose): """Convert a geometry_msgs/PoseStamped object to a tf2_geometry_msgs/PoseStamped object. """ new_pose = tf2_geometry_msgs.PoseStamped() new_pose.header.stamp = pose.header.stamp new_pose.header.frame_id = pose.header.frame_id new_pose.pose.orientation = pose.pose.orientation new_pose.pose.position = pose.pose.position return new_pose
def do_transform(self,point_trans, from_frame, to_frame): tf_buffer = self.tf_buffer listener = self.listener pose_stamped = tf2_geometry_msgs.PoseStamped() pose_stamped.pose = point_trans pose_stamped.header.frame_id = from_frame pose_stamped.header.stamp = rospy.Time.now() trans = tf_buffer.lookup_transform(to_frame ,from_frame, rospy.Time()) pose_transformed = tf2_geometry_msgs.do_transform_pose(pose_stamped, trans) return pose_transformed.pose
def transform_pose(input_pose, from_frame, to_frame): tf_buffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tf_buffer) pose_stamped = tf2_geometry_msgs.PoseStamped() pose_stamped.pose = input_pose pose_stamped.header.frame_id = from_frame pose_stamped.header.stamp = rospy.Time.now() try: output_pose_stamped = tf_buffer.transform(pose_stamped, to_frame, rospy.Duration(1)) return output_pose_stamped.pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): raise
def build_pose_msg(frame, position, orientation = [0,0,0,1]): p = Pose() p.position.x = position[0] p.position.y = position[1] p.position.z = position[2] # Make sure the quaternion is valid and normalized p.orientation.x = orientation[0] p.orientation.y = orientation[1] p.orientation.z = orientation[2] p.orientation.w = orientation[3] pose_stamped = tf2_geometry_msgs.PoseStamped() pose_stamped.pose = p pose_stamped.header.frame_id = frame pose_stamped.header.stamp = rospy.Time.now() return pose_stamped
def transform_pose(input_pose, from_frame, to_frame): # **Assuming /tf2 topic is being broadcasted tf_buffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tf_buffer) pose_stamped = tf2_geometry_msgs.PoseStamped() pose_stamped.pose = input_pose pose_stamped.header.frame_id = from_frame pose_stamped.header.stamp = rospy.Time.now() try: # ** It is important to wait for the listener to start listening. Hence the rospy.Duration(1) output_pose_stamped = tf_buffer.transform(pose_stamped, to_frame, rospy.Duration(1)) return output_pose_stamped.pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): raise
def transform_pose_between_frames(input_pose, from_frame, to_frame): """ Transform pose from one frame to another """ #init tf2 to transform between frames tf_buffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tf_buffer) pose_stamped = tf2_geometry_msgs.PoseStamped() pose_stamped.pose = input_pose pose_stamped.header.frame_id = from_frame # pose_stamped.header.stamp = rospy.Time.now() try: # ** It is important to wait for the listener to start listening. Hence the rospy.Duration(1) output_pose_stamped = tf_buffer.transform(pose_stamped, to_frame, rospy.Duration(1)) return output_pose_stamped.pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): raise
def __init__(self): # Load ODAS configuration self._configuration = self._load_configuration(rospy.get_param('~configuration_path')) if self._verify_sst_configuration(): # Stamped Pose Message containing the converted Sound Source Tracking (SST) position from ODAS. self._sst_input_PoseStamped = tf2_geometry_msgs.PoseStamped() self._sst_input_PoseStamped.pose.position.x = 0 self._sst_input_PoseStamped.pose.position.y = 0 self._sst_input_PoseStamped.pose.position.z = 0 # Subscribe to the Sound Source Tracking from ODAS Server self._sst_sub = rospy.Subscriber('sst', OdasSstArrayStamped, self._sst_cb, queue_size=10) # ODAS SST Publisher for PoseStamped self._sst_pose_pub = rospy.Publisher('sst_pose', tf2_geometry_msgs.PoseStamped, queue_size=1) if self._verify_ssl_configuration(): # Subscribe to the Sound Source Localization and Sound Source Tracking from ODAS Server self._ssl_sub = rospy.Subscriber('ssl', OdasSslArrayStamped, self._ssl_cb, queue_size=10) # ODAS SSL Publisher for PointCloud2 self._ssl_pcl_pub = rospy.Publisher("ssl_pcl2", PointCloud2, queue_size=500)
def uwb_pose_cov_sub_3d(self, ps_cov): if self.yaw_zero is None: return ps = tf2_geom.PoseStamped() ps.header = ps_cov.header ps.pose = ps_cov.pose.pose ps_tf = self.tf_buffer.transform(ps, self.car_frame_id) x = ps_tf.pose.position.x # + self.x0 y = ps_tf.pose.position.y # + self.y0 z = ps_tf.pose.position.z self.odom_offset.header.stamp = rospy.Time.now() self.odom_offset.pose.pose.position.x = x self.odom_offset.pose.pose.position.y = y self.odom_offset.pose.pose.position.z = z self.odom_offset.pose.pose.orientation.x = self.relative_quat[0] self.odom_offset.pose.pose.orientation.y = self.relative_quat[1] self.odom_offset.pose.pose.orientation.z = self.relative_quat[2] self.odom_offset.pose.pose.orientation.w = self.relative_quat[3] self.odom_offset.pose.covariance = self.cov_mat(ps_cov, self.odom) self.pub_odom(self.odom_offset).publish("/odom_uwb_3d")
def callback(detectionarray): #pub = rospy.Subscriber('tag_detections', AprilTagDetection , chatter_callback) #rospy.init_node('odom_tag8',anonymous = True) #msg = rospy.wait_for_message("/tag_detections", Pose) #rospy.init_node('odom_tag3',anonymous = True) #tf_buffer = tf2_ros.Buffer() #listener = tf2_ros.TransformListener(tf_buffer) #rate = rospy.Rate(1) # 10hz try: frame_id = str(detectionarray.detections[0].id) tag_frame = "tag_" + frame_id[1:2] #Getting the number out of frame_id except (IndexError): return None pose_stamped = tf2_geometry_msgs.PoseStamped() pose_stamped.pose = detectionarray.detections[0].pose.pose.pose pose_stamped.header.frame_id = "tag_" + frame_id[1:2] pose_stamped.header.stamp = detectionarray.header.stamp #pose_stamped.header = detectionarray.header #trans = tf_buffer.lookup_transform("odom",tag_frame,detectionarray.header.stamp) # print(trans) #transformed = tf2_geometry_msgs.PoseStamped() #rate = rospy.Rate(1) #one message per second #size = detectionarray.header.seq try: transformed = tf_buffer.transform(pose_stamped, "odom") return transformed except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): raise
def main(): try: print "" print "----------------------------------------------------------" print "Pick and Place, adapted from the Moveit Tutorial" print "----------------------------------------------------------" print "Press Ctrl-D to exit at any time" print "" print "============ Starting ..." tutorial = MoveGroupPythonIntefaceTutorial() # TRANSFORM LISTENER FOR ARUCO PLACEMENT start = [-0.41415453925863344, 0.3715584449139969, 0.7361325368581666, -1.17314415671765, -0.2620980800028647, 1.6138702165508845, 1.7717436871266141] start1 = [-2.0834039414342937, -0.6766622038076456, -1.9461502955504797, 1.1706543877977795, -2.3508053969552263, 2.053198583117345, 0.9989990642125767, 0.0, 0.0] start2 = [-2.2619144388837618, -0.6842947961826499, -1.7804109003559019, 1.0170783702821486, 0.8496650848667218, -2.0557343545221545, -2.0685291190378554, 0.0004475501727254161, -0.0004475501727254161] start3 = [-1.14158059591, 0.672824882423, -0.885950393325, 1.41331959244, 2.48458651934, 2.05816060307, 2.38714031805, 0.000334007592543, -0.000334007592543] tutorial.go_to_joint_state(start3) #TF listener = tf.TransformListener() listener.waitForTransform("world", "tool_link_ee", rospy.Time(), rospy.Duration(4.0)) (trans0, rot0) = listener.lookupTransform('world', 'tool_link_ee', rospy.Time(0)) #TF2 tfBuffer = tf2.Buffer() list = tf2.TransformListener(tfBuffer) tobj = tfBuffer.lookup_transform('world', 'tool_link_ee', rospy.Time(0), rospy.Duration(4.0)) #From earlier measurement, position of marker relative to camera #camera -> marker trans1 = [0.0135586191948, -0.0600579480895, 0.61578447076] rot1 = [0.999398125078, -0.00114191325631, 0.0196456334891, 0.0285680365118] cam_to_aruco = tf2_geometry_msgs.PoseStamped() cam_to_aruco.header.frame_id = "tool_link_ee" cam_to_aruco.pose.orientation.x = rot1[0] cam_to_aruco.pose.orientation.y = rot1[1] cam_to_aruco.pose.orientation.z = rot1[2] cam_to_aruco.pose.orientation.w = rot1[3] cam_to_aruco.pose.position.x = trans1[0] cam_to_aruco.pose.position.y = trans1[1] cam_to_aruco.pose.position.z = trans1[2] aruco_pose_transformed = tf2_geometry_msgs.do_transform_pose(cam_to_aruco, tobj) print("From world to marker: ") print(str(aruco_pose_transformed)) atran = aruco_pose_transformed.pose.position arot = aruco_pose_transformed.pose.orientation print("New pose:" + str(atran) + str(arot)) #Get the global pose object aruco_res = tutorial.aruco_res #Some hardcoded values since we know what they are "supposed" to be #To use the captures values, use the values in the comments aruco_res.header.frame_id = "world" aruco_res.pose.orientation.x = 0 #arot.x aruco_res.pose.orientation.y = 0 #arot.y aruco_res.pose.orientation.z = 0 #arot.z aruco_res.pose.orientation.w = 1 #arot.w aruco_res.pose.position.x = atran.x aruco_res.pose.position.y = atran.y aruco_res.pose.position.z = 0.93 #atran.z raw_input("Press `Enter` to add marker") tutorial.add_marker() print(str(aruco_res)) print "============ Press `Enter` to move the robot ..." tutorial.remove_objects() raw_input() tutorial.add_base() start = [-0.41415453925863344, 0.3715584449139969, 0.7361325368581666, -1.17314415671765, -0.2620980800028647, 1.6138702165508845, 1.7717436871266141] start1 = [-2.0834039414342937, -0.6766622038076456, -1.9461502955504797, 1.1706543877977795, -2.3508053969552263, 2.053198583117345, 0.9989990642125767, 0.0, 0.0] start2 = [-2.2619144388837618, -0.6842947961826499, -1.7804109003559019, 1.0170783702821486, 0.8496650848667218, -2.0557343545221545, -2.0685291190378554, 0.0004475501727254161, -0.0004475501727254161] #tutorial.go_to_pose_goal() #go to marker # tutorial.go_to_joint_state(start2) # print "============ Going to first object " # first = [0.5472679466193449, 1.169132990300362, -0.9911353951578363, -1.3482463949438375, 0.985989433873197, 1.1751899751206594, -0.5343455639834627] # tutorial.go_to_base() # #tutorial.go_to_object_pose('base') # print("went to base") # tutorial.go_to_pose_goal() print "============ Press `Enter` to go to wheel0 object" raw_input() wheel0_pos = [0.8756192498248672, 1.4651931415513568, -1.4451235064740584, -1.4011732602564302, 1.4101140508232206, 1.4483739241363474, -0.49654546305974134] # tutorial.go_to_joint_state(wheel0_pos) tutorial.go_to_w0() # tutorial.go_to_pose_goal() print "============ Attaching object ..." obj = 'wheel0' tutorial.attach_object('wheel0', 4) print "Object attached" print "============ Press `Enter` to go to placement position" raw_input() detach_wheel0 = [-2.928892028186479, -0.8667535063312142, 2.766639956107351, -1.4389499482037884, 0.36402599303087707, 0.9026336410084178, -0.267337618883517] tutorial.go_to_w0_detach() # tutorial.go_to_joint_state(detach_wheel0) print "============ Detaching object ..." tutorial.detach_object('wheel0', 4) print "============ Press `Enter` to go to wheel1 object" raw_input() wheel1_pos = [2.777018462599235, -1.9403007681278754, 2.057705339335132, 1.351776997117338, 1.0980508289490314, 1.9419208629705356, 0.8961924514111098] tutorial.go_to_w1() # tutorial.go_to_joint_state(wheel1_pos) print "============ Attaching object ..." tutorial.attach_object('wheel1', 4) print "Object attached" print "============ Press `Enter` to go to placement position" raw_input() detach_wheel1 = [-2.8412117332213644, -0.8680279973914196, 2.766639956107351, -1.4361070139357122, 0.3633691742618234, 0.9038608222108321, -0.17821458778484808] tutorial.go_to_w1_detach() # tutorial.go_to_joint_state(detach_wheel1) print "============ Detaching object ..." tutorial.detach_object('wheel1', 4) print "============ Press `Enter` to go to top object" raw_input() top_pos = [2.838910966607277, -1.875108243516905, -0.7736556348885344, -0.8728112300764034, 0.784507729561571, -1.9057429119325142, -2.840182772195697] # tutorial.go_to_joint_state(top_pos) tutorial.go_to_top() raw_input() print "============ Attaching object ..." tutorial.attach_object('top', 4) print "Object attached" print "============ Press `Enter` to go to placement position" raw_input() detach_top = [2.414945732149793, -1.4984348557319669, 1.6105012683134586, 1.4422988405977757, -1.5044930583570189, -1.6187787856485503, -2.4218961878085894] # tutorial.go_to_joint_state(detach_top) tutorial.go_to_top_detach() print "============ Detaching object ..." tutorial.detach_object('top', 4) print "============ Press `Enter` to go to screw" raw_input() tutorial.go_to_screw() raw_input() print "============ Attaching object ..." tutorial.attach_object('screw', 4) print "Object attached" print "============ Press `Enter` to go to placement position" raw_input() detach_top = [2.414945732149793, -1.4984348557319669, 1.6105012683134586, 1.4422988405977757, -1.5044930583570189, -1.6187787856485503, -2.4218961878085894] # tutorial.go_to_joint_state(detach_top) tutorial.go_to_screw_detach() print "============ Detaching object ..." tutorial.detach_object('screw', 4) print "============ The end! Press `Enter` to go to start position" raw_input() tutorial.go_to_joint_state(start) print "============ Press `Enter` to all remove objects from the planning scene" raw_input() tutorial.remove_objects() except rospy.ROSInterruptException: return except KeyboardInterrupt: return