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