def poseCallback(pose):
try:
trans = tfBuffer.lookup_transform(pose.header.frame_id, "map", rospy.Time(0))
newPose = tf2_geometry_msgs.do_transform_pose(pose,trans)
pub.publish(newPose)
rospy.loginfo(newPose)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
rospy.logerr("exception thrown %s", e)
return
def transform_pose(pose, tf):
""" Transform the given pose with the given transform """
# do_transform_pose expects a stamped pose, but it ignores the header
if isinstance(pose, geometry_msgs.Pose2D):
p = to_pose3d(
pose, frame='dummy') # just force to_pose3d return a stamped pose
elif isinstance(pose, geometry_msgs.Pose):
p = geometry_msgs.PoseStamped(None, pose)
elif isinstance(pose, geometry_msgs.PoseStamped):
p = pose
else:
raise rospy.ROSException(
"Input parameter pose is not a valid geometry_msgs pose object")
return tf2_geometry_msgs.do_transform_pose(p, tf)
def receivePose(self, pose):
transform = self.tf_buffer.lookup_transform("world2D",
pose.header.frame_id, #source frame
rospy.Time(0), #get the tf at first available time
rospy.Duration(1.0)) #wait for 1 second
self.robotPTAMPose = tf2_geometry_msgs.do_transform_pose(pose, transform).pose
#self.robotPTAMPose = pose.pose
self.rlSafePath.header.stamp = rospy.Time.now()
self.safe_path_pub.publish(self.rlSafePath)
self.rlUnsafePath.header.stamp = rospy.Time.now()
self.unsafe_path_pub.publish(self.rlUnsafePath)
self.lookahead_path_pub.publish(self.lookaheadPath)
def transform_coordinates(self,
coord,
from_frame='base_scan',
to_frame='odom'):
''' Gets frame transform at latest time '''
p1 = PoseStamped()
p1.header.frame_id = from_frame
p1.pose.position.x = coord[0]
p1.pose.position.y = coord[1] # Not sure this is right
p1.pose.position.z = coord[2]
p1.pose.orientation.w = 1.0 # Neutral orientation
transform = self.tf_buffer.lookup_transform(to_frame, from_frame,
rospy.Time(0),
rospy.Duration(1.0))
return tf2_geometry_msgs.do_transform_pose(p1, transform)
def base_scan_pose(self):
base_scan_position = {"translation": {}, "rotation": {}}
pose_transformed = tf2_geometry_msgs.do_transform_pose(
self.pose, self.transform_data)
print(pose_transformed)
for i in ['x', 'y', 'z']:
# exec(self.pose.pose.position."+ i +"+ self.transform_data.translation."+ i)
# exec("self.pose.pose.orientation."+ i +"+ self.transform_data.rotation."+ i)
base_scan_position["translation"][i] = getattr(
pose_transformed.pose.position, i)
for u in ['x', 'y', 'z', 'w']:
base_scan_position["rotation"][u] = getattr(
pose_transformed.pose.orientation, u)
return base_scan_position
def displacement_from_target(self,
target=None,
current=None,
frame_id=None):
if target is None:
target = self._setpoint_pos
if current is None:
current = self.get_position()
if frame_id is not None:
transform1 = self.tf_buffer.lookup_transform(
frame_id, target.header.frame_id, rospy.Time(0),
rospy.Duration(1))
transform2 = self.tf_buffer.lookup_transform(
frame_id, current.header.frame_id, rospy.Time(0),
rospy.Duration(1))
target = tf2_geometry_msgs.do_transform_pose(target, transform1)
current = tf2_geometry_msgs.do_transform_pose(current, transform2)
# Should probably allow Pose or PoseStamped objects
target = target.pose.position
current = current.pose.position
return Vector3(target.x - current.x, target.y - current.y,
target.z - current.z)
def pose_callback(self, msg):
rospy.loginfo("Received at goal message!")
# Copying for simplicity
position = msg.pose.position
quat = msg.pose.orientation
rospy.loginfo("Point Position: [ %f, %f, %f ]" %
(position.x, position.y, position.z))
rospy.loginfo("Quat Orientation: [ %f, %f, %f, %f]" %
(quat.x, quat.y, quat.z, quat.w))
transform = self.tf_buffer.lookup_transform(
'base',
msg.header.frame_id, #source frame
rospy.Time(0), #get the tf at first available time
rospy.Duration(1.0)) #wait for 1 second
pose_transformed = tf2_geometry_msgs.do_transform_pose(msg, transform)
self.marker.header.frame_id = pose_transformed.header.frame_id
self.marker.type = self.marker.ARROW
self.marker.action = self.marker.ADD
self.marker.scale.x = 0.25
self.marker.scale.y = 0.03
self.marker.scale.z = 0.03
self.marker.color.a = 1.0
self.marker.color.r = 1.0
self.marker.color.g = 1.0
self.marker.color.b = 0.0
self.marker.pose.orientation = pose_transformed.pose.orientation
self.marker.pose.position = pose_transformed.pose.position
self.marker.id = 0
hdr = msg.header
poses = {
'right': pose_transformed,
}
# self.flagPub.publish(str(self._performedMotion))
if not self._performedMotion:
self._performedMotion = True
succ, resp = self.ik_solver(poses)
if succ:
rospy.loginfo("Simple IK call passed!")
print "Moving to joint location"
joint_pos = np.array(resp.joints[0].position)
self.move_to_joint_position(joint_pos)
else:
rospy.logerr("Simple IK call FAILED")
def check_direction(cj_injection_message):
global listener, tfBuffer
trans = None
try:
trans = tfBuffer.lookup_transform('world', prefix, rospy.Time(0))
except:
rospy.logwarn("tf lookup -- {} not found".format(prefix))
if trans != None:
cj_local_coord = PoseStamped()
cj_local_coord = tf2_geometry_msgs.do_transform_pose(
cj_injection_message, trans)
rospy.loginfo(cj_local_coord)
heading = atan2(cj_local_coord.pose.position.y,
cj_local_coord.pose.position.x)
rospy.loginfo(heading)
def transform_3D_point_to_world_frame(self, xyz_point_from_sensor,
sensor_frame_id):
pose_stamped = geometry_msgs.msg.PoseStamped()
pose_stamped.header.frame_id = sensor_frame_id
pose_stamped.pose.orientation.w = 1.0
pose_stamped.pose.position.x = xyz_point_from_sensor[0]
pose_stamped.pose.position.y = xyz_point_from_sensor[1]
pose_stamped.pose.position.z = xyz_point_from_sensor[2]
transform = self.tf_buffer.lookup_transform(
"world", pose_stamped.header.frame_id, rospy.Time(0),
rospy.Duration(1.0))
pose_transformed = tf2_geometry_msgs.do_transform_pose(
pose_stamped, transform)
return pose_transformed
def main():
try:
tf_buffer = tf2_ros.Buffer(rospy.Duration(1.0)) #tf buffer length
tf_listener = tf2_ros.TransformListener(tf_buffer)
tf_transform = tf_buffer.lookup_transform(
"world",
"dumb_link_2", #source frame
rospy.Time(0), #get the tf at first available time
rospy.Duration(4.0))
pose_transformed = tf2_geometry_msgs.do_transform_pose(
bottle_pose, tf_transform)
print(pose_transformed)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print("tf not working")
def transform_pose(pose, trans, rot):
t = TransformStamped()
t.transform.translation.x = trans[0]
t.transform.translation.y = trans[1]
t.transform.translation.z = trans[2]
t.transform.rotation.x = rot[0]
t.transform.rotation.y = rot[1]
t.transform.rotation.z = rot[2]
t.transform.rotation.w = rot[3]
pose_stamped = PoseStamped()
pose_stamped.pose = pose
pose_transformed = tf2_geometry_msgs.do_transform_pose(pose_stamped, t)
return pose_transformed.pose
def map_to_odom_transform():
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer
"""
global new_translation, new_rotation
(translation, rotation) = convert_pose_inverse_transform(robot_pose)
map_wrt_base_pose = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation))
map_wrt_base_pose.header.stamp=rospy.Time.now()
map_wrt_base_pose.header.frame_id=base_frame
# access/look-up the transform odom (target/new frame) wrt base(source/current frame):
# tf_buffer.lookup_transform(target_frame, source_frame, query_time, timeout_secs)
ts_odom_wrt_base = tf_buffer.lookup_transform(odom_frame, base_frame, rospy.Time(), rospy.Duration(1.0)) # wait 1s for transform to become available
map_wrt_odom_pose = tf2_geometry_msgs.do_transform_pose(map_wrt_base_pose, ts_odom_wrt_base)
# invert map_wrt_odom_to get odom_wrt_map and store as tuple position, quaternion
(new_translation, new_rotation) = convert_pose_inverse_transform(map_wrt_odom_pose)
def transform_object_pose_to_robot_rf(self):
#kinect camera axis not the same as the robot axis so we could have
#to perform the necessary transforms first to get both axes aligned
#and then to transform camera rf to robot's rf
#goal_pose is the final pose of the marker wrt the robot's rf
tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0))
tf_listener = tf2_ros.TransformListener(tf_buffer)
transform = tf_buffer.lookup_transform('base', 'camera_link',
rospy.Time(0),
rospy.Duration(1.0))
trans_pose = tf2_geometry_msgs.do_transform_pose(
self.object_pose, transform)
return trans_pose
def send_goal(goal_pt):
global goal, wp_tolerance
try:
transform = tf_buffer.lookup_transform('odom', 'map', rospy.Time(0))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
return
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = 'map'
pose_stamped.pose.position.x = goal_pt[0]
pose_stamped.pose.position.y = goal_pt[1]
pose_transformed = tf2_geometry_msgs.do_transform_pose(
pose_stamped, transform)
odom_point = PointStamped()
# odom_point.header.frame_id = "odom"
# odom_point.point.x = pose_transformed.pose.position.x
# odom_point.point.y = pose_transformed.pose.position.y
odom_point.header.frame_id = "map"
odom_point.point.x = goal_pt[0]
odom_point.point.y = goal_pt[1]
odom_point.point.z = 0
odom_point.header.stamp = rospy.Time.now()
gps_pub.publish(odom_point)
# print odom_point
oldX = goal.target_pose.pose.position.x
oldY = goal.target_pose.pose.position.y
newX = pose_transformed.pose.position.x
newY = pose_transformed.pose.position.y
dist_between_goals = math.sqrt(
math.pow(oldX - newX, 2) + math.pow(oldY - newY, 2))
# print dist_between_goals,wp_tolerance
if dist_between_goals > wp_tolerance:
goal.target_pose.header.frame_id = "odom"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = newX
goal.target_pose.pose.position.y = newY
goal.target_pose.pose.orientation.x = 0
goal.target_pose.pose.orientation.y = 0
goal.target_pose.pose.orientation.z = 0
goal.target_pose.pose.orientation.w = 1
client.send_goal(goal)
print "Goal sent"
print goal
def callback_velocity(self, msg):
# rospy.loginfo("Received dimmensions and velocities msg")
# Logger the received msg
flag = False
pose_input = PoseStamped()
pose_input.header = 'base_link'
pose_input.pose = msg.pose
# Transform the pose in car frame to the map frame so we can show the right direction
try:
transform = self.tf_buffer.lookup_transform(
'map',
'base_link', #source frame
rospy.Time(0), #get the tf at first available time
rospy.Duration(1.0)) #wait for 1 second
flag = True
pose_transformed = tf2_geometry_msgs.do_transform_pose(
pose_input, transform)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
flag = False
pose_transformed = msg.pose
# Calculate the velocity from axis
moving_vel = math.sqrt(msg.vel_x**2 + msg.vel_y**2)
# Check if the object is a moving object
if moving_vel > STATIC_VEL_OBJ:
x = pose_transformed.pose.position.x
y = pose_transformed.pose.position.y
if x > 0:
x_dir = 'north'
else:
x_dir = 'south'
if y > 0:
y_dir = 'west'
else:
y_dir = 'east'
# Check if the object is bigger than 2 m each side then must be a moving car
if msg.dimension.x > 2 or msg.dimension.y > 2:
obj_type = 'car'
else:
obj_type = 'person'
# Log the moving objects
rospy.loginfo(
'There is a %s moving in direction %s %s with velocity %3f m/s'
% (obj_type, x_dir, y_dir, moving_vel))
def MoveHand(x, y, z, xO, yO, zO, side, time):
if m.isnan(x):
print Cont.O.BOLD + Cont.O.UNDERLINE + Cont.O.RED + "NAN Value received, Not Moving Arms" + Cont.O.END
return False
try:
global transform, transformP
global tf_buffer
transform = tf_buffer.lookup_transform("world", "head", rospy.Time(0),
rospy.Duration(1.0))
headPoint = PoseStamped()
headPoint.header.frame_id = "head"
headPoint.pose.position.x = x
headPoint.pose.position.y = y
headPoint.pose.position.z = z
worldPoint = tf2_geometry_msgs.do_transform_pose(headPoint, transform)
POSITION = [
worldPoint.pose.position.x, worldPoint.pose.position.y,
worldPoint.pose.position.z
]
transformP = tf_buffer.lookup_transform("world", "pelvis",
rospy.Time(0),
rospy.Duration(1.0))
temp = tf.transformations.euler_from_quaternion(
(transformP.transform.rotation.x, transformP.transform.rotation.y,
transformP.transform.rotation.z, transformP.transform.rotation.w))
ORIENTATION = [xO + temp[0], yO + temp[1], zO + temp[2]]
quat = tf.transformations.quaternion_from_euler(
ORIENTATION[0], ORIENTATION[1], ORIENTATION[2])
Cont.aC.HandMsgMaker(time, side, POSITION, quat)
print Cont.O.color.GREEN + "Published Hand Trajectory" + Cont.O.color.END
stuff = String()
stuff.data = "Published Hand Trajectory"
conn.publish(stuff)
POINT = PointStamped()
POINT.point.x = POSITION[0]
POINT.point.y = POSITION[1]
POINT.point.z = POSITION[2]
POINT.header.frame_id = "world"
PointPUB.publish(POINT)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
print "HELP"
rate.sleep()
def transform_microwave_marker_pose_to_robot_rf(self):
#kinect camera axi is not the same as the robot axis so we could have
#to perform the necessary transforms first to get both axes aligned
#and then to transform camera rf to robot's rf
#goal_pose is the final pose of the marker wrt the robot's rf
marker_pose = PoseStamped()
marker_pose.pose.position.y = self.microwave_marker_pose.pose.position.y
marker_pose.pose.position.x = self.microwave_marker_pose.pose.position.x
marker_pose.pose.position.z = self.microwave_marker_pose.pose.position.z
marker_pose.pose.orientation = self.microwave_marker_pose.pose.orientation
tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0))
tf_listener = tf2_ros.TransformListener(tf_buffer)
transform = tf_buffer.lookup_transform('base', 'camera_link',rospy.Time(0),
rospy.Duration(1.0))
self.microwave_goal_pose = tf2_geometry_msgs.do_transform_pose(marker_pose, transform)
def separator_on_shelf_layer(self,
separator_pose,
width_threshold=0.035,
height_threshold=0.06):
"""
:param separator_pose: pose of separator in floor frame
:type separator_pose: PoseStamped
:param width_threshold: all separators that are this close to the width edge are filtered.
:type width_threshold: float
:type height_threshold: float
:return: bool
"""
separator_pose = do_transform_pose(separator_pose, self.T_map___layer)
x = separator_pose.pose.position.x
z = separator_pose.pose.position.z
return width_threshold <= x and x <= self.current_shelf_layer_width - width_threshold and \
-height_threshold <= z and z <= height_threshold
def arm_homing():
global arm
jointTarget = [0.947, 5.015, 4.95, 1.144, 11.425, 4.870, 7.281]
arm.move_to_joint_pose(jointTarget)
rospy.sleep(3)
transform = tfBuffer.lookup_transform('linear_actuator_link',
'right_ee_link', rospy.Time(0),
rospy.Duration(1.0))
ps = geometry_msgs.msg.PoseStamped()
ps.header.stamp = rospy.Time.now()
pt = tf2_geometry_msgs.do_transform_pose(ps, transform)
pt.pose.orientation.w = 1.0
pt.pose.orientation.x = 0.0
pt.pose.orientation.y = 0.0
pt.pose.orientation.z = 0.0
pt.pose.position.z += 0.1
return pt.pose
def transformPose(self, out_frame_id, pose_in):
# in tf2, frames do not have the initial slash
if (pose_in.header.frame_id[0] == '/'):
pose_in.header.frame_id = pose_in.header.frame_id[1:]
# in tf2, frames do not have the initial slash
if (out_frame_id[0] == '/'):
out_frame_id = out_frame_id[1:]
try:
transform = self.listenerBuffer.lookup_transform(out_frame_id, pose_in.header.frame_id, rospy.Time.now(), rospy.Duration(4.0))
pose_out = do_transform_pose(pose_in, transform)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
rospy.logwarn("[%s] transform from (%s) to (%s) failed: (%s).", rospy.get_name(), pose_in.header.frame_id, out_frame_id, e)
pose_out = None
return pose_out
def transform(self, passed_stamped_pose):
""" Apply a transform to a given pose. """
# Creating / Updating transform with latest translation and rotation.
transform = TransformStamped()
transform.header = rospy.get_rostime()
transform.transform.translation = Point(self.translation[0],
self.translation[1], 0.0)
transform.transform.rotation = Quaternion(self.rotation[0],
self.rotation[1],
self.rotation[2],
self.rotation[3])
# pose = PoseStamped(passed_stamped_pose.header, passed_stamped_pose.pose)
pose = tf2_geometry_msgs.do_transform_pose(passed_stamped_pose,
transform)
return pose
def grasp(self, msg):
camera_frame_pose = geometry_msgs.msg.PoseStamped()
camera_frame_pose.header = msg.header
camera_frame_pose.pose = msg.poses[0]
print camera_frame_pose
transform = self.tf_buffer.lookup_transform('base',
'head_camera_fixed',
msg.header.stamp,
rospy.Duration(1.0))
pose_transformed = tf2_geometry_msgs.do_transform_pose(camera_frame_pose, transform)
print pose_transformed
roll, pitch, yaw = tf.transformations.euler_from_quaternion([0, 0, 1, 0])
roll += 0
pitch += -1.57
yaw += 1.57
print roll, pitch, yaw
quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
print quat
# pose = [0.5, 0.2, 0.2]
pose = [pose_transformed.pose.position.x, pose_transformed.pose.position.y, 0.4]
if not self._move_arm_to_pose(pose, quat):
rospy.logwarn("Planning failed. Exiting")
return
pose[2] -= 0.2
self._move_arm_to_pose(pose, quat)
self.gripper.command_position(0, block=True)
pose[2] += 0.2
self._move_arm_to_pose(pose, quat)
rospy.sleep(2)
pose[2] -= 0.2
self._move_arm_to_pose(pose, quat)
self.gripper.command_position(100, block=True)
pose[2] += 0.2
self._move_arm_to_pose(pose, quat)
self._move_arm_to_pose(self.home_pose, quat)
print "Finished"
self.subscriber.unregister()
def transform_trajectory(trajectory, transformations):
''' translate each point in trajectory by a transformation interpolated to the correct time, return the transformed trajectory'''
# for each point in trajectory, find the interpolated transformation, then transform the trajectory point
matching_transforms = get_interpolations(trajectory, transformations)
res = []
for i in range(len(matching_transforms)):
trans = matching_transforms[i]
traj_pose = trajectory[i]
transformed = tf2_geometry_msgs.do_transform_pose(traj_pose, trans)
res.append(transformed)
return res
def targetUpdate(
data
): #transforms target to map frame as that is the only frame guaranteed to be static
global target
transf = tfBuffer.lookup_transform(MAP_FRAME,
data.header.frame_id,
data.header.stamp,
timeout=rospy.Duration(2))
data = tf2_geometry_msgs.do_transform_pose(data, transf)
target = data
target.header.frame_id = MAP_FRAME
global controller_done
controller_done = False
global near_target
near_target = False
print("received new target: (map_frame)" + str(target))
pass
def getRobotPoseSt():
global tfBuffer, listener
destFrame = "world"
inPoseStamped = PoseStamped()
inPoseStamped.pose.orientation.w = 1
inPoseStamped.header.stamp = rospy.Time.now()
inPoseStamped.header.frame_id = "actor00/base_link"
absPoseStamped = None
t0 = rospy.Time()
try:
trans = tfBuffer.lookup_transform(destFrame, inPoseStamped.header.frame_id, t0, rospy.Duration(5.0))
absPoseStamped = tf2_geometry_msgs.do_transform_pose(inPoseStamped, trans)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
rospy.logerr("TF between %s and %s not ready: [%s]" % (destFrame, inPoseStamped.header.frame_id, str(e)))
return absPoseStamped
def on_enter(self, userdata):
self.pose_transformed = None
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
trans = tf_buffer.lookup_transform(self._after, self._before,
rospy.Time(0), rospy.Duration(1.0))
before_pose = userdata.target_pose
pose = PoseStamped()
pose.pose = before_pose
transformed = tf2_geometry_msgs.do_transform_pose(pose, trans)
self.pose_transformed = transformed.pose
print self.pose_transformed
try:
self.omni_base.go_rel(0.0, self.pose_transformed.position.y, 0.0,
10.0)
except:
print("Time out")
def goal_callback(self, msg):
rospy.logdebug("Received goal pose: %s", str(msg))
try:
if self.simulation:
# Simulated
trans = self.tf_buffer.lookup_transform('map', 'mechROS_base_link', rospy.Time(0), rospy.Duration(1.0))
pose_transformed = tf2_geometry_msgs.do_transform_pose(msg, trans)
pose_transformed.header.frame_id = 'map'
self.target_pose = pose_transformed
else:
# Real robot
self.target_pose = msg
self.signal.set()
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rospy.loginfo('Could not find transform')
def execute(self, ud):
rospy.logdebug("Entered 'SET_OBJECT_TARGET' state.")
new_target = ''
is_new_obj = False
# loop through all found objects
for i in range(len(ud.sot_object_array)):
# get object info, mainly its name
try:
req = GetObjectInformationRequest()
req.type = ud.sot_object_array[i].type
res = self.ork_srv(req)
# if the object hasn't been grasped yet, make the object the current grasp target
is_new_obj = True
if any(res.information.name in s for s in ud.sot_picked_objects):
is_new_obj = False
if is_new_obj:
new_target = res.information.name
except rospy.ServiceException:
rospy.logerr("GetObjectInformation service request failed.")
# if a new object has been detected
if new_target and is_new_obj:
# get the transform from ork's frame to odom
transform = self.tf_buffer.lookup_transform('odom', ud.sot_ork_frame, rospy.Time(0))
# get the object's pose in odom frame
ud.sot_grasp_target_pose = do_transform_pose(ud.sot_object_array[i].pose.pose, transform)
ud.sot_target_object = new_target
ud.sot_grasp_target_pose.pose.position.z += 0.08
ud.sot_grasp_target_pose.pose.position.x -= 0.15
print "OBJECT NAME : " + new_target
print "POSE : "
print ud.sot_grasp_target_pose
check_call([PATH_TO_PDF_CREATOR], shell=True)
check_call(['~/sara_ws/src/PDF_creator/create_pdf.sh'], shell=True)
return 'target_set'
# rospy.logerr("Could not get transform from " + ud.sot_ork_frame + " to 'odom'.")
return 'no_new_object'
def check_direction():
global listener, tfBuffer, cj_injection_message
speed = 0.1 # default speed m/s
rot_speed = 0.5 # default rot speed sec/radian
min_duration = 2.0 # minimum time [sec] for single trajectory
duration = default_duration = 2 # sec
trans = None
try:
trans = tfBuffer.lookup_transform(prefix, 'world', rospy.Time(0))
except:
rospy.logwarn("tf lookup -- {} not found".format(prefix))
if trans != None:
cj_local_coord = PoseStamped()
cj_local_coord = tf2_geometry_msgs.do_transform_pose(cj_injection_message, trans)
# rospy.loginfo(cj_local_coord)
heading = atan2(cj_local_coord.pose.position.y, cj_local_coord.pose.position.x)
# rospy.loginfo(heading)
distance = sqrt(pow(cj_local_coord.pose.position.x, 2) + pow(cj_local_coord.pose.position.y, 2))
duration = distance / speed # #calculate required time for this motion
# rospy.logdebug("in check_direction: before if: [{},{}]".format(heading, duration))
if duration < min_duration:
duration = min_duration
q = (cj_local_coord.pose.orientation.x,
cj_local_coord.pose.orientation.y,
cj_local_coord.pose.orientation.z,
cj_local_coord.pose.orientation.w)
euler = euler_from_quaternion(q, axes='sxyz')
rotation_yaw = abs(euler[2])
duration_yaw = rotation_yaw / rot_speed
# rospy.logdebug("in check_direction: duration_yaw: {}".format(duration_yaw))
# rospy.logdebug("in check_direction: duration: {}".format(duration))
if duration_yaw > duration:
duration = duration_yaw
# rospy.logdebug("in check_direction. returning: [{},{}]".format(heading, duration))
return [heading, duration]
else:
rospy.logerr("in check_direction: transform is None")
return [0, duration]
def my_callback(event):
try:
transf = tfBuffer.lookup_transform("map",
BASE_FRAME,
rospy.Time(),
timeout=rospy.Duration(2))
rotVec = PoseStamped()
rotVec.header.frame_id = "map"
rotVec.pose.orientation.w = 1.0
rotVec = tf2_geometry_msgs.do_transform_pose(rotVec, transf)
quaternion = [
rotVec.pose.orientation.x, rotVec.pose.orientation.y,
rotVec.pose.orientation.z, rotVec.pose.orientation.w
]
euler = tf.transformations.euler_from_quaternion(quaternion)
yaw = euler[2]
estimated_orientation.z = yaw
travel_x = last_cmdvel.x * UPDATE_PERIOD
travel_y = last_cmdvel.y * UPDATE_PERIOD
travel_x = (travel_x * cos(estimated_orientation.z) +
travel_y * sin(estimated_orientation.z))
travel_y = (travel_x * sin(estimated_orientation.z) +
travel_y * cos(estimated_orientation.z))
estimated_position.x = estimated_position.x + travel_x / 2
estimated_position.y = estimated_position.y + travel_y / 2
omega = last_cmdvel.z * UPDATE_PERIOD
if not has_odom:
estimated_orientation.z += omega
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = ODOM_FRAME
odom_quat = tf.transformations.quaternion_from_euler(
estimated_orientation.z, 0, 0, "rzyx")
odom.pose.pose = Pose(
Point(estimated_position.x, estimated_position.y, 0),
Quaternion(*odom_quat))
odom.child_frame_id = BASE_FRAME
odom.twist.twist = Twist(Vector3(travel_x, travel_y, 0),
Vector3(0, 0, omega))
odom_pub.publish(odom)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as ex:
print(ex)
pass
def get_detector_results(self, request):
"""
Args:
request (GetDetectorResultsRequest):
Returns:
GetDetectorResultsResponse
"""
if self.currently_busy.is_set():
return GetDetectorResultsResponse(status=ServiceStatus(BUSY=True))
self.currently_busy.set()
detections = Detections()
try:
rois = []
poses = []
for pose in self.obj_poses:
transform = self.tf_buffer.lookup_transform(self.ref_frame, 'odom', rospy.Time())
pose = tf2_geometry_msgs.do_transform_pose(pose, transform).pose
x1 = pose.position.x - 0.03
x2 = pose.position.x + 0.03
y1 = pose.position.y - 0.03
y2 = pose.position.y + 0.03
z1 = pose.position.z - 0.03
z2 = pose.position.z + 0.03
roi = RegionOfInterest(x1=x1, y1=y1, z1=z1, x2=x2, y2=y2, z2=z2)
rois.append(roi)
poses.append(pose)
self.currently_busy.clear()
# Fill in the detections message
for roi, pose, track_id in zip(rois, poses, self.obj_ids):
seg_roi = SegmentOfInterest(x=[], y=[])
score = 1.0
detections.objects.append(Detection(roi=roi, seg_roi=seg_roi, pose=pose,
id=self._new_id(), track_id=track_id,
confidence=score, class_name=self.classes[0]))
return GetDetectorResultsResponse(status=ServiceStatus(OKAY=True), results=detections)
except Exception as e:
print("GazeboRenderedBerriesServer error: ", e)
return GetDetectorResultsResponse(status=ServiceStatus(ERROR=True), results=detections)
def _odom_callback(self, msg):
with self._lock:
try:
transform = self.tf_buffer.lookup_transform(
'base_link', msg.child_frame_id, rospy.Time.now(),
rospy.Duration(1.0))
except Exception as e:
rospy.logerr('ERROR in TransformOdomNode: ' + str(e))
raise
stamped_point = PoseStamped()
stamped_point.pose = msg.pose.pose
final_pose = tf2_geometry_msgs.do_transform_pose(
stamped_point, transform)
msg_final = self._construct_transform(final_pose, msg)
self._transform_broadcaster.sendTransform(msg_final)
def handle_req_s_tr(self,req):
resp = LookAtTSResponse()
try:
# Get position from the request
pos = req.desired_s_tr.transform.translation
# Lookup the transform between frames
trans = self.tfBuffer.lookup_transform(self.base, req.desired_s_tr.child_frame_id, rospy.Time(0))
# Apply the transform to the requested position
pose = PoseStamped()
pose.pose.position = Point(pos.x, pos.y, pos.z)
pose.pose.orientation = req.desired_s_tr.transform.rotation
trans = tf2_geometry_msgs.do_transform_pose(pose, trans).pose.position
# Send off the request
resp.success = self.lookat(trans)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
print 'Cannot lookupTransform'
resp.success = False
return resp
def __init__(self, from_frame, to_frame):
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = from_frame
pose_stamped.header.stamp = rospy.Time.now()
tf_buffer = tf2_ros.Buffer(rospy.Duration(1.0)) # tf buffer length
tf_listener = tf2_ros.TransformListener(tf_buffer)
while not rospy.is_shutdown():
try:
rospy.sleep(0.1)
transform = tf_buffer.lookup_transform(to_frame,
from_frame, # source frame
rospy.Time(0), # get the tf at first available time
rospy.Duration(0.1)) # wait for 1 second
pose_transformed = tf2_geometry_msgs.do_transform_pose(pose_stamped, transform)
pose_pub.publish(pose_transformed)
except Exception as e:
rospy.logwarn(e)
continue
def execute(self, ud):
loop_again = True
while loop_again:
self.mutex.acquire()
if self.msg_received:
loop_again = False
self.mutex.release()
rospy.sleep(rospy.Duration(1))
transform = self.tf_buffer.lookup_transform('odom', self.tables.header.frame_id, rospy.Time(0))
drop_pose_z = 1.5
tmp = PoseStamped()
lst = []
for t in range(len(self.tables.tables)):
tmp.pose = self.tables.tables[t].pose
tmp.header.frame_id = self.tables.header.frame_id
tmp.header.stamp = rospy.Time.now()
eval_pose = do_transform_pose(tmp, transform)
rpy = tf_conversions.transformations.euler_from_quaternion(eval_pose.pose.orientation)
if rpy[2]**2 > 0.90:
lst.append([(eval_pose.pose.position.z - drop_pose_z)**2, eval_pose])
lst.sort()
ud.drop_pose = lst[0][1]
print ud.drop_pose
return 'drop_pose_acquired'
def do_playback_keyframe_demo(self, goal):
rospy.loginfo("Received playback demo")
## input: keyframe demo goal has bagfile name and eef_only bool
self.start_time = time.time()
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
tfBroadcaster = tf2_ros.TransformBroadcaster()
# Check if the bag file is valid
# Example path if necessary
bag_path = os.path.expanduser(goal.bag_file_name)
if (not os.path.isfile(bag_path)):
error_msg = "Playback bagfile does not exist: %s" % bag_path
self.server.set_aborted(self.result, error_msg)
rospy.logerr(error_msg)
return
with rosbag.Bag(bag_path) as bag:
self.bag = bag
# Check the number of playback - and warn/stop if we have more than X number of keyframes
if self.bag.get_message_count() > self.KEYFRAME_THRESHOLD:
error_msg = "Playback Keyframe Demo aborted due to too many frames: %d" % self.bag.get_message_count()
self.server.set_aborted(self.result, error_msg)
rospy.logerr(error_msg)
return
keyframe_count = 0
# Check if we have a gripper topic. If so add it to playback
all_topics = self.bag.get_type_and_topic_info().topics.keys()
playback_topics = [goal.target_topic]
GRIPPER_TOPIC = 'gripper/stat'
gripper_topics = [x for x in all_topics if GRIPPER_TOPIC in x]
playback_topics.extend(gripper_topics)
OBJECT_LOCATION_TOPIC = "object_location"
playback_topics.append(OBJECT_LOCATION_TOPIC)
gripper_msgs = dict()
self.gripper_pos = dict()
while not self.server.is_preempt_requested():
# Cycle through the topics and messages and store them into list for ordering
all_messages = dict()
for topic, msg, t in self.bag.read_messages(topics=playback_topics):
if topic not in all_messages:
all_messages[topic] = []
all_messages[topic].append(msg)
# Pull out the playback topic
playback_list = all_messages[goal.target_topic]
for gripper_topic in gripper_topics: # either one or two grippers
gripper_msgs[gripper_topic] = all_messages[gripper_topic]
# Actually set the gripper value
pos = gripper_msgs[gripper_topic][0].requested_position
self.gripper_helper(gripper_topic, pos)
for msg_count in xrange(len(playback_list)):
msg = playback_list[msg_count]
if goal.zero_marker:
zeroMarker = None
if not OBJECT_LOCATION_TOPIC in all_messages:
rospy.logerr("Playback specified a zero marker but no object locations were found in keyframe #{}".format(msg_count))
self.server.set_aborted(text="Object locations missing")
return
for i, location in enumerate(all_messages[OBJECT_LOCATION_TOPIC][msg_count].objects):
if location.label == goal.zero_marker:
zeroMarker = location
break
if not zeroMarker:
rospy.logerr("Specified zero marker not found in .bag file in keyframe #{}".format(msg_count))
self.server.set_aborted(text="Zero marker not found in .bag file")
return
try:
currentZero = tfBuffer.lookup_transform("map", goal.zero_marker, rospy.Time(0), timeout=rospy.Duration(5)).transform
except tf2_ros.LookupException:
rospy.logerr("Specified label \"{}\" not found in current scene. Disable locate objects to play back absolute keyframe positions.".format(zeroMarker.label))
self.server.set_aborted(text="Specified label not found")
return
rospy.loginfo("Using zero marker \"{}\" (prob: {:.1%}) with position (x: {:.2f}, y: {:.2f}, z: {:.2f})".format(
zeroMarker.label,
zeroMarker.probability,
zeroMarker.pose.position.x,
zeroMarker.pose.position.y,
zeroMarker.pose.position.z
))
rospy.loginfo("Found corresponding zero marker in current frame (x: {:.2f}, y: {:.2f}, z: {:.2f})".format(
currentZero.translation.x,
currentZero.translation.y,
currentZero.translation.z
))
else:
rospy.loginfo("No zero marker passed for keyframe #{}".format(msg_count))
# Check if we need to convert the msg into joint values
if goal.eef_only:
# Ask the arm planner to plan for that joint target from current position
pt = Pose(msg.position, msg.orientation)
if zeroMarker:
# Remap the point to the global map frame for adjustment
baseLinkToMap = tfBuffer.lookup_transform("map", "base_link", rospy.Time(0), timeout=rospy.Duration(5))
mapToBaseLink = tfBuffer.lookup_transform("base_link", "map", rospy.Time(0), timeout=rospy.Duration(5))
ptAbsolute = tf2_geometry_msgs.do_transform_pose(PoseStamped(pose=pt), baseLinkToMap)
# Compensate for new zero marker position
ptAbsolute.pose.position.x += (currentZero.translation.x - zeroMarker.pose.position.x)
ptAbsolute.pose.position.y += (currentZero.translation.y - zeroMarker.pose.position.y)
ptAbsolute.pose.position.z += (currentZero.translation.z - zeroMarker.pose.position.z)
pt = tf2_geometry_msgs.do_transform_pose(ptAbsolute, mapToBaseLink).pose
rospy.loginfo("Moving EEF to adjusted position: (x: {:.2f}, y: {:.2f}, z: {:.2f})".format(
pt.position.x,
pt.position.y,
pt.position.z
))
plan = self.arm_planner.plan_poseTargetInput(pt)
else:
# Pull out the joint values for the arm from the message
pts = self._get_arm_joint_values(msg)
# Ask the arm planner to plan for that joint target from current position
plan = self.arm_planner.plan_jointTargetInput(pts)
# Check if we have a valid plan
if plan == None or len(plan.joint_trajectory.points) < 1:
print "Error: no plan found"
else:
rospy.loginfo("Executing Keyframe: %d" % keyframe_count)
self.sendPlan(plan)
keyframe_count+=1
# Execute Gripper if needed
for gripper_topic in gripper_topics:
pos = gripper_msgs[gripper_topic][msg_count].requested_position
if abs(pos - self.gripper_pos[gripper_topic]) > self.GRIPPER_THRESHOLD:
# Actually set the gripper value
self.gripper_helper(gripper_topic, pos)
self.result.time_elapsed = rospy.Duration.from_sec(time.time() - self.start_time)
complete_msg = "Playback Keyframe Demo completed successfully"
self.server.set_succeeded(self.result, complete_msg)
rospy.loginfo(complete_msg)
return
self.result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time)
self.server.set_preempted(self.result, "playback keyframe demo preempted")
print 'preempted'
return
def pick_and_place_aruco(self, string_operation):
success = False
if string_operation == "pick":
self.prepare_robot_pandp()
rospy.sleep(2.0)
while not rospy.is_shutdown() and self.aruco_pose_rcv == False:
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection...")
rospy.sleep(1.0)
aruco_pose = self.aruco_pose
aruco_pose.header.frame_id = self.strip_leading_slash(aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform("base_footprint",
ps.header.frame_id,
rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
rospy.loginfo("Setting cube pose based on Aruco detection")
self.pick_g.object_pose.pose.position = aruco_ps.pose.position
self.pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(self.pick_g))
self.pick_g.object_pose.header.frame_id = 'base_footprint'
self.pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(self.pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(self.pick_g))
self.pick_as.send_goal_and_wait(self.pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
success = False
else:
success = True
self.prepare_robot_nav()
return success
if string_operation == "place":
# Place the object on table in front
rospy.loginfo("Placing aruco marker")
self.place_as.send_goal_and_wait(self.pick_g)
rospy.loginfo("Done!")
result = self.place_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to place, not trying further")
success = False
else:
success = True
return success
