def main():
rospy.init_node('tf_test')
rospy.loginfo("Node for testing actionlib server")
#from_link = '/head_color_camera_l_link'
#to_link = '/base_link'
from_link = '/base_link'
to_link = '/map'
tfl = TransformListener()
rospy.sleep(5)
t = rospy.Time(0)
mpose = PoseStamped()
mpose.pose.position.x = 1
mpose.pose.position.y = 0
mpose.pose.position.z = 0
mpose.pose.orientation.x = 0
mpose.pose.orientation.y = 0
mpose.pose.orientation.z = 0
mpose.pose.orientation.w = 0
mpose.header.frame_id = from_link
mpose.header.stamp = rospy.Time.now()
rospy.sleep(5)
mpose_transf = None
rospy.loginfo('Waiting for transform for some time...')
tfl.waitForTransform(to_link,from_link,t,rospy.Duration(5))
if tfl.canTransform(to_link,from_link,t):
mpose_transf = tfl.transformPose(to_link,mpose)
print mpose_transf
else:
rospy.logerr('Transformation is not possible!')
sys.exit(0)
def subscriber_callback(data):
occupancyMap = transformation(data.data, data.info.width, data.info.height)
way_points = find_goals(occupancyMap, data.info.width, data.info.height)
result = random.choice(way_points)
try:
planMaker = rospy.ServiceProxy('move_base/make_plan', nav_srv.GetPlan)
listener = TransformListener()
listener.waitForTransform("base_link", "map", rospy.Time(), rospy.Duration(4.0))
t = listener.getLatestCommonTime("base_link", "map")
position, quaternion = listener.lookupTransform("base_link", "map", t)
pos_x = position[0]
pos_y = position[1]
pos_z = position[2]
goal_robot = create_goal((result[1]-2000)*data.info.resolution,(result[0]-2000)*data.info.resolution)
#Make plan with 0.5 meter flexibility, from target pose and current pose (with same header)
start_pose = create_message(pos_x,pos_y,pos_z,quaternion)
plan = planMaker(
start_pose,
goal_robot.target_pose,
0.5)
action_server = actionlib.SimpleActionClient('move_base', mb_msg.MoveBaseAction);
action_server.wait_for_server()
send_goal(goal_robot, action_server)
except rospy.ServiceException, e:
print "plan service call failed: %s"%e
class MocapObject:
def __init__(self, name):
self.name = name
self.tf = '/vicon/' + name + '/' + name
self.tl = TransformListener()
self.pose = np.array([0., 0., 0.])
self.orient = np.array([0, 0, 0]) # Euler angles
# for velocity:
sub = message_filters.Subscriber(self.tf, TransformStamped)
self.cache = message_filters.Cache(sub, 100)
self.vel = np.array([0, 0, 0])
def position(self):
self.tl.waitForTransform("/world", self.tf, rospy.Time(0),
rospy.Duration(1))
position, quaternion = self.tl.lookupTransform("/world", self.tf,
rospy.Time(0))
self.pose = np.array(position)
return np.array(position)
def orientation(self):
self.tl.waitForTransform("/world", self.tf, rospy.Time(0),
rospy.Duration(1))
position, quaternion = self.tl.lookupTransform("/world", self.tf,
rospy.Time(0))
self.orient = get_angles(np.array(quaternion))
return get_angles(np.array(quaternion))
def __init__(self, target_link):
rospy.init_node('GetJointPose')
tf_listener = TransformListener()
base_link = "base_link"
print "waiting for transform"
tf_listener.waitForTransform (target_link, base_link, rospy.Time(), rospy.Duration(4.0))
print "done waiting"
if not tf_listener.frameExists("base_link"):
print "ERROR NO FRAME base_link"
return
elif not tf_listener.frameExists(target_link):
print "ERROR NO FRAME" + target_link
return
else:
t = tf_listener.getLatestCommonTime("/base_link", target_link)
joint_pose = geometry_msgs.msg.PoseStamped()
joint_pose.header.frame_id = target_link
joint_pose.pose.orientation.w = 1.0 # Neutral orientation
pose_from_base = tf_listener.transformPose("/base_link", joint_pose)
print "Position from " + base_link + " to " + target_link
print pose_from_base
class RTCRobot_Pose:
#############################################################################
def __init__(self):
rospy.init_node('rtcrobot_pose') #, anonymous=True
nodename = rospy.get_name()
self.map_frame = rospy.get_param("map_frame", "/map")
self.base_frame = rospy.get_param("base_frame", "/base_footprint")
self.publish_frequency = rospy.get_param("publish_frequency", 10)
self.p_pub = rospy.Publisher()
if(is_stamped):
p_pub = rospy.Publisher('robot_pose', PoseStamped, queue_size=1)
else:
p_pub = ospy.Publisher('robot_pose', Pose, queue_size=1)
self.rate = self.publish_frequency
self.listener = TransformListener()
self.listener.waitForTransform(self.map_frame,self.base_frame,rospy.Duration(secs = 1.0))
rtcrobot_pose
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = self.map_frame
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.pose.orientation.x = rot[0]
pose_stamped.pose.orientation.y = rot[1]
pose_stamped.pose.orientation.z = rot[2]
pose_stamped.pose.orientation.w = rot[3]
class mocap_object:
def __init__(self, name):
self.name = name
self.tf = '/vicon/' + name + '/' + name
self.tl = TransformListener()
self.pose = np.array([0, 0, 0])
self.orient = np.array([0, 0, 0])
def position(self):
self.tl.waitForTransform("/world", self.tf, rospy.Time(0),
rospy.Duration(1))
position, quaternion = self.tl.lookupTransform("/world", self.tf,
rospy.Time(0))
self.pose = position
return np.array(position)
def orientation(self):
self.tl.waitForTransform("/world", self.tf, rospy.Time(0),
rospy.Duration(1))
position, quaternion = self.tl.lookupTransform("/world", self.tf,
rospy.Time(0))
self.orient = get_angles(np.array(quaternion))
return get_angles(np.array(quaternion))
def publish_position(self):
# publish_pose(self.pose, self.orient, self.name+"_pose")
publish_cylinder(self.pose, self.orient, 0.15, self.name + "_cylinder")
def listener_server(data):
"""Function that obtains the position of the robot once the service is called.
@param data: input of the service:
@param data.writePose: boolean whether or not to get the position
@param data.argument: string with reason to save the position"""
if data.writePose:
try:
tfmine = TransformListener()
now = rospy.Time()
tfmine.waitForTransform("/map", "/base_link", now,
rospy.Duration(1))
time = tfmine.getLatestCommonTime("/map", "/base_link")
posit, quaternion = tfmine.lookupTransform("/map", "/base_link",
time)
yaw = trans.euler_from_quaternion(quaternion)[2]
except Exception as e:
print "Error at lookup getting position:"
print e
return False
try:
line = str(posit[0]) + "," + str(
posit[1]) + "," + str(yaw) + "," + str(data.argument) + "\n"
file = open("waypoints.txt", 'a')
file.write(line)
return True
except Exception as e:
print "Error writing position to file:"
print e
return False
else:
return True
def transform(pos):
"""
Convert coordinate in camera frame to world frame
:param pos: coordinate of beacon in camera frame
:param baseframe: robot pose (coordinate, orientation)
:return: coordinate of beacon in world frame
"""
p1 = PoseStamped()
p1.header.frame_id = "camera_link"
p1.pose.orientation.w = 1.0
p1.pose.position.x = pos[2]
p1.pose.position.y = pos[0]
p1.pose.position.z = pos[1]
tf = TransformListener()
tf.waitForTransform("/camera_link", "/map", rospy.Time(0),
rospy.Duration(4.0))
while not rospy.is_shutdown():
try:
tf.waitForTransform("/camera_link", "/map", rospy.Time(0),
rospy.Duration(4.0))
break
except:
print("except")
pass
p_in_base = tf.transformPose("/map", p1)
return [
p_in_base.pose.position.x, p_in_base.pose.position.y,
p_in_base.pose.position.z
]
class myNode:
def __init__(self, *args):
self.tf = TransformListener()
# rospy.Subscriber(...)
# ...
def some_method(self):
if self.tf.frameExists(
"left_fingertip_sensor_s0") and self.tf.frameExists("world"):
t = self.tf.getLatestCommonTime("left_fingertip_sensor_s0",
"world")
position, quaternion = self.tf.lookupTransform(
"left_fingertip_sensor_s0", "world", t)
print position, quaternion
else:
print "not found frame"
def fury(self):
# now = rospy.Time.now()
self.tf.waitForTransform("pbase_link", "world", rospy.Time(0),
rospy.Duration(10.0))
print "waited"
(trans, rot) = self.tf.lookupTransform("left_fingertip_sensor_s0",
"world", rospy.Time(0))
print trans, rot
return trans
def locations(self, frame1, frame2):
self.tf.waitForTransform(frame1, frame2, rospy.Time(0),
rospy.Duration(10.0))
print "waited"
(trans, rot) = self.tf.lookupTransform(frame1, frame2, rospy.Time(0))
print trans, rot
class Mocap_object: # superclass
def __init__(self, name):
self.name = name
self.tf = '/vicon/'+name+'/'+name
self.tl = TransformListener()
self.pose = np.array([0,0,0])
self.orient = np.array([0,0,0])
# for velocity:
sub = message_filters.Subscriber(self.tf, TransformStamped)
self.cache = message_filters.Cache(sub, 100)
self.vel = np.array([0,0,0])
def position(self):
self.tl.waitForTransform("/world", self.tf, rospy.Time(0), rospy.Duration(1))
position, quaternion = self.tl.lookupTransform("/world", self.tf, rospy.Time(0))
self.pose = np.array(position)
return np.array(position)
def orientation(self):
self.tl.waitForTransform("/world", self.tf, rospy.Time(0), rospy.Duration(1))
position, quaternion = self.tl.lookupTransform("/world", self.tf, rospy.Time(0))
self.orient = get_angles(np.array(quaternion))
return get_angles(np.array(quaternion))
def publish_position(self):
publish_pose(self.pose, self.orient, self.name+"_pose")
def velocity(self):
aver_interval = 0.1 # sec
msg_past = self.cache.getElemAfterTime(self.cache.getLatestTime() - rospy.rostime.Duration(aver_interval))
msg_now = self.cache.getElemAfterTime(self.cache.getLatestTime())
if (msg_past is not None) and (msg_now is not None) and (msg_now.header.stamp != msg_past.header.stamp):
vel = vel_estimation_TransformStamped(msg_past, msg_now)
self.vel = vel
class PoopTransformer:
def __init__(self):
self.subscriber = rospy.Subscriber('/poop_perception', PointCloud,
self._callback)
self.transformer = TransformListener()
self.publisher = rospy.Publisher('/poop_perception_odom_combined',
PointCloud)
def _callback(self, data):
self.transformer.waitForTransform('odom_combined', 'map', Time.now(),
rospy.Duration(2))
new_data = PointCloud()
new_data.header.stamp = Time.now()
new_data.header.frame_id = 'odom_combined'
new_data.points = [
self._map_to_odom_combined(p, data.header.stamp)
for p in data.points
]
new_data.channels = data.channels
self.publisher.publish(new_data)
def _map_to_odom_combined(self, point_in_map, stamp):
"""Takes and returns a Point32."""
ps = PointStamped()
ps.header.frame_id = 'map'
ps.header.stamp = stamp
ps.point.x = point_in_map.x
ps.point.y = point_in_map.y
ps.point.z = point_in_map.z
self.transformer.waitForTransform('odom_combined', 'map', stamp,
rospy.Duration(2))
point_in_odom = self.transformer.transformPoint('odom_combined', ps)
z = 25 if point_in_map.z == 25 else point_in_map.z
return Point32(point_in_odom.point.x, point_in_odom.point.y, z)
def turnRadians(angle_radians, angular_vel, clockwise):
twist = Twist()
cmd_vel = rospy.Publisher("cmd_vel_mux/input/teleop", Twist, queue_size=10)
listener = TransformListener()
listener.waitForTransform("/base_link", "/odom", rospy.Time(0),
rospy.Duration(1))
(start_t, start_r) = listener.lookupTransform("/base_link", "/odom",
rospy.Time(0))
start_transform = t.concatenate_matrices(t.translation_matrix(start_t),
t.quaternion_matrix(start_r))
if clockwise:
twist.angular.z = -abs(angular_vel)
else:
twist.angular.z = abs(angular_vel)
rate = rospy.Rate(10)
done = False
while not done:
cmd_vel.publish(twist)
rate.sleep()
(curr_t, curr_r) = listener.lookupTransform("/base_link", "/odom",
rospy.Time(0))
current_transform = t.concatenate_matrices(
t.translation_matrix(curr_t), t.quaternion_matrix(curr_r))
relative = numpy.dot(t.inverse_matrix(start_transform),
current_transform)
rot_moved, dire, point = t.rotation_from_matrix(relative)
print("angle=%s, moved=%s,stop=%s" %
(str(angle_radians), str(rot_moved),
str(rot_moved / 2 > angle_radians)))
if abs(rot_moved) > abs(angle_radians):
done = True
break
return done, "Done!"
class PoseRegistrator:
def __init__(self):
self.qrHypothesis = None
self.endEffectorHypothesis = None
self.tf = TransformListener()
self.errorTrans = None
self.mut = threading.Lock()
def qrCodeCallback(self, qrPose):
try:
#Wait for a transform from the ruler-measured QR code frame to the detected QR code pose
#self.tf.waitForTransform("qr_code_frame", qrPose.header.frame_id, rospy.Time(0), rospy.Duration(0))
tmp = qrPose.header.frame_id
qrPose.header.frame_id = "qr_code_frame"
self.tf.waitForTransform("qr_code_frame", qrPose.header.frame_id, rospy.Time(0), rospy.Duration(0))
with self.mut:
self.errorTrans = self.tf.transformPose(tmp, qrPose)
self.errorTrans = self.errorTrans.pose
except Exception as e:
#The failure mode of waitForTransform is to throw a generic Exception. Grrrr.
rospy.logwarn("Ignored exception %s", e.message)
return
def getTransSample(self):
with self.mut:
return self.errorTrans
def forward(distance, speed):
twist = Twist()
cmd_vel = rospy.Publisher("cmd_vel_mux/input/teleop", Twist, queue_size=10)
listener = TransformListener()
listener.waitForTransform("/base_link", "/odom", rospy.Time(0),
rospy.Duration(1))
(start_t, start_r) = listener.lookupTransform("/base_link", "/odom",
rospy.Time())
start_transform = t.concatenate_matrices(t.translation_matrix(start_t),
t.quaternion_matrix(start_r))
twist.linear.x = abs(speed)
rate = rospy.Rate(10)
done = False
# for i in range(int((10*distance)/speed)):
# cmd_vel.publish(twist)
# rate.sleep()
while not done:
cmd_vel.publish(twist)
rate.sleep()
(curr_t, curr_r) = listener.lookupTransform("/base_link", "/odom",
rospy.Time(0))
current_transform = t.concatenate_matrices(
t.translation_matrix(curr_t), t.quaternion_matrix(curr_r))
relative = numpy.dot(t.inverse_matrix(start_transform),
current_transform)
(x, y, z) = t.translation_from_matrix(relative)
print("distance=%s, moved=%s,stop=%s" %
(str(distance), str(x), str(abs(x) > abs(distance))))
if abs(x) > abs(distance):
done = True
break
return done, "Made it"
def getPose():
"""
Subscriber function for turtlebot pose
:return: pose as geometry_msgs/Pose message
"""
pos = None
rot = None
tf = TransformListener()
pose = Pose()
while pos is None or rot is None:
try:
tf.waitForTransform("/map", "/base_footprint", rospy.Time.now(),
rospy.Duration(4.0))
pos, rot = tf.lookupTransform("/map", "/base_footprint",
rospy.Time())
except Exception as e:
print(e)
pass
pose.position.x = pos[0]
pose.position.y = pos[1]
pose.position.z = pos[2]
pose.orientation.x = rot[0]
pose.orientation.y = rot[1]
pose.orientation.z = rot[2]
pose.orientation.w = rot[3]
return pose
def turnAbs(direction, angular_vel):
EAST = 0
NORTH = radians(90)
WEST = radians(180)
SOUTH = radians(270)
target_angle = EAST
if direction == 'NORTH':
target_angle = NORTH
elif direction == 'SOUTH':
target_angle = SOUTH
elif direction == 'EAST':
target_angle = EAST
elif direction == 'WEST':
target_angle = WEST
else:
rospy.logerr("Direction is not correct")
return False
# Get current location of robot
listener = TransformListener()
try:
listener.waitForTransform("/base_link", "/map", rospy.Time(0),
rospy.Duration(5))
except Exception:
rospy.logerr("Need map transform for TurnAbs to work")
return False
(start_t, start_r) = listener.lookupTransform("/base_link", "/map",
rospy.Time(0))
q = (start_r[0], start_r[1], start_r[2], start_r[3])
yaw = t.euler_from_quaternion(q)[2]
(rot, clockwise) = calculateTurnAngleRadians(target_angle, yaw)
return turnRadians(rot, angular_vel, clockwise)
class myNode:
def __init__(self, *args):
self.tf = TransformListener()
# rospy.Subscriber(...)
# ...
def locations(self, frame1, frame2):
self.tf.waitForTransform(frame1, frame2, rospy.Time(0),
rospy.Duration(0.2))
# print "waited"
(trans, rot) = self.tf.lookupTransform(frame1, frame2, rospy.Time(0))
# print trans,rot
return trans
# print(frame1, frame1)
def add_point(self, frame1, frame2):
global points_z
self.tf.waitForTransform(frame1, frame2, rospy.Time(0),
rospy.Duration(1))
(alu, rot) = self.tf.lookupTransform(frame1, frame2, rospy.Time(0))
# print trans,rot
points[0] = alu[0]
points[1] = alu[1]
points[2] = alu[2]
points[3] = 255 << 16 | 255 << 8 | 255
points_z = np.vstack([points_z, points])
def sub_command(self, sub_command):
print sub_command
class FTSCalibSampler:
def __init__(self, filename='poses.txt'):
print('init')
self.tf = TransformListener()
self.base_link = 'base_link'
self.tool_link = 'fts_toolside'
self.pose_cnt = 0
self.file = open(filename, 'w+')
try:
self.tf.waitForTransform(self.base_link, self.tool_link,
rospy.Time(), rospy.Duration(5.0))
except tf.Exception: # likely a timeout
print(
"Timeout while waiting for the TF transformation with the map!"
" Is someone publishing TF tansforms?\n ROS positions won't be available."
)
self.tf_running = False
def update(self):
if self.tf.frameExists(self.base_link) and self.tf.frameExists(
self.tool_link):
t = self.tf.getLatestCommonTime(self.base_link, self.tool_link)
position, quaternion = self.tf.lookupTransform(
self.base_link, self.tool_link, t)
self.new_pose = position + list(euler_from_quaternion(quaternion))
print(self.new_pose)
def write(self):
self.file.write('pose{}: {}\n'.format(self.pose_cnt,
str(self.new_pose)))
self.pose_cnt += 1
class ChallengeProblemLogger(object):
_knownObstacles = {}
_placedObstacle = False
_lastgzlog = 0.0
_tf_listener = None
def __init__(self,name):
self._name = name;
self._experiment_started = False
self._tf_listener = TransformListener()
# Subscribe to robot pose ground truth from gazebo
rospy.Subscriber("/gazebo/model_states", ModelStates, callback=self.gazebo_model_monitor,
queue_size=1)
# Whenever we get a report from Gazebo, map the gazebo coordinate to map coordinates and
# log this
# Only do this every second - this should be accurate enough
# TODO: model is assumed to be the third in the list. Need to make this based
# on the array to account for obstacles (maybe)
def gazebo_model_monitor(self, data):
if (len(data.pose))<=2:
return
data_time = rospy.get_rostime().to_sec()
if ((self._lastgzlog == 0.0) | (data_time - self._lastgzlog >= 1)):
# Only do this every second
# Get the turtlebot model state information (assumed to be indexed at 2)
tb_pose = data.pose[2]
tb_position = tb_pose.position
self._lastgzlog = data_time
# Do this only if the transform exists
if self._tf_listener.frameExists("/base_link") and self._tf_listener.frameExists("/map"):
self._tf_listener.waitForTransform("/map", "/base_link", rospy.Time(0), rospy.Duration(1))
(trans,rot) = self._tf_listener.lookupTransform("/map", "/base_link",rospy.Time(0))
rospy.loginfo("BRASS | Turtlebot | {},{}".format(trans[0], trans[1]))
# Log any obstacle information, but do it only once. This currently assumes one obstacle
# TODO: test this
if len(data.name) > 3:
addedObstacles = {}
removedObstacles = self._knownObstacles.copy()
for obs in range(3, len(data.name)-1):
if (data.name[obs] not in self._knownObstacles):
addedObstacles[data.name[obs]] = obs
else:
self._knownObstacles[data.name[obs]] = obs
del removedObstacles[data.name[obs]]
for key, value in removedObstacles.iteritems():
rospy.logInfo("BRASS | Obstacle {} | removed".format(key))
del self._knownObstacles[key]
for key, value in addedObstacles.iteritems():
obs_pos = data.pose[value].position
rospy.logInfo ("BRASS | Obstacle {} | {},{}".format(key, obs_pos.x, obs_pos.y))
self._knownObstacles[key] = value
class FTPNode:
def __init__(self, *args):
print("init")
self.tf = TransformListener()
self.tt = Transformer()
rospy.Subscriber("/trackedHumans", TrackedHumans, self.pos_callback)
self.publisher = rospy.Publisher("directionmarker_array", MarkerArray)
def pos_callback(self, data):
rospy.loginfo("on updated pos, face robot towards guy...")
print("hi")
if (len(data.trackedHumans) > 0):
print(data.trackedHumans[0].location.point.x)
try:
self.tf.waitForTransform(data.trackedHumans[0].location.header.frame_id, "/base_link", rospy.Time.now(), rospy.Duration(2.0))
pp = self.tf.transformPoint("/base_link", data.trackedHumans[0].location)
print "Original:"
print [data.trackedHumans[0].location.point]
print "Transform:"
print pp.point
phi = math.atan2(pp.point.y, pp.point.x)
sss.move_base_rel("base", [0,0,phi])
print phi*180/math.pi
markerArray = MarkerArray()
marker = Marker()
marker.header.stamp = rospy.Time();
marker.ns = "my_namespace";
marker.id = 0;
marker.header.frame_id = "/base_link"
marker.type = marker.ARROW
marker.action = marker.ADD
marker.scale.x = .1
marker.scale.y = .1
marker.scale.z = .1
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
p1 = Point()
p1.x = 0
p1.y = 0
p1.z = 0
p2 = Point()
p2.x = pp.point.x
p2.y = pp.point.y
p2.z = 0
marker.points = [p1,p2]
#marker.pose.position.x = 1
#marker.pose.position.y = 0
#marker.pose.position.z = 1
#marker.pose.orientation.w = 1
markerArray.markers.append(marker)
self.publisher.publish(markerArray)
print "try ended"
except Exception as e:
print e
class CamPixelToPointServer:
def __init__(self):
self.camera_model = PinholeCameraModel()
self.bridge = CvBridge()
self.camera_model.fromCameraInfo(SubscriberValue('camera_info', CameraInfo).value)
self.depth_image = SubscriberValue('camera_depth_image', Image, transform=self.bridge.imgmsg_to_cv2)
self.service = rospy.Service('cam_pixel_to_point', CamPixelToPoint, self.handle_service)
self.tf_listener = TransformListener()
print('Service is ready.')
def handle_service(self, req): # type: (CamPixelToPoint) -> CamPixelToPointResponse
x, y = int(req.cam_pixel.x), int(req.cam_pixel.y)
methods = [self.read_depth_simple,
# self.read_depth_average,
self.read_depth_as_floor_depth]
for method in methods:
d = method(x, y)
if not np.isnan(d):
break
pos = np.array(self.camera_model.projectPixelTo3dRay((x, y))) * d
point = PointStamped()
point.header.frame_id = self.camera_model.tfFrame()
point.point.x, point.point.y, point.point.z = pos[0], pos[1], pos[2]
return CamPixelToPointResponse(point)
def read_depth_simple(self, x, y): # (int, int) -> float
return self.depth_image.value[y, x]
def read_depth_average(self, x, y): # (int, int) -> float
print('Fallback to average')
s = 5
return np.nanmean(self.depth_image.value[y-s:y+s, x-s:x+s])
def read_depth_as_floor_depth(self, x, y): # (int, int) -> float
print('Fallback to floor model')
min_distance = 10.0
# Extend the camera ray until it passes through where the floor should be. Use its length as the depth.
camera_origin = PointStamped()
camera_origin.header.frame_id = self.camera_model.tfFrame()
camera_origin.point.x, camera_origin.point.y, camera_origin.point.z = 0.0, 0.0, 0.0
point_along_ray = PointStamped()
point_along_ray.header.frame_id = self.camera_model.tfFrame()
point_along_ray.point.x, point_along_ray.point.y, point_along_ray.point.z = self.camera_model.projectPixelTo3dRay((x, y))
self.tf_listener.waitForTransform('base_footprint', self.camera_model.tfFrame(), rospy.Time(rospy.get_time()), rospy.Duration(1))
camera_origin = self.tf_listener.transformPoint('base_footprint', camera_origin)
point_along_ray = self.tf_listener.transformPoint('base_footprint', point_along_ray)
camera_origin = np_from_poinstamped(camera_origin)
point_along_ray = np_from_poinstamped(point_along_ray)
ray_dir = point_along_ray - camera_origin
# Assuming this transformation was orthogonal, |ray_dir| = 1, at least approximately
d = camera_origin[1]/max(-ray_dir[1], camera_origin[1]/min_distance)
if d <= 0.01:
d = np.nan
return d
class Leader():
def __init__(self, goals):
rospy.init_node('leader', anonymous=True)
self.worldFrame = rospy.get_param("~worldFrame", "/world")
self.frame = rospy.get_param("~frame")
self.leaderFrame = rospy.get_param("~leaderFrame", "/leader")
self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1)
# self.leaderAdvertise=rospy.Publisher('leaderPosition',PoseStamped,queue_size=1)
self.listener = TransformListener()
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
self.goals = goals
self.takeoffFlag = 0
self.goalIndex = 0
rospy.loginfo("demo start!!!!!!!")
def cmdVelCallback(self, data):
if data.linear.z != 0.0 and self.takeoffFlag == 0:
self.takeoffFlag = 1
rospy.sleep(10)
self.takeoffFlag = 2
def run(self):
self.listener.waitForTransform(self.worldFrame, self.frame,
rospy.Time(), rospy.Duration(5.0))
goal = PoseStamped()
goal.header.seq = 0
goal.header.frame_id = self.worldFrame
while not rospy.is_shutdown():
self.calc_goal(goal, self.goalIndex)
self.pubGoal.publish(goal)
# self.leaderAdvertise.publish(goal)
t = self.listener.getLatestCommonTime(self.worldFrame, self.frame)
if self.listener.canTransform(self.worldFrame, self.frame, t):
position, quaternion = self.listener.lookupTransform(
self.worldFrame, self.frame, t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
if self.takeoffFlag == 1:
self.goalIndex = 0
elif self.takeoffFlag == 2 and self.goalIndex < len(
self.goals) - 1:
rospy.sleep(self.goals[self.goalIndex][4] * 2)
rospy.loginfo(self.goalIndex)
self.goalIndex += 1
def calc_goal(self, goal, index):
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = self.goals[index][0]
goal.pose.position.y = self.goals[index][1]
goal.pose.position.z = self.goals[index][2]
quaternion = tf.transformations.quaternion_from_euler(
0, 0, self.goals[index][3])
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
class LaunchObserver(object):
"""
Keeps track of the flying/landed state of a single drone, and publishes
a tf message keeping track of the coordinates from which the drone most recently launched.
"""
def __init__(self):
self.tfl = TransformListener()
self.tfb = TransformBroadcaster()
self.flying_state_sub = rospy.Subscriber(
'states/ardrone3/PilotingState/FlyingStateChanged',
Ardrone3PilotingStateFlyingStateChanged, self.on_flying_state)
self.is_flying = True
self.RATE = 5 # republish hz
self.saved_translation = [0, 0, 0] # In meters
self.saved_yaw = 0 # In radians
self.name = rospy.get_namespace().strip('/')
self.base_link = self.name + '/base_link'
self.launch = self.name + '/launch'
self.odom = self.name + '/odom'
def on_flying_state(self, msg):
self.is_flying = msg.state in [
Ardrone3PilotingStateFlyingStateChanged.state_takingoff,
Ardrone3PilotingStateFlyingStateChanged.state_hovering,
Ardrone3PilotingStateFlyingStateChanged.state_flying,
Ardrone3PilotingStateFlyingStateChanged.state_landing,
Ardrone3PilotingStateFlyingStateChanged.state_emergency_landing
]
def spin(self):
r = rospy.Rate(self.RATE)
self.tfl.waitForTransform(self.odom, self.base_link, rospy.Time(0),
rospy.Duration.from_sec(99999))
while not rospy.is_shutdown():
if not self.is_flying:
# On the ground, update the transform
pos, quat = self.tfl.lookupTransform(self.base_link, self.odom,
rospy.Time(0))
pos[2] = 0
self.saved_translation = pos
_, _, self.saved_yaw = euler_from_quaternion(quat)
time = max(rospy.Time.now(),
self.tfl.getLatestCommonTime(
self.odom, self.base_link)) + (
2 * rospy.Duration.from_sec(1.0 / self.RATE))
self.tfb.sendTransform(self.saved_translation,
quaternion_from_euler(0, 0, self.saved_yaw),
time, self.odom, self.launch)
r.sleep()
class Follower():
def __init__(self, leaderFrame, radius=0.5, phase=0, pointNum=2000):
rospy.init_node('follower', anonymous=True)
self.worldFrame = rospy.get_param("~worldFrame", "/world")
self.frame = rospy.get_param("~frame")
self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1)
# rospy.Subscriber("/"+leaderName+'/leaderPosition',PoseStamped,self.followerSubCB)
self.listener = TransformListener()
self.goal = PoseStamped()
self.leaderFrame = leaderFrame
self.radius = radius
self.phase = 0
self.pointNum = 2000
self.goalIndex = 0
rospy.loginfo("demo start!!!!!!!")
def run(self):
self.listener.waitForTransform(self.worldFrame, self.frame,
rospy.Time(), rospy.Duration(5.0))
self.goal = PoseStamped()
self.goal.header.seq = 0
self.goal.header.frame_id = self.worldFrame
self.goal.pose.orientation.w = 1
while not rospy.is_shutdown():
self.pubGoal.publish(self.goal)
# rospy.loginfo(self.goal)
# rospy.loginfo(self.worldFrame)
# rospy.loginfo(self.leaderFrame)
# rospy.loginfo(self.frame)
t = self.listener.getLatestCommonTime(self.worldFrame,
self.leaderFrame)
if self.listener.canTransform(self.worldFrame, self.leaderFrame,
t):
position, quaternion = self.listener.lookupTransform(
self.worldFrame, self.leaderFrame, t)
# rospy.loginfo(position)
# rospy.loginfo(quaternion)
self.followerGoalGenerate(position, quaternion)
rospy.sleep(0.02)
def followerGoalGenerate(self, position, quaternion):
# rospy.loginfo("info received!")
angle = self.goalIndex / self.pointNum * 2 * math.pi + self.phase
self.goal.header.seq += 1
self.goal.header.stamp = rospy.Time.now()
self.goal.pose.position.x = position[0] + self.radius * math.cos(angle)
self.goal.pose.position.y = position[1] + self.radius * math.sin(angle)
# self.goal.pose.position.z=position.z
self.goal.pose.position.z = 0.8
self.goal.pose.orientation.w = quaternion[3]
self.goal.pose.orientation.x = quaternion[0]
self.goal.pose.orientation.y = quaternion[1]
self.goal.pose.orientation.z = quaternion[2]
self.goalIndex = self.goalIndex + 1
def asMatrix(self, target_frame, source_frame):
tran = TransformListener()
tran.waitForTransform(target_frame=target_frame,
source_frame=source_frame,
time=rospy.Time(0),
timeout=rospy.Duration(4.0))
translation, rotation = tran.lookupTransform(target_frame=target_frame,
source_frame=source_frame,
time=rospy.Time(0))
return self.fromTranslationRotation(translation, rotation)
class CameraPointer(object):
def __init__(self, side, camera_ik):
self.side = side
self.camera_ik = camera_ik
self.joint_names = self.joint_angles_act = self.joint_angles_des = None
self.tfl = TransformListener()
self.joint_state_sub = rospy.Subscriber('/{0}_arm_controller/state'.format(self.side), JointTrajectoryControllerState, self.joint_state_cb)
self.joint_traj_pub = rospy.Publisher('/{0}_arm_controller/command'.format(self.side), JointTrajectory)
# Wait for joint information to become available
while self.joint_names is None and not rospy.is_shutdown():
rospy.sleep(0.5)
rospy.loginfo("[{0}] Waiting for joint state from arm controller.".format(rospy.get_name()))
#Set rate limits on a per-joint basis
self.max_vel_rot = [np.pi]*len(self.joint_names)
self.target_sub = rospy.Subscriber('{0}/lookat_ik/goal'.format(rospy.get_name()), PointStamped, self.goal_cb)
rospy.loginfo("[{0}] Ready.".format(rospy.get_name()))
def joint_state_cb(self, jtcs):
if self.joint_names is None:
self.joint_names = jtcs.joint_names
self.joint_angles_act = jtcs.actual.positions
self.joint_angles_des = jtcs.desired.positions
def goal_cb(self, pt_msg):
rospy.loginfo("[{0}] New LookatIK goal received.".format(rospy.get_name()))
if (pt_msg.header.frame_id != self.camera_ik.base_frame):
try:
now = pt_msg.header.stamp
self.tfl.waitForTransform(pt_msg.header.frame_id,
self.camera_ik.base_frame,
now, rospy.Duration(1.0))
pt_msg = self.tfl.transformPoint(self.camera_ik.base_frame, pt_msg)
except (LookupException, ConnectivityException, ExtrapolationException):
rospy.logwarn("[{0}] TF Error tranforming point from {1} to {2}".format(rospy.get_name(),
pt_msg.header.frame_id,
self.camera_ik.base_frame))
target = np.array([pt_msg.point.x, pt_msg.point.y, pt_msg.point.z])
# Get IK Solution
current_angles = list(copy.copy(self.joint_angles_act))
iksol = self.camera_ik.lookat_ik(target, current_angles[:len(self.camera_ik.arm_indices)])
# Start with current angles, then replace angles in camera IK with IK solution
# Use desired joint angles to avoid sagging over time
jtp = JointTrajectoryPoint()
jtp.positions = list(copy.copy(self.joint_angles_des))
for i, joint_name in enumerate(self.camera_ik.arm_joint_names):
jtp.positions[self.joint_names.index(joint_name)] = iksol[i]
deltas = np.abs(np.subtract(jtp.positions, current_angles))
duration = np.max(np.divide(deltas, self.max_vel_rot))
jtp.time_from_start = rospy.Duration(duration)
jt = JointTrajectory()
jt.joint_names = self.joint_names
jt.points.append(jtp)
rospy.loginfo("[{0}] Sending Joint Angles.".format(rospy.get_name()))
self.joint_traj_pub.publish(jt)
class Normal():
def __init__(self):
rospy.init_node('follower', anonymous=True)
self.worldFrame = rospy.get_param("~worldFrame", "/world")
self.frame = rospy.get_param("~frame")
self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1)
# rospy.Subscriber("/"+leaderName+'/leaderPosition',PoseStamped,self.followerSubCB)
self.listener = TransformListener()
self.goal=PoseStamped()
# 第一件事情，跟随这个目标，这个目标的格式应该是一个frame
self.leaderFrame=rospy.get_param("~leaderFrame","")
self.offsetX=rospy.get_param("~offsetX","0")
self.offsetY=rospy.get_param("~offsetY","0")
# self.offsetZ=rospy.get_param("~offsetZ","0")
# 第二件事情，广播自身的位置吧
# self.pubAttitude=rospy.Publisher('pose',)
# 第三件事情，侦听manager节点的状态信息
self.takeoffFlag = 0
self.goalIndex = 0
rospy.loginfo("demo start!!!!!!!")
def run(self):
self.listener.waitForTransform(self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0))
self.goal = PoseStamped()
self.goal.header.seq = 0
self.goal.header.frame_id = self.worldFrame
self.goal.pose.orientation.w=1
while not rospy.is_shutdown():
self.pubGoal.publish(self.goal)
# rospy.loginfo(self.goal)
# rospy.loginfo(self.worldFrame)
# rospy.loginfo(self.leaderFrame)
# rospy.loginfo(self.frame)
t = self.listener.getLatestCommonTime(self.worldFrame, self.leaderFrame)
if self.listener.canTransform(self.worldFrame, self.leaderFrame, t):
position, quaternion = self.listener.lookupTransform(self.worldFrame, self.leaderFrame, t)
# rospy.loginfo(position)
# rospy.loginfo(quaternion)
self.followerGoalGenerate(position,quaternion)
rospy.sleep(0.02)
def followerGoalGenerate(self,position,quaternion):
# rospy.loginfo("info received!")
self.goal.header.seq += 1
self.goal.header.stamp = rospy.Time.now()
self.goal.pose.position.x=position[0]+float(self.offsetX)
self.goal.pose.position.y=position[1]+float(self.offsetY)
# self.goal.pose.position.z=position.z
self.goal.pose.position.z=0.8
self.goal.pose.orientation.w=quaternion[3]
self.goal.pose.orientation.x=quaternion[0]
self.goal.pose.orientation.y=quaternion[1]
self.goal.pose.orientation.z=quaternion[2]
class publish_tag_tf(object):
def __init__(self):
rospy.init_node('republish_tag_tf')
self.tf = TransformListener()
self.br = TransformBroadcaster()
rospy.Subscriber("/tag_detections", AprilTagDetectionArray, self.tags_callback, queue_size=10)
self.tag_detections = rospy.Publisher('/selective_grasping/tag_detections', Int16MultiArray, queue_size=10)
self.detections_list = np.zeros(8)
def tags_callback(self, msg):
detections = len(msg.detections)
self.detections_list = np.zeros(8)
for idx in range(detections):
detected_id = msg.detections[idx].id[0]
self.detections_list[detected_id] = 1
def publish(self, tag):
try:
self.tf.waitForTransform("camera_link", tag, rospy.Time(0), rospy.Duration(1.0)) # rospy.Time.now()
ptFinal, oriFinal = self.tf.lookupTransform("camera_link", tag, rospy.Time(0))
oriFinal_euler = euler_from_quaternion(oriFinal)
new_ori = [0, -0.244, 0]
new_ori[0] = oriFinal_euler[2]
new_ori = quaternion_from_euler(new_ori[0], new_ori[1], new_ori[2])
self.br.sendTransform((ptFinal[2],
-ptFinal[0],
-ptFinal[1]),
new_ori,
rospy.Time.now(),
tag + "_corrected",
"camera_link")
except:
pass
def detection_main(self):
tags = ['tag_0', 'tag_1', 'tag_2', 'tag_3',
'tag_4', 'tag_5', 'tag_6', 'tag_7']
tags_to_send = Int16MultiArray()
rate = rospy.Rate(1)
while not rospy.is_shutdown():
print(self.detections_list)
tags_to_send.data = self.detections_list
self.tag_detections.publish(tags_to_send)
for idx, detected_id in enumerate(self.detections_list):
if detected_id:
self.publish(tags[idx])
else:
print("Tag ID: '{}' not found".format(tags[idx]))
rate.sleep()
class FaceInteractionDemo(object):
def __init__(self):
self.move_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)
self.lock = RLock()
self.head = Head()
self.expressions = Expressions()
self.tf = TransformListener()
self.lastFacePos = None
rospy.sleep(0.5)
# Gets an equivalent point in the reference frame "destFrame"
def getPointStampedInFrame(self, pointStamped, destFrame):
# Wait until we can transform the pointStamped to destFrame
self.tf.waitForTransform(pointStamped.header.frame_id, destFrame, rospy.Time(), rospy.Duration(4.0))
# Set point header to the last common frame time between its frame and destFrame
pointStamped.header.stamp = self.tf.getLatestCommonTime(pointStamped.header.frame_id, destFrame)
# Transform the point to destFrame
return self.tf.transformPoint(destFrame, pointStamped)
def onFacePos(self, posStamped):
self.lastFacePos = posStamped
def navigateTo(self):
if (self.lastFacePos is not None and self.lock.acquire(blocking=False)):
self.expressions.nod_head()
pointStamped = self.getPointStampedInFrame(pointStamped, "base_link")
distance_to_base = sqrt(pointStamped.point.x ** 2 + pointStamped.point.y ** 2 + pointStamped.point.z ** 2)
unit_vector = { "x": pointStamped.point.x / distance_to_base,
"y": pointStamped.point.y / distance_to_base }
pointStamped.point.x = unit_vector["x"] * (distance_to_base - 0.5)
pointStamped.point.y = unit_vector["y"] * (distance_to_base - 0.5)
quaternion = Quaternion()
quaternion.w = 1
pose = Pose()
pose.position = pointStamped.point
pose.orientation = quaternion
poseStamped = PoseStamped()
poseStamped.header = pointStamped.header
pointStamped.pose = pose
self.move_pub.publish(poseStamped)
self.lock.release()
def onFaceCount(self, int8):
if (int8.data > 0):
self.expressions.be_happy()
else:
self.expressions.be_sad()
self.expressions.be_neutral()
class FiducialTablierCalibrator(object):
def __init__(self, camera, fiducial):
self._camera = camera
self._fiducial = fiducial
self._tf_list = TransformListener()
def __call__(self):
now = rospy.Time(0)
self._tf_list.waitForTransform(self._camera, self._fiducial, now,
rospy.Duration(3.0))
return self._tf_list.lookupTransform(self._camera, self._fiducial, now)
class Trajectory_generator(object):
# Generate the object's trajectory from the central point of the detected box, then project
def __init__(self,node_name="point_reproject"):
self.point = PointStamped()
self.node_name = node_name
#self.listener = tf.TransformListener()
rospy.init_node(self.node_name)
self.tf = TransformListener()
self.point_sub = rospy.Subscriber("/rgbdt_fusion/resColor/full",DetectionFull,self.Point_callback)
self.point_pub = rospy.Publisher("point_reproject",PointStamped,queue_size = 2)
def Point_callback(self,detectionsfull):
#self.listener.waitForTransform("/map","/velodyne",rospy.Time(),rospy.Duration(0.5))
#try:
# now = rospy.Time.now()
# self.listener.waitForTransform("/map","/velodyne",now,rospy.Duration(0.5))
# (trans,rot) = listener.lookupTransform("/map","/velodyne",now)
time = rospy.Time.now()
rospy.logerr(time)
if (detectionsfull.detections.size >= 1):
for i in range(detectionsfull.detections.size):
if detectionsfull.detections.data[i].object_class == "person":
point_stamped = detectionsfull.detections.data[i].ptStamped
#self.tf.waitForTransform("base_link", "thermal_camera", rospy.Time.now(), rospy.Duration(4.0))
try:
now = rospy.Time.now()
self.tf.waitForTransform("map","thermal_camera", now,rospy.Duration(4.0))
point_transformed = self.tf.transformPoint("map",point_stamped)
self.point = point_transformed
'''self.path.header = point_transformed.header
pose = PoseStamped()
pose.header = point_transformed.header
pose.pose.position.x = point_transformed.point.x
pose.pose.position.y = point_transformed.point.y
pose.pose.position.z = point_transformed.point.z
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
self.path.poses.append(pose)'''
self.point_pub.publish(self.point)
rospy.loginfo("The pointhas been published!")
#break
except(tf.ConnectivityException,tf.LookupException,tf.ExtrapolationException,rospy.ROSTimeMovedBackwardsException):
rospy.loginfo("Some Exceptions happend!")
else:
rospy.logdebug("There is no detection of person available!")
class drone:
def __init__(self, name, leader=False):
self.name = name
self.tf = '/vicon/' + name + '/' + name
self.leader = leader
self.tl = TransformListener()
self.pose = np.array([0, 0, 0])
self.orient = np.array([0, 0, 0])
self.sp = np.array([0, 0, 0])
self.path = Path()
self.obstacle_update_status = [False, None]
self.rad_imp = radius_impedance_model()
def position(self):
self.tl.waitForTransform("/world", self.tf, rospy.Time(0),
rospy.Duration(1))
position, quaternion = self.tl.lookupTransform("/world", self.tf,
rospy.Time(0))
self.pose = position
return np.array(position)
def orientation(self):
self.tl.waitForTransform("/world", self.tf, rospy.Time(0),
rospy.Duration(1))
position, quaternion = self.tl.lookupTransform("/world", self.tf,
rospy.Time(0))
self.orient = get_angles(np.array(quaternion))
return get_angles(np.array(quaternion))
def publish_sp(self):
publish_pose(self.sp, np.array([0, 0, 0]), self.name + "_sp")
def publish_position(self):
publish_pose(self.pose, self.orient, self.name + "_pose")
def publish_path(self, limit=1000):
publish_path(self.path, self.sp, self.orient, self.name + "_path",
limit)
def fly(self):
# if self.leader:
# limits = np.array([ 2, 2, 2.5 ])
# np.putmask(self.sp, self.sp >= limits, limits)
# np.putmask(self.sp, self.sp <= -limits, -limits)
publish_goal_pos(self.sp, 0, "/" + self.name)
def update_radius_imp(self, delta):
if self.rad_imp.inside:
self = impedance_obstacle_delta(self, delta)
self.sp += self.rad_imp.pose
def main():
rospy.init_node('tf_test')
rospy.loginfo("Node for testing actionlib server")
#from_link = '/head_color_camera_l_link'
#to_link = '/base_link'
from_link = '/base_link'
to_link = '/map'
tfl = TransformListener()
rospy.sleep(5)
t = rospy.Time(0)
mpose = PoseStamped()
mpose.pose.position.x = 1
mpose.pose.position.y = 0
mpose.pose.position.z = 0
mpose.pose.orientation.x = 0
mpose.pose.orientation.y = 0
mpose.pose.orientation.z = 0
mpose.pose.orientation.w = 0
mpose.header.frame_id = from_link
mpose.header.stamp = rospy.Time.now()
rospy.sleep(5)
mpose_transf = None
rospy.loginfo('Waiting for transform for some time...')
tfl.waitForTransform(to_link, from_link, t, rospy.Duration(5))
if tfl.canTransform(to_link, from_link, t):
mpose_transf = tfl.transformPose(to_link, mpose)
print mpose_transf
else:
rospy.logerr('Transformation is not possible!')
sys.exit(0)
def transform_point(src_frame, target_frame, point, tf_listener=None):
if tf_listener is None:
tf_listener = TransformListener()
p = PointStamped()
p.header.frame_id = src_frame
p.header.stamp = rospy.Time(0)
p.point = Point(*point)
tf_listener.waitForTransform(target_frame, src_frame, rospy.Time(0),
rospy.Duration(4.0))
newp = tf_listener.transformPoint(target_frame, p)
p = newp.point
return numpy.array([p.x, p.y, p.z])
class Demo():
def __init__(self, goals):
rospy.init_node('demo', anonymous=True)
self.worldFrame = rospy.get_param("~worldFrame", "/world")
self.frame = rospy.get_param("~frame")
self.pubGoal = rospy.Publisher(
'goal', PoseStamped,
queue_size=1) #publish to topic goal with msg type PoseStamped
self.listener = TransformListener(
) #tflisterner is a method with functions relating to transforms
self.goals = goals #Assign nX5 matrix containing target waypoints
self.goalIndex = 0 # start with the first row of entries (first waypoint)
def run(self):
self.listener.waitForTransform(
self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0)
) #Find the transform from world frame to body frame, returns bool on if it can find a transform
goal = PoseStamped()
goal.header.seq = 0
goal.header.frame_id = self.worldFrame
while not rospy.is_shutdown():
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = self.goals[self.goalIndex][0]
goal.pose.position.y = self.goals[self.goalIndex][1]
goal.pose.position.z = self.goals[self.goalIndex][2]
quaternion = tf.transformations.quaternion_from_euler(
0, 0, self.goals[self.goalIndex][3])
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
self.pubGoal.publish(goal)
t = self.listener.getLatestCommonTime(self.worldFrame, self.frame)
if self.listener.canTransform(self.worldFrame, self.frame, t):
position, quaternion = self.listener.lookupTransform(
self.worldFrame, self.frame, t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
#If within position error bound, sleep and then move to next waypoint
if math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.2 \
and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.2 \
and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.2 \
and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10) \
and self.goalIndex < len(self.goals) - 1:
rospy.sleep(self.goals[self.goalIndex][4])
self.goalIndex += 1
class Demo:
def __init__(self, goals):
rospy.init_node("demo", anonymous=True)
self.frame = rospy.get_param("~frame")
self.pubGoal = rospy.Publisher("goal", PoseStamped, queue_size=1)
self.listener = TransformListener()
self.goals = goals
self.goalIndex = 0
def run(self):
self.listener.waitForTransform("/world", self.frame, rospy.Time(), rospy.Duration(5.0))
goal = PoseStamped()
goal.header.seq = 0
goal.header.frame_id = "world"
while not rospy.is_shutdown():
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = self.goals[self.goalIndex][0]
goal.pose.position.y = self.goals[self.goalIndex][1]
goal.pose.position.z = self.goals[self.goalIndex][2]
quaternion = tf.transformations.quaternion_from_euler(0, 0, self.goals[self.goalIndex][3])
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
self.pubGoal.publish(goal)
t = self.listener.getLatestCommonTime("/world", self.frame)
if self.listener.canTransform("/world", self.frame, t):
position, quaternion = self.listener.lookupTransform("/world", self.frame, t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
if (
math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.3
and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.3
and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.3
and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10)
and self.goalIndex < len(self.goals) - 1
):
rospy.sleep(self.goals[self.goalIndex][4])
self.goalIndex += 1
class TwistToPoseConverter(object):
def __init__(self):
self.ee_frame = rospy.get_param('~ee_frame', "/l_gripper_tool_frame")
self.twist_sub = rospy.Subscriber('/twist_in', TwistStamped, self.twist_cb)
self.pose_pub = rospy.Publisher('/pose_out', PoseStamped)
self.tf_listener = TransformListener()
def get_ee_pose(self, frame='/torso_lift_link', time=None):
"""Get current end effector pose from tf."""
try:
now = rospy.Time.now() if time is None else time
self.tf_listener.waitForTransform(frame, self.ee_frame, now, rospy.Duration(4.0))
pos, quat = self.tf_listener.lookupTransform(frame, self.ee_frame, now)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[%s] TF Failure getting current end-effector pose:\r\n %s"
%(rospy.get_name(), e))
return None, None
return pos, quat
def twist_cb(self, ts):
"""Get current end effector pose and augment with twist command."""
cur_pos, cur_quat = self.get_ee_pose(ts.header.frame_id)
ps = PoseStamped()
ps.header.frame_id = ts.header.frame_id
ps.header.stamp = ts.header.stamp
ps.pose.position.x = cur_pos[0] + ts.twist.linear.x
ps.pose.position.y = cur_pos[1] + ts.twist.linear.y
ps.pose.position.z = cur_pos[2] + ts.twist.linear.z
twist_quat = trans.quaternion_from_euler(ts.twist.angular.x,
ts.twist.angular.y,
ts.twist.angular.z)
final_quat = trans.quaternion_multiply(twist_quat, cur_quat)
ps.pose.orientation = Quaternion(*final_quat)
try:
self.tf_listener.waitForTransform('/torso_lift_link', ps.header.frame_id,
ps.header.stamp, rospy.Duration(3.0))
pose_out = self.tf_listener.transformPose('/torso_lift_link', ps)
self.pose_pub.publish(pose_out)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[%s] TF Failure transforming goal pose to torso_lift_link:\r\n %s"
%(rospy.get_name(), e))
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Antons Rebguns
#
import roslib; roslib.load_manifest('wubble2_robot')
import rospy
import math
from tf import TransformListener
if __name__ == '__main__':
rospy.init_node('gripper_opening', anonymous=True)
listener = TransformListener()
start = rospy.Time.now().to_sec()
listener.waitForTransform('/L7_wrist_roll_link', '/left_fingertip_link', rospy.Time(0), rospy.Duration(0.1))
middle = rospy.Time.now().to_sec()
listener.waitForTransform('/L7_wrist_roll_link', '/right_fingertip_link', rospy.Time(0), rospy.Duration(0.1))
end = rospy.Time.now().to_sec()
print middle-start,end-start
start = rospy.Time.now().to_sec()
l_pos,l_ori = listener.lookupTransform('/L7_wrist_roll_link', '/left_fingertip_link', rospy.Time(0))
middle = rospy.Time.now().to_sec()
r_pos,r_ori = listener.lookupTransform('/L7_wrist_roll_link', '/right_fingertip_link', rospy.Time(0))
end = rospy.Time.now().to_sec()
print middle-start,end-start
dx = l_pos[0] - r_pos[0]
dy = l_pos[1] - r_pos[1]
dz = l_pos[2] - r_pos[2]
class ORKTabletop(object):
""" Listens to the table and object messages from ORK. Provides ActionServer
that assembles table and object into same message.
NB - the table is an axis-aligned bounding box in the kinect's frame"""
def __init__(self, name):
self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.callback)
self.pub = rospy.Publisher('/recognized_object_array_as_point_cloud', PointCloud2)
self.pose_pub = rospy.Publisher('/recognized_object_array_as_pose_stamped', PoseStamped)
# We listen for ORK's MarkerArray of tables on this topic
self.table_topic = "/marker_tables"
self.tl = TransformListener()
# create messages that are used to publish feedback/result.
# accessed by multiple threads
self._result = TabletopResult()
self.result_lock = threading.Lock()
# used s.t. we don't return a _result message that hasn't been updated yet.
self.has_data = False
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name, TabletopAction,
execute_cb=self.execute_cb, auto_start=False)
self._as.start()
# TODO: Is this really the best structure for handling the callbacks?
# Would it be possible to have separate callbacks for table and objects, each updating a most-recently-seen variable?
# Or maybe use the message_filters.TimeSynchronizer class if corresponding table/object data has identical timestamps?
def callback(self, data):
rospy.loginfo("Objects %d", data.objects.__len__())
table_corners = []
# obtain table_offset and table_pose
to = rospy.wait_for_message(self.table_topic, MarkerArray);
# obtain Table corners ...
rospy.loginfo("Tables hull size %d", to.markers.__len__())
if not to.markers:
rospy.loginfo("No tables detected")
return
else:
# NB - D says that ORK has this set to filter based on height.
# IIRC, it's 0.6-0.8m above whatever frame is set as the floor
point_array_size = 4 # for the 4 corners of the bounding box
for i in range (0, point_array_size):
p = Point()
p.x = to.markers[0].points[i].x
p.y = to.markers[0].points[i].y
p.z = to.markers[0].points[i].z
table_corners.append(p)
# this is a table pose at the edge close to the robot, in the center of x axis
table_pose = PoseStamped()
table_pose.header = to.markers[0].header
table_pose.pose = to.markers[0].pose
# Determine table dimensions
rospy.loginfo('calculating table pose bounding box in frame: %s' % table_pose.header.frame_id)
min_x = sys.float_info.max
min_y = sys.float_info.max
max_x = -sys.float_info.max
max_y = -sys.float_info.max
for i in range (table_corners.__len__()):
if table_corners[i].x > max_x:
max_x = table_corners[i].x
if table_corners[i].y > max_y:
max_y = table_corners[i].y
if table_corners[i].x < min_x:
min_x = table_corners[i].x
if table_corners[i].y < min_y:
min_y = table_corners[i].y
table_dim = Point()
# TODO: if we don't (can't!) compute the height, should we at least give it non-zero depth?
# (would also require shifting the observed centroid down by table_dim.z/2)
table_dim.z = 0.0
table_dim.x = abs(max_x - min_x)
table_dim.y = abs(max_y - min_y)
print "Dimensions: ", table_dim.x, table_dim.y
# Temporary frame used for transformations
table_link = 'table_link'
# centroid of a table in table_link frame
centroid = PoseStamped()
centroid.header.frame_id = table_link
centroid.header.stamp = table_pose.header.stamp
centroid.pose.position.x = (max_x + min_x)/2.
centroid.pose.position.y = (max_y + min_y)/2.
centroid.pose.position.z = 0.0
centroid.pose.orientation.x = 0.0
centroid.pose.orientation.y = 0.0
centroid.pose.orientation.z = 0.0
centroid.pose.orientation.w = 1.0
# generate transform from table_pose to our newly-defined table_link
tt = TransformStamped()
tt.header = table_pose.header
tt.child_frame_id = table_link
tt.transform.translation = table_pose.pose.position
tt.transform.rotation = table_pose.pose.orientation
self.tl.setTransform(tt)
self.tl.waitForTransform(table_link, table_pose.header.frame_id, table_pose.header.stamp, rospy.Duration(3.0))
if self.tl.canTransform(table_pose.header.frame_id, table_link, table_pose.header.stamp):
centroid_table_pose = self.tl.transformPose(table_pose.header.frame_id, centroid)
self.pose_pub.publish(centroid_table_pose)
else:
rospy.logwarn("No transform between %s and %s possible",table_pose.header.frame_id, table_link)
return
# transform each object into desired frame; add to list of clusters
cluster_list = []
for i in range (data.objects.__len__()):
rospy.loginfo("Point clouds %s", data.objects[i].point_clouds.__len__())
pc = PointCloud2()
pc = data.objects[i].point_clouds[0]
arr = pointclouds.pointcloud2_to_array(pc, 1)
arr_xyz = pointclouds.get_xyz_points(arr)
arr_xyz_trans = []
for j in range (arr_xyz.__len__()):
ps = PointStamped();
ps.header.frame_id = table_link
ps.header.stamp = table_pose.header.stamp
ps.point.x = arr_xyz[j][0]
ps.point.y = arr_xyz[j][1]
ps.point.z = arr_xyz[j][2]
if self.tl.canTransform(table_pose.header.frame_id, table_link, table_pose.header.stamp):
ps_in_kinect_frame = self.tl.transformPoint(table_pose.header.frame_id, ps)
else:
rospy.logwarn("No transform between %s and %s possible",table_pose.header.frame_id, table_link)
return
arr_xyz_trans.append([ps_in_kinect_frame.point.x, ps_in_kinect_frame.point.y, ps_in_kinect_frame.point.z])
pc_trans = PointCloud2()
pc_trans = pointclouds.xyz_array_to_pointcloud2(np.asarray([arr_xyz_trans]),
table_pose.header.stamp, table_pose.header.frame_id)
self.pub.publish(pc_trans)
cluster_list.append(pc_trans)
rospy.loginfo("cluster size %d", cluster_list.__len__())
# finally - save all data in the object that'll be sent in response to actionserver requests
with self.result_lock:
self._result.objects = cluster_list
self._result.table_dims = table_dim
self._result.table_pose = centroid_table_pose
self.has_data = True
def execute_cb(self, goal):
rospy.loginfo('Executing ORKTabletop action')
# want to get the NEXT data coming in, rather than the current one.
with self.result_lock:
self.has_data = False
rr = rospy.Rate(1.0)
while not rospy.is_shutdown() and not self._as.is_preempt_requested():
with self.result_lock:
if self.has_data:
break
rr.sleep()
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
elif rospy.is_shutdown():
self._as.set_aborted()
else:
with self.result_lock:
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
class MarkerProcessor(object):
def __init__(self, use_dummy_transform=False):
rospy.init_node('star_center_positioning_node')
if use_dummy_transform:
self.odom_frame_name = "odom_dummy"
else:
self.odom_frame_name = "odom"
self.marker_locators = {}
self.add_marker_locator(MarkerLocator(0,(0.0,0.0),0))
self.add_marker_locator(MarkerLocator(1,(1.4/1.1,2.0/1.1),0))
self.marker_sub = rospy.Subscriber("ar_pose_marker",
ARMarkers,
self.process_markers)
self.odom_sub = rospy.Subscriber("odom", Odometry, self.process_odom, queue_size=10)
self.star_pose_pub = rospy.Publisher("STAR_pose",PoseStamped,queue_size=10)
self.continuous_pose = rospy.Publisher("STAR_pose_continuous",PoseStamped,queue_size=10)
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
def add_marker_locator(self, marker_locator):
self.marker_locators[marker_locator.id] = marker_locator
def process_odom(self, msg):
p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=self.odom_frame_name),
pose=msg.pose.pose)
try:
STAR_pose = self.tf_listener.transformPose("STAR", p)
STAR_pose.header.stamp = msg.header.stamp
self.continuous_pose.publish(STAR_pose)
except Exception as inst:
print "error is", inst
def process_markers(self, msg):
for marker in msg.markers:
# do some filtering basd on prior knowledge
# we know the approximate z coordinate and that all angles but yaw should be close to zero
euler_angles = euler_from_quaternion((marker.pose.pose.orientation.x,
marker.pose.pose.orientation.y,
marker.pose.pose.orientation.z,
marker.pose.pose.orientation.w))
angle_diffs = TransformHelpers.angle_diff(euler_angles[0],pi), TransformHelpers.angle_diff(euler_angles[1],0)
if (marker.id in self.marker_locators and
2.4 <= marker.pose.pose.position.z <= 2.6 and
fabs(angle_diffs[0]) <= .2 and
fabs(angle_diffs[1]) <= .2):
locator = self.marker_locators[marker.id]
xy_yaw = locator.get_camera_position(marker)
orientation_tuple = quaternion_from_euler(0,0,xy_yaw[2])
pose = Pose(position=Point(x=xy_yaw[0],y=xy_yaw[1],z=0),
orientation=Quaternion(x=orientation_tuple[0], y=orientation_tuple[1], z=orientation_tuple[2], w=orientation_tuple[3]))
# TODO: use markers timestamp instead of now() (unfortunately, not populated currently by ar_pose)
pose_stamped = PoseStamped(header=Header(stamp=rospy.Time.now(),frame_id="STAR"),pose=pose)
try:
offset, quaternion = self.tf_listener.lookupTransform("/base_link", "/base_laser_link", rospy.Time(0))
except Exception as inst:
print "Error", inst
return
# TODO: use frame timestamp instead of now()
pose_stamped_corrected = deepcopy(pose_stamped)
pose_stamped_corrected.pose.position.x -= offset[0]*cos(xy_yaw[2])
pose_stamped_corrected.pose.position.y -= offset[0]*sin(xy_yaw[2])
self.star_pose_pub.publish(pose_stamped_corrected)
self.fix_STAR_to_odom_transform(pose_stamped_corrected)
def fix_STAR_to_odom_transform(self, msg):
""" Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """
(translation, rotation) = TransformHelpers.convert_pose_inverse_transform(msg.pose)
p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=rospy.Time(),frame_id="base_link"))
try:
self.tf_listener.waitForTransform("odom","base_link",rospy.Time(),rospy.Duration(1.0))
except Exception as inst:
print "whoops", inst
return
print "got transform"
self.odom_to_STAR = self.tf_listener.transformPose("odom", p)
(self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_STAR.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map to odom transformation.
This is necessary so things like move_base can work properly. """
if not(hasattr(self,'translation') and hasattr(self,'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame_name, "STAR")
def run(self):
r = rospy.Rate(10)
while not rospy.is_shutdown():
self.broadcast_last_transform()
r.sleep()
class GripperActionController():
def __init__(self, controller_namespace, controllers):
self.controller_namespace = controller_namespace
self.controllers = {}
for c in controllers:
self.controllers[c.controller_namespace] = c
def initialize(self):
l_controller_name = rospy.get_param('~left_controller_name', 'left_finger_controller')
r_controller_name = rospy.get_param('~right_controller_name', 'right_finger_controller')
if l_controller_name not in self.controllers:
rospy.logerr('left_controller_name not found, requested: [%s], available: %s' % (l_controller_name, self.controllers))
return False
if r_controller_name not in self.controllers:
rospy.logerr('right_controller_name not found, requested: [%s], available: %s' % (r_controller_name, self.controllers))
return False
self.l_finger_controller = self.controllers[l_controller_name]
self.r_finger_controller = self.controllers[r_controller_name]
if self.l_finger_controller.port_namespace != self.r_finger_controller.port_namespace:
rospy.logerr('%s and %s are connected to different serial ports, this is unsupported' % (l_controller_name, r_controller_name))
return False
# left and right finger are assumed to be connected to the same port
self.dxl_io = self.l_finger_controller.dxl_io
self.l_finger_state = self.l_finger_controller.joint_state
self.r_finger_state = self.r_finger_controller.joint_state
self.l_max_speed = self.l_finger_controller.joint_max_speed
self.r_max_speed = self.r_finger_controller.joint_max_speed
return True
def start(self):
self.tf_listener = TransformListener()
# Publishers
self.gripper_opening_pub = rospy.Publisher('gripper_opening', Float64)
self.l_finger_ground_distance_pub = rospy.Publisher('left_finger_ground_distance', Float64)
self.r_finger_ground_distance_pub = rospy.Publisher('right_finger_ground_distance', Float64)
# Pressure sensors
# 0-3 - left finger
# 4-7 - right finger
num_sensors = 8
self.pressure = [0.0] * num_sensors
[rospy.Subscriber('/interface_kit/124427/sensor/%d' % i, Float64Stamped, self.process_pressure_sensors, i) for i in range(num_sensors)]
# pressure sensors are at these values when no external pressure is applied
self.l_zero_pressure = [0.0, 0.0, 0.0, 0.0]
self.r_zero_pressure = [0.0, 0.0, 130.0, 0.0]
self.lr_zero_pressure = self.l_zero_pressure + self.r_zero_pressure
self.l_total_pressure_pub = rospy.Publisher('left_finger_pressure', Float64)
self.r_total_pressure_pub = rospy.Publisher('right_finger_pressure', Float64)
self.lr_total_pressure_pub = rospy.Publisher('total_pressure', Float64)
self.close_gripper = False
self.dynamic_torque_control = False
self.lower_pressure = 800.0
self.upper_pressure = 1000.0
# IR sensor
self.gripper_ir_pub = rospy.Publisher('gripper_distance_sensor', Float64)
self.ir_distance = 0.0
rospy.Subscriber('/interface_kit/106950/sensor/7', Float64Stamped, self.process_ir_sensor)
# Temperature monitor and torque control thread
Thread(target=self.gripper_monitor).start()
# Start gripper opening monitoring thread
Thread(target=self.calculate_gripper_opening).start()
self.action_server = actionlib.SimpleActionServer('wubble_gripper_action',
WubbleGripperAction,
execute_cb=self.process_gripper_action,
auto_start=False)
self.action_server.start()
rospy.loginfo('gripper_freespin_controller: ready to accept goals')
def stop(self):
pass
def __send_motor_command(self, l_desired_torque, r_desired_torque):
l_motor_id = self.l_finger_controller.motor_id
r_motor_id = self.r_finger_controller.motor_id
l_trq = self.l_finger_controller.spd_rad_to_raw(l_desired_torque)
r_trq = self.r_finger_controller.spd_rad_to_raw(r_desired_torque)
self.dxl_io.set_multi_speed( ( (l_motor_id,l_trq), (r_motor_id,r_trq) ) )
def activate_gripper(self, command, torque_limit):
if command == WubbleGripperGoal.CLOSE_GRIPPER:
l_desired_torque = -torque_limit * self.l_max_speed
r_desired_torque = torque_limit * self.r_max_speed
else: # assume opening
l_desired_torque = torque_limit * self.l_max_speed
r_desired_torque = -torque_limit * self.r_max_speed
self.__send_motor_command(l_desired_torque, r_desired_torque)
return max(l_desired_torque, r_desired_torque)
def gripper_monitor(self):
rospy.loginfo('Gripper temperature monitor and torque control thread started successfully')
motors_overheating = False
max_pressure = 8000.0
r = rospy.Rate(150)
while not rospy.is_shutdown():
l_total_pressure = max(0.0, sum(self.pressure[:4]) - sum(self.l_zero_pressure))
r_total_pressure = max(0.0, sum(self.pressure[4:]) - sum(self.r_zero_pressure))
pressure = l_total_pressure + r_total_pressure
self.l_total_pressure_pub.publish(l_total_pressure)
self.r_total_pressure_pub.publish(r_total_pressure)
self.lr_total_pressure_pub.publish(pressure)
#----------------------- TEMPERATURE MONITOR ---------------------------#
l_temp = max(self.l_finger_state.motor_temps)
r_temp = max(self.r_finger_state.motor_temps)
if l_temp >= 75 or r_temp >= 75:
if not motors_overheating:
rospy.logwarn('Disabling gripper motors torque [LM: %dC, RM: %dC]' % (l_temp, r_temp))
self.__send_motor_command(-0.5, 0.5)
motors_overheating = True
else:
motors_overheating = False
#########################################################################
# don't do torque control if not requested or
# when the gripper is open
# or when motors are too hot
if not self.dynamic_torque_control or \
not self.close_gripper or \
motors_overheating:
r.sleep()
continue
#----------------------- TORQUE CONTROL -------------------------------#
l_current = self.l_finger_state.goal_pos
r_current = self.r_finger_state.goal_pos
if pressure > self.upper_pressure: # release
pressure_change_step = abs(pressure - self.upper_pressure) / max_pressure
l_goal = min( self.l_max_speed, l_current + pressure_change_step)
r_goal = max(-self.r_max_speed, r_current - pressure_change_step)
if l_goal < self.l_max_speed or r_goal > -self.r_max_speed:
if self.close_gripper:
self.__send_motor_command(l_goal, r_goal)
rospy.logdebug('>MAX pressure is %.2f, LT: %.2f, RT: %.2f, step is %.2f' % (pressure, l_current, r_current, pressure_change_step))
elif pressure < self.lower_pressure: # squeeze
pressure_change_step = abs(pressure - self.lower_pressure) / max_pressure
l_goal = max(-self.l_max_speed, l_current - pressure_change_step)
r_goal = min( self.r_max_speed, r_current + pressure_change_step)
if l_goal > -self.l_max_speed or r_goal < self.r_max_speed:
if self.close_gripper:
self.__send_motor_command(l_goal, r_goal)
rospy.logdebug('<MIN pressure is %.2f, LT: %.2f, RT: %.2f, step is %.2f' % (pressure, l_current, r_current, pressure_change_step))
########################################################################
r.sleep()
def calculate_gripper_opening(self):
timeout = rospy.Duration(5)
last_reported = rospy.Time(0)
r = rospy.Rate(50)
while not rospy.is_shutdown():
try:
map_frame_id = 'base_footprint'
palm_frame_id = 'L7_wrist_roll_link'
l_fingertip_frame_id = 'left_fingertip_link'
r_fingertip_frame_id = 'right_fingertip_link'
self.tf_listener.waitForTransform(palm_frame_id, l_fingertip_frame_id, rospy.Time(0), rospy.Duration(0.2))
self.tf_listener.waitForTransform(palm_frame_id, r_fingertip_frame_id, rospy.Time(0), rospy.Duration(0.2))
l_pos, _ = self.tf_listener.lookupTransform(palm_frame_id, l_fingertip_frame_id, rospy.Time(0))
r_pos, _ = self.tf_listener.lookupTransform(palm_frame_id, r_fingertip_frame_id, rospy.Time(0))
dx = l_pos[0] - r_pos[0]
dy = l_pos[1] - r_pos[1]
dz = l_pos[2] - r_pos[2]
dist = math.sqrt(dx*dx + dy*dy + dz*dz)
self.gripper_opening_pub.publish(dist)
l_pos, _ = self.tf_listener.lookupTransform(map_frame_id, l_fingertip_frame_id, rospy.Time(0))
r_pos, _ = self.tf_listener.lookupTransform(map_frame_id, r_fingertip_frame_id, rospy.Time(0))
self.l_finger_ground_distance_pub.publish(l_pos[2])
self.r_finger_ground_distance_pub.publish(r_pos[2])
except LookupException as le:
rospy.logerr(le)
except ConnectivityException as ce:
rospy.logerr(ce)
except tf.Exception as e:
if rospy.Time.now() - last_reported > timeout:
rospy.logerr('TF exception: %s' % str(e))
last_reported = rospy.Time.now()
r.sleep()
def process_pressure_sensors(self, msg, i):
self.pressure[i] = msg.data
def process_ir_sensor(self, msg):
"""
Given a raw sensor value from Sharp IR sensor (4-30cm model)
returns an actual distance in meters.
"""
if msg.data < 80: self.ir_distance = 1.0 # too far
elif msg.data > 530: self.ir_distance = 0.0 # too close
else: self.ir_distance = 20.76 / (msg.data - 11) # just right
self.gripper_ir_pub.publish(self.ir_distance)
def process_gripper_action(self, req):
r = rospy.Rate(500)
timeout = rospy.Duration(2.0)
if req.command == WubbleGripperGoal.CLOSE_GRIPPER:
self.dynamic_torque_control = req.dynamic_torque_control
self.lower_pressure = req.pressure_lower
self.upper_pressure = req.pressure_upper
desired_torque = self.activate_gripper(req.command, req.torque_limit)
if not self.dynamic_torque_control:
start_time = rospy.Time.now()
while not rospy.is_shutdown() and \
rospy.Time.now() - start_time < timeout and \
(not within_tolerance(self.l_finger_state.load, -req.torque_limit, 0.01) or
not within_tolerance(self.r_finger_state.load, -req.torque_limit, 0.01)):
r.sleep()
else:
if desired_torque > 1e-3: rospy.sleep(1.0 / desired_torque)
self.close_gripper = True
self.action_server.set_succeeded()
elif req.command == WubbleGripperGoal.OPEN_GRIPPER:
self.close_gripper = False
self.activate_gripper(req.command, req.torque_limit)
start_time = rospy.Time.now()
while not rospy.is_shutdown() and \
rospy.Time.now() - start_time < timeout and \
(self.l_finger_state.current_pos < 0.8 or
self.r_finger_state.current_pos > -0.8):
r.sleep()
self.__send_motor_command(0.5, -0.5)
self.action_server.set_succeeded()
else:
msg = 'Unrecognized command: %d' % req.command
rospy.logerr(msg)
self.action_server.set_aborted(text=msg)
class ServoingServer(object):
def __init__(self):
rospy.init_node('relative_servoing')
rospy.Subscriber('robot_pose_ekf/odom_combined',
PoseWithCovarianceStamped,
self.update_position)
#rospy.Subscriber('/base_scan', LaserScan, self.base_laser_cb)
self.servoing_as = actionlib.SimpleActionServer('servoing_action',
ServoAction,
self.goal_cb, False)
self.vel_out = rospy.Publisher('base_controller/command', Twist)
self.tfl = TransformListener()
self.goal_out = rospy.Publisher('/servo_dest', PoseStamped, latch=True)
self.left_out = rospy.Publisher('/left_points', PointCloud)
self.right_out = rospy.Publisher('/right_points', PointCloud)
self.front_out = rospy.Publisher('/front_points', PointCloud)
#Initialize variables, so as not to spew errors before seeing a goal
self.at_goal = False
self.rot_safe = True
self.bfp_goal = PoseStamped()
self.odom_goal = PoseStamped()
self.x_max = 0.5
self.x_min = 0.05
self.y_man = 0.3
self.y_min = 0.05
self.z_max = 0.5
self.z_min = 0.05
self.ang_goal = 0.0
self.ang_thresh_small = 0.01
self.ang_thresh_big = 0.04
self.ang_thresh = self.ang_thresh_big
self.retreat_thresh = 0.3
self.curr_pos = PoseWithCovarianceStamped()
self.dist_thresh = 0.15
self.left = [[],[]]
self.right = [[],[]]
self.front = [[],[]]
self.servoing_as.start()
def goal_cb(self, goal):
self.update_goal(goal.goal)
update_rate = rospy.Rate(40)
command = Twist()
while not (rospy.is_shutdown() or self.at_goal):
command.linear.x = self.get_trans_x()
command.linear.y = self.get_trans_y()
command.angular.z = self.get_rot()
(x,y,z) = self.avoid_obstacles()
if x is not None:
command.linear.x = x
if y is not None:
command.linear.y = y
command.angular.z += z
if command.linear.y > 0:
if not self.left_clear():
command.linear.y = 0.0
elif command.linear.y < 0:
if not self.right_clear():
command.linear.y = 0.0
#print "Sending vel_command: \r\n %s" %self.command
self.vel_out.publish(command)
rospy.sleep(0.01) #Min sleep
update_rate.sleep() #keep pace
if self.at_goal:
print "Arrived at goal"
result = ServoResult()
result.arrived = Bool(True)
self.servoing_as.set_succeeded(result)
def update_goal(self, msg):
msg.header.stamp = rospy.Time.now()
if not self.tfl.waitForTransform(msg.header.frame_id, '/base_footprint',
msg.header.stamp, rospy.Duration(30)):
rospy.logwarn('Cannot find /base_footprint transform')
self.bfp_goal = self.tfl.transformPose('/base_footprint', msg)
if not self.tfl.waitForTransform(msg.header.frame_id, 'odom_combined',
msg.header.stamp, rospy.Duration(30)):
rospy.logwarn('Cannot find /odom_combined transform')
self.odom_goal = self.tfl.transformPose('/odom_combined', msg)
self.goal_out.publish(self.odom_goal)
ang_to_goal = math.atan2(self.bfp_goal.pose.position.y,
self.bfp_goal.pose.position.x)
#(current angle in odom, plus the robot-relative change to face goal)
self.ang_goal = self.curr_ang[2] + ang_to_goal
rospy.logwarn(self.odom_goal)
rospy.logwarn(self.ang_goal)
print "New Goal: \r\n %s" %self.bfp_goal
def update_position(self, msg):
if not self.servoing_as.is_active():
return
self.curr_ang=tft.euler_from_quaternion([msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w])
# Normalized via unwrap relative to 0; (keeps between -pi/pi)
self.ang_diff = np.unwrap([0, self.ang_goal - self.curr_ang[2]])[1]
#print "Ang Diff: %s" %self.ang_diff
self.dist_to_goal = ((self.odom_goal.pose.position.x-
msg.pose.pose.position.x)**2 +
(self.odom_goal.pose.position.y-
msg.pose.pose.position.y)**2)**(1./2)
rospy.logwarn('Distance to goal (msg): ')
rospy.logwarn(msg)
rospy.logwarn('Distance to goal (odom_goal): ')
rospy.logwarn(self.odom_goal)
if ((self.dist_to_goal < self.dist_thresh) and
(abs(self.ang_diff) < self.ang_thresh)):
self.at_goal = True
else:
self.at_goal = False
def base_laser_cb(self, msg):
max_angle = msg.angle_max
ranges = np.array(msg.ranges)
angles = np.arange(msg.angle_min, msg.angle_max, msg.angle_increment)
#Filter out noise(<0.003), points >1m, leaves obstacles
near_angles = np.extract(np.logical_and(ranges<1, ranges>0.003),
angles)
near_ranges = np.extract(np.logical_and(ranges<1, ranges>0.003),
ranges)
self.bad_side = np.sign(near_angles[np.argmax(abs(near_angles))])
#print "bad side: %s" %bad_side # (1 (pos) = left, -1 = right)
#print "Min in Ranges: %s" %min(ranges)
#if len(near_ranges) > 0:
xs = near_ranges * np.cos(near_angles)
ys = near_ranges * np.sin(near_angles)
#print "xs: %s" %xs
self.points = np.vstack((xs,ys))
#print "Points: %s" %points
self.bfp_points = np.vstack((np.add(0.275, xs),ys))
#print "bfp Points: %s" %bfp_points
self.bfp_dists = np.sqrt(np.add(np.square(self.bfp_points[0][:]),
np.square(self.bfp_points[1][:])))
#print min(self.bfp_dists)
if len(self.bfp_dists) >0:
if min(self.bfp_dists) > 0.5:
self.rot_safe = True
else:
self.rot_safe = False
else:
self.rot_safe = True
self.left = np.vstack((xs[np.nonzero(ys>0.35)[0]],
ys[np.nonzero(ys>0.35)[0]]))
self.right= np.vstack((xs[np.nonzero(ys<-0.35)[0]],
ys[np.nonzero(ys<-0.35)[0]]))
self.front = np.vstack(
(np.extract(np.logical_and(ys<0.35,ys>-0.35),xs),
np.extract(np.logical_and(ys<0.35, ys>-0.35),ys)))
front_dist = (self.front[:][0]**2+self.front[:][1]**2)**(1/2)
##Testing and Visualization:###
if len(self.left[:][0]) > 0:
leftScan = PointCloud()
leftScan.header.frame_id = '/base_laser_link'
leftScan.header.stamp = rospy.Time.now()
for i in range(len(self.left[0][:])):
pt = Point32()
pt.x = self.left[0][i]
pt.y = self.left[1][i]
pt.z = 0
leftScan.points.append(pt)
self.left_out.publish(leftScan)
if len(self.right[:][0]) > 0:
rightScan = PointCloud()
rightScan.header.frame_id = '/base_laser_link'
rightScan.header.stamp = rospy.Time.now()
for i in range(len(self.right[:][0])):
pt = Point32()
pt.x = self.right[0][i]
pt.y = self.right[1][i]
pt.z = 0
rightScan.points.append(pt)
self.right_out.publish(rightScan)
if len(self.front[:][0]) > 0:
frontScan = PointCloud()
frontScan.header.frame_id = 'base_laser_link'
frontScan.header.stamp = rospy.Time.now()
for i in range(len(self.front[:][0])):
pt = Point32()
pt.x = self.front[0][i]
pt.y = self.front[1][i]
pt.z = 0
frontScan.points.append(pt)
self.front_out.publish(frontScan)
def get_rot(self):
if abs(self.ang_diff) < self.ang_thresh:
self.ang_thresh = self.ang_thresh_big
return 0.0
else:
self.ang_thresh = self.ang_thresh_small
if self.rot_safe:
return np.sign(self.ang_diff)*np.clip(abs(0.35*self.ang_diff),
0.1, 0.5)
else:
fdbk = ServoFeedback()
fdbk.current_action = String("Cannot Rotate, obstacles nearby")
self.servoing_as.publish_feedback(fdbk)
return 0.0
def get_trans_x(self):
if (abs(self.ang_diff) < math.pi/6 and
self.dist_to_goal > self.dist_thresh):
return np.clip(self.dist_to_goal*0.125, 0.05, 0.3)
else:
return 0.0
def get_trans_y(self):
# Determine left/right movement speed for strafing obstacle avoidance
push_from_left = push_from_right = 0.0
if len(self.left[:][0]) > 0:
lefts = np.extract(self.left[:][1]<0.45, self.left[:][1])
if len(lefts) > 0:
push_from_left = -0.45 + min(lefts)
if len(self.right[:][0]) > 0:
rights = np.extract(self.right[:][1]>-0.45, self.right[:][1])
if len(rights) > 0:
push_from_right = 0.45 + max(rights)
slide = push_from_right + push_from_left
#print "Slide speed (m/s): %s" %slide
return np.sign(slide)*np.clip(abs(slide), 0.04, 0.07)
def avoid_obstacles(self):
##Determine rotation to avoid obstacles in front of robot#
x = y = None
z = 0.
if len(self.front[0][:]) > 0:
if min(self.front[0][:]) < self.retreat_thresh:
#(round-up on corner-to-corner radius of robot) -
# 0.275 (x diff from base laser link to base footprint)
#print "front[0][:] %s" %self.front[0][:]
front_dists = np.sqrt(np.add(np.square(self.front[0][:]),
np.square(self.front[1][:])))
min_x = self.front[0][np.argmin(front_dists)]
min_y = self.front[1][np.argmin(front_dists)]
#print "min x/y: %s,%s" %(min_x, min_y)
x = -np.sign(min_x)*np.clip(abs(min_x),0.05,0.1)
y = -np.sign(min_y)*np.clip(abs(min_y),0.05,0.1)
z = 0.
# This should probably be avoided...
fdbk = ServoFeedback()
fdbk.current_action = String("TOO CLOSE: Back up slowly...")
self.servoing_as.publish_feedback(fdbk)
self.retreat_thresh = 0.4
elif min(self.front[0][:]) < 0.45:
self.retreat_thresh = 0.3
fdbk = ServoFeedback()
fdbk.current_action=String("Turning Away from obstacles in front")
self.servoing_as.publish_feedback(fdbk)
lfobs = self.front[0][np.logical_and(self.front[1]>0,
self.front[0]<0.45)]
rfobs = self.front[0][np.logical_and(self.front[1]<0,
self.front[0]<0.45)]
if len(lfobs) == 0:
y = 0.07
elif len(rfobs) == 0:
y = -0.07
weight = np.reciprocal(np.sum(np.reciprocal(rfobs)) -
np.sum(np.reciprocal(lfobs)))
if weight > 0:
z = 0.05
else:
z = -0.05
else:
self.retreat_thresh = 0.3
return (x,y,z)
def left_clear(self): # Base Laser cannot see obstacles beside the back edge of the robot, which could cause problems, especially just after passing through doorways...
if len(self.left[0][:])>0:
#Find points directly to the right or left of the robot (not in front or behind)
# x<0.1 (not in front), x>-0.8 (not behind)
left_obs = self.left[:, self.left[1,:]<0.4] #points too close.
if len(left_obs[0][:])>0:
left_obs = left_obs[:, np.logical_and(left_obs[0,:]<0.1,
left_obs[0,:]>-0.8)]
if len(left_obs[:][0])>0:
fdbk = ServoFeedback()
fdbk.current_action = String("Obstacle to the left, cannot move.")
self.servoing_as.publish_feedback(fdbk)
return False
return True
def right_clear(self):
if len(self.right[0][:])>0:
#Find points directly to the right or left of the robot (not in front or behind)
# x<0.1 (not in front), x>-0.8 (not behind)
right_obs = self.right[:, self.right[1,:]>-0.4] #points too close.
if len(right_obs[0][:])>0:
right_obs = right_obs[:, np.logical_and(right_obs[0,:]<0.1,
right_obs[0,:]>-0.8)]
if len(right_obs[:][0])>0:
fdbk = ServoFeedback()
fdbk.current_action = String("Obstacle immediately to the right, cannot move.")
self.servoing_as.publish_feedback(fdbk)
return False
return True
class jt_task_utils:
def __init__(self, tf=None):
if tf is None:
self.tf = TransformListener()
else:
self.tf = tf
#### SERVICES ####
rospy.loginfo("Waiting for utility_frame_services")
try:
rospy.wait_for_service("/l_utility_frame_update", 3.0)
rospy.wait_for_service("/r_utility_frame_update", 3.0)
self.update_frame = [
rospy.ServiceProxy("/l_utility_frame_update", FrameUpdate),
rospy.ServiceProxy("/r_utility_frame_update", FrameUpdate),
]
rospy.loginfo("Found utility_frame_services")
except:
rospy.logwarn("Left or Right Utility Frame Service Not available")
#### Action Clients ####
self.ft_move_client = actionlib.SimpleActionClient("l_cart/ft_move_action", FtMoveAction)
rospy.loginfo("Waiting for l_cart/ft_move_action server")
if self.ft_move_client.wait_for_server(rospy.Duration(3)):
rospy.loginfo("Found l_cart/ft_move_action server")
else:
rospy.logwarn("Cannot find l_cart/ft_move_action server")
self.ft_move_r_client = actionlib.SimpleActionClient("r_cart/ft_move_action", FtMoveAction)
rospy.loginfo("Waiting for r_cart/ft_move_action server")
if self.ft_move_r_client.wait_for_server(rospy.Duration(3)):
rospy.loginfo("Found r_cart/ft_move_action server")
else:
rospy.logwarn("Cannot find r_cart/ft_move_action server")
self.ft_hold_client = actionlib.SimpleActionClient("ft_hold_action", FtHoldAction)
rospy.loginfo("Waiting for ft_hold_action server")
if self.ft_hold_client.wait_for_server(rospy.Duration(3)):
rospy.loginfo("Found ft_hold_action server")
else:
rospy.logwarn("Cannot find ft_hold_action server")
#### SUBSCRIBERS ####
self.curr_state = [PoseStamped(), PoseStamped()]
rospy.Subscriber("/l_cart/state/x", PoseStamped, self.get_l_state)
rospy.Subscriber("/r_cart/state/x", PoseStamped, self.get_r_state)
rospy.Subscriber("/wt_l_wrist_command", Point, self.rot_l_wrist)
rospy.Subscriber("/wt_r_wrist_command", Point, self.rot_r_wrist)
rospy.Subscriber("/wt_left_arm_pose_commands", Point, self.trans_l_hand)
rospy.Subscriber("/wt_right_arm_pose_commands", Point, self.trans_r_hand)
# self.ft_wrench = WrenchStamped()
# self.force_stopped = False
# self.ft_z_thresh = -2
# self.ft_mag_thresh = 5
# rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.get_ft_state)
#### PUBLISHERS ####
self.goal_pub = [
rospy.Publisher("l_cart/command_pose", PoseStamped),
rospy.Publisher("r_cart/command_pose", PoseStamped),
]
self.posture_pub = [
rospy.Publisher("l_cart/command_posture", Float64MultiArray),
rospy.Publisher("r_cart/command_posture", Float64MultiArray),
]
#### STATIC DATA ####
self.postures = {
"off": [],
"mantis": [0, 1, 0, -1, 3.14, -1, 3.14],
"elbowupr": [-0.79, 0, -1.6, 9999, 9999, 9999, 9999],
"elbowupl": [0.79, 0, 1.6, 9999, 9999, 9999, 9999],
"old_elbowupr": [-0.79, 0, -1.6, -0.79, 3.14, -0.79, 5.49],
"old_elbowupl": [0.79, 0, 1.6, -0.79, 3.14, -0.79, 5.49],
"elbowdownr": [-0.028262, 1.294634, -0.2578564, -1.549888, -31.27891385, -1.05276449, -1.8127318],
"elbowdownl": [-0.008819572, 1.28348282, 0.2033844, -1.5565279, -0.09634, -1.023502, 1.799089],
}
def get_l_state(self, ps): # WORKING, TESTED
self.curr_state[0] = ps
def get_r_state(self, ps): # WORKING, TESTED
self.curr_state[1] = ps
# def get_ft_state(self, ws):
# self.ft_wrench = ws
# self.ft_mag = math.sqrt(ws.wrench.force.x**2 + ws.wrench.force.y**2 + ws.wrench.force.z**2)
# if ws.wrench.force.z < self.ft_z_thresh:
# self.force_stopped = True
# # rospy.logwarn("Z force threshold exceeded")
# if self.ft_mag > self.ft_mag_thresh:
# self.force_stopped = True
# rospy.logwarn("Total force threshold exceeded")
def rot_l_wrist(self, pt):
out_pose = deepcopy(self.curr_state[0])
q_r = transformations.quaternion_about_axis(pt.x, (1, 0, 0)) # Hand frame roll (hand roll)
q_p = transformations.quaternion_about_axis(pt.y, (0, 1, 0)) # Hand frame pitch (wrist flex)
q_h = transformations.quaternion_multiply(q_r, q_p)
q_f = transformations.quaternion_about_axis(pt.y, (1, 0, 0)) # Forearm frame rot (forearm roll)
if pt.x or pt.y:
self.tf.waitForTransform(
out_pose.header.frame_id, "l_wrist_roll_link", out_pose.header.stamp, rospy.Duration(3.0)
)
hand_pose = self.tf.transformPose("l_wrist_roll_link", out_pose)
q_hand_pose = (
hand_pose.pose.orientation.x,
hand_pose.pose.orientation.y,
hand_pose.pose.orientation.z,
hand_pose.pose.orientation.w,
)
q_h_rot = transformations.quaternion_multiply(q_h, hand_pose.pose.orientation)
hand_pose.pose.orientation = Quaternion(*q_h_rot)
out_pose = self.tf.transformPose(out_pose.header.frame_id, hand_pose)
if pt.z:
self.tf.waitForTransform(
out_pose.header.frame_id, "l_forearm_roll_link", out_pose.header.stamp, rospy.Duration(3.0)
)
hand_pose = self.tf.transformPose("l_forearm_roll_link", out_pose)
q_hand_pose = (
hand_pose.pose.orientation.x,
hand_pose.pose.orientation.y,
hand_pose.pose.orientation.z,
hand_pose.pose.orientation.w,
)
q_f_rot = transformations.quaternion_multiply(q_f, hand_pose.pose.orientation)
hand_pose.pose.orientation = Quaternion(*q_f_rot)
out_pose = self.tf.transformPose(out_pose.header.frame_id, hand_pose)
wrist_traj = self.build_trajectory(out_pose, arm=0)
# TODO: Add Action Goal Sender to Move along trajectory!!!!!!!!!!!!!!!!
def rot_r_wrist(self, pt):
out_pose = deepcopy(self.curr_state[1])
q_r = transformations.quaternion_about_axis(-pt.x, (1, 0, 0)) # Hand frame roll (hand roll)
q_p = transformations.quaternion_about_axis(-pt.y, (0, 1, 0)) # Hand frame pitch (wrist flex)
q_h = transformations.quaternion_multiply(q_r, q_p)
q_f = transformations.quaternion_about_axis(-pt.y, (1, 0, 0)) # Forearm frame rot (forearm roll)
if pt.x or pt.y:
self.tf.waitForTransform(
out_pose.header.frame_id, "r_wrist_roll_link", out_pose.header.stamp, rospy.Duration(3.0)
)
hand_pose = self.tf.transformPose("r_wrist_roll_link", out_pose)
q_hand_pose = (
hand_pose.pose.orientation.x,
hand_pose.pose.orientation.y,
hand_pose.pose.orientation.z,
hand_pose.pose.orientation.w,
)
q_h_rot = transformations.quaternion_multiply(q_h, hand_pose.pose.orientation)
hand_pose.pose.orientation = Quaternion(*q_h_rot)
out_pose = self.tf.transformPose(out_pose.header.frame_id, hand_pose)
if pt.z:
self.tf.waitForTransform(
out_pose.header.frame_id, "r_forearm_roll_link", out_pose.header.stamp, rospy.Duration(3.0)
)
hand_pose = self.tf.transformPose("r_forearm_roll_link", out_pose)
q_hand_pose = (
hand_pose.pose.orientation.x,
hand_pose.pose.orientation.y,
hand_pose.pose.orientation.z,
hand_pose.pose.orientation.w,
)
q_f_rot = transformations.quaternion_multiply(q_f, hand_pose.pose.orientation)
hand_pose.pose.orientation = Quaternion(*q_f_rot)
out_pose = self.tf.transformPose(out_pose.header.frame_id, hand_pose)
wrist_traj = self.build_trajectory(out_pose, arm=1)
# TODO: Add Action Goal Sender to Move along trajectory!!!!!!!!!!!!!!!!
def trans_l_hand(self, pt):
print "Moving Left Hand with JT Task Controller"
out_pose = PoseStamped()
out_pose.header.frame_id = self.curr_state[0].header.frame_id
out_pose.header.stamp = rospy.Time.now()
out_pose.pose.position.x = self.curr_state[0].pose.position.x + pt.x
out_pose.pose.position.y = self.curr_state[0].pose.position.y + pt.y
out_pose.pose.position.z = self.curr_state[0].pose.position.z + pt.z
out_pose.pose.orientation = self.curr_state[0].pose.orientation
trans_traj = self.build_trajectory(out_pose, arm=0)
self.ft_move_client.send_goal(FtMoveGoal(trans_traj, 0.0, True))
self.ft_move_client.wait_for_result(rospy.Duration(0.025 * len(trans_traj)))
def trans_r_hand(self, pt):
out_pose = PoseStamped()
out_pose.header.frame_id = self.curr_state[1].header.frame_id
out_pose.header.stamp = rospy.Time.now()
out_pose.pose.position.x = self.curr_state[1].pose.position.x + pt.x
out_pose.pose.position.y = self.curr_state[1].pose.position.y + pt.y
out_pose.pose.position.z = self.curr_state[1].pose.position.z + pt.z
out_pose.pose.orientation = self.curr_state[1].pose.orientation
trans_traj = self.build_trajectory(out_pose, arm=0)
self.ft_move_client.send_goal(FtMoveGoal(trans_traj, 0.0, True))
self.ft_move_client.wait_for_result(rospy.Duration(0.025 * len(trans_traj)))
def send_posture(self, posture="off", arm=0): # WORKING, TESTED TODO: SLOW TRANSITION (if possible)
if "elbow" in posture:
if arm == 0:
posture = posture + "l"
elif arm == 1:
posture = posture + "r"
self.posture_pub[arm].publish(Float64MultiArray(data=self.postures[posture]))
def send_traj(self, poses, arm=0):
send_rate = rospy.Rate(50)
##!!!!!!!!!!!! MUST BALANCE SEND RATE WITH SPACING IN 'BUILD_TRAJECTORY' FOR CONTROL OF VELOCITY !!!!!!!!!!!!
finished = False
count = 0
while not (rospy.is_shutdown() or finished):
self.goal_pub[arm].publish(poses[count])
count += 1
send_rate.sleep()
if count == len(poses):
finished = True
def send_traj_to_contact(self, poses, arm=0):
send_rate = rospy.Rate(20)
##!!!!!!!!!!!! MUST BALANCE SEND RATE WITH SPACING IN 'BUILD_TRAJECTORY' FOR CONTROL OF VELOCITY !!!!!!!!!!!!
finished = False
count = 0
while not (rospy.is_shutdown() or finished or self.force_stopped):
self.goal_pub[arm].publish(poses[count])
count += 1
send_rate.sleep()
if count == len(poses):
finished = True
def build_trajectory(self, finish, start=None, arm=0, space=0.001, steps=None): # WORKING, TESTED
##!!!!!!!!!!!! MUST BALANCE SPACING WITH SEND RATE IN 'SEND_TRAJ' FOR CONTROL OF VELOCITY !!!!!!!!!!!!
if start is None: # if given one pose, use current position as start
start = self.curr_state[arm]
dist = self.calc_dist(start, finish, arm=arm) # Total distance to travel
if steps is None:
steps = int(math.ceil(dist / space))
fracs = np.linspace(0, 1, steps) # A list of fractional positions along course
print "Steps: %s" % steps
poses = [PoseStamped() for i in xrange(steps)]
xs = np.linspace(start.pose.position.x, finish.pose.position.x, steps)
ys = np.linspace(start.pose.position.y, finish.pose.position.y, steps)
zs = np.linspace(start.pose.position.z, finish.pose.position.z, steps)
qs = [start.pose.orientation.x, start.pose.orientation.y, start.pose.orientation.z, start.pose.orientation.w]
qf = [
finish.pose.orientation.x,
finish.pose.orientation.y,
finish.pose.orientation.z,
finish.pose.orientation.w,
]
for i, frac in enumerate(fracs):
poses[i].header.stamp = rospy.Time.now()
poses[i].header.frame_id = start.header.frame_id
poses[i].pose.position = Point(xs[i], ys[i], zs[i])
new_q = transformations.quaternion_slerp(qs, qf, frac)
poses[i].pose.orientation = Quaternion(*new_q)
# rospy.loginfo("Planning straight-line path, please wait")
# self.wt_log_out.publish(data="Planning straight-line path, please wait")
return poses
def pose_frame_move(self, pose, x, y=0, z=0, arm=0): # FINISHED, UNTESTED
self.update_frame[arm](pose)
if arm == 0:
frame = "lh_utility_frame"
elif arm == 1:
frame = "rh_utility_frame"
pose.header.stamp = rospy.Time.now()
self.tf.waitForTransform(pose.header.frame_id, frame, pose.header.stamp, rospy.Duration(3.0))
framepose = self.tf.transformPose(frame, pose)
framepose.pose.position.x += x
framepose.pose.position.y += y
framepose.pose.position.z += z
self.dist = math.sqrt(x ** 2 + y ** 2 + z ** 2)
self.tf.waitForTransform(frame, pose.header.frame_id, pose.header.stamp, rospy.Duration(3.0))
return self.tf.transformPose(pose.header.frame_id, framepose)
def calc_dist(self, ps1, ps2=None, arm=0): # FINISHED, UNTESTED
if ps2 is None:
ps2 = self.curr_pose()
p1 = ps1.pose.position
p2 = ps2.pose.position
wrist_dist = math.sqrt((p1.x - p2.x) ** 2 + (p1.y - p2.y) ** 2 + (p1.z - p2.z) ** 2)
self.update_frame[arm](ps2)
ps2.header.stamp = rospy.Time(0)
np2 = self.tf.transformPose("lh_utility_frame", ps2)
np2.pose.position.x += 0.21
self.tf.waitForTransform(np2.header.frame_id, "torso_lift_link", rospy.Time.now(), rospy.Duration(3.0))
p2 = self.tf.transformPose("torso_lift_link", np2)
self.update_frame[arm](ps1)
ps1.header.stamp = rospy.Time(0)
np1 = self.tf.transformPose("lh_utility_frame", ps1)
np1.pose.position.x += 0.21
self.tf.waitForTransform(np1.header.frame_id, "torso_lift_link", rospy.Time.now(), rospy.Duration(3.0))
p1 = self.tf.transformPose("torso_lift_link", np1)
p1 = p1.pose.position
p2 = p2.pose.position
finger_dist = math.sqrt((p1.x - p2.x) ** 2 + (p1.y - p2.y) ** 2 + (p1.z - p2.z) ** 2)
dist = max(wrist_dist, finger_dist)
print "Calculated Distance: ", dist
return dist
def test(self):
print "Testing..."
rospy.sleep(1)
#### TEST STATE GRABBING ####
print "Left Current Pose:"
print self.curr_state[0]
print "Right Current Pose:"
print self.curr_state[1]
#### TEST FORCE STATE GRABBING ####
print "Current Force Wrench:"
print self.ft_wrench
print "Current Force Magnitude:"
print self.ft_mag
#### TEST LEFT ARM GOAL SENDING ####
l_pose = PoseStamped()
l_pose.header.frame_id = "torso_lift_link"
l_pose.pose.position = Point(0.6, 0.3, 0.1)
l_pose.pose.orientation = Quaternion(1, 0, 0, 0)
raw_input("send left arm goal")
self.goal_pub[0].publish(l_pose)
#### TEST RIGHT ARM GOAL SENDING
# r_pose = PoseStamped()
# r_pose.header.frame_id = 'torso_lift_link'
# r_pose.pose.position = Point(0.6, -0.3, 0.1)
# r_pose.pose.orientation = Quaternion(1,0,0,0)
# raw_input("send right arm goal")
# self.goal_pub[1].publish(r_pose)
#### TEST POSE SETTING ####
# raw_input("Left Elbow Up")
# self.send_posture('elbowup',0)
# raw_input("Right Elbow Up")
# self.send_posture('elbowup',1)
# raw_input("Left Elbow Down")
# self.send_posture('elbowdown',0)
# raw_input("Right Elbow Down")
# self.send_posture('elbowdown',1)
# raw_input("Both Postures Off")
# self.send_posture(arm=0)
# self.send_posture(arm=1)
# print "Postures adjusted"
#### TEST TRAJECTORY MOTION ####
l_pose2 = PoseStamped()
l_pose2.header.frame_id = "torso_lift_link"
l_pose2.pose.position = Point(0.8, 0.3, 0.1)
l_pose2.pose.orientation = Quaternion(1, 0, 0, 0)
raw_input("Left trajectory")
# l_pose2 = self.pose_frame_move(self.curr_state[0], -0.1, arm=0)
traj = self.build_trajectory(l_pose2)
self.send_traj_to_contact(traj)
# r_pose2 = PoseStamped()
# r_pose2.header.frame_id = 'torso_lift_link'
# r_pose2.pose.position = Point(0.8, -0.15, -0.3)
# r_pose2.pose.orientation = Quaternion(0,0.5,0.5,0)
# raw_input("Right trajectory")
# r_pose2 = self.pose_frame_move(self.curr_state[1], -0.1, arm=1)
# traj = self.build_trajectory(r_pose2, arm=1)
# self.send_traj(traj,1)
#### RECONFIRM POSITION ####
print "New Left Pose:"
print self.curr_state[0]
print "New Right Pose:"
print self.curr_state[1]
class SimpleMover(object):
# TODO: Actually calculate something for this
steering_timeout = 5
def __init__(self, param_ns="", listener=None, stop_function=None):
if listener is not None:
self.tf = listener
else:
self.tf = TransformListener()
rospy.sleep(2.0)
self.param_ns = param_ns
self.default_stop_function = stop_function
self.max_velocity = rospy.get_param(param_ns + 'max_velocity', 0.5)
self.acceleration = rospy.get_param(param_ns + 'acceleration', 0.5)
self.deceleration = self.acceleration
self.stop_deceleration = rospy.get_param(param_ns + 'stop_deceleration', 1.5)
self.loop_rate = rospy.get_param(param_ns + 'loop_rate', 10.0)
self.steering_angle_epsilon = rospy.get_param(param_ns + 'steering_angle_epsilon', 0.01)
self.velocity_epsilon = rospy.get_param(param_ns + 'velocity_epsilon', 0.001)
self.running = False
self.stop_requested = False
#this is a hack to allow changes in strafe direction
self.strafe_angle = None
self.current_position = None
#get stern wheel transform
try:
self.tf.waitForTransform('base_link', 'stern_suspension',
rospy.Time(0), rospy.Duration(5.0))
pos, quat = self.tf.lookupTransform("base_link","stern_suspension",rospy.Time(0))
self.stern_offset = np.abs(pos[0])
except (tf.Exception):
rospy.logwarn("SIMPLE_MOTION: Failed to get stern_suspension transform")
return
#publishers and subscribers
self.publisher = rospy.Publisher("twist", Twist, queue_size=1)
rospy.Subscriber("odometry", Odometry, self.odometry_callback, None, 1)
rospy.Subscriber("platform_joint_state", JointState, self.joint_state_callback, None, 1)
def odometry_callback(self, msg):
"""
Called to store present robot position and yaw
"""
quaternion = np.zeros(4)
quaternion[0] = msg.pose.pose.orientation.x
quaternion[1] = msg.pose.pose.orientation.y
quaternion[2] = msg.pose.pose.orientation.z
quaternion[3] = msg.pose.pose.orientation.w
self.current_yaw = euler_from_quaternion(quaternion)[-1]
self.current_position = np.r_[msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z]
self.current_omega = msg.twist.twist.angular.z
self.current_speed = np.hypot( msg.twist.twist.linear.x,
msg.twist.twist.linear.y)
self.current_angle = np.arctan2(msg.twist.twist.linear.y,
msg.twist.twist.linear.x)
def joint_state_callback(self, msg):
"""
Remember current robot steering angles
"""
pos_dict = dict(zip(msg.name,msg.position))
vel_dict = dict(zip(msg.name,msg.velocity))
self.stern_pos = pos_dict["stern_steering_joint"]
self.port_pos = pos_dict["port_steering_joint"]
self.starboard_pos = pos_dict["starboard_steering_joint"]
self.stern_vel = vel_dict["stern_steering_joint"]
self.port_vel = vel_dict["port_steering_joint"]
self.starboard_vel = vel_dict["starboard_steering_joint"]
def spin_publisher(self, velocity, sign):
twist = Twist()
twist.angular.z = sign*velocity/self.stern_offset
self.publisher.publish(twist)
#set the strafe angle, and the publisher should just send the robot off at a new angle
def set_strafe_angle(self, new_angle):
self.strafe_angle = new_angle
def strafe_publisher(self, velocity):
twist = Twist()
twist.linear.x = velocity*np.cos(self.strafe_angle)
twist.linear.y = velocity*np.sin(self.strafe_angle)
self.publisher.publish(twist)
def execute_spin(self, rotation, max_velocity=None, acceleration=None, stop_function=None):
return util.unwind(
self.execute(
lambda start_yaw=self.current_yaw:(self.current_yaw-start_yaw-rotation)*self.stern_offset,
dict(stern=-np.pi/2*np.sign(rotation),
port=0.0,
starboard=0.0),
lambda v, sign=np.sign(rotation) : self.spin_publisher(v, sign),
max_velocity,
acceleration,
stop_function)/self.stern_offset)
def execute_strafe(self, angle, distance, max_velocity=None, acceleration=None, stop_function=None):
angle = util.unwind(angle)
self.strafe_angle = angle
#now calculate the angles of the wheel pods
if (-np.pi/2<=angle<=np.pi/2):
target_angle = angle
elif (np.pi/2 < angle):
target_angle = angle - np.pi
elif (angle < -np.pi/2):
target_angle = angle + np.pi
return self.execute(
lambda start_position=self.current_position: np.abs(distance-np.linalg.norm(start_position-self.current_position)),
dict(stern=target_angle,
port=target_angle,
starboard=target_angle),
lambda v : self.strafe_publisher(v),
max_velocity,
acceleration,
stop_function)
#use with caution, strafes at constant velocity until external stop is called
def execute_continuous_strafe(self, angle, max_velocity=None, acceleration=None, stop_function=None):
angle = util.unwind(angle)
self.strafe_angle = angle
#now calculate the angles of the wheel pods
if (-np.pi/2<=angle<=np.pi/2):
target_angle = angle
elif (np.pi/2 < angle):
target_angle = angle - np.pi
elif (angle < -np.pi/2):
target_angle = angle + np.pi
return self.execute(
lambda: np.inf ,
dict(stern=target_angle,
port=target_angle,
starboard=target_angle),
lambda v : self.strafe_publisher(v),
max_velocity,
acceleration,
stop_function)
def execute(self, error, target, publisher, max_velocity=None, acceleration=None, stop_function=None):
rospy.logdebug("SIMPLE_MOTION: {}, starting execute".format(self.param_ns))
rate = rospy.Rate(self.loop_rate)
#load defaults if no kwargs present
if max_velocity is None:
max_velocity = self.max_velocity
if acceleration is None:
acceleration = self.acceleration
self.deceleration = acceleration
if not callable(stop_function):
stop_function = self.default_stop_function
# if one step of accel overshoots half the distance, give up
if np.abs(error()/2.) < 0.5*acceleration/self.loop_rate**2:
return error()
#prepare to start main loop
self.stop_requested = False
self.running = False
steering_timeout_time = rospy.Time.now() + rospy.Duration(self.steering_timeout)
velocity = lambda:self.velocity_epsilon
v = velocity()
while ((not self.should_stop(stop_function)) and (v != 0)):
# Run at some rate, ~10Hz
rate.sleep()
#wait until correct steering angles are achieved
if ((np.abs(self.stern_pos-target['stern'])>self.steering_angle_epsilon or
np.abs(self.port_pos-target['port'])>self.steering_angle_epsilon or
np.abs(self.starboard_pos-target['starboard'])>self.steering_angle_epsilon) and
not self.running):
rospy.logdebug("SIMPLE_MOTION: {}, Turning to target angles: {:f} {:f} {:f}".format(
self.param_ns,
np.abs(self.stern_pos-target['stern']),
np.abs(self.port_pos-target['port']),
np.abs(self.starboard_pos-target['starboard'])))
if (rospy.Time.now()>steering_timeout_time):
publisher(0)
raise TimeoutException('Steering move failed to complete before timeout: {!s}'.format(steering_timeout_time))
#we have achieved proper steering angle, but not started the motion yet
#get the velocity function with total_distance as the current error
elif not self.running:
start_time = rospy.Time.now()
velocity = lambda start_time = start_time, d = np.abs(error()) : self.velocity_of_time( d, (rospy.Time.now() - start_time).to_sec(), acceleration, max_velocity)
self.running = True
v = velocity()
publisher(v)
#decel to zero (this is necessary after a stop is requested)
while v > 0:
#if an estop was requested, deceleration will be stop_deceleration
v = np.max((v - (self.deceleration/self.loop_rate), 0))
publisher(v)
rate.sleep()
#make sure the servos have received the last zero v twist
start_time = rospy.Time.now()
while not rospy.is_shutdown():
rospy.sleep(0.1)
if (self.current_omega < self.velocity_epsilon) \
and (self.current_speed < self.velocity_epsilon):
break
if (rospy.Time.now() - start_time) > rospy.Duration(2.0):
rospy.logwarn("SIMPLE_MOTION {} waited 2 seconds for \
odometry to report v < epsilon. Exiting anyway. \
Robot is probably out of control.".format(self.param_ns))
self.running = False
self.stop_requested = False
rospy.logdebug("SIMPLE_MOTION: {}, exiting execute".format(self.param_ns))
return error()
def should_stop(self, stop_function):
external_stop = (callable(stop_function) and stop_function())
if external_stop:
self.stop_requested = True
return self.stop_requested
def velocity_of_time(self, total_distance, t, acceleration, max_velocity):
#if total_distance is np.inf, set it equal to accel ramps distance +
#distance traveled at max_velocity. This make this loop get to max_v and go forever!
if np.isinf(total_distance):
total_distance = max_velocity**2/acceleration + t*max_velocity
if( max_velocity**2/2./acceleration > total_distance/2. ):
# cannot accel to max velocity, compute peak velocity
vmax = np.sqrt(2*(total_distance/2.)*acceleration)
# and time of max velocity
taccel = vmax/acceleration
if t < taccel:
vel = t*acceleration
elif t < 2*taccel:
vel = vmax-(t-taccel)*acceleration
else:
vel = 0
else:
taccel = max_velocity/acceleration
daccel = max_velocity**2/2./acceleration
dconst = total_distance-2*daccel
tconst = dconst/max_velocity
if t < taccel:
vel = t*acceleration
elif t < taccel+tconst:
vel = max_velocity
elif t < 2*taccel+tconst:
vel = max_velocity - acceleration*(t-taccel-tconst)
else:
vel = 0
return vel
def decel_at_current(self, acceleration):
timeout = np.max((self.current_speed/acceleration,
self.current_omega*self.stern_offset/acceleration))
if timeout > 1./self.loop_rate:
timeout_time = rospy.Time.now() + rospy.Duration( timeout*1.5 )
tolerance = self.acceleration*self.loop_rate
self.stop_requested = False
accel_per_loop = acceleration/self.loop_rate
angle = self.current_angle
while ((rospy.Time().now() < timeout_time) and
(not self.stop_requested) and
(self.current_speed>tolerance)):
if (self.current_omega*self.stern_offset>tolerance):
raise TimeoutException("Could not stop before adjusting steering")
omega = np.sign(self.current_omega)*np.min((np.abs(self.current_omega) -
accel_per_loop/self.stern_offset,0))
speed = np.min((self.current_speed - accel_per_loop, 0))
twist = Twist()
twist.angular.z = omega
twist.linear.x = speed*np.cos(angle)
twist.linear.y = speed*np.sin(angle)
self.publisher.publish(twist)
# publish a final 0
self.publisher.publish(Twist())
def is_running(self):
return self.running
def stop(self):
self.stop_requested = True
def estop(self):
self.deceleration = self.stop_deceleration
self.stop_requested = True
class ArmActions():
def __init__(self):
rospy.init_node('left_arm_actions')
self.tfl = TransformListener()
self.aul = l_arm_utils.ArmUtils(self.tfl)
#self.fth = ft_handler.FTHandler()
rospy.loginfo("Waiting for l_utility_frame_service")
try:
rospy.wait_for_service('/l_utility_frame_update', 7.0)
self.aul.update_frame = rospy.ServiceProxy('/l_utility_frame_update', FrameUpdate)
rospy.loginfo("Found l_utility_frame_service")
except:
rospy.logwarn("Left Utility Frame Service Not available")
rospy.loginfo('Waiting for Pixel_2_3d Service')
try:
rospy.wait_for_service('/pixel_2_3d', 7.0)
self.p23d_proxy = rospy.ServiceProxy('/pixel_2_3d', Pixel23d, True)
rospy.loginfo("Found pixel_2_3d service")
except:
rospy.logwarn("Pixel_2_3d Service Not available")
#Low-level motion requests: pass directly to arm_utils
rospy.Subscriber('wt_left_arm_pose_commands', Point, self.torso_frame_move)
rospy.Subscriber('wt_left_arm_angle_commands', JointTrajectoryPoint, self.aul.send_traj_point)
#More complex motion scripts, defined here using arm_util functions
rospy.Subscriber('norm_approach_left', PoseStamped, self.norm_approach)
rospy.Subscriber('wt_grasp_left_goal', PoseStamped, self.grasp)
#rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.wipe)
rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.force_wipe_agg)
rospy.Subscriber('wt_swipe_left_goals', PoseStamped, self.swipe)
rospy.Subscriber('wt_lin_move_left', Float32, self.hand_move)
rospy.Subscriber('wt_adjust_elbow_left', Float32, self.aul.adjust_elbow)
rospy.Subscriber('wt_surf_wipe_left_points', Point, self.force_surf_wipe)
rospy.Subscriber('wt_poke_left_point', PoseStamped, self.poke)
rospy.Subscriber('wt_contact_approach_left', PoseStamped, self.approach_to_contact)
self.wt_log_out = rospy.Publisher('wt_log_out', String)
self.test_pose = rospy.Publisher('test_pose', PoseStamped, latch=True)
self.say = rospy.Publisher('text_to_say', String)
self.wipe_started = False
self.surf_wipe_started = False
self.wipe_ends = [PoseStamped(), PoseStamped()]
#FORCE_TORQUE ADDITIONS
#rospy.Subscriber('pr2_netft_zeroer/wrench_zeroed', WrenchStamped, self.ft_preprocess)
#self.rezero_wrench = rospy.Publisher('pr2_netft_zeroer/rezero_wrench', Bool)
rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.ft_preprocess)
rospy.Subscriber('wt_ft_goal', Float32, self.set_force_goal)
self.wt_force_out = rospy.Publisher('wt_force_out', Float32)
self.ft_rate_limit = rospy.Rate(30)
self.ft = WrenchStamped()
self.ft_mag = 0.
self.ft_mag_que = deque([0]*10,10)
self.ft_sm_mag = 0.
self.ft_case = None
self.force_goal_mean =3 #1.42
self.force_goal_std= 0.625
self.stop_maintain = False
self.force_wipe_started = False
self.force_wipe_start = PoseStamped()
self.force_wipe_appr = PoseStamped()
def set_force_goal(self, msg):
self.force_goal_mean = msg.data
def ft_preprocess(self, ft):
self.ft = ft
self.ft_mag = math.sqrt(ft.wrench.force.x**2 + ft.wrench.force.y**2 + ft.wrench.force.z**2)
self.ft_mag_que.append(self.ft_mag)
self.ft_sm_mag = np.mean(self.ft_mag_que)
#print 'Force Magnitude: ', self.ft_mag
self.wt_force_out.publish(self.ft_mag)
def approach_to_contact(self, ps, overshoot=0.05):
ps.pose.position.z += 0.02
self.stop_maintain = True
self.aul.update_frame(ps)
appr = self.aul.find_approach(ps, 0.15)
goal = self.aul.find_approach(ps, -overshoot)
(appr_reachable, ik_goal) = self.aul.full_ik_check(appr)
(goal_reachable, ik_goal) = self.aul.full_ik_check(goal)
if appr_reachable and goal_reachable:
traj = self.aul.build_trajectory(goal,appr,tot_points=200)
#prep = self.aul.move_arm_to(appr)
self.aul.blind_move(appr, 3)
rospy.sleep(3)
if True: # if prep:
self.adjust_forearm(traj.points[0].positions)
#self.rezero_wrench.publish(data=True)
curr_traj_point = self.advance_to_contact(traj)
if not curr_traj_point is None:
self.maintain_norm_force2(traj, curr_traj_point)
#self.maintain_force_position(self.aul.hand_frame_move(0.05))
#self.twist_wipe(); self.aul.blind_move(appr)
else:
self.aul.send_traj_point(traj.points[0], 4)
else:
rospy.loginfo("Cannot reach desired 'move-to-contact' point")
self.wt_log_out.publish(data="Cannot reach desired 'move-to-contact' point")
def advance_to_contact(self, traj):
self.stop_maintain = False
curr_traj_point = 0
advance_rate = rospy.Rate(10)
while not (rospy.is_shutdown() or self.stop_maintain):
if not (curr_traj_point >= len(traj.points)):
self.aul.send_traj_point(traj.points[curr_traj_point], 0.3)
curr_traj_point += 1
advance_rate.sleep()
else:
rospy.loginfo("Moved past expected contact, but no contact found! Returning to start")
self.wt_log_out.publish(data="Moved past expected contact, but no contact found! Returning to start")
self.stop_maintain = True
return None
if self.ft_mag > 2.5:
self.stop_maintain = True
print "Contact Detected"
return curr_traj_point
def maintain_norm_force2(self, traj, curr_traj_point=0, mean=3, std=1):
self.stop_maintain = False
maintain_rate = rospy.Rate(100)
while not (rospy.is_shutdown() or self.stop_maintain):
if self.ft_mag > 12:
rospy.loginfo("Force Too High, ending behavior")
self.wt_log_out.publish(data="Force too high, ending behavior")
break
#print "mean: ", mean, "stds: ", std, "force: ", self.ft_mag
if self.ft_mag < mean - std:
curr_traj_point += 1
curr_traj_point = np.clip(curr_traj_point, 0, len(traj.points))
#print curr_traj_point
print "Low"
if not (curr_traj_point >= len(traj.points)):
self.aul.send_traj_point(traj.points[curr_traj_point], 0.3)
rospy.sleep(0.3)
#else:
# rospy.loginfo("Force too low, but extent of the trajectory is reached")
# self.wt_log_out.publish(data="Force too low, but extent of the trajectory is reached")
# self.stop_maintain = True
elif self.ft_mag > mean + std:
print "High"
steps = int(round((self.ft_mag/std)))
curr_traj_point -= steps
#print curr_traj_point
curr_traj_point = np.clip(curr_traj_point, 0, len(traj.points))
if curr_traj_point >= 0:
self.aul.send_traj_point(traj.points[curr_traj_point], 0.3)
rospy.sleep(0.3)
else:
rospy.loginfo("Beginning of trajectory reached, cannot back away further")
self.wt_log_out.publish(data="Beginning of trajectory reached, cannot back away further")
#self.stop_maintain = True
maintain_rate.sleep()
mean = self.force_goal_mean
std = self.force_goal_std
print "Returning to start position"
self.aul.send_traj_point(traj.points[0], 4)
def maintain_norm_force(self, traj, curr_traj_point=0, mean=0, std=1):
self.stop_maintain = False
maintain_rate = rospy.Rate(100)
while not (rospy.is_shutdown() or self.stop_maintain):
#print "mean: ", mean, "stds: ", std, "force: ", self.ft_mag
if self.ft_mag < mean - std:
curr_traj_point += 1
np.clip(curr_traj_point, 0, len(traj.points))
if not (curr_traj_point >= len(traj.points)):
print curr_traj_point
self.aul.send_traj_point(traj.points[curr_traj_point], 0.1)
rospy.sleep(0.1)
else:
rospy.loginfo("Force too low, but extent of the trajectory is reached")
self.wt_log_out.publish(data="Force too low, but extent of the trajectory is reached")
stopped = True
elif self.ft_mag > mean + std:
curr_traj_point -= 1
np.clip(curr_traj_point, 0, len(traj.points))
if curr_traj_point >= 0:
print curr_traj_point
self.aul.send_traj_point(traj.points[curr_traj_point], 0.1)
rospy.sleep(0.1)
else:
rospy.loginfo("Beginning of trajectory reached, cannot back away further")
self.wt_log_out.publish(data="Beginning of trajectory reached, cannot back away further")
stopped = True
maintain_rate.sleep()
mean = self.force_goal_mean
std = self.force_goal_std
# def maintain_net_force(self, mean=0, std=3):
# self.stop_maintain = False
# maintain_rate = rospy.Rate(100)
# while not (rospy.is_shutdown() or self.stop_maintain):
# if self.ft_mag > mean + 8:
# curr_angs = self.aul.joint_state_act.positions
# try:
# x_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0.02))).solution.joint_state.position, curr_angs)
# x_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(-0.02))).solution.joint_state.position, curr_angs)
# y_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0.02, 0))).solution.joint_state.position, curr_angs)
# y_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, -0.02, 0))).solution.joint_state.position, curr_angs)
# z_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0, 0.02))).solution.joint_state.position, curr_angs)
# z_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0, -0.02))).solution.joint_state.position, curr_angs)
# #print 'x: ', x_plus,'\r\n', x_minus
# #print 'y: ', y_plus,'\r\n', y_minus
# #print 'z: ', z_plus,'\r\n', z_minus
# ft_sum = self.ft_mag
# parts = np.divide([self.ft.wrench.force.x, self.ft.wrench.force.y, self.ft.wrench.force.z], ft_sum)
# print 'parts', parts
# ends = [[x_plus,x_minus],[y_plus, y_minus],[z_plus,z_minus]]
# side = [[0]*7]*3
# for i, part in enumerate(parts):
# if part >=0:
# side[i] = ends[i][0]
# else:
# side[i] = ends[i][1]
#
# ang_out = curr_angs
# for i in range(3):
# ang_out -= np.average(side, 0, parts)
# except:
# print 'Near Joint Limits!'
# self.aul.send_joint_angles(ang_out)
#
# #print 'x: ', x_plus, x_minus
# maintain_rate.sleep()
# mean = self.force_goal_mean
# std = self.force_goal_std
def maintain_force_position(self,pose = None, mean=3, std=1):
self.stop_maintain = False
if pose is None:
goal = self.aul.curr_pose()
else:
goal = pose
goal_ort = [goal.pose.orientation.x,goal.pose.orientation.y,goal.pose.orientation.z,goal.pose.orientation.w]
error = PoseStamped()
maintain_rate = rospy.Rate(250)
while not (rospy.is_shutdown() or self.stop_maintain):
current = self.aul.curr_pose()
current_ort = [current.pose.orientation.x, current.pose.orientation.y, current.pose.orientation.z, current.pose.orientation.w]
error.pose.position.x = current.pose.position.x - goal.pose.position.x
error.pose.position.y = current.pose.position.y - goal.pose.position.y
error.pose.position.z = current.pose.position.z - goal.pose.position.z
error_mag = math.sqrt(error.pose.position.x**2 + error.pose.position.y**2 + error.pose.position.z**2)
#out = deepcopy(goal)
out = PoseStamped()
out.header.frame_id = goal.header.frame_id
out.header.stamp = rospy.Time.now()
out.pose.position = Point(goal.pose.position.x, goal.pose.position.y, goal.pose.position.z)
self.test_pose.publish(out)
if all(np.array(self.ft_mag_que) < mean - std) and error_mag > 0.005:
#print 'Force Too LOW'
out.pose.position.x += 0.990*error.pose.position.x
out.pose.position.y += 0.990*error.pose.position.y
out.pose.position.z += 0.990*error.pose.position.z
ori = transformations.quaternion_slerp(goal_ort, current_ort, 0.990)
out.pose.orientation = Quaternion(*ori)
self.aul.fast_move(out,0.0038)
elif all(np.array(self.ft_mag_que) > mean + std):
#print 'Moving to avoid force'
current.pose.position.x += self.ft.wrench.force.x/9000
current.pose.position.y += self.ft.wrench.force.y/9000
current.pose.position.z += self.ft.wrench.force.z/9000
self.aul.fast_move(current,0.0038)
else:
pass
#print "Force in desired range"
mean = self.force_goal_mean
std = self.force_goal_std
#rospy.sleep(0.001)
maintain_rate.sleep()
#def maintain_force_position(self,pose = None, mean=3, std=1):
# self.stop_maintain = False
# if pose is None:
# goal = self.aul.curr_pose()
# else:
# goal = pose
# self.rezero_wrench.publish(data=True)
# maintain_rate = rospy.Rate(500)
# kp = 0.07
# kd = 0.03
# ki = 0.0
# error = PoseStamped()
# old_error = PoseStamped()
# sum_error_x = deque([0]*10,10)
# sum_error_y = deque([0]*10,10)
# sum_error_z = deque([0]*10,10)
# while not (rospy.is_shutdown() or self.stop_maintain):
# current = self.aul.curr_pose()
# error.pose.position.x = current.pose.position.x - goal.pose.position.x
# error.pose.position.y = current.pose.position.y - goal.pose.position.y
# error.pose.position.z = current.pose.position.z - goal.pose.position.z
# sum_error_x.append(error.pose.position.x)
# sum_error_y.append(error.pose.position.y)
# sum_error_z.append(error.pose.position.z)
# print "Force: ", self.ft_sm_mag, min(self.ft_mag_que), max(self.ft_mag_que)
# print "Error: ", error.pose.position
# print self.ft_mag_que, mean-std
# print self.ft_mag_que < mean-std
# break
# if all([self.ft_mag_que < mean - std]):
# print 'Force Too LOW'
# current.pose.position.x += kp*error.pose.position.x + kd*(error.pose.position.x - old_error.pose.position.x) + ki*np.sum(sum_error_x)
# current.pose.position.x += kp*error.pose.position.y + kd*(error.pose.position.y - old_error.pose.position.y) + ki*np.sum(sum_error_y)
# current.pose.position.x += kp*error.pose.position.z + kd*(error.pose.position.z - old_error.pose.position.z) + ki*np.sum(sum_error_z)
# self.aul.fast_move(current, 0.02)
# self.test_pose.publish(current)
# if all([i > mean + std for i in self.ft_mag_que]):
# print 'Moving to avoid force'
# current.pose.position.x += self.ft.wrench.force.x/10000
# current.pose.position.y += self.ft.wrench.force.y/10000
# current.pose.position.z += self.ft.wrench.force.z/10000
# self.aul.fast_move(current, 0.02)
# self.test_pose.publish(current)
# old_error = error
# mean = self.force_goal_mean
# std = self.force_goal_std
# maintain_rate.sleep()
def maintain_net_force(self, mean=3, std=1):
self.stop_maintain = False
maintain_rate = rospy.Rate(500)
#self.rezero_wrench.publish(data=True)
while not (rospy.is_shutdown() or self.stop_maintain):
if self.ft_mag > mean + 3:
print 'Moving to avoid force'
print "Force: ", self.ft_mag
goal = self.aul.curr_pose()
goal.pose.position.x += np.clip(self.ft.wrench.force.x/5000, -0.001, 0.001)
goal.pose.position.y += np.clip(self.ft.wrench.force.y/5000, -0.001, 0.001)
goal.pose.position.z += np.clip(self.ft.wrench.force.z/5000, -0.001, 0.001)
self.test_pose.publish(goal)
self.aul.fast_move(goal, 0.02)
mean = self.force_goal_mean
std = self.force_goal_std
maintain_rate.sleep()
def mannequin(self):
mannequin_rate=rospy.Rate(100)
pose = PoseStamped()
while not rospy.is_shutdown():
#joints = np.add(np.array(self.aul.joint_state_act.positions), np.clip(np.array(self.aul.joint_state_err.positions), -0.05, 0.05))
joints = self.aul.joint_state_act.positions
print joints
#raw_input('Review Joint Angles')
self.aul.send_joint_angles(joints, 0.00001)
pose.header.stamp = rospy.Time.now()
self.test_pose.publish(pose)
mannequin_rate.sleep()
def force_wipe_agg(self, ps):
ps.pose.position.z += 0.02
self.aul.update_frame(ps)
rospy.sleep(0.1)
pose = self.aul.find_approach(ps, 0)
(goal_reachable, ik_goal) = self.aul.ik_check(pose)
if goal_reachable:
if not self.force_wipe_started:
appr = self.aul.find_approach(ps, 0.20)
(appr_reachable, ik_goal) = self.aul.ik_check(appr)
self.test_pose.publish(appr)
if appr_reachable:
self.force_wipe_start = pose
self.force_wipe_appr = appr
self.force_wipe_started = True
else:
rospy.loginfo("Cannot reach approach point, please choose another")
self.wt_log_out.publish(data="Cannot reach approach point, please choose another")
self.say.publish(data="I cannot get to a safe approach for there, please choose another point")
else:
ps1, ps2 = self.align_poses(self.force_wipe_start, pose)
self.force_wipe_prep(ps1, ps2)
# self.force_wipe_prep(self.force_wipe_start, pose)
self.force_wipe_started = False
else:
rospy.loginfo("Cannot reach wiping point, please choose another")
self.wt_log_out.publish(data="Cannot reach wiping point, please choose another")
self.say.publish(data="I cannot reach there, please choose another point")
def force_wipe_prep(self, ps_start, ps_finish, travel = 0.05):
ps_start.header.stamp = rospy.Time.now()
ps_finish.header.stamp = rospy.Time.now()
ps_start_far = self.aul.hand_frame_move(travel, 0, 0, ps_start)
ps_start_near = self.aul.hand_frame_move(-travel, 0, 0, ps_start)
ps_finish_far = self.aul.hand_frame_move(travel, 0, 0, ps_finish)
ps_finish_near = self.aul.hand_frame_move(-travel, 0, 0, ps_finish)
n_points = int(math.ceil(self.aul.calc_dist(ps_start, ps_finish)*9000))
print 'n_points: ', n_points
mid_traj = self.aul.build_trajectory(ps_finish, ps_start, tot_points=n_points, jagged=False)
near_traj = self.aul.build_trajectory(ps_finish_near, ps_start_near, tot_points=n_points, jagged=False)
far_traj = self.aul.build_trajectory(ps_finish_far, ps_start_far, tot_points=n_points, jagged=False)
self.force_wipe(mid_traj, near_traj, far_traj)
def force_surf_wipe(self, point):
self.fsw_poses = self.prep_surf_wipe(point)
if not self.fsw_poses is None:
near_poses = far_poses = [PoseStamped() for i in xrange(len(self.fsw_poses))]
for i, p in enumerate(self.fsw_poses):
near_poses[i]=self.aul.pose_frame_move(p, -0.05)
far_poses[i]=self.aul.pose_frame_move(p, 0.05)
near_traj = self.aul.fill_ik_traj(near_poses)
mid_traj = self.aul.fill_ik_traj(self.fsw_poses)
far_traj = self.aul.fill_ik_traj(far_poses)
print 'Trajectories Found'
self.force_wipe(mid_traj, near_traj, far_traj)
def adjust_forearm(self, in_angles):
print 'cur angles: ', self.aul.joint_state_act.positions, 'angs: ', in_angles
print np.abs(np.subtract(self.aul.joint_state_act.positions, in_angles))
if np.max(np.abs(np.subtract(self.aul.joint_state_act.positions,in_angles)))>math.pi:
self.say.publish(data="Adjusting for-arm roll")
print "Evasive Action!"
angles = list(self.aul.joint_state_act.positions)
flex = in_angles[5]
angles[5] = -0.1
self.aul.send_joint_angles(angles, 4)
angles[4] = in_angles[4]
self.aul.send_joint_angles(angles,6)
angles[5] = flex
self.aul.send_joint_angles(angles, 4)
def force_wipe(self, mid_traj, near_traj, far_traj):
near_angles = [list(near_traj.points[i].positions) for i in range(len(near_traj.points))]
mid_angles = [list(mid_traj.points[i].positions) for i in range(len(mid_traj.points))]
far_angles = [list(far_traj.points[i].positions) for i in range(len(far_traj.points))]
print 'lens: nmf: ', len(near_angles), len(mid_angles), len(far_angles)
fmn_diff = np.abs(np.subtract(near_angles, far_angles))
fmn_max = np.max(fmn_diff, axis=0)
print 'fmn_max: ', fmn_max
if any(fmn_max >math.pi/2):
rospy.loginfo("TOO LARGE OF AN ANGLE CHANGE ALONG GRADIENT, IK REGION PROBLEMS!")
self.wt_log_out.publish(data="The path requested required large movements (IK Limits cause angle wrapping)")
self.say.publish(data="Large motion detected, cancelling. Please try again.")
return
for i in range(7):
n_max = max(np.abs(np.diff(near_angles,axis=0)[i]))
m_max = max(np.abs(np.diff(mid_angles,axis=0)[i]))
f_max = max(np.abs(np.diff(far_angles,axis=0)[i]))
n_mean = 4*np.mean(np.abs(np.diff(near_angles,axis=0)[i]))
m_mean = 4*np.mean(np.abs(np.diff(mid_angles,axis=0)[i]))
f_mean = 4*np.mean(np.abs(np.diff(far_angles,axis=0)[i]))
print 'near: max: ', n_max, 'mean: ', n_mean
print 'mid: max: ', m_max, 'mean: ', m_mean
print 'far: max: ', f_max, 'mean: ', f_mean
if (n_max >= n_mean) or (m_max >= m_mean) or (f_max >= f_mean):
rospy.logerr("TOO LARGE OF AN ANGLE CHANGE ALONG PATH, IK REGION PROBLEMS!")
self.wt_log_out.publish(data="The path requested required large movements (IK Limits cause angle wrapping)")
self.say.publish(data="Large motion detected, cancelling. Please try again.")
return
near_diff = np.subtract(near_angles, mid_angles).tolist()
far_diff = np.subtract(far_angles, mid_angles).tolist()
self.say.publish(data="Moving to approach point")
appr = self.force_wipe_appr
appr.pose.orientation = self.aul.get_fk(near_angles[0]).pose.orientation
#prep = self.aul.move_arm_to(appr)
self.aul.blind_move(appr)
#rospy.sleep(3)
if True: #prep:
self.adjust_forearm(near_angles[0])
self.say.publish(data="Making Approach.")
bias = 2
self.aul.send_joint_angles(np.add(mid_angles[0],np.multiply(bias, near_diff[0])), 3.5)
#self.rezero_wrench.publish(data=True)
rospy.sleep(1)
wipe_rate = rospy.Rate(1000)
self.stop_maintain = False
count = 0
lap = 0
max_laps = 4
mean = self.force_goal_mean
std = self.force_goal_std
self.say.publish(data="Wiping")
single_dir = False#True
time = rospy.Time.now().to_sec()
while not (rospy.is_shutdown() or self.stop_maintain) and (count + 1 <= len(mid_angles)) and (lap < max_laps):
if self.ft_mag > 10:
angles_out = np.add(mid_angles[0], np.multiply(2, near_diff[0]))
self.aul.send_joint_angles(angles_out,2)
rospy.loginfo("Force Too High, ending behavior")
self.wt_log_out.publish(data="Force too high, ending behavior")
break
#print "mean: ", mean, "std: ", std, "force: ", self.ft_mag
if self.ft_mag >= mean + std:
# print 'Force too high!'
bias += (self.ft_mag/500)
elif self.ft_mag < mean - std:
# print 'Force too low'
bias -= max(0.003,(self.ft_mag/1500))
else:
# print 'Force Within Range'
count += 1
bias = np.clip(bias, -1, 2)
if bias > 0.:
diff = near_diff[count]
else:
diff = far_diff[count]
angles_out = np.add(mid_angles[count], np.multiply(abs(bias), diff))
self.aul.send_joint_angles(angles_out, 0.0025)
#rospy.sleep(0.0000i1)
#print "Rate: ", (1/ (rospy.Time.now().to_sec() - time))
#time = rospy.Time.now().to_sec()
wipe_rate.sleep()
mean = self.force_goal_mean
std = self.force_goal_std
if count + 1>= len(mid_angles):
if single_dir:
bias = 1
pose = self.aul.curr_pose()
pose = self.aul.hand_frame_move(-0.01)
rot = transformations.quaternion_about_axis(math.radians(-10), (0,1,0))
q = transformations.quaternion_multiply([pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w],rot)
pose.pose.orientation = Quaternion(*q)
self.aul.blind_move(pose)
#goal = self.aul.ik_pose_proxy(self.aul.form_ik_request(pose))
#if goal.error_code.val == 1:
# self.aul.send_angles_wrap(goal.solution.joint_state.position)
#angles_out = list(self.aul.joint_state_act.positions)
#angles_out[4] += 0.05
# self.aul.send_joint_angles(angles_out,3)
angles_out = np.add(mid_angles[count], np.multiply(bias, near_diff[count]))
self.aul.send_joint_angles(angles_out,5)
angles_out = np.add(mid_angles[0], np.multiply(bias, near_diff[0]))
self.aul.send_joint_angles(angles_out,5)
else:
mid_angles.reverse()
near_diff.reverse()
far_diff.reverse()
lap += 1
#if lap == 3:
# self.say.publish(data="Hold Still, you rascal!")
count = 0
rospy.sleep(0.5)
self.say.publish(data="Pulling away")
angles_out = np.add(mid_angles[0], np.multiply(2, near_diff[0]))
self.aul.send_joint_angles(angles_out,5)
rospy.sleep(5)
self.say.publish(data="Finished wiping. Thank you")
def torso_frame_move(self, msg):
self.stop_maintain = True
goal = self.aul.curr_pose()
goal.pose.position.x += msg.x
goal.pose.position.y += msg.y
goal.pose.position.z += msg.z
self.aul.blind_move(goal)
def hand_move(self, f32):
self.stop_maintain = True
hfm_pose = self.aul.hand_frame_move(f32.data)
self.aul.blind_move(hfm_pose)
def norm_approach(self, pose):
self.stop_maintain = True
self.aul.update_frame(pose)
appr = self.aul.find_approach(pose, 0.15)
self.aul.move_arm_to(appr)
def grasp(self, ps):
self.stop_maintain = True
rospy.loginfo("Initiating Grasp Sequence")
self.wt_log_out.publish(data="Initiating Grasp Sequence")
self.aul.update_frame(ps)
approach = self.aul.find_approach(ps, 0.15)
rospy.loginfo("approach: \r\n %s" %approach)
at_appr = self.aul.move_arm_to(approach)
rospy.loginfo("arrived at approach: %s" %at_appr)
if at_appr:
opened = self.aul.gripper(0.09)
if opened:
rospy.loginfo("making linear approach")
#hfm_pose = self.aul.hand_frame_move(0.23)
hfm_pose = self.aul.find_approach(ps,-0.02)
self.aul.blind_move(hfm_pose)
self.aul.wait_for_stop(2)
closed = self.aul.gripper(0.0)
if not closed:
rospy.loginfo("Couldn't close completely: Grasp likely successful")
hfm_pose = self.aul.hand_frame_move(-0.23)
self.aul.blind_move(hfm_pose)
else:
pass
def prep_surf_wipe(self, point):
pixel_u = point.x
pixel_v = point.y
test_pose = self.p23d_proxy(pixel_u, pixel_v).pixel3d
self.aul.update_frame(test_pose)
test_pose = self.aul.find_approach(test_pose, 0)
(reachable, ik_goal) = self.aul.full_ik_check(test_pose)
if reachable:
if not self.surf_wipe_started:
start_pose = test_pose
self.surf_wipe_start = [pixel_u, pixel_v, start_pose]
self.surf_wipe_started = True
rospy.loginfo("Received valid starting position for wiping action")
self.wt_log_out.publish(data="Received valid starting position for wiping action")
return None#Return after 1st point, wait for second
else:
rospy.loginfo("Received valid ending position for wiping action")
self.wt_log_out.publish(data="Received valid ending position for wiping action")
self.surf_wipe_started = False #Continue on successful 2nd point
else:
rospy.loginfo("Cannot reach wipe position, please try another")
self.wt_log_out.publish(data="Cannot reach wipe position, please try another")
return None#Return on invalid point, wait for another
dist = self.aul.calc_dist(self.surf_wipe_start[2],test_pose)
print 'dist', dist
num_points = dist/0.01
print 'num_points', num_points
us = np.round(np.linspace(self.surf_wipe_start[0], pixel_u, num_points))
vs = np.round(np.linspace(self.surf_wipe_start[1], pixel_v, num_points))
surf_points = [PoseStamped() for i in xrange(len(us))]
print "Surface Points", [us,vs]
for i in xrange(len(us)):
pose = self.p23d_proxy(us[i],vs[i]).pixel3d
self.aul.update_frame(pose)
surf_points[i] = self.aul.find_approach(pose,0)
print i+1, '/', len(us)
return surf_points
#self.aul.blind_move(surf_points[0])
#self.aul.wait_for_stop()
#for pose in surf_points:
# self.aul.blind_move(pose,2.5)
# rospy.sleep(2)
# #self.aul.wait_for_stop()
#self.aul.hand_frame_move(-0.1)
def twist_wipe(self):
angles = list(self.aul.joint_state_act.positions)
count = 0
while count < 3:
angles[6] = -6.7
self.aul.send_joint_angles(angles)
while self.aul.joint_state_act.positions[6] > -6.6:
rospy.sleep(0.1)
angles[6] = 0.8
self.aul.send_joint_angles(angles)
while self.aul.joint_state_act.positions[6] < 0.7:
rospy.sleep(0.1)
count += 1
def poke(self, ps):
self.stop_maintain = True
self.aul.update_frame(ps)
appr = self.aul.find_approach(ps,0.15)
touch = self.aul.find_approach(ps,0)
prepared = self.aul.move_arm_to(appr)
if prepared:
self.aul.blind_move(touch)
self.aul.wait_for_stop()
rospy.sleep(7)
self.aul.blind_move(appr)
def swipe(self, ps):
traj = self.prep_wipe(ps)
if traj is not None:
self.stop_maintain = True
self.wipe_move(traj, 1)
def wipe(self, ps):
traj = self.prep_wipe(ps)
if traj is not None:
self.stop_maintain = True
self.wipe_move(traj, 4)
def prep_wipe(self, ps):
#print "Prep Wipe Received: %s" %pa
self.aul.update_frame(ps)
print "Updating frame to: %s \r\n" %ps
if not self.wipe_started:
self.wipe_appr_seed = ps
self.wipe_ends[0] = self.aul.find_approach(ps, 0)
print "wipe_end[0]: %s" %self.wipe_ends[0]
(reachable, ik_goal) = self.aul.full_ik_check(self.wipe_ends[0])
if not reachable:
rospy.loginfo("Cannot find approach for initial wipe position, please try another")
self.wt_log_out.publish(data="Cannot find approach for initial wipe position, please try another")
return None
else:
self.wipe_started = True
rospy.loginfo("Received starting position for wiping action")
self.wt_log_out.publish(data="Received starting position for wiping action")
return None
else:
self.wipe_ends[1] = self.aul.find_approach(ps, 0)
self.wipe_ends.reverse()
(reachable, ik_goal) = self.aul.full_ik_check(self.wipe_ends[1])
if not reachable:
rospy.loginfo("Cannot find approach for final wipe position, please try another")
self.wt_log_out.publish(data="Cannot find approach for final wipe position, please try another")
return None
else:
rospy.loginfo("Received End position for wiping action")
self.wt_log_out.publish(data="Received End position for wiping action")
####### REMOVED AND REPLACED WITH ALIGN FUNCTION ##############
self.wipe_ends[0], self.wipe_ends[1] = self.align_poses(self.wipe_ends[0],self.wipe_ends[1])
self.aul.update_frame(self.wipe_appr_seed)
appr = self.aul.find_approach(self.wipe_appr_seed, 0.15)
appr.pose.orientation = self.wipe_ends[1].pose.orientation
prepared = self.aul.move_arm_to(appr)
if prepared:
#self.aul.advance_to_contact()
self.aul.blind_move(self.wipe_ends[1])
traj = self.aul.build_trajectory(self.wipe_ends[1], self.wipe_ends[0])
wipe_traj = self.aul.build_follow_trajectory(traj)
self.aul.wait_for_stop()
self.wipe_started = False
return wipe_traj
#self.wipe(wipe_traj)
else:
rospy.loginfo("Failure reaching start point, please try again")
self.wt_log_out.publish(data="Failure reaching start point, please try again")
def align_poses(self, ps1, ps2):
self.aul.update_frame(ps1)
ps2.header.stamp = rospy.Time.now()
self.tfl.waitForTransform(ps2.header.frame_id, 'lh_utility_frame', rospy.Time.now(), rospy.Duration(3.0))
ps2_in_ps1 = self.tfl.transformPose('lh_utility_frame', ps2)
ang = math.atan2(-ps2_in_ps1.pose.position.z, -ps2_in_ps1.pose.position.y)+(math.pi/2)
q_st_rot = transformations.quaternion_about_axis(ang, (1,0,0))
q_st_new = transformations.quaternion_multiply([ps1.pose.orientation.x, ps1.pose.orientation.y, ps1.pose.orientation.z, ps1.pose.orientation.w],q_st_rot)
ps1.pose.orientation = Quaternion(*q_st_new)
self.aul.update_frame(ps2)
ps1.header.stamp = rospy.Time.now()
self.tfl.waitForTransform(ps1.header.frame_id, 'lh_utility_frame', rospy.Time.now(), rospy.Duration(3.0))
ps1_in_ps2 = self.tfl.transformPose('lh_utility_frame', ps1)
ang = math.atan2(ps1_in_ps2.pose.position.z, ps1_in_ps2.pose.position.y)+(math.pi/2)
q_st_rot = transformations.quaternion_about_axis(ang, (1,0,0))
q_st_new = transformations.quaternion_multiply([ps2.pose.orientation.x, ps2.pose.orientation.y, ps2.pose.orientation.z, ps2.pose.orientation.w],q_st_rot)
ps2.pose.orientation = Quaternion(*q_st_new)
return ps1, ps2
def wipe_move(self, traj_goal, passes=4):
times = []
for i in range(len(traj_goal.trajectory.points)):
times.append(traj_goal.trajectory.points[i].time_from_start)
count = 0
while count < passes:
traj_goal.trajectory.points.reverse()
for i in range(len(times)):
traj_goal.trajectory.points[i].time_from_start = times[i]
traj_goal.trajectory.header.stamp = rospy.Time.now()
assert traj_goal.trajectory.points[0] != []
self.aul.l_arm_follow_traj_client.send_goal(traj_goal)
self.aul.l_arm_follow_traj_client.wait_for_result(rospy.Duration(20))
rospy.sleep(0.5)# Pause at end of swipe
count += 1
rospy.loginfo("Done Wiping")
self.wt_log_out.publish(data="Done Wiping")
hfm_pose = self.aul.hand_frame_move(-0.15)
self.aul.blind_move(hfm_pose)
class Shaving_manager():
def __init__(self):
self.tf = TransformListener()
self.jtarms = jttask_utils.jt_task_utils(self.tf)
self.controller_switcher = ControllerSwitcher()
##### Subscribers ####
self.disc_sub = rospy.Subscriber('wt_disc_shaving_step', Int8, self.disc_change_pose)
self.disc_sub.impl.add_callback(self.cancel_goals, None)
self.cont_sub = rospy.Subscriber('wt_cont_shaving_step', Point, self.cont_change_pose)
self.cont_sub.impl.add_callback(self.cancel_goals, None)
self.loc_sub = rospy.Subscriber('wt_shave_location', Int8, self.set_shave_pose)
self.loc_sub.impl.add_callback(self.cancel_goals, None)
rospy.Subscriber('wt_stop_shaving', Bool, self.stop_shaving)
#### Services ####
rospy.loginfo("Waiting for get_head_pose service")
try:
rospy.wait_for_service('/get_head_pose', 5.0)
self.get_pose_proxy = rospy.ServiceProxy('/get_head_pose', GetHeadPose)
rospy.loginfo("Found get_head_pose service")
except:
rospy.logwarn("get_head_pose service not available")
rospy.loginfo("Waiting for ellipsoid_command service")
try:
rospy.wait_for_service('/ellipsoid_command', 5.0)
self.ellipsoid_command_proxy = rospy.ServiceProxy('/ellipsoid_command', EllipsoidCommand)
rospy.loginfo("Found ellipsoid_command service")
except:
rospy.logwarn("ellipsoid_command service not available")
#### Publishers ####
self.wt_log_out = rospy.Publisher('wt_log_out', String)
self.test_out = rospy.Publisher('test_pose_1', PoseStamped)
self.rezero_wrench = rospy.Publisher('/netft_gravity_zeroing/rezero_wrench', Bool)
#### Data ####
self.hand_offset = 0.195
self.angle_tool_offset = 0.03
self.inline_tool_offset = 0.085
self.selected_pose = 0
self.poses = {
0 : ["near_ear",0.],
1 : ["upper_cheek",0.],
2 : ["middle_cheek",0.],
3 : ["jaw_bone",0.],
4 : ["back_neck",0.],
5 : ["nose",0.],
6 : ["chin",0.],
7 : ["mouth_corner", 0.]
}
#self.ft_wrench = WrenchStamped()
# self.force_unsafe = False
# self.ft_activity_thresh = 3
# self.ft_safety_thresh = 12
# rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.get_ft_state)
# rospy.Subscriber('/netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.get_ft_state)
if POSTURE:
rospy.sleep(3)
print 'Sending Posture'
self.jtarms.send_posture(posture='elbowup', arm=0)
# def get_ft_state(self, ws):
# self.ft_wrench = ws
# self.ft_mag = math.sqrt(ws.wrench.force.x**2 + ws.wrench.force.y**2 + ws.wrench.force.z**2)
# if self.ft_mag > self.ft_safety_thresh:
# self.force_unsafe = True
# if self.ft_mag > self.ft_activity_thresh:
# self.ft_activity = rospy.Time.now()
def stop_shaving(self, msg):
self.cancel_goals(msg)
rospy.loginfo("Stopping Shaving Behavior")
self.wt_log_out.publish(data="Stopping Shaving Behavior")
self.controller_switcher.carefree_switch('l','%s_arm_controller')
def cancel_goals(self, msg):
self.jtarms.ft_move_client.cancel_all_goals() #Stop what we're doing, as we're about to do something different, and stopping soon may be desired
self.jtarms.ft_hold_client.cancel_all_goals()
rospy.loginfo("Cancelling Active Shaving Goals")
self.wt_log_out.publish(data="Cancelling Active Shaving Goals")
def cont_change_pose(self, step):
self.controller_switcher.carefree_switch('l','%s_cart')
self.new_pose = True
req = EllipsoidCommandRequest()
req.change_latitude = int(-step.y)
req.change_longitude = int(-step.x)
req.change_height = int(step.z)
print req
print type(req.change_latitude)
self.ellipsoid_command_proxy.call(req)
rospy.loginfo("Moving Across Ellipsoid")
self.wt_log_out.publish("Moving Across Ellipsoid")
def disc_change_pose(self, step):
self.new_pose = True
self.selected_pose += step.data
if self.selected_pose > 7:
self.selected_pose = 7
self.wt_log_out.publish(data="Final position in sequence, there is no next position")
return
elif self.selected_pose < 0:
self.selected_pose = 0
self.wt_log_out.publish(data="First position in sequence, there is no previous position")
return
rospy.loginfo("Moving to "+self.poses[self.selected_pose][0])
self.wt_log_out.publish(data="Moving to "+self.poses[self.selected_pose][0])
ellipse_pose = self.get_pose_proxy(self.poses[self.selected_pose][0], self.poses[self.selected_pose][1]).pose
self.adjust_pose(ellipse_pose)
def set_shave_pose(self, num):
self.selected_pose = num.data
rospy.loginfo("Moving to "+self.poses[self.selected_pose][0])
self.wt_log_out.publish(data="Moving to "+self.poses[self.selected_pose][0])
ellipse_pose = self.get_pose_proxy(self.poses[self.selected_pose][0], self.poses[self.selected_pose][1]).pose
self.adjust_pose(ellipse_pose)
def adjust_pose(self, ellipse_pose):
self.controller_switcher.carefree_switch('l','%s_cart')
self.jtarms.ft_move_client.cancel_all_goals() #Stop what we're doing, as we're about to do something different, and stopping soon may be desired
self.jtarms.ft_hold_client.cancel_all_goals()
#self.test_out.publish(ellipse_pose)
print "New Position Received, should stop current action"
self.tf.waitForTransform(ellipse_pose.header.frame_id, 'torso_lift_link', ellipse_pose.header.stamp, rospy.Duration(3.0))
torso_pose = self.tf.transformPose('torso_lift_link', ellipse_pose)
if TOOL == 'inline':
goal_pose = self.jtarms.pose_frame_move(torso_pose, -(self.hand_offset + self.inline_tool_offset), arm=0)
approach_pose = self.jtarms.pose_frame_move(goal_pose, -0.15, arm=0)
elif TOOL == '90':
goal_pose = self.jtarms.pose_frame_move(torso_pose, 0.015)
elif TOOL == '45':
goal_pose = torso_pose
approach_pose = self.jtarms.pose_frame_move(goal_pose, -0.15, arm=0)
traj_to_approach = self.jtarms.build_trajectory(approach_pose, arm=0)
traj_appr_to_goal = self.jtarms.build_trajectory(goal_pose, approach_pose, arm=0)
self.ft_move(traj_to_approach)
self.rezero_wrench.publish(data=True)
self.ft_move(traj_appr_to_goal, ignore_ft=False)
retreat_traj = self.jtarms.build_trajectory(approach_pose, arm=0)
escape_traj = self.jtarms.build_trajectory(approach_pose, arm=0, steps=30)
self.jtarms.ft_hold_client.send_goal(FtHoldGoal(2.,8,10,rospy.Duration(10)))#activity_thresh, z_unsafe, mag_unsafe, timeout
print "Waiting for hold result"
finished = self.jtarms.ft_hold_client.wait_for_result(rospy.Duration(0))
print "Finished before Timeout: %s" %finished
print "Done Waiting"
hold_result = self.jtarms.ft_hold_client.get_result()
print "Hold Result:"
print hold_result
if hold_result.unsafe:
self.ft_move(escape_traj)
elif hold_result.timeout:
self.ft_move(retreat_traj)
def ft_move(self, traj, ft_thresh=2, ignore_ft=True):
self.jtarms.ft_move_client.send_goal(FtMoveGoal(traj, ft_thresh, ignore_ft),feedback_cb=self.ft_move_feedback)
finished_within_timeout = self.jtarms.ft_move_client.wait_for_result(rospy.Duration(0.25*len(traj)))
if finished_within_timeout:
result = self.jtarms.ft_move_client.get_result()
if result.all_sent:
rospy.loginfo("Full Trajectory Completed")
self.wt_log_out.publish(data="Full Trajectory Completed")
elif result.contact:
rospy.loginfo("Stopped Trajectory upon contact")
self.wt_log_out.publish(data="Stopped Trajectory upon contact")
else:
self.jtarms.ft_move_client.cancel_goal()
rospy.loginfo("Failed to complete movement within timeout period.")
self.wt_log_out.publish(data="Failed to complete movement within timeout period.")
def ft_move_feedback(self, fdbk):
pct = 100.*float(fdbk.current)/float(fdbk.total)
#rospy.loginfo("Movement is %2.0f percent complete." %pct)
self.wt_log_out.publish(data="Movement to %s is %2.0f percent complete."
%(self.poses[self.selected_pose][0], pct))
#def hold_ft_aware(self, escape_pose, arm=0):
# self.jtarms.force_stopped = False
# self.force_unsafe = False
# escape_traj = self.jtarms.build_trajectory(escape_pose)
# time_since_activity = rospy.Duration(0)
# self.ft_activity = rospy.Time.now()
# while not (rospy.is_shutdown() or self.force_unsafe or time_since_activity>rospy.Duration(10) or self.new_pose):
# time_since_activity = rospy.Time.now()-self.ft_activity
# rospy.sleep(0.01)
# if self.force_unsafe:
# rospy.loginfo("Unsafe High Forces, moving away!")
# self.wt_log_out.publish(data="Unsafe High Forces, moving away!")
# self.jtarms.goal_pub[arm].publish(escape_pose)
# return
# if time_since_activity>rospy.Duration(10):
# rospy.loginfo("10s of inactivity, moving away")
# self.wt_log_out.publish(data="10s of inactivity, moving away")
# if self.new_pose:
# rospy.loginfo("New Pose Received")
# self.wt_log_out.publish(data="New Pose Received")
# self.new_pose = False
# self.jtarms.send_traj(escape_traj)
def test(self):
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'torso_lift_link'
goal_pose.header.stamp = rospy.Time.now()
goal_pose.pose.position = Point(0.8, 0.3, 0.0)
goal_pose.pose.orientation = Quaternion(1,0,0,0)
approach_pose = self.jtarms.pose_frame_move(goal_pose, -0.15, arm=0)
traj_to_approach = self.jtarms.build_trajectory(approach_pose, arm=0)
traj_appr_to_goal = self.jtarms.build_trajectory(goal_pose, approach_pose, arm=0)
raw_input("Move to approach pose")
self.jtarms.send_traj(traj_to_approach)
self.jtarms.force_stopped = False
self.force_unsafe = False
raw_input("Move to contact pose")
self.jtarms.force_stopped = False
self.force_unsafe = False
self.jtarms.send_traj_to_contact(traj_appr_to_goal)
raw_input("Hold under force constraints")
self.jtarms.force_stopped = False
self.force_unsafe = False
self.hold_ft_aware(approach_pose, arm=0)
rospy.loginfo("Finished Testing")
class NormalApproach():
standoff = 0.368 #0.2 + 0.168 (dist from wrist to fingertips)
frame = 'base_footprint'
px = py = pz = 0;
qx = qy = qz = 0;
qw = 1;
def __init__(self):
rospy.init_node('normal_approach_right')
rospy.Subscriber('norm_approach_right', PoseStamped, self.update_frame)
rospy.Subscriber('r_hand_pose', PoseStamped, self.update_curr_pose)
rospy.Subscriber('wt_lin_move_right',Float32, self.linear_move)
self.goal_out = rospy.Publisher('wt_right_arm_pose_commands', PoseStamped)
self.move_arm_out = rospy.Publisher('wt_move_right_arm_goals', PoseStamped)
self.tf = TransformListener()
self.tfb = TransformBroadcaster()
self.wt_log_out = rospy.Publisher('wt_log_out', String )
def update_curr_pose(self, msg):
self.currpose = msg;
def update_frame(self, pose):
self.standoff = 0.368
self.frame = pose.header.frame_id
self.px = pose.pose.position.x
self.py = pose.pose.position.y
self.pz = pose.pose.position.z
self.qx = pose.pose.orientation.x
self.qy = pose.pose.orientation.y
self.qz = pose.pose.orientation.z
self.qw = pose.pose.orientation.w
self.tfb.sendTransform((self.px,self.py,self.pz),(self.qx,self.qy,self.qz,self.qw), rospy.Time.now(), "pixel_3d_frame", self.frame)
self.find_approach(pose)
def find_approach(self, msg):
self.pose_in = msg
self.tf.waitForTransform('pixel_3d_frame','base_footprint', rospy.Time(0), rospy.Duration(3.0))
self.tfb.sendTransform((self.pose_in.pose.position.x, self.pose_in.pose.position.y, self.pose_in.pose.position.z),(self.pose_in.pose.orientation.x, self.pose_in.pose.orientation.y, self.pose_in.pose.orientation.z, self.pose_in.pose.orientation.w), rospy.Time.now(), "pixel_3d_frame", self.pose_in.header.frame_id)
self.tf.waitForTransform('pixel_3d_frame','r_wrist_roll_link', rospy.Time.now(), rospy.Duration(3.0))
goal = PoseStamped()
goal.header.frame_id = 'pixel_3d_frame'
goal.header.stamp = rospy.Time.now()
goal.pose.position.z = self.standoff
goal.pose.orientation.x = 0
goal.pose.orientation.y = 0.5*math.sqrt(2)
goal.pose.orientation.z = 0
goal.pose.orientation.w = 0.5*math.sqrt(2)
#print "Goal:\r\n %s" %goal
self.tf.waitForTransform(goal.header.frame_id, 'torso_lift_link', rospy.Time.now(), rospy.Duration(3.0))
appr = self.tf.transformPose('torso_lift_link', goal)
# print "Appr: \r\n %s" %appr
self.wt_log_out.publish(data="Normal Approach with right hand: Trying to move WITH motion planning")
self.move_arm_out.publish(appr)
def linear_move(self, msg):
print "Linear Move: Right Arm: %s m Step" %msg.data
self.tf.waitForTransform(self.currpose.header.frame_id, 'r_wrist_roll_link', self.currpose.header.stamp, rospy.Duration(3.0))
newpose = self.tf.transformPose('r_wrist_roll_link', self.currpose)
newpose.pose.position.x += msg.data
step_goal = self.tf.transformPose(self.currpose.header.frame_id, newpose)
self.goal_out.publish(step_goal)
class Predictor(object):
def __init__(self):
rospy.loginfo("Initializing biternion predictor")
self.counter = 0
modelname = rospy.get_param("~model", "head_50_50")
weightsname = abspath(expanduser(rospy.get_param("~weights", ".")))
rospy.loginfo("Predicting using {} & {}".format(modelname, weightsname))
topic = rospy.get_param("~topic", "/biternion")
self.pub = rospy.Publisher(topic, HeadOrientations, queue_size=3)
self.pub_vis = rospy.Publisher(topic + '/image', ROSImage, queue_size=3)
self.pub_pa = rospy.Publisher(topic + "/pose", PoseArray, queue_size=3)
self.pub_tracks = rospy.Publisher(topic + "/tracks", TrackedPersons, queue_size=3)
# Ugly workaround for "jumps back in time" that the synchronizer sometime does.
self.last_stamp = rospy.Time()
# Create and load the network.
netlib = import_module(modelname)
self.net = netlib.mknet()
self.net.__setstate__(np.load(weightsname))
self.net.evaluate()
self.aug = netlib.mkaug(None, None)
self.preproc = netlib.preproc
self.getrect = netlib.getrect
# Do a fake forward-pass for precompilation.
im = cutout(np.zeros((480,640,3), np.uint8), 0, 0, 150, 450)
im = next(self.aug.augimg_pred(self.preproc(im), fast=True))
self.net.forward(np.array([im]))
rospy.loginfo("BiternionNet initialized")
src = rospy.get_param("~src", "tra")
subs = []
if src == "tra":
subs.append(message_filters.Subscriber(rospy.get_param("~tra", "/TODO"), TrackedPersons2d))
elif src == "ubd":
subs.append(message_filters.Subscriber(rospy.get_param("~ubd", "/upper_body_detector/detections"), UpperBodyDetector))
else:
raise ValueError("Unknown source type: " + src)
rgb = rospy.get_param("~rgb", "/head_xtion/rgb/image_rect_color")
subs.append(message_filters.Subscriber(rgb, ROSImage))
subs.append(message_filters.Subscriber(rospy.get_param("~d", "/head_xtion/depth/image_rect_meters"), ROSImage))
subs.append(message_filters.Subscriber('/'.join(rgb.split('/')[:-1] + ['camera_info']), CameraInfo))
tra3d = rospy.get_param("~tra3d", "")
if src == "tra" and tra3d:
subs.append(message_filters.Subscriber(tra3d, TrackedPersons))
self.listener = TransformListener()
ts = message_filters.ApproximateTimeSynchronizer(subs, queue_size=5, slop=0.5)
ts.registerCallback(self.cb)
def cb(self, src, rgb, d, caminfo, *more):
# Ugly workaround because approximate sync sometimes jumps back in time.
if rgb.header.stamp <= self.last_stamp:
rospy.logwarn("Jump back in time detected and dropped like it's hot")
return
self.last_stamp = rgb.header.stamp
detrects = get_rects(src)
# Early-exit to minimize CPU usage if possible.
#if len(detrects) == 0:
# return
# If nobody's listening, why should we be computing?
if 0 == sum(p.get_num_connections() for p in (self.pub, self.pub_vis, self.pub_pa, self.pub_tracks)):
return
header = rgb.header
bridge = CvBridge()
rgb = bridge.imgmsg_to_cv2(rgb)[:,:,::-1] # Need to do BGR-RGB conversion manually.
d = bridge.imgmsg_to_cv2(d)
imgs = []
for detrect in detrects:
detrect = self.getrect(*detrect)
det_rgb = cutout(rgb, *detrect)
det_d = cutout(d, *detrect)
# Preprocess and stick into the minibatch.
im = subtractbg(det_rgb, det_d, 1.0, 0.5)
im = self.preproc(im)
imgs.append(im)
sys.stderr.write("\r{}".format(self.counter)) ; sys.stderr.flush()
self.counter += 1
# TODO: We could further optimize by putting all augmentations in a
# single batch and doing only one forward pass. Should be easy.
if len(detrects):
bits = [self.net.forward(batch) for batch in self.aug.augbatch_pred(np.array(imgs), fast=True)]
preds = bit2deg(ensemble_biternions(bits)) - 90 # Subtract 90 to correct for "my weird" origin.
# print(preds)
else:
preds = []
if 0 < self.pub.get_num_connections():
self.pub.publish(HeadOrientations(
header=header,
angles=list(preds),
confidences=[0.83] * len(imgs)
))
# Visualization
if 0 < self.pub_vis.get_num_connections():
rgb_vis = rgb[:,:,::-1].copy()
for detrect, alpha in zip(detrects, preds):
l, t, w, h = self.getrect(*detrect)
px = int(round(np.cos(np.deg2rad(alpha))*w/2))
py = -int(round(np.sin(np.deg2rad(alpha))*h/2))
cv2.rectangle(rgb_vis, (detrect[0], detrect[1]), (detrect[0]+detrect[2],detrect[1]+detrect[3]), (0,255,255), 1)
cv2.rectangle(rgb_vis, (l,t), (l+w,t+h), (0,255,0), 2)
cv2.line(rgb_vis, (l+w//2, t+h//2), (l+w//2+px,t+h//2+py), (0,255,0), 2)
# cv2.putText(rgb_vis, "{:.1f}".format(alpha), (l, t+25), cv2.FONT_HERSHEY_SIMPLEX, 1, (255,0,255), 2)
vismsg = bridge.cv2_to_imgmsg(rgb_vis, encoding='rgb8')
vismsg.header = header # TODO: Seems not to work!
self.pub_vis.publish(vismsg)
if 0 < self.pub_pa.get_num_connections():
fx, cx = caminfo.K[0], caminfo.K[2]
fy, cy = caminfo.K[4], caminfo.K[5]
poseArray = PoseArray(header=header)
for (dx, dy, dw, dh, dd), alpha in zip(get_rects(src, with_depth=True), preds):
dx, dy, dw, dh = self.getrect(dx, dy, dw, dh)
# PoseArray message for boundingbox centres
poseArray.poses.append(Pose(
position=Point(
x=dd*((dx+dw/2.0-cx)/fx),
y=dd*((dy+dh/2.0-cy)/fy),
z=dd
),
# TODO: Use global UP vector (0,0,1) and transform into frame used by this message.
orientation=Quaternion(*quaternion_about_axis(np.deg2rad(alpha), [0, -1, 0]))
))
self.pub_pa.publish(poseArray)
if len(more) == 1 and 0 < self.pub_tracks.get_num_connections():
t3d = more[0]
try:
self.listener.waitForTransform(header.frame_id, t3d.header.frame_id, rospy.Time(), rospy.Duration(1))
for track, alpha in zip(t3d.tracks, preds):
track.pose.pose.orientation = self.listener.transformQuaternion(t3d.header.frame_id, QuaternionStamped(
header=header,
# TODO: Same as above!
quaternion=Quaternion(*quaternion_about_axis(np.deg2rad(alpha), [0, -1, 0]))
)).quaternion
self.pub_tracks.publish(t3d)
except TFException:
pass
from visualization_msgs.msg import MarkerArray
import operator
from tf.transformations import euler_from_quaternion
## reality: make sure that person to align to is not robot itself
## align to person with lowest id (except robot)
rospy.init_node('face_to_person')
sss=simple_script_server()
#sss.init("base")
#sss.move("arm","folded")
listener = TransformListener()
# listener1 = TransformListener()
publisher = rospy.Publisher("followedPerson", MarkerArray)
publisher1 = rospy.Publisher("diameter", MarkerArray)
listener.waitForTransform('/map','/base_link',rospy.Time(),rospy.Duration(5))
target_angle = 0
trackedPerson = ""
personsToFollow = ['Richard'] # can be a list
def mydist(rob, hum):
return math.sqrt( (rob[0]-hum.location.point.x)**2 +
(rob[1]-hum.location.point.y)**2 )
def trackedHumans_callback(data):
print "calllback"
global trackedPerson
global target_angle
class RegistrationLoader(object):
WORLD_FRAME = "odom_combined"
HEAD_FRAME = "head_frame"
def __init__(self):
self.head_pose = None
self.head_pc_reg = None
self.head_frame_tf = None
self.head_frame_bcast = TransformBroadcaster()
self.tfl = TransformListener()
self.init_reg_srv = rospy.Service("/initialize_registration", HeadRegistration, self.init_reg_cb)
self.confirm_reg_srv = rospy.Service("/confirm_registration", ConfirmRegistration, self.confirm_reg_cb)
self.head_registration_r = rospy.ServiceProxy("/head_registration_r", HeadRegistration)
self.head_registration_l = rospy.ServiceProxy("/head_registration_l", HeadRegistration)
self.feedback_pub = rospy.Publisher("/feedback", String)
self.test_pose = rospy.Publisher("/test_head_pose", PoseStamped)
self.reg_dir = rospy.get_param("~registration_dir", "")
self.subject = rospy.get_param("~subject", None)
def publish_feedback(self, msg):
rospy.loginfo("[%s] %s" % (rospy.get_name(), msg))
self.feedback_pub.publish(msg)
def init_reg_cb(self, req):
# TODO REMOVE THIS FACE SIDE MESS
self.publish_feedback("Performing Head Registration. Please Wait.")
self.face_side = rospy.get_param("~face_side1", 'r')
bag_str = self.reg_dir + '/' + '_'.join([self.subject, self.face_side, "head_transform"]) + ".bag"
rospy.loginfo("[%s] Loading %s" %(rospy.get_name(), bag_str))
try:
bag = rosbag.Bag(bag_str, 'r')
for topic, msg, ts in bag.read_messages():
head_tf = msg
assert (head_tf is not None), "Error reading head transform bagfile"
bag.close()
except Exception as e:
self.publish_feedback("Registration failed: Error loading saved registration.")
rospy.logerr("[%s] Cannot load registration parameters from %s:\r\n%s" %
(rospy.get_name(), bag_str, e))
return (False, PoseStamped())
if self.face_side == 'r':
head_registration = self.head_registration_r
else:
head_registration = self.head_registration_l
try:
rospy.loginfo("[%s] Requesting head registration for %s at pixel (%d, %d)." %(rospy.get_name(),
self.subject,
req.u, req.v))
self.head_pc_reg = head_registration(req.u, req.v).reg_pose
if ((self.head_pc_reg.pose.position == Point(0.0, 0.0, 0.0)) and
(self.head_pc_reg.pose.orientation == Quaternion(0.0, 0.0, 0.0, 1.0))):
raise rospy.ServiceException("Unable to find a good match.")
self.head_pc_reg = None
except rospy.ServiceException as se:
self.publish_feedback("Registration failed: %s" %se)
return (False, PoseStamped())
pc_reg_mat = PoseConv.to_homo_mat(self.head_pc_reg)
head_tf_mat = PoseConv.to_homo_mat(Transform(head_tf.transform.translation,
head_tf.transform.rotation))
self.head_pose = PoseConv.to_pose_stamped_msg(pc_reg_mat**-1 * head_tf_mat)
self.head_pose.header.frame_id = self.head_pc_reg.header.frame_id
self.head_pose.header.stamp = self.head_pc_reg.header.stamp
side = "right" if (self.face_side == 'r') else "left"
self.publish_feedback("Registered head using %s cheek model, please check and confirm." %side)
# rospy.loginfo('[%s] Head frame registered at:\r\n %s' %(rospy.get_name(), self.head_pose))
self.test_pose.publish(self.head_pose)
return (True, self.head_pose)
def confirm_reg_cb(self, req):
if self.head_pose is None:
raise rospy.ServiceException("Head has not been registered.");
return False
try:
hp = copy.copy(self.head_pose)
now = rospy.Time.now() + rospy.Duration(0.5)
self.tfl.waitForTransform(self.WORLD_FRAME, hp.header.frame_id, now, rospy.Duration(10))
hp.header.stamp = now
hp_world = self.tfl.transformPose(self.WORLD_FRAME, hp)
pos = (hp_world.pose.position.x, hp_world.pose.position.y, hp_world.pose.position.z)
quat = (hp_world.pose.orientation.x, hp_world.pose.orientation.y,
hp_world.pose.orientation.z, hp_world.pose.orientation.w)
self.head_frame_tf = (pos, quat)
self.publish_feedback("Head registration confirmed.");
return True
except Exception as e:
rospy.logerr("[%s] Error: %s" %(rospy.get_name(), e))
raise rospy.ServiceException("Error confirming head registration.")
class FaceADLsManager(object):
def __init__(self):
self.ellipsoid = EllipsoidSpace()
self.controller_switcher = ControllerSwitcher()
self.ee_frame = '/l_gripper_shaver305_frame'
self.head_pose = None
self.tfl = TransformListener()
self.is_forced_retreat = False
self.pose_traj_pub = rospy.Publisher('/haptic_mpc/goal_pose_array', PoseArray)
self.global_input_sub = rospy.Subscriber("/face_adls/global_move", String,
async_call(self.global_input_cb))
self.local_input_sub = rospy.Subscriber("/face_adls/local_move", String,
async_call(self.local_input_cb))
self.clicked_input_sub = rospy.Subscriber("/face_adls/clicked_move", PoseStamped,
async_call(self.global_input_cb))
self.feedback_pub = rospy.Publisher('/face_adls/feedback', String)
self.global_move_poses_pub = rospy.Publisher('/face_adls/global_move_poses',
StringArray, latch=True)
self.controller_enabled_pub = rospy.Publisher('/face_adls/controller_enabled',
Bool, latch=True)
self.enable_controller_srv = rospy.Service("/face_adls/enable_controller",
EnableFaceController, self.enable_controller_cb)
self.request_registration = rospy.ServiceProxy("/request_registration", RequestRegistration)
self.enable_mpc_srv = rospy.ServiceProxy('/haptic_mpc/enable_mpc', EnableHapticMPC)
self.ell_params_pub = rospy.Publisher("/ellipsoid_params", EllipsoidParams, latch=True)
def stop_move_cb(msg):
self.stop_moving()
self.stop_move_sub = rospy.Subscriber("/face_adls/stop_move", Bool, stop_move_cb, queue_size=1)
def stop_moving(self):
"""Send empty PoseArray to skin controller to stop movement"""
for x in range(2):
self.pose_traj_pub.publish(PoseArray()) #Send empty msg to skin controller
cart_traj_msg = [PoseConv.to_pose_msg(self.current_ee_pose(self.ee_frame))]
head = Header()
head.frame_id = '/torso_lift_link'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
rospy.loginfo( "Sent stop moving commands!")
def register_ellipse(self, mode, side):
reg_e_params = EllipsoidParams()
try:
now = rospy.Time.now()
self.tfl.waitForTransform("/base_link", "/head_frame", now, rospy.Duration(10.))
pos, quat = self.tfl.lookupTransform("/head_frame", "/base_frame", now)
self.head_pose = PoseStamped()
self.head_pose.header.stamp = now
self.head_pose.header.frame_id = "/base_link"
self.head_pose.pose.position = Point(*pos)
self.head_pose.pose.orientation = Quaternion(*quat)
except:
rospy.logwarn("[%s] Head registration not loaded yet." %rospy.get_name())
return (False, reg_e_params)
reg_prefix = rospy.get_param("~registration_prefix", "")
registration_files = rospy.get_param("~registration_files", None)
if mode not in registration_files:
rospy.logerr("[%s] Mode not in registration_files parameters" % (rospy.get_name()))
return (False, reg_e_params)
try:
bag_str = reg_prefix + registration_files[mode][side]
rospy.loginfo("[%s] Loading %s" %(rospy.get_name(), bag_str))
bag = rosbag.Bag(bag_str, 'r')
e_params = None
for topic, msg, ts in bag.read_messages():
e_params = msg
assert e_params is not None
bag.close()
except:
rospy.logerr("[%s] Cannot load registration parameters from %s" %(rospy.get_name(), bag_str))
return (False, reg_e_params)
head_reg_mat = PoseConv.to_homo_mat(self.head_pose)
ell_reg = PoseConv.to_homo_mat(Transform(e_params.e_frame.transform.translation,
e_params.e_frame.transform.rotation))
reg_e_params.e_frame = PoseConv.to_tf_stamped_msg(head_reg_mat**-1 * ell_reg)
reg_e_params.e_frame.header.frame_id = self.head_reg_tf.header.frame_id
reg_e_params.e_frame.child_frame_id = '/ellipse_frame'
reg_e_params.height = e_params.height
reg_e_params.E = e_params.E
self.ell_params_pub.publish(reg_e_params)
self.ell_frame = reg_e_params.e_frame
return (True, reg_e_params)
def current_ee_pose(self, link, frame='/torso_lift_link'):
"""Get current end effector pose from tf"""
# print "Getting pose of %s in frame: %s" %(link, frame)
try:
now = rospy.Time.now()
self.tfl.waitForTransform(frame, link, now, rospy.Duration(8.0))
pos, quat = self.tfl.lookupTransform(frame, link, now)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[face_adls_manager] TF Failure getting current end-effector pose: %s" %e)
return None
return pos, quat
def publish_feedback(self, message=None):
if message is not None:
rospy.loginfo("[face_adls_manager] %s" % message)
self.feedback_pub.publish(message)
def enable_controller_cb(self, req):
if req.enable:
face_adls_modes = rospy.get_param('/face_adls_modes', None)
params = face_adls_modes[req.mode]
self.face_side = rospy.get_param('/face_side', 'r') # TODO Make more general
self.trim_retreat = req.mode == "shaving"
self.flip_gripper = self.face_side == 'r' and req.mode == "shaving"
bounds = params['%s_bounds' % self.face_side]
self.ellipsoid.set_bounds(bounds['lat'], bounds['lon'], bounds['height'])#TODO: Change Back
#self.ellipsoid.set_bounds((-np.pi,np.pi), (-np.pi,np.pi), (0,100))
success, e_params = self.register_ellipse(req.mode, self.face_side)
if not success:
self.publish_feedback(Messages.NO_PARAMS_LOADED)
return EnableFaceControllerResponse(False)
reg_pose = PoseConv.to_pose_stamped_msg(e_params.e_frame)
try:
now = rospy.Time.now()
reg_pose.header.stamp = now
self.tfl.waitForTransform(reg_pose.header.frame_id, '/base_link',
now, rospy.Duration(8.0))
ellipse_frame_base = self.tfl.transformPose('/base_link', reg_pose)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[face_adls_manager] TF Failure converting ellipse to base frame: %s" %e)
self.ellipsoid.load_ell_params(ellipse_frame_base, e_params.E,
e_params.is_oblate, e_params.height)
global_poses_dir = rospy.get_param("~global_poses_dir", "")
global_poses_file = params["%s_ell_poses_file" % self.face_side]
global_poses_resolved = roslaunch.substitution_args.resolve_args(
global_poses_dir + "/" + global_poses_file)
self.global_poses = rosparam.load_file(global_poses_resolved)[0][0]
self.global_move_poses_pub.publish(sorted(self.global_poses.keys()))
#Check rotating gripper based on side of body
if self.flip_gripper:
self.gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0)
print "Rotating gripper by PI around x-axis"
else:
self.gripper_rot = trans.quaternion_from_euler(0, 0, 0)
print "NOT Rotating gripper around x-axis"
# check if arm is near head
# cart_pos, cart_quat = self.current_ee_pose(self.ee_frame)
# ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
# equals = ell_pos == self.ellipsoid.enforce_bounds(ell_pos)
# print ell_pos, equals
# if self.ellipsoid._lon_bounds[0] >= 0 and ell_pos[1] >= 0:
# arm_in_bounds = np.all(equals)
# else:
# ell_lon_1 = np.mod(ell_pos[1], 2 * np.pi)
# min_lon = np.mod(self.ellipsoid._lon_bounds[0], 2 * np.pi)
# arm_in_bounds = (equals[0] and
# equals[2] and
# (ell_lon_1 >= min_lon or ell_lon_1 <= self.ellipsoid._lon_bounds[1]))
arm_in_bounds = True
self.setup_force_monitor()
success = True
if not arm_in_bounds:
self.publish_feedback(Messages.ARM_AWAY_FROM_HEAD)
success = False
#Switch back to l_arm_controller if necessary
if self.controller_switcher.carefree_switch('l', '%s_arm_controller', reset=False):
print "Controller switch to l_arm_controller succeeded"
self.publish_feedback(Messages.ENABLE_CONTROLLER)
else:
print "Controller switch to l_arm_controller failed"
success = False
self.controller_enabled_pub.publish(Bool(success))
req = EnableHapticMPCRequest()
req.new_state = 'enabled'
resp = self.enable_mpc_srv.call(req)
else:
self.stop_moving()
self.controller_enabled_pub.publish(Bool(False))
rospy.loginfo(Messages.DISABLE_CONTROLLER)
req = EnableHapticMPCRequest()
req.new_state = 'disabled'
self.enable_mpc_srv.call(req)
success = False
return EnableFaceControllerResponse(success)
def setup_force_monitor(self):
self.force_monitor = ForceCollisionMonitor()
# registering force monitor callbacks
def dangerous_cb(force):
self.stop_moving()
curr_pose = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
ell_pos, _ = self.ellipsoid.pose_to_ellipsoidal(curr_pose)
if ell_pos[2] < SAFETY_RETREAT_HEIGHT * 0.9:
self.publish_feedback(Messages.DANGEROUS_FORCE %force)
self.retreat_move(SAFETY_RETREAT_HEIGHT,
SAFETY_RETREAT_VELOCITY,
forced=True)
self.force_monitor.register_dangerous_cb(dangerous_cb)
def timeout_cb(time):
start_time = rospy.get_time()
ell_pos, _ = self.ellipsoid.pose_to_ellipsoidal(self.current_ee_pose(self.ee_frame, '/ellipse_frame'))
rospy.loginfo("ELL POS TIME: %.3f " % (rospy.get_time() - start_time) + str(ell_pos)
+ " times: %.3f %.3f" % (self.force_monitor.last_activity_time, rospy.get_time()))
if ell_pos[2] < RETREAT_HEIGHT * 0.9 and self.force_monitor.is_inactive():
self.publish_feedback(Messages.TIMEOUT_RETREAT % time)
self.retreat_move(RETREAT_HEIGHT, LOCAL_VELOCITY)
self.force_monitor.register_timeout_cb(timeout_cb)
def contact_cb(force):
self.force_monitor.update_activity()
if not self.is_forced_retreat:
self.stop_moving()
self.publish_feedback(Messages.CONTACT_FORCE % force)
rospy.loginfo("[face_adls_manZger] Arm stopped due to making contact.")
self.force_monitor.register_contact_cb(contact_cb)
self.force_monitor.start_activity()
self.force_monitor.update_activity()
self.is_forced_retreat = False
def retreat_move(self, height, velocity, forced=False):
if forced:
self.is_forced_retreat = True
cart_pos, cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
#print "Retreat EP:", ell_pos
latitude = ell_pos[0]
if self.trim_retreat:
latitude = min(latitude, TRIM_RETREAT_LATITUDE)
#rospy.loginfo("[face_adls_manager] Retreating from current location.")
retreat_pos = [latitude, ell_pos[1], height]
retreat_pos = self.ellipsoid.enforce_bounds(retreat_pos)
retreat_quat = [0,0,0,1]
if forced:
cart_path = self.ellipsoid.ellipsoidal_to_pose((retreat_pos, retreat_quat))
cart_msg = [PoseConv.to_pose_msg(cart_path)]
else:
ell_path = self.ellipsoid.create_ellipsoidal_path(ell_pos,
ell_quat,
retreat_pos,
retreat_quat,
velocity=0.01,
min_jerk=True)
cart_path = [self.ellipsoid.ellipsoidal_to_pose(ell_pose) for ell_pose in ell_path]
cart_msg = [PoseConv.to_pose_msg(pose) for pose in cart_path]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_msg)
self.pose_traj_pub.publish(pose_array)
self.is_forced_retreat = False
def global_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
rospy.loginfo("[face_adls_manager] Performing global move.")
if type(msg) == String:
self.publish_feedback(Messages.GLOBAL_START %msg.data)
goal_ell_pose = self.global_poses[msg.data]
elif type(msg) == PoseStamped:
try:
self.tfl.waitForTransform(msg.header.frame_id, '/ellipse_frame', msg.header.stamp, rospy.Duration(8.0))
goal_cart_pose = self.tfl.transformPose('/ellipse_frame', msg)
goal_cart_pos, goal_cart_quat = PoseConv.to_pos_quat(goal_cart_pose)
flip_quat = trans.quaternion_from_euler(-np.pi/2, np.pi, 0)
goal_cart_quat_flipped = trans.quaternion_multiply(goal_cart_quat, flip_quat)
goal_cart_pose = PoseConv.to_pose_stamped_msg('/ellipse_frame', (goal_cart_pos, goal_cart_quat_flipped))
self.publish_feedback('Moving around ellipse to clicked position).')
goal_ell_pose = self.ellipsoid.pose_to_ellipsoidal(goal_cart_pose)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[face_adls_manager] TF Failure getting clicked position in ellipse_frame:\r\n %s" %e)
return
curr_cart_pos, curr_cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal((curr_cart_pos, curr_cart_quat))
retreat_ell_pos = [curr_ell_pos[0], curr_ell_pos[1], RETREAT_HEIGHT]
retreat_ell_quat = trans.quaternion_multiply(self.gripper_rot, [1.,0.,0.,0.])
approach_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], RETREAT_HEIGHT]
approach_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
final_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], goal_ell_pose[0][2] - HEIGHT_CLOSER_ADJUST]
final_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
cart_ret_pose = self.ellipsoid.ellipsoidal_to_pose((retreat_ell_pos, retreat_ell_quat))
cart_ret_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_ret_pose)
cart_app_pose = self.ellipsoid.ellipsoidal_to_pose((approach_ell_pos, approach_ell_quat))
cart_app_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_app_pose)
cart_goal_pose = self.ellipsoid.ellipsoidal_to_pose((final_ell_pos, final_ell_quat))
cart_goal_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_goal_pose)
retreat_ell_traj = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
retreat_ell_pos, retreat_ell_quat,
velocity=0.01, min_jerk=True)
transition_ell_traj = self.ellipsoid.create_ellipsoidal_path(retreat_ell_pos, retreat_ell_quat,
approach_ell_pos, approach_ell_quat,
velocity=0.01, min_jerk=True)
approach_ell_traj = self.ellipsoid.create_ellipsoidal_path(approach_ell_pos, approach_ell_quat,
final_ell_pos, final_ell_quat,
velocity=0.01, min_jerk=True)
full_ell_traj = retreat_ell_traj + transition_ell_traj + approach_ell_traj
full_cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in full_ell_traj]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in full_cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
ps = PoseStamped()
ps.header = head
ps.pose = cart_traj_msg[0]
self.force_monitor.update_activity()
def local_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
self.stop_moving()
button_press = msg.data
curr_cart_pos, curr_cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal((curr_cart_pos, curr_cart_quat))
if button_press in ell_trans_params:
self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
change_trans_ep = ell_trans_params[button_press]
goal_ell_pos = [curr_ell_pos[0]+change_trans_ep[0],
curr_ell_pos[1]+change_trans_ep[1],
curr_ell_pos[2]+change_trans_ep[2]]
ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
goal_ell_pos, curr_ell_quat,
velocity=ELL_LOCAL_VEL, min_jerk=True)
elif button_press in ell_rot_params:
self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
change_rot_ep = ell_rot_params[button_press]
rot_quat = trans.quaternion_from_euler(*change_rot_ep)
goal_ell_quat = trans.quaternion_multiply(curr_ell_quat, rot_quat)
ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
curr_ell_pos, goal_ell_quat,
velocity=ELL_ROT_VEL, min_jerk=True)
elif button_press == "reset_rotation":
self.publish_feedback(Messages.ROT_RESET_START)
ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
curr_ell_pos,(0.,0.,0.,1.),
velocity=ELL_ROT_VEL, min_jerk=True)
else:
rospy.logerr("[face_adls_manager] Unknown local ellipsoidal command")
cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in ell_path]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
self.force_monitor.update_activity()
class RobotHapticStateServer():
## Constructor for robot haptic state server
def __init__(self, opt, node_name=None):
self.opt = opt
# Set up all ros comms to start with
self.node_name = node_name
self.tf_listener = None
self.state_pub = None
self.rate = 100.0 # 100 Hz.
self.msg_seq = 0 # Sequence counter
# ROS Param server paths.
self.base_path = "haptic_mpc"
# Skin data
self.skin_topic_list = [] # List of topic names
self.skin_client = None
# Robot object. Contains all the subscribers and robot specific kinematics, etc
self.robot = None
# Joint data
self.joint_names = []
self.joint_angles = []
self.desired_joint_angles = []
self.joint_velocities = []
self.joint_stiffness = []
self.joint_damping = []
self.joint_data_lock = threading.RLock()
self.joint_names = []
# End effector pose
self.end_effector_position = None
self.end_effector_orient_cart = None
self.end_effector_orient_quat = None
self.torso_pose = geom_msgs.Pose()
# Jacobian storage
self.Jc = None # Contact jacobians
self.Je = None # End effector jacobian
self.trim_threshold = 1.0 #this is 1.0 for forces
# Jacobian MultiArray to Matrix converter
self.ma_to_m = multiarray_to_matrix.MultiArrayConverter()
# Initialise various parameters.
self.initComms()
self.initRobot(self.opt.robot)
## Initialise all robot specific parameters (skin topics, robot interfaces, etc). Calls the appropriate init function.
def initRobot(self, robot_type="pr2"):
if robot_type == "pr2":
self.initPR2()
elif robot_type == "cody":
self.initCody()
elif robot_type == "cody5dof":
self.initCody(5)
elif robot_type == "sim3" or robot_type == "sim3_nolim" or robot_type == "sim_equal_links_1":
self.initSim(robot_type)
elif robot_type == "crona":
self.initCrona()
elif robot_type == "darpa":
self.initDarpa()
elif robot_type == "kreacher":
self.initKreacher()
elif robot_type == "darci":
self.initDarci()
elif robot_type == "darci_sim":
self.initDarciSim()
else:
rospy.logerr("RobotHapticState: Invalid robot type specified")
sys.exit()
## Initialise parameters for the state publisher when used on single link Darpa arm
def initDarpa(self):
# Robot kinematic classes and skin clients. These are specific to each robot
import hrl_darpa_arm.darpa_arm_robot_client
#import pr2_arm_darpa_m3_deprecated as pr2_arm # DEPRECATED
# Load parameters from ROS Param server
self.robot_path = "/darpa"
self.skin_topic_list = rospy.get_param(self.base_path +
self.robot_path +
'/skin_list/' + self.opt.sensor)
self.torso_frame = rospy.get_param(self.base_path +
self.robot_path +
'/torso_frame' )
self.inertial_frame = rospy.get_param(self.base_path +
self.robot_path +
'/inertial_frame')
rospy.loginfo("RobotHapticState: Initialising Darpa haptic state publisher" +
"with the following skin topics: \n%s"
%str(self.skin_topic_list))
self.skin_client = sc.TaxelArrayClient(self.skin_topic_list,
self.torso_frame,
self.tf_listener)
self.skin_client.setTrimThreshold(self.trim_threshold)
rospy.loginfo("RobotHapticState: Initialising robot interface")
# Create the robot object. Provides interfaces to the robot arm and various kinematic functions.
self.robot = hrl_darpa_arm.darpa_arm_robot_client.SingleLinkRobotClient()
self.joint_limits_max = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' +
'/max')
self.joint_limits_min = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' +
'/min')
# Push the arm specific param to the location the controller looks.
self.setControllerJointLimits(self.joint_limits_max, self.joint_limits_min)
## Initialise parameters for the state publisher when used on kreacher
def initKreacher(self):
# Robot kinematic classes and skin clients. These are specific to each robot
import hrl_darpa_arm.darpa_arm_robot_client
#import pr2_arm_darpa_m3_deprecated as pr2_arm # DEPRECATED
# Load parameters from ROS Param server
self.robot_path = "/kreacher"
self.skin_topic_list = rospy.get_param(self.base_path +
self.robot_path +
'/skin_list/' + self.opt.sensor)
self.torso_frame = rospy.get_param(self.base_path +
self.robot_path +
'/torso_frame' )
self.inertial_frame = rospy.get_param(self.base_path +
self.robot_path +
'/inertial_frame')
rospy.loginfo("RobotHapticState: Initialising Darpa haptic state publisher" +
"with the following skin topics: \n%s"
%str(self.skin_topic_list))
self.skin_client = sc.TaxelArrayClient(self.skin_topic_list,
self.torso_frame,
self.tf_listener)
self.skin_client.setTrimThreshold(self.trim_threshold)
rospy.loginfo("RobotHapticState: Initialising robot interface")
# Create the robot object. Provides interfaces to the robot arm and various kinematic functions.
self.robot = hrl_darpa_arm.darpa_arm_robot_client.KreacherRobotClient()
self.joint_limits_max = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' +
'/max')
self.joint_limits_min = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' +
'/min')
# Push the arm specific param to the location the controller looks.
self.setControllerJointLimits(self.joint_limits_max, self.joint_limits_min)
def initDarci(self):
print "DARCI actual robot not implemented in Quasi-Static MPC"
sys.exit(1)
def initDarciSim(self):
# Robot kinematic classes and skin clients. These are specific to each robot
import urdf_arm_darpa_m3 as urdf_arm
# Load parameters from ROS Param server
self.robot_path = "/darci_sim"
self.skin_topic_list = rospy.get_param(self.base_path +
self.robot_path +
'/skin_list/' + self.opt.sensor)
self.torso_frame = rospy.get_param(self.base_path +
self.robot_path +
'/torso_frame' )
self.inertial_frame = rospy.get_param(self.base_path +
self.robot_path +
'/inertial_frame')
self.end_effector_frame = rospy.get_param(self.base_path +
self.robot_path +
'/end_effector_frame')
rospy.loginfo("RobotHapticState: Initialising PR2 haptic state publisher" +
"with the following skin topics: \n%s"
%str(self.skin_topic_list))
self.skin_client = sc.TaxelArrayClient(self.skin_topic_list,
self.torso_frame,
self.tf_listener)
self.skin_client.setTrimThreshold(self.trim_threshold)
rospy.loginfo("RobotHapticState: Initialising robot interface")
if not self.opt.arm:
rospy.logerr("RobotHapticState: No arm specified for Darci Sim")
sys.exit()
# Create the robot object. Provides interfaces to the robot arm and various kinematic functions.
self.robot = urdf_arm.URDFArm(self.opt.arm,
self.tf_listener,
self.torso_frame,
self.end_effector_frame)
# Push joint angles to the param server.
if self.opt.arm in ['l', 'r']:
arm_path = '/left'
if self.opt.arm == 'r':
arm_path = '/right'
self.joint_limits_max = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' +
arm_path + '/max')
self.joint_limits_min = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' +
arm_path + '/min')
# Push the arm specific param to the location the controller looks.
self.setControllerJointLimits(self.joint_limits_max, self.joint_limits_min)
## Initialise parameters for the state publisher when used on the PR2.
def initPR2(self):
# Robot kinematic classes and skin clients. These are specific to each robot
import urdf_arm_darpa_m3 as urdf_arm
# Load parameters from ROS Param server
self.robot_path = "/pr2"
self.skin_topic_list = rospy.get_param(self.base_path +
self.robot_path +
'/skin_list/' + self.opt.sensor)
self.torso_frame = rospy.get_param(self.base_path +
self.robot_path +
'/torso_frame' )
self.inertial_frame = rospy.get_param(self.base_path +
self.robot_path +
'/inertial_frame')
self.end_effector_frame = rospy.get_param(self.base_path +
self.robot_path +
'/end_effector_frame')
rospy.loginfo("RobotHapticState: Initialising PR2 haptic state publisher" +
"with the following skin topics: \n%s"
%str(self.skin_topic_list))
self.skin_client = sc.TaxelArrayClient(self.skin_topic_list,
self.torso_frame,
self.tf_listener)
self.skin_client.setTrimThreshold(self.trim_threshold)
rospy.loginfo("RobotHapticState: Initialising robot interface")
if not self.opt.arm:
rospy.logerr("RobotHapticState: No arm specified for PR2")
sys.exit()
# Create the robot object. Provides interfaces to the robot arm and various kinematic functions.
self.robot = urdf_arm.URDFArm(self.opt.arm,
self.tf_listener,
self.torso_frame,
self.end_effector_frame)
# Push joint angles to the param server.
if self.opt.arm in ['l', 'r']:
arm_path = '/left'
if self.opt.arm == 'r':
arm_path = '/right'
self.joint_limits_max = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' +
arm_path + '/max')
self.joint_limits_min = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' +
arm_path + '/min')
# Push the arm specific param to the location the controller looks.
self.setControllerJointLimits(self.joint_limits_max, self.joint_limits_min)
## Initialise parameters for the state publisher when used on Cody.
def initCody(self, num_of_joints=7):
import hrl_cody_arms.cody_arm_client
# Load the skin list from the param server
self.robot_path = '/cody'
self.skin_topic_list = rospy.get_param(self.base_path +
self.robot_path +
'/skin_list/' + self.opt.sensor)
self.torso_frame = rospy.get_param(self.base_path +
self.robot_path +
'/torso_frame' )
self.inertial_frame = rospy.get_param(self.base_path +
self.robot_path +
'/inertial_frame')
rospy.loginfo("RobotHapticState: Initialising Cody haptic state publisher" +
" with the following skin topics: \n%s"
%str(self.skin_topic_list))
self.skin_client = sc.TaxelArrayClient(self.skin_topic_list,
self.torso_frame,
self.tf_listener)
self.skin_client.setTrimThreshold(self.trim_threshold)
rospy.loginfo("RobotHapticState: Initialising robot interface")
if not self.opt.arm:
rospy.logerr("RobotHapticState: No arm specified for Cody")
sys.exit()
if num_of_joints == 7:
self.robot = hrl_cody_arms.cody_arm_client.CodyArmClient_7DOF(self.opt.arm)
elif num_of_joints == 5:
self.robot = hrl_cody_arms.cody_arm_client.CodyArmClient_5DOF(self.opt.arm)
# Push joint angles to the param server.
if self.opt.arm in ['l', 'r']:
arm_path = '/left'
if self.opt.arm == 'r':
arm_path = '/right'
self.joint_limits_max = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' +
arm_path + '/max')
self.joint_limits_min = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' +
arm_path + '/min')
# Push the arm specific param to the location the controller looks.
self.setControllerJointLimits(self.joint_limits_max, self.joint_limits_min)
## Initialise parameters for the state publisher when used in simulation
#with the 3DOF arm.
def initSim(self, robot_type='sim3'):
import gen_sim_arms as sim_robot
# Load the skin list from the param server
if robot_type == 'sim3':
import hrl_common_code_darpa_m3.robot_config.three_link_planar_capsule as sim_robot_config
self.robot_path = '/sim3'
elif robot_type == 'sim3_nolim':
import hrl_common_code_darpa_m3.robot_config.three_link_planar_capsule_nolim as sim_robot_config
self.robot_path = '/sim3'
elif robot_type == 'sim_equal_links_1':
import hrl_common_code_darpa_m3.robot_config.multi_link_one_planar as sim_robot_config
self.robot_path = '/sim_equal_links_1'
self.skin_topic_list = rospy.get_param(self.base_path +
self.robot_path +
'/skin_list/' + self.opt.sensor)
self.torso_frame = rospy.get_param(self.base_path +
self.robot_path +
'/torso_frame')
self.inertial_frame = rospy.get_param(self.base_path +
self.robot_path +
'/inertial_frame')
rospy.loginfo("RobotHapticState: Initialising Sim haptic state publisher" +
"with the following skin topics: \n%s"
%str(self.skin_topic_list))
self.skin_client = sc.TaxelArrayClient(self.skin_topic_list,
self.torso_frame,
self.tf_listener)
self.skin_client.setTrimThreshold(self.trim_threshold)
# TODO: Add config switching here.
rospy.loginfo("RobotHapticState: Initialising Sim robot interface")
sim_config = sim_robot_config
self.robot = sim_robot.ODESimArm(sim_config)
self.joint_limits_max = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits/max')
self.joint_limits_min = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits/min')
# Push the arm specific param to the location the controller looks.
self.setControllerJointLimits(self.joint_limits_max, self.joint_limits_min)
#Initialise parameters for the state publisher when used in simulation
#with the 7DOF cody arm.
def initSimCody(self):
import cody_arm_darpa_m3 as cody_arm
# Load the skin list from the param server
self.robot_path = '/simcody'
self.skin_topic_list = rospy.get_param(self.base_path +
self.robot_path +
'/skin_list')
self.torso_frame = rospy.get_param(self.base_path +
self.robot_path +
'/torso_frame' )
self.inertial_frame = rospy.get_param(self.base_path +
self.robot_path +
'/inertial_frame')
rospy.loginfo("RobotHapticState: Initialising Sim haptic state publisher" +
"with the following skin topics: \n%s"
%str(self.skin_topic_list))
self.skin_client = sc.TaxelArrayClient(self.skin_topic_list,
self.torso_frame,
self.tf_listener)
self.skin_client.setTrimThreshold(self.trim_threshold)
# TODO: Add config switching here.
rospy.loginfo("RobotHapticState: Initialising robot interface")
if not self.opt.arm:
rospy.logerr("RobotHapticState: No arm specified for Sim Cody")
sys.exit()
self.robot = cody_arm.CodyArmClient(self.opt.arm)
def initCrona(self):
import urdf_arm_darpa_m3 as urdf_arm
self.robot_path = "/crona"
#self.skin_topic_list = rospy.get_param(self.base_path +
# self.robot_path +
# '/skin_list/' + self.opt.sensor)
self.skin_topic_list = None
self.torso_frame = rospy.get_param(self.base_path +
self.robot_path +
'/torso_frame' )
self.end_effector_frame = rospy.get_param(self.base_path +
self.robot_path +
'/end_effector_frame' )
self.inertial_frame = rospy.get_param(self.base_path +
self.robot_path +
'/inertial_frame')
self.skin_client = sc.TaxelArrayClient([],
self.torso_frame,
self.tf_listener)
rospy.loginfo("RobotHapticState: Initialising CRONA haptic state publisher with the following skin topics: \n%s"
%str(self.skin_topic_list))
rospy.loginfo("RobotHapticState: Initialising robot interface")
if not self.opt.arm:
rospy.logerr("RobotHapticState: No arm specified for CRONA")
sys.exit()
self.robot = urdf_arm.URDFArm(self.opt.arm,
self.tf_listener,
base_link=self.torso_frame,
end_link=self.end_effector_frame)
self.skins = []
self.Jc = []
# Push joint angles to the param server.
if self.opt.arm in ['l', 'r']:
arm_path = '/left'
if self.opt.arm == 'r':
arm_path = '/right'
self.joint_limits_max = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' +
arm_path + '/max')
self.joint_limits_min = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' +
arm_path + '/min')
# Push the arm specific param to the location the controller looks.
self.setControllerJointLimits(self.joint_limits_max, self.joint_limits_min)
# Initialise publishers for the robot haptic state,
# the current gripper pose, and a TF listener.
# NB: The skin client and robot clients will have their own
# publishers/subscribers specific to them.
def initComms(self):
if self.node_name != None:
rospy.init_node(self.node_name)
self.tf_listener = TransformListener()
self.state_pub = rospy.Publisher('haptic_mpc/robot_state',
haptic_msgs.RobotHapticState)
self.gripper_pose_pub = rospy.Publisher('haptic_mpc/gripper_pose',
geom_msgs.PoseStamped)
## Pushes the given joint limits to a known location on the param server.
# The control loads these on startup.
def setControllerJointLimits(self, joint_limits_max, joint_limits_min):
# Push the arm specific param to the location the controller looks.
rospy.set_param(self.base_path + '/joint_limits/max', joint_limits_max)
rospy.set_param(self.base_path + '/joint_limits/min', joint_limits_min)
# Returns a header type with the current timestamp.
# Does not set the frame_id
def getMessageHeader(self):
header = Header()
header.stamp = rospy.get_rostime()
return header
# Updates the stored end effector Jacobian from the current joint angles
# and end effector position
def updateEndEffectorJacobian(self):
self.Je = [self.robot.kinematics.jacobian(self.joint_angles, self.end_effector_position)]
#pos = self.robot.kinematics.forward(self.joint_angles,end_link=self.opt.arm+'_forearm_roll_link')
#self.Je = [self.robot.kinematics.jacobian(self.joint_angles, pos[:3,3])]
## Compute contact Jacobians based on the provided taxel array dictionary
# @param skin_data Dictionary containing taxel array messages indexed by topic name
def updateContactJacobians(self, skin_data):
# loc_l = list of taxel locations relative the "torso_lift_link" frame.
# jt_l = list of joints beyond which the jacobian columns are zero.
# loc_l. jt_l from skin client.
Jc_l = []
loc_l, jt_l = self.getTaxelLocationAndJointList(skin_data)
if len(loc_l) != len(jt_l):
rospy.logfatal("Haptic State Publisher: Dimensions don't match. %s, %s" % (len(loc_l), len(jt_l)))
sys.exit()
for jt_li, loc_li in it.izip(jt_l, loc_l):
Jc = self.robot.kinematics.jacobian(self.joint_angles, loc_li)
Jc[:, jt_li+1:] = 0.0
Jc = Jc[0:3, 0:len(self.joint_stiffness)] # trim the jacobian to suit the number of DOFs.
Jc_l.append(Jc)
self.Jc = Jc_l
## Returns a Pose object for the torso pose in the stated inertial frame
def updateTorsoPose(self):
# Get the transformation from the desired frame to current frame
self.tf_listener.waitForTransform(self.inertial_frame, self.torso_frame,
rospy.Time(), rospy.Duration(10.0))
t1, q1 = self.tf_listener.lookupTransform(self.inertial_frame,
self.torso_frame,
rospy.Time(0))
torso_pose = geom_msgs.Pose()
torso_pose.position = geom_msgs.Point(*t1)
torso_pose.orientation = geom_msgs.Quaternion(*q1)
return torso_pose
## Store latest joint states from the specified robot class
# @var joint_names: Joint names
# @var joint_angles: Joint angles
# @var joint_velocities: Joint velocities
# @var joint_stiffness: Joint stiffness
# @var joint_damping: Joint damping
# @var q_des: Desired joint angles
def updateJointStates(self):
self.joint_names = self.robot.get_joint_names()
self.joint_angles = self.robot.get_joint_angles()
self.joint_stiffness, self.joint_damping = self.robot.get_joint_impedance()
self.joint_velocities = self.robot.get_joint_velocities()
q_des = self.robot.get_ep()
if q_des != None:
self.desired_joint_angles = q_des
# Compute and store the end effector position, orientation, and jacobian
# from the current joint angles.
def updateEndEffectorPose(self):
pos, rot = self.robot.kinematics.FK(self.joint_angles)
self.end_effector_position = pos
self.end_effector_orient_cart = rot
self.end_effector_orient_quat = tr.matrix_to_quaternion(rot)
## Returns a list of taxel locations and a list of joint numbers after which the
# joint torque will have no effect on the contact force
# @param skin_data Dictionary of TaxelArrays indexed by topic
# @retval locations List of taxel locations where a force is present
# @retval joint_nums List of joints after which the joint torque will have no effect on the contact force
# @return These arrays will both be the same length (as the joint number corresponds
def getTaxelLocationAndJointList(self, skin_data):
locations = []
joint_nums = []
for ta_msg in skin_data.values():
# Get points list
ta_locs = self.skin_client.getContactLocationsFromTaxelArray(ta_msg)
# Create list of joints beyond which the joint torque has no effect on contact force
ta_jts = []
for contact_index in range(len(ta_msg.centers_x)):
jt_num = len(self.joint_angles)-1 # Index of last joint, 0 indexed.
# Verify we have the same number of link names as taxel contacts If not, make no assumptions about joints.
if len(ta_msg.link_names) >= len(ta_msg.centers_x):
link_name = ta_msg.link_names[contact_index]
# Iterate over the known joint names looking for the link this is associated with.
# NB: links should be named based on their joint.
# NB:
for idx, joint_name in enumerate(self.joint_names):
if joint_name in link_name:
jt_num = idx
break #
ta_jts.append(jt_num)
# Attach these lists to the end of the global list (incorporating multiple taxel arrays)
locations.extend(ta_locs)
joint_nums.extend(ta_jts)
return locations, joint_nums
## Modify taxel data for PR2 specific situations
# TODO: Survy to implement selective taxel ignoring.
# @param skin_data Dictionary containing taxel array messages indexed by topic name
def modifyPR2Taxels(self, skin_data):
#print "modifyPR2Taxels"
return skin_data
## Modifies data from the taxel array based on robot specific configurations.
# An example of this is ignoring the PR2 wrist taxels when the wrist
# is near its joint limit as the wrist itself will trigger the taxel.
# @param skin_data Dict containing taxel array messages indexed by topic name
# @return skin_data Modified dictionary containing taxel array messages
def modifyRobotSpecificTaxels(self, skin_data):
if self.opt.robot == 'pr2':
return self.modifyPR2Taxels(skin_data)
return skin_data # If this is running on a different robot, don't modify the data.
## Calls all the sub-component updates
def updateHapticState(self):
self.updateJointStates()
#self.torso_pose = self.updateTorsoPose()
self.updateEndEffectorPose()
self.updateEndEffectorJacobian()
# Skin sensor calculations.
# Get the latest skin data from the skin client
skin_data = self.skin_client.getTrimmedSkinData()
full_skin_data = self.skin_client.getSkinData()
# Trim skin_data based on specific robot state (eg wrist configuration).
skin_data = self.modifyRobotSpecificTaxels(skin_data)
self.updateContactJacobians(skin_data)
# Add the list of TaxelArray messages to the message
self.skins = skin_data.values()
self.full_skins = full_skin_data.values()
## Build the haptic state message data structure
# @return haptic_state_msg Haptic State message object containing relevant data
def getHapticStateMessage(self):
# Update all haptic state data (jacobians etc)
self.updateHapticState()
msg = haptic_msgs.RobotHapticState()
msg.header = self.getMessageHeader()
msg.header.frame_id = self.torso_frame
# TODO Locking on data? - check these are copies.
# Joint states
# self.updateJointStates()
msg.joint_names = self.joint_names
msg.joint_angles = self.joint_angles
msg.desired_joint_angles = self.desired_joint_angles
msg.joint_velocities = self.joint_velocities
msg.joint_stiffness = self.joint_stiffness
msg.joint_damping = self.joint_damping
msg.torso_pose = self.torso_pose #self.updateTorsoPose()
# End effector calculations
# self.updateEndEffectorPose()
msg.hand_pose.position = geom_msgs.Point(*(self.end_effector_position.A1))
msg.hand_pose.orientation = geom_msgs.Quaternion(*self.end_effector_orient_quat)
# self.updateEndEffectorJacobian()
msg.end_effector_jacobian = self.ma_to_m.matrixListToMultiarray(self.Je)
# # Skin sensor calculations.
# # Get the latest skin data from the skin client
# skin_data = self.skin_client.getTrimmedSkinData()
# # Trim skin_data based on specific robot state (eg wrist configuration).
# skin_data = self.modifyRobotSpecificTaxels(skin_data)
# # Add the list of TaxelArray messages to the message
# msg.skins = skin_data.values()
# self.updateContactJacobians(skin_data)
# Add the list of TaxelArray messages to the message
msg.skins = self.skins
msg.contact_jacobians = self.ma_to_m.matrixListToMultiarray(self.Jc)
return msg
## Build and publish the haptic state message.
def publishRobotState(self):
msg = self.getHapticStateMessage()
# Publish the newly formed state message
for i in range(len(msg.joint_names)):
msg.joint_names[i] = str(msg.joint_names[i]) #testing
self.state_pub.publish(msg)
# Publish gripper pose for debug purposes
ps_msg = geom_msgs.PoseStamped()
ps_msg.header = self.getMessageHeader()
ps_msg.header.frame_id = self.torso_frame
ps_msg.pose.position = geom_msgs.Point(*(self.end_effector_position.A1))
ps_msg.pose.orientation = geom_msgs.Quaternion(*self.end_effector_orient_quat)
self.gripper_pose_pub.publish(ps_msg)
## Handler for Ctrl-C signals. Some of the ROS spin loops don't respond well to
# Ctrl-C without this.
def signal_handler(self, signal, frame):
print 'Ctrl+C pressed - exiting'
sys.exit(0)
## Start the state publisher
def start(self):
signal.signal(signal.SIGINT, self.signal_handler) # Catch Ctrl-Cs
rospy.loginfo("RobotHapticState: Starting Robot Haptic State publisher")
rate = rospy.Rate(self.rate) # 100Hz, nominally.
# Blocking sleep to prevent the node publishing until joint states
# are read by the robot client.
rospy.loginfo("RobotHapticState: Waiting for robot state")
joint_stiffness, joint_damping = self.robot.get_joint_impedance()
while (self.robot.get_joint_angles() == None or
self.robot.get_joint_velocities() == None or
joint_stiffness == None):
joint_stiffness, joint_damping = self.robot.get_joint_impedance()
rate.sleep()
rospy.loginfo("RobotHapticState: Got robot state")
while self.robot.get_ep() == None:
rospy.sleep(0.001)
rospy.loginfo("RobotHapticState: Setting desired joint angles to current joint_angles")
self.robot.set_ep(self.robot.get_joint_angles())
rospy.loginfo("RobotHapticState: Starting publishing")
while not rospy.is_shutdown():
self.publishRobotState()
# rospy.spin() # Blocking spin for debug/dev purposes
rate.sleep()
class DepthImageCreator(object):
def __init__(self, use_depth_only):
self.use_depth_only = use_depth_only
self.depth_image_lock = Lock()
self.image_list_lock = Lock()
self.image_list = []
self.image_list_max_size = 100
self.downsample_factor = 2
self.tf = TransformListener()
rospy.Subscriber("/color_camera/camera_info",
CameraInfo,
self.process_camera_info,
queue_size=10)
rospy.Subscriber("/point_cloud",
PointCloud,
self.process_point_cloud,
queue_size=10)
rospy.Subscriber("/color_camera/image_raw/compressed",
CompressedImage,
self.process_image,
queue_size=10)
self.clicked_point_pub = rospy.Publisher("/clicked_point",PointStamped,queue_size=10)
self.camera_info = None
self.P = None
self.depth_image = None
self.image = None
self.last_image_timestamp = None
self.click_timestamp = None
self.depth_timestamp = None
cv2.namedWindow("depth_feed")
cv2.namedWindow("image_feed")
cv2.namedWindow("combined_feed")
cv2.setMouseCallback('image_feed',self.handle_click)
cv2.setMouseCallback('combined_feed',self.handle_combined_click)
def handle_click(self,event,x,y,flags,param):
if event == cv2.EVENT_LBUTTONDOWN:
self.click_timestamp = self.last_image_timestamp
self.click_coords = (x*self.downsample_factor,y*self.downsample_factor)
def process_image(self,msg):
self.image_list_lock.acquire()
np_arr = np.fromstring(msg.data, np.uint8)
self.last_image_timestamp = msg.header.stamp
self.image = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
if len(self.image_list) == self.image_list_max_size:
self.image_list.pop(0)
self.image_list.append((msg.header.stamp,self.image))
self.image_list_lock.release()
def process_camera_info(self, msg):
self.camera_info = msg
self.P = np.array(msg.P).reshape((3,4))
self.K = np.array(msg.K).reshape((3,3))
# TODO: this is necessary due to a mistake in intrinsics_server.py
self.D = np.array([msg.D[0],msg.D[1],0,0,msg.D[2]])
def get_nearest_image_temporally(self,timestamp):
self.image_list_lock.acquire()
diff_list = []
for im_stamp,image in self.image_list:
diff_list.append((abs((im_stamp-timestamp).to_sec()),image))
closest_temporally = min(diff_list,key=lambda t: t[0])
print closest_temporally[0]
self.image_list_lock.release()
return closest_temporally[1]
def handle_combined_click(self,event,x,y,flags,param):
if event == cv2.EVENT_LBUTTONDOWN:
try:
self.depth_image_lock.acquire()
click_coords = (x*self.downsample_factor,y*self.downsample_factor)
distances = []
for i in range(self.projected_points.shape[0]):
dist = (self.projected_points[i,0,0] - click_coords[0])**2 + (self.projected_points[i,0,1] - click_coords[1])**2
distances.append(dist)
three_d_coord = self.points_3d[:,np.argmin(distances)]
# again, we have to reshuffle the coordinates due to differences in ROS Tango coordinate systems
point_msg = PointStamped(header=Header(stamp=self.depth_image_timestamp,
frame_id="depth_camera"),
point=Point(y=three_d_coord[0],
z=three_d_coord[1],
x=three_d_coord[2]))
self.tf.waitForTransform("depth_camera",
"odom",
self.depth_image_timestamp,
rospy.Duration(1.0))
transformed_coord = self.tf.transformPoint('odom', point_msg)
self.clicked_point_pub.publish(transformed_coord)
self.depth_image_lock.release()
except Exception as ex:
print "Encountered an errror! ", ex
self.depth_image_lock.release()
def process_point_cloud(self, msg):
self.depth_image_lock.acquire()
try:
if self.P == None:
self.depth_image_lock.release()
return
self.depth_image_timestamp = msg.header.stamp
self.depth_image = np.zeros((self.camera_info.height, self.camera_info.width, 3),dtype=np.uint8)
self.points_3d = np.zeros((3,len(msg.points))).astype(np.float32)
depths = []
for i,p in enumerate(msg.points):
# this is weird due to mismatches between Tango coordinate system and ROS
self.points_3d[:,i] = np.array([p.y, p.z, p.x])
depths.append(p.x)
self.projected_points, dc = cv2.projectPoints(self.points_3d.T,
(0,0,0),
(0,0,0),
self.K,
self.D)
if self.click_timestamp != None and msg.header.stamp > self.click_timestamp:
distances = []
for i in range(self.projected_points.shape[0]):
dist = (self.projected_points[i,0,0] - self.click_coords[0])**2 + (self.projected_points[i,0,1] - self.click_coords[1])**2
distances.append(dist)
three_d_coord = self.points_3d[:,np.argmin(distances)]
# again, we have to reshuffle the coordinates due to differences in ROS Tango coordinate systems
point_msg = PointStamped(header=Header(stamp=msg.header.stamp,
frame_id="depth_camera"),
point=Point(y=three_d_coord[0],
z=three_d_coord[1],
x=three_d_coord[2]))
transformed_coord = self.tf.transformPoint('odom', point_msg)
self.clicked_point_pub.publish(transformed_coord)
self.click_timestamp = None
# do equalization
depths_equalized = np.zeros((len(depths),1))
for idx,(i,depth) in enumerate(sorted(enumerate(depths), key=lambda x: -x[1])):
depths_equalized[i] = idx/float(len(depths)-1)
for i in range(self.projected_points.shape[0]):
if not(np.isnan(self.projected_points[i,0,0])) and not(np.isnan(self.projected_points[i,0,1])):
self.depth_image[int(self.projected_points[i,0,1]),int(self.projected_points[i,0,0]),:] = int(depths_equalized[i]*255.0)
self.depth_image_lock.release()
except Exception as ex:
print "Encountered an errror! ", ex
self.depth_image_lock.release()
def run(self):
r = rospy.Rate(10)
while not(rospy.is_shutdown()):
cv2.waitKey(5)
if self.depth_image != None:
# dilate the depth image for display since it is so sparse
if not(self.depth_image_lock.locked()):
kernel = np.ones((5,5),'uint8')
self.depth_image_lock.acquire()
cv2.imshow("depth_feed", cv2.resize(cv2.dilate(self.depth_image, kernel),(self.depth_image.shape[1]/self.downsample_factor,
self.depth_image.shape[0]/self.downsample_factor)))
self.depth_image_lock.release()
if self.image != None:
cv2.imshow("image_feed", cv2.resize(self.image,(self.image.shape[1]/self.downsample_factor,
self.image.shape[0]/self.downsample_factor)))
if not(self.use_depth_only) and self.image != None and self.depth_image != None:
kernel = np.ones((3,3),'uint8')
self.depth_image_lock.acquire()
nearest_image = self.get_nearest_image_temporally(self.depth_image_timestamp)
ret, depth_threshed = cv2.threshold(self.depth_image,1,255,cv2.THRESH_BINARY)
combined_img = (cv2.dilate(depth_threshed,kernel)*0.2 + nearest_image*0.8).astype(dtype=np.uint8)
cv2.imshow("combined_feed", cv2.resize(combined_img,(self.image.shape[1]/self.downsample_factor,
self.image.shape[0]/self.downsample_factor)))
self.depth_image_lock.release()
if self.depth_image != None and self.use_depth_only:
kernel = np.ones((5,5),'uint8')
self.depth_image_lock.acquire()
ret, depth_threshed = cv2.threshold(self.depth_image,1,255,cv2.THRESH_BINARY)
combined_img = (cv2.dilate(depth_threshed,kernel)).astype(dtype=np.uint8)
cv2.imshow("combined_feed", cv2.resize(combined_img,(combined_img.shape[1]/self.downsample_factor,
combined_img.shape[0]/self.downsample_factor)))
self.depth_image_lock.release()
r.sleep()
class ArmActions():
def __init__(self):
rospy.init_node('left_arm_actions')
self.tfl = TransformListener()
self.aul = l_arm_utils.ArmUtils(self.tfl)
#self.fth = ft_handler.FTHandler()
rospy.loginfo("Waiting for l_utility_frame_service")
try:
rospy.wait_for_service('/l_utility_frame_update', 7.0)
self.aul.update_frame = rospy.ServiceProxy('/l_utility_frame_update', FrameUpdate)
rospy.loginfo("Found l_utility_frame_service")
except:
rospy.logwarn("Left Utility Frame Service Not available")
rospy.loginfo('Waiting for Pixel_2_3d Service')
try:
rospy.wait_for_service('/pixel_2_3d', 7.0)
self.p23d_proxy = rospy.ServiceProxy('/pixel_2_3d', Pixel23d, True)
rospy.loginfo("Found pixel_2_3d service")
except:
rospy.logwarn("Pixel_2_3d Service Not available")
#Low-level motion requests: pass directly to arm_utils
rospy.Subscriber('wt_left_arm_pose_commands', Point, self.torso_frame_move)
rospy.Subscriber('wt_left_arm_angle_commands', JointTrajectoryPoint, self.aul.send_traj_point)
#More complex motion scripts, defined here using arm_util functions
rospy.Subscriber('norm_approach_left', PoseStamped, self.norm_approach)
rospy.Subscriber('wt_grasp_left_goal', PoseStamped, self.grasp)
#rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.wipe)
rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.force_wipe_agg)
rospy.Subscriber('wt_swipe_left_goals', PoseStamped, self.swipe)
rospy.Subscriber('wt_lin_move_left', Float32, self.hand_move)
rospy.Subscriber('wt_adjust_elbow_left', Float32, self.aul.adjust_elbow)
rospy.Subscriber('wt_surf_wipe_left_points', Point, self.prep_surf_wipe)
rospy.Subscriber('wt_poke_left_point', PoseStamped, self.poke)
rospy.Subscriber('wt_contact_approach_left', PoseStamped, self.approach_to_contact)
self.wt_log_out = rospy.Publisher('wt_log_out', String)
self.test_pose = rospy.Publisher('test_pose', PoseStamped, latch=True)
self.say = rospy.Publisher('text_to_say', String)
self.wipe_started = False
self.surf_wipe_started = False
self.wipe_ends = [PoseStamped(), PoseStamped()]
#FORCE_TORQUE ADDITIONS
rospy.Subscriber('netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.ft_preprocess)
self.rezero_wrench = rospy.Publisher('netft_gravity_zeroing/rezero_wrench', Bool)
#rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.ft_preprocess)
rospy.Subscriber('wt_ft_goal', Float32, self.set_force_goal)
self.wt_force_out = rospy.Publisher('wt_force_out', Float32)
self.ft_rate_limit = rospy.Rate(30)
self.ft = WrenchStamped()
self.ft_mag = 0.
self.ft_case = None
self.force_goal_mean = 1.42
self.force_goal_std= 0.625
self.stop_maintain = False
self.force_wipe_started = False
self.force_wipe_start = PoseStamped()
self.force_wipe_appr = PoseStamped()
def set_force_goal(self, msg):
self.force_goal_mean = msg.data
def ft_preprocess(self, ft):
self.ft = ft
self.ft_mag = math.sqrt(ft.wrench.force.x**2 + ft.wrench.force.y**2 + ft.wrench.force.z**2)
#print 'Force Magnitude: ', self.ft_mag
self.wt_force_out.publish(self.ft_mag)
def approach_to_contact(self, ps, overshoot=0.05):
self.stop_maintain = True
self.aul.update_frame(ps)
appr = self.aul.find_approach(ps, 0.15)
goal = self.aul.find_approach(ps, -overshoot)
(appr_reachable, ik_goal) = self.aul.full_ik_check(appr)
(goal_reachable, ik_goal) = self.aul.full_ik_check(goal)
if appr_reachable and goal_reachable:
traj = self.aul.build_trajectory(goal,appr,tot_points=600)
prep = self.aul.move_arm_to(appr)
if prep:
curr_traj_point = self.advance_to_contact(traj)
self.maintain_norm_force(traj, curr_traj_point)
else:
rospy.loginfo("Cannot reach desired 'move-to-contact' point")
self.wt_log_out.publish(data="Cannot reach desired 'move-to-contact' point")
def advance_to_contact(self, traj):
completed = False
curr_traj_point = 0
advance_rate = rospy.Rate(30)
while not (rospy.is_shutdown() or completed):
if not (curr_traj_point >= len(traj.points)):
self.aul.send_traj_point(traj.points[curr_traj_point], 0.05)
#hfm_pose = self.aul.hand_frame_move(0.003)
#self.aul.blind_move(hfm_pose,0.9)
advance_rate.sleep()
curr_traj_point += 1
else:
rospy.loginfo("Moved past expected contact, but no contact found!")
self.wt_log_out.publish(data="Moved past expected contact, but no contact found!")
completed = True
if self.ft.wrench.force.z < -1.5:
completed = True
return curr_traj_point
def maintain_norm_force(self, traj, curr_traj_point=0, mean=0, std=1):
self.stop_maintain = False
maintain_rate = rospy.Rate(100)
while not (rospy.is_shutdown() or self.stop_maintain):
if self.ft.wrench.force.z > mean + std:
curr_traj_point += 1
if not (curr_traj_point >= len(traj.points)):
self.aul.send_traj_point(traj.points[curr_traj_point], 0.033)
rospy.sleep(0.033)
else:
rospy.loginfo("Force too low, but extent of the trajectory is reached")
self.wt_log_out.publish(data="Force too low, but extent of the trajectory is reached")
stopped = True
elif self.ft.wrench.force.z < mean - std:
curr_traj_point -= 1
if curr_traj_point >= 0:
self.aul.send_traj_point(traj.points[curr_traj_point], 0.033)
rospy.sleep(0.033)
else:
rospy.loginfo("Beginning of trajectory reached, cannot back away further")
self.wt_log_out.publish(data="Beginning of trajectory reached, cannot back away further")
stopped = True
maintain_rate.sleep()
mean = -self.force_goal_mean
std = self.force_goal_std
# def maintain_net_force(self, mean=0, std=3):
# self.stop_maintain = False
# maintain_rate = rospy.Rate(100)
# while not (rospy.is_shutdown() or self.stop_maintain):
# if self.ft_mag > mean + 8:
# curr_angs = self.aul.joint_state_act.positions
# try:
# x_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0.02))).solution.joint_state.position, curr_angs)
# x_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(-0.02))).solution.joint_state.position, curr_angs)
# y_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0.02, 0))).solution.joint_state.position, curr_angs)
# y_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, -0.02, 0))).solution.joint_state.position, curr_angs)
# z_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0, 0.02))).solution.joint_state.position, curr_angs)
# z_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0, -0.02))).solution.joint_state.position, curr_angs)
# #print 'x: ', x_plus,'\r\n', x_minus
# #print 'y: ', y_plus,'\r\n', y_minus
# #print 'z: ', z_plus,'\r\n', z_minus
# ft_sum = self.ft_mag
# parts = np.divide([self.ft.wrench.force.x, self.ft.wrench.force.y, self.ft.wrench.force.z], ft_sum)
# print 'parts', parts
# ends = [[x_plus,x_minus],[y_plus, y_minus],[z_plus,z_minus]]
# side = [[0]*7]*3
# for i, part in enumerate(parts):
# if part >=0:
# side[i] = ends[i][0]
# else:
# side[i] = ends[i][1]
#
# ang_out = curr_angs
# for i in range(3):
# ang_out -= np.average(side, 0, parts)
# except:
# print 'Near Joint Limits!'
# self.aul.send_joint_angles(ang_out)
#
# #print 'x: ', x_plus, x_minus
# maintain_rate.sleep()
# mean = self.force_goal_mean
# std = self.force_goal_std
def maintain_net_force(self, mean=0, std=8):
self.stop_maintain = False
maintain_rate = rospy.Rate(100)
while not (rospy.is_shutdown() or self.stop_maintain):
if self.ft_mag > mean + 3:
#print 'Moving to avoid force'
goal = PoseStamped()
goal.header.frame_id = 'l_netft_frame'
goal.header.stamp = rospy.Time(0)
goal.pose.position.x = -np.clip(self.ft.wrench.force.x/2500, -0.1, 0.1)
goal.pose.position.y = -np.clip(self.ft.wrench.force.y/2500, -0.1, 0.1)
goal.pose.position.z = -np.clip(self.ft.wrench.force.z/2500, -0.1, 0.1)+0.052
goal = self.tfl.transformPose('/torso_lift_link', goal)
goal.header.stamp = rospy.Time.now()
goal.pose.orientation = self.aul.curr_pose().pose.orientation
self.test_pose.publish(goal)
self.aul.fast_move(goal, 0.2)
rospy.sleep(0.05)
maintain_rate.sleep()
mean = self.force_goal_mean
std = self.force_goal_std
#def mannequin(self):
#mannequin_rate=rospy.Rate(10)
#while not rospy.is_shutdown():
#joints = np.add(np.array(self.aul.joint_state_act.positions),
# np.clip(np.array(self.aul.joint_state_err.positions), -0.05, 0.05))
#joints = self.aul.joint_state_act.positions
#print joints
#raw_input('Review Joint Angles')
#self.aul.send_joint_angles(joints,0.1)
#mannequin_rate.sleep()
def force_wipe_agg(self, ps):
self.aul.update_frame(ps)
rospy.sleep(0.1)
pose = self.aul.find_approach(ps, 0)
(goal_reachable, ik_goal) = self.aul.ik_check(pose)
if goal_reachable:
if not self.force_wipe_started:
appr = self.aul.find_approach(ps, 0.20)
(appr_reachable, ik_goal) = self.aul.ik_check(appr)
self.test_pose.publish(appr)
if appr_reachable:
self.force_wipe_start = pose
self.force_wipe_appr = appr
self.force_wipe_started = True
else:
rospy.loginfo("Cannot reach approach point, please choose another")
self.wt_log_out.publish(data="Cannot reach approach point, please choose another")
self.say.publish(data="I cannot get to a safe approach for there, please choose another point")
else:
self.force_wipe(self.force_wipe_start, pose)
self.force_wipe_started = False
else:
rospy.loginfo("Cannot reach wiping point, please choose another")
self.wt_log_out.publish(data="Cannot reach wiping point, please choose another")
self.say.publish(data="I cannot reach there, please choose another point")
def force_wipe(self, ps_start, ps_finish, travel = 0.05):
ps_start.header.stamp = rospy.Time.now()
ps_finish.header.stamp = rospy.Time.now()
ps_start_far = self.aul.hand_frame_move(travel, 0, 0, ps_start)
ps_start_near = self.aul.hand_frame_move(-travel, 0, 0, ps_start)
ps_finish_far = self.aul.hand_frame_move(travel, 0, 0, ps_finish)
ps_finish_near = self.aul.hand_frame_move(-travel, 0, 0, ps_finish)
n_points = int(math.ceil(self.aul.calc_dist(ps_start, ps_finish)*7000))
print 'n_points: ', n_points
mid_traj = self.aul.build_trajectory(ps_finish, ps_start, tot_points=n_points)
near_traj = self.aul.build_trajectory(ps_finish_near, ps_start_near, tot_points=n_points)
far_traj = self.aul.build_trajectory(ps_finish_far, ps_start_far, tot_points=n_points)
near_angles = [list(near_traj.points[i].positions) for i in range(n_points)]
mid_angles = [list(mid_traj.points[i].positions) for i in range(n_points)]
far_angles = [list(far_traj.points[i].positions) for i in range(n_points)]
fmn_diff = np.abs(np.subtract(near_angles, far_angles))
fmn_max = np.max(fmn_diff, axis=0)
print 'fmn_max: ', fmn_max
if any(fmn_max >math.pi/2):
rospy.loginfo("TOO LARGE OF AN ANGLE CHANGE ALONG GRADIENT, IK REGION PROBLEMS!")
self.wt_log_out.publish(data="The path requested required large movements (IK Limits cause angle wrapping)")
self.say.publish(data="Large motion detected, cancelling. Please try again.")
return
for i in range(7):
n_max = max(np.abs(np.diff(near_angles,axis=0)[i]))
m_max = max(np.abs(np.diff(mid_angles,axis=0)[i]))
f_max = max(np.abs(np.diff(far_angles,axis=0)[i]))
n_mean = 4*np.mean(np.abs(np.diff(near_angles,axis=0)[i]))
m_mean = 4*np.mean(np.abs(np.diff(mid_angles,axis=0)[i]))
f_mean = 4*np.mean(np.abs(np.diff(far_angles,axis=0)[i]))
print 'near: max: ', n_max, 'mean: ', n_mean
print 'mid: max: ', m_max, 'mean: ', m_mean
print 'far: max: ', f_max, 'mean: ', f_mean
if (n_max >= n_mean) or (m_max >= m_mean) or (f_max >= f_mean):
rospy.logerr("TOO LARGE OF AN ANGLE CHANGE ALONG PATH, IK REGION PROBLEMS!")
self.wt_log_out.publish(data="The path requested required large movements (IK Limits cause angle wrapping)")
self.say.publish(data="Large motion detected, cancelling. Please try again.")
return
near_diff = np.subtract(near_angles, mid_angles).tolist()
far_diff = np.subtract(far_angles, mid_angles).tolist()
self.say.publish(data="Moving to approach point")
prep = self.aul.move_arm_to(self.force_wipe_appr)
if prep:
print 'cur angles: ', self.aul.joint_state_act.positions, 'near angs: ', near_angles[0]
print np.abs(np.subtract(self.aul.joint_state_act.positions, near_angles[0]))
if np.max(np.abs(np.subtract(self.aul.joint_state_act.positions,near_angles[0])))>math.pi:
self.say.publish(data="Adjusting for-arm roll")
print "Evasive Action!"
angles = list(self.aul.joint_state_act.positions)
flex = angles[5]
angles[5] = -0.1
self.aul.send_joint_angles(angles, 4)
rospy.sleep(4)
angles[4] = near_angles[0][4]
self.aul.send_joint_angles(angles,6)
rospy.sleep(6)
angles[5] = flex
self.aul.send_joint_angles(angles, 4)
rospy.sleep(4)
self.say.publish(data="Making Approach.")
self.aul.send_joint_angles(np.add(mid_angles[0],near_diff[0]), 7.5)
rospy.sleep(7)
self.rezero_wrench.publish(data=True)
rospy.sleep(1)
wipe_rate = rospy.Rate(400)
self.stop_maintain = False
count = 0
lap = 0
max_laps = 4
mean = self.force_goal_mean
std = self.force_goal_std
bias = 1
self.say.publish(data="Wiping")
while not (rospy.is_shutdown() or self.stop_maintain) and (count + 1 <= n_points) and (lap < max_laps):
#print "mean: ", mean, "std: ", std, "force: ", self.ft_mag
if self.ft_mag >= mean + std:
# print 'Force too high!'
bias += (self.ft_mag/3500)
elif self.ft_mag < mean - std:
# print 'Force too low'
bias -= max(0.001,(self.ft_mag/3500))
else:
# print 'Force Within Range'
count += 1
bias = np.clip(bias, -1, 1)
if bias > 0.:
diff = near_diff[count]
else:
diff = far_diff[count]
angles_out = np.add(mid_angles[count], np.multiply(abs(bias), diff))
self.aul.send_joint_angles(angles_out, 0.008)
rospy.sleep(0.0075)
mean = self.force_goal_mean
std = self.force_goal_std
if count + 1>= n_points:
count = 0
mid_angles.reverse()
near_diff.reverse()
far_diff.reverse()
lap += 1
#if lap == 3:
# self.say.publish(data="Hold Still, you rascal!")
rospy.sleep(0.5)
self.say.publish(data="Pulling away")
self.aul.send_joint_angles(near_angles[0],5)
rospy.sleep(5)
self.say.publish(data="Finished wiping. Thank you")
def torso_frame_move(self, msg):
self.stop_maintain = True
goal = self.aul.curr_pose()
goal.pose.position.x += msg.x
goal.pose.position.y += msg.y
goal.pose.position.z += msg.z
self.aul.blind_move(goal)
def hand_move(self, f32):
self.stop_maintain = True
hfm_pose = self.aul.hand_frame_move(f32.data)
self.aul.blind_move(hfm_pose)
def norm_approach(self, pose):
self.stop_maintain = True
self.aul.update_frame(pose)
appr = self.aul.find_approach(pose, 0.15)
self.aul.move_arm_to(appr)
def grasp(self, ps):
self.stop_maintain = True
rospy.loginfo("Initiating Grasp Sequence")
self.wt_log_out.publish(data="Initiating Grasp Sequence")
self.aul.update_frame(ps)
approach = self.aul.find_approach(ps, 0.15)
rospy.loginfo("approach: \r\n %s" %approach)
at_appr = self.aul.move_arm_to(approach)
rospy.loginfo("arrived at approach: %s" %at_appr)
if at_appr:
opened = self.aul.gripper(0.09)
if opened:
rospy.loginfo("making linear approach")
#hfm_pose = self.aul.hand_frame_move(0.23)
hfm_pose = self.aul.find_approach(ps,-0.02)
self.aul.blind_move(hfm_pose)
self.aul.wait_for_stop(2)
closed = self.aul.gripper(0.0)
if not closed:
rospy.loginfo("Couldn't close completely: Grasp likely successful")
hfm_pose = self.aul.hand_frame_move(-0.23)
self.aul.blind_move(hfm_pose)
else:
pass
def prep_surf_wipe(self, point):
pixel_u = point.x
pixel_v = point.y
test_pose = self.p23d_proxy(pixel_u, pixel_v).pixel3d
self.aul.update_frame(test_pose)
test_pose = self.aul.find_approach(test_pose, 0)
(reachable, ik_goal) = self.aul.full_ik_check(test_pose)
if reachable:
if not self.surf_wipe_started:
start_pose = test_pose
self.surf_wipe_start = [pixel_u, pixel_v, start_pose]
self.surf_wipe_started = True
rospy.loginfo("Received valid starting position for wiping action")
self.wt_log_out.publish(data="Received valid starting position for wiping action")
return #Return after 1st point, wait for second
else:
rospy.loginfo("Received valid ending position for wiping action")
self.wt_log_out.publish(data="Received valid ending position for wiping action")
self.surf_wipe_started = False #Continue on successful 2nd point
else:
rospy.loginfo("Cannot reach wipe position, please try another")
self.wt_log_out.publish(data="Cannot reach wipe position, please try another")
return #Return on invalid point, wait for another
dist = self.aul.calc_dist(self.surf_wipe_start[2],test_pose)
print 'dist', dist
num_points = dist/0.02
print 'num_points', num_points
us = np.round(np.linspace(self.surf_wipe_start[0], pixel_u, num_points))
vs = np.round(np.linspace(self.surf_wipe_start[1], pixel_v, num_points))
surf_points = [PoseStamped() for i in xrange(len(us))]
print "Surface Points", [us,vs]
for i in xrange(len(us)):
pose = self.p23d_proxy(us[i],vs[i]).pixel3d
self.aul.update_frame(pose)
surf_points[i] = self.aul.find_approach(pose,0)
print i+1, '/', len(us)
self.aul.blind_move(surf_points[0])
self.aul.wait_for_stop()
for pose in surf_points:
self.aul.blind_move(pose,2.5)
rospy.sleep(2)
#self.aul.wait_for_stop()
self.aul.hand_frame_move(-0.1)
def poke(self, ps):
self.stop_maintain = True
self.aul.update_frame(ps)
appr = self.aul.find_approach(ps,0.15)
touch = self.aul.find_approach(ps,0)
prepared = self.aul.move_arm_to(appr)
if prepared:
self.aul.blind_move(touch)
self.aul.wait_for_stop()
rospy.sleep(7)
self.aul.blind_move(appr)
def swipe(self, ps):
traj = self.prep_wipe(ps)
if traj is not None:
self.stop_maintain = True
self.wipe_move(traj, 1)
def wipe(self, ps):
traj = self.prep_wipe(ps)
if traj is not None:
self.stop_maintain = True
self.wipe_move(traj, 4)
def prep_wipe(self, ps):
#print "Prep Wipe Received: %s" %pa
self.aul.update_frame(ps)
print "Updating frame to: %s \r\n" %ps
if not self.wipe_started:
self.wipe_appr_seed = ps
self.wipe_ends[0] = self.aul.find_approach(ps, 0)
print "wipe_end[0]: %s" %self.wipe_ends[0]
(reachable, ik_goal) = self.aul.full_ik_check(self.wipe_ends[0])
if not reachable:
rospy.loginfo("Cannot find approach for initial wipe position, please try another")
self.wt_log_out.publish(data="Cannot find approach for initial wipe position, please try another")
return None
else:
self.wipe_started = True
rospy.loginfo("Received starting position for wiping action")
self.wt_log_out.publish(data="Received starting position for wiping action")
return None
else:
self.wipe_ends[1] = self.aul.find_approach(ps, 0)
self.wipe_ends.reverse()
(reachable, ik_goal) = self.aul.full_ik_check(self.wipe_ends[1])
if not reachable:
rospy.loginfo("Cannot find approach for final wipe position, please try another")
self.wt_log_out.publish(data="Cannot find approach for final wipe position, please try another")
return None
else:
rospy.loginfo("Received End position for wiping action")
self.wt_log_out.publish(data="Received End position for wiping action")
self.aul.update_frame(self.wipe_ends[0])
self.wipe_ends[1].header.stamp = rospy.Time.now()
self.tfl.waitForTransform(self.wipe_ends[1].header.frame_id, 'lh_utility_frame', rospy.Time.now(), rospy.Duration(3.0))
fin_in_start = self.tfl.transformPose('lh_utility_frame', self.wipe_ends[1])
ang = math.atan2(-fin_in_start.pose.position.z, -fin_in_start.pose.position.y)+(math.pi/2)
q_st_rot = transformations.quaternion_about_axis(ang, (1,0,0))
q_st_new = transformations.quaternion_multiply([self.wipe_ends[0].pose.orientation.x, self.wipe_ends[0].pose.orientation.y, self.wipe_ends[0].pose.orientation.z, self.wipe_ends[0].pose.orientation.w],q_st_rot)
self.wipe_ends[0].pose.orientation.x = q_st_new[0]
self.wipe_ends[0].pose.orientation.y = q_st_new[1]
self.wipe_ends[0].pose.orientation.z = q_st_new[2]
self.wipe_ends[0].pose.orientation.w = q_st_new[3]
self.aul.update_frame(self.wipe_ends[1])
self.wipe_ends[0].header.stamp = rospy.Time.now()
self.tfl.waitForTransform(self.wipe_ends[0].header.frame_id, 'lh_utility_frame', rospy.Time.now(), rospy.Duration(3.0))
start_in_fin = self.tfl.transformPose('lh_utility_frame', self.wipe_ends[0])
ang = math.atan2(start_in_fin.pose.position.z, start_in_fin.pose.position.y)+(math.pi/2)
q_st_rot = transformations.quaternion_about_axis(ang, (1,0,0))
q_st_new = transformations.quaternion_multiply([self.wipe_ends[1].pose.orientation.x, self.wipe_ends[1].pose.orientation.y, self.wipe_ends[1].pose.orientation.z, self.wipe_ends[1].pose.orientation.w],q_st_rot)
self.wipe_ends[1].pose.orientation.x = q_st_new[0]
self.wipe_ends[1].pose.orientation.y = q_st_new[1]
self.wipe_ends[1].pose.orientation.z = q_st_new[2]
self.wipe_ends[1].pose.orientation.w = q_st_new[3]
self.aul.update_frame(self.wipe_appr_seed)
appr = self.aul.find_approach(self.wipe_appr_seed, 0.15)
appr.pose.orientation = self.wipe_ends[1].pose.orientation
prepared = self.aul.move_arm_to(appr)
if prepared:
#self.aul.advance_to_contact()
self.aul.blind_move(self.wipe_ends[1])
traj = self.aul.build_trajectory(self.wipe_ends[1], self.wipe_ends[0])
wipe_traj = self.aul.build_follow_trajectory(traj)
self.aul.wait_for_stop()
self.wipe_started = False
return wipe_traj
#self.wipe(wipe_traj)
else:
rospy.loginfo("Failure reaching start point, please try again")
self.wt_log_out.publish(data="Failure reaching start point, please try again")
def wipe_move(self, traj_goal, passes=4):
times = []
for i in range(len(traj_goal.trajectory.points)):
times.append(traj_goal.trajectory.points[i].time_from_start)
count = 0
while count < passes:
traj_goal.trajectory.points.reverse()
for i in range(len(times)):
traj_goal.trajectory.points[i].time_from_start = times[i]
traj_goal.trajectory.header.stamp = rospy.Time.now()
assert traj_goal.trajectory.points[0] != []
self.aul.l_arm_follow_traj_client.send_goal(traj_goal)
self.aul.l_arm_follow_traj_client.wait_for_result(rospy.Duration(20))
rospy.sleep(0.5)# Pause at end of swipe
count += 1
rospy.loginfo("Done Wiping")
self.wt_log_out.publish(data="Done Wiping")
hfm_pose = self.aul.hand_frame_move(-0.15)
self.aul.blind_move(hfm_pose)
def broadcast_pose(self):
broadcast_rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.pose_out.publish(self.aul.curr_pose())
broadcast_rate.sleep()
class MarkerProcessor(object):
def __init__(self, use_dummy_transform=False):
rospy.init_node('star_center_positioning_node')
if use_dummy_transform:
self.odom_frame_name = ROBOT_NAME+"_odom_dummy"
else:
self.odom_frame_name = ROBOT_NAME+"_odom"
self.marker_locators = {}
self.add_marker_locator(MarkerLocator(0,(-6*12*2.54/100.0,0),0))
self.add_marker_locator(MarkerLocator(1,(0.0,0.0),pi))
self.add_marker_locator(MarkerLocator(2,(0.0,10*12*2.54/100.0),0))
self.add_marker_locator(MarkerLocator(3,(-6*12*2.54/100.0,6*12*2.54/100.0),0))
self.add_marker_locator(MarkerLocator(4,(0.0,6*12*2.54/100.0),pi))
self.pose_correction = rospy.get_param('~pose_correction',0.0)
self.phase_offset = rospy.get_param('~phase_offset',0.0)
self.is_flipped = rospy.get_param('~is_flipped',False)
srv = Server(STARPoseConfig, self.config_callback)
self.marker_sub = rospy.Subscriber("ar_pose_marker",
ARMarkers,
self.process_markers)
self.odom_sub = rospy.Subscriber(ROBOT_NAME+"/odom", Odometry, self.process_odom, queue_size=10)
self.star_pose_pub = rospy.Publisher(ROBOT_NAME+"/STAR_pose",PoseStamped,queue_size=10)
self.continuous_pose = rospy.Publisher(ROBOT_NAME+"/STAR_pose_continuous",PoseStamped,queue_size=10)
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
def add_marker_locator(self, marker_locator):
self.marker_locators[marker_locator.id] = marker_locator
def config_callback(self, config, level):
self.phase_offset = config.phase_offset
self.pose_correction = config.pose_correction
return config
def process_odom(self, msg):
p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=self.odom_frame_name),
pose=msg.pose.pose)
try:
STAR_pose = self.tf_listener.transformPose("STAR", p)
STAR_pose.header.stamp = msg.header.stamp
self.continuous_pose.publish(STAR_pose)
except Exception as inst:
print "error is", inst
def process_markers(self, msg):
for marker in msg.markers:
# do some filtering basd on prior knowledge
# we know the approximate z coordinate and that all angles but yaw should be close to zero
euler_angles = euler_from_quaternion((marker.pose.pose.orientation.x,
marker.pose.pose.orientation.y,
marker.pose.pose.orientation.z,
marker.pose.pose.orientation.w))
angle_diffs = TransformHelpers.angle_diff(euler_angles[0],pi), TransformHelpers.angle_diff(euler_angles[1],0)
print angle_diffs, marker.pose.pose.position.z
if (marker.id in self.marker_locators and
3.0 <= marker.pose.pose.position.z <= 3.6 and
fabs(angle_diffs[0]) <= .4 and
fabs(angle_diffs[1]) <= .4):
print "FOUND IT!"
locator = self.marker_locators[marker.id]
xy_yaw = list(locator.get_camera_position(marker))
if self.is_flipped:
print "WE ARE FLIPPED!!!"
xy_yaw[2] += pi
print self.pose_correction
print self.phase_offset
xy_yaw[0] += self.pose_correction*cos(xy_yaw[2]+self.phase_offset)
xy_yaw[1] += self.pose_correction*sin(xy_yaw[2]+self.phase_offset)
orientation_tuple = quaternion_from_euler(0,0,xy_yaw[2])
pose = Pose(position=Point(x=-xy_yaw[0],y=-xy_yaw[1],z=0),
orientation=Quaternion(x=orientation_tuple[0], y=orientation_tuple[1], z=orientation_tuple[2], w=orientation_tuple[3]))
# TODO: use markers timestamp instead of now() (unfortunately, not populated currently by ar_pose)
pose_stamped = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id="STAR"),pose=pose)
# TODO: use frame timestamp instead of now()
self.star_pose_pub.publish(pose_stamped)
self.fix_STAR_to_odom_transform(pose_stamped)
def fix_STAR_to_odom_transform(self, msg):
""" Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """
(translation, rotation) = TransformHelpers.convert_pose_inverse_transform(msg.pose)
p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=rospy.Time(),frame_id=ROBOT_NAME+"_base_link"))
try:
self.tf_listener.waitForTransform(ROBOT_NAME+"_odom",ROBOT_NAME+"_base_link",rospy.Time(),rospy.Duration(1.0))
except Exception as inst:
print "whoops", inst
return
print "got transform"
self.odom_to_STAR = self.tf_listener.transformPose(ROBOT_NAME+"_odom", p)
(self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_STAR.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map to odom transformation.
This is necessary so things like move_base can work properly. """
if not(hasattr(self,'translation') and hasattr(self,'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame_name, "STAR")
def run(self):
r = rospy.Rate(10)
while not rospy.is_shutdown():
self.broadcast_last_transform()
r.sleep()
class ros_cv_testing_node:
def __init__(self):
# Get information for this particular camera
camera_info = get_single('head_camera/depth_registered/camera_info', CameraInfo)
print camera_info
# Set information for the camera
self.cam_model = PinholeCameraModel()
self.cam_model.fromCameraInfo(camera_info)
# Subscribe to the camera's image topic and depth image topic
self.rgb_image_sub = rospy.Subscriber("head_camera/rgb/image_raw",
Image, self.on_rgb)
self.depth_image_sub = rospy.Subscriber("head_camera/depth_registered/image_raw",
Image, self.on_depth)
# Publish where the button is in the base link frame
self.point_pub = rospy.Publisher('button_point', PointStamped, queue_size=10)
# Set up connection to CvBridge
self.bridge = CvBridge()
# Open up viewing windows
cv2.namedWindow('depth')
cv2.namedWindow('rgb')
# Set up the class variables
self.rgb_image = None
self.rgb_image_time = None
self.depth_image = None
self.center = None
self.return_point = PointStamped()
self.tf_listener = TransformListener()
def on_rgb(self, image):
# Convert image to something that cv2 can work with
try:
self.rgb_image = self.bridge.imgmsg_to_cv2(image, 'bgr8')
except e:
print e
return
self.rgb_image_time = image.header.stamp
# Get height and width of image
h = self.rgb_image.shape[0]
w = self.rgb_image.shape[1]
# Create empty image
color_dst = numpy.empty((h,w), 'uint8')
# Convert picture to grayscale
cv2.cvtColor(self.rgb_image, cv2.COLOR_RGB2GRAY, color_dst)
# Find circles given the camera image
dp = 1.1
minDist = 90
circles = cv2.HoughCircles(color_dst, cv2.cv.CV_HOUGH_GRADIENT, dp, minDist)
# If no circles are detected then exit the program
if circles == None:
print "No circles found using these parameters"
sys.exit()
circles = numpy.uint16(numpy.around(circles))
# Draw the center of the circle closest to the top
ycoord = []
for i in circles[0,:]:
ycoord.append(i[1])
# Find the top circle in the frame
top_circ_y_coor = min(ycoord)
x_coor = 0
y_coor = 0
for i in circles[0,:]:
if i[1] == top_circ_y_coor:
# draw the center of the circle
#cv2.circle(self.rgb_image,(i[0],i[1]),2,(0,0,255),3)
# draw outer circle
#cv2.circle(self.rgb_image,(i[0],i[1]),i[2],(0,255,0),2)
x_coor = i[0]
y_coor = i[1]
cv2.circle(self.rgb_image,(327,247),2,(0,0,255),3)
# Set the coordinates for the center of the circle
self.center = (x_coor, y_coor)
#self.center = (328,248)
cv2.imshow('rgb', self.rgb_image)
cv2.waitKey(1)
def on_depth(self, image):
# Use numpy so that cv2 can use image
self.depth_image = numpy.fromstring(image.data, dtype='float32').reshape((480,640))
nmask = numpy.isnan(self.depth_image)
#Find the minimum and the maximum of the depth image
Dmin = self.depth_image[~nmask].min()
Dmax = self.depth_image[~nmask].max()
# If a circle has been found find its depth and apply that to the ray
if self.center!=None:
ray = self.cam_model.projectPixelTo3dRay(self.center)
depth = self.depth_image[self.center[1]][self.center[0]]
# If the depth is not a number exit
if math.isnan(depth):
print "Incomplete information on depth at this point"
return
# Otherwise mulitply the depth by the unit vector of the ray
else:
print "depth", depth
cam_coor = [depth*ray[0],depth*ray[1],depth*ray[2]]
#print "camera frame coordinate:", cam_coor
else:
return
# Rescale to [0,1]
cv2.imshow('depth', self.depth_image / 2.0)
cv2.waitKey(1)
# Only use depth if the RGB image is running
if self.rgb_image_time is None:
rospy.loginfo('not using depth cause no RGB yet')
return
time_since_rgb = image.header.stamp - self.rgb_image_time
# Only use depth if it is close in time to the RGB image
if time_since_rgb > rospy.Duration(0.5):
rospy.loginfo('not using depth because time since RGB is ' + str(time_since_rgb))
#return
# Find transform from camera frame to world frame
self.tf_listener.waitForTransform(self.cam_model.tfFrame(), "base_link",
image.header.stamp, rospy.Duration(1.0))
# Set up the point to be published
self.return_point.header.stamp = image.header.stamp
self.return_point.header.frame_id = self.cam_model.tfFrame()
self.return_point.point.x = cam_coor[0]
self.return_point.point.y = cam_coor[1]
self.return_point.point.z = cam_coor[2]
# Transform point to the baselink frame
self.return_point = self.tf_listener.transformPoint("base_link", self.return_point)
print "world frame coordinate:", self.return_point.point.x, \
self.return_point.point.y,self.return_point.point.z, "\n"
self.point_pub.publish(self.return_point)
class PR2Greeter:
def __init__(self, debug=False, online = True, robot_frame="odom_combined"):
self._tf = TransformListener()
self._online = online
# self.snd_handle = SoundClient()
if self._online:
#self._interface = ROSpeexInterface()
#self._interface.init()
self._speech_client = SpeechSynthesisClient_NICT()
else:
self.snd_handle = SoundClient()
rospy.sleep(1)
self.say('Hello world!')
rospy.sleep(1)
self._debug = debug
self._robot_frame = robot_frame
self._point_sub = rospy.Subscriber('nearest_face', PointStamped, self.face_cb)
self._head_action_cl = actionlib.SimpleActionClient('/head_traj_controller/point_head_action', pr2_controllers_msgs.msg.PointHeadAction)
self._torso_action_cl = actionlib.SimpleActionClient('/torso_controller/position_joint_action', pr2_controllers_msgs.msg.SingleJointPositionAction)
self._left_arm = MoveGroupCommander("left_arm")
self._right_arm = MoveGroupCommander("right_arm")
print "r.f.: " + self._left_arm.get_pose_reference_frame()
self.face = None
# self.face_from = rospy.Time(0)
self.face_last_dist = 0
self.last_invited_at = rospy.Time(0)
self._person_prob_left = 0
self.l_home_pose = [0.283, 0.295, 0.537, -1.646, 0.468, -1.735]
self.l_wave_1 = [-0.1, 0.6, 1.15, -1.7, -0.97, -1.6]
self.l_wave_2 = [-0.1, 0.6, 1.15, 1.7, -0.97, 1.6]
self.r_home_pose = [0.124, -0.481, 0.439, -1.548, 0.36, -0.035]
self.r_advert = [0.521, -0.508, 0.845, -1.548, 0.36, -0.035]
self.no_face_random_delay = None
self._initialized = False
self._timer = rospy.Timer(rospy.Duration(1.0), self.timer)
self._move_buff = Queue.Queue()
self._head_buff = Queue.Queue()
self._move_thread = threading.Thread(target=self.movements)
self._move_thread.daemon = True
self._move_thread.start()
self._head_thread = threading.Thread(target=self.head)
self._head_thread.daemon = True
self._head_thread.start()
self.new_face = False
self.face_last_dist = 0.0
self.face_counter = 0
self.actions = [self.stretchingAction,
self.introduceAction,
self.waveHandAction,
self.lookAroundAction,
self.lookAroundAction,
self.lookAroundAction,
self.advertAction,
self.numberOfFacesAction]
self.goodbye_strings = ["Thanks for stopping by.",
"Enjoy the event.",
"See you later!",
"Have a nice day!"]
self.invite_strings = ["Hello. It's nice to see you.",
"Come here and take some flyer.",
"I hope you are enjoying the event."]
rospy.loginfo("Ready")
def say(self, text):
if self._online:
#self._interface.say(text, 'en', 'nict')
data = self._speech_client.request(text, 'en')
try:
tmp_file = open('output_tmp.dat', 'wb')
tmp_file.write(data)
tmp_file.close()
# play sound
subprocess.check_output(['ffplay', 'output_tmp.dat', '-autoexit', '-nodisp'], stderr=subprocess.STDOUT)
except IOError:
rospy.logerr('file io error.')
except OSError:
rospy.logerr('ffplay is not installed.')
except subprocess.CalledProcessError:
rospy.logerr('ffplay return error value.')
except:
rospy.logerr('unknown exception.')
else:
self.snd_handle.say(text)
def getRandomFromArray(self, arr):
idx = random.randint(0, len(arr) - 1)
return arr[idx]
def stretchingAction(self):
self.say("I'm bit tired. Let's do some stretching.")
self._torso_action_cl.wait_for_server()
goal = pr2_controllers_msgs.msg.SingleJointPositionGoal()
goal.position = 0.195
goal.max_velocity = 0.5
try:
self._torso_action_cl.send_goal(goal)
self._torso_action_cl.wait_for_result(rospy.Duration.from_sec(10.0))
except:
rospy.logerr("torso action error (up)")
# TODO move arms
self.say("I feel much better now!")
goal.position = 0.0
try:
self._torso_action_cl.send_goal(goal)
self._torso_action_cl.wait_for_result(rospy.Duration.from_sec(10.0))
except:
rospy.logerr("torso action error (down)")
def numberOfFacesAction(self):
string = "Today I already saw " + str(self.face_counter) + "face"
if self.face_counter != 1:
string = string + "s"
string = string + "."
self.say(string)
rospy.sleep(1)
def advertAction(self):
self.say("Hello. Here are some posters for you.")
self.go(self._right_arm, self.r_advert)
rospy.sleep(1)
def introduceAction(self):
self.say("Hello. I'm PR2 robot. Come here to check me.")
rospy.sleep(1)
def waveHandAction(self):
self.say("I'm here. Please come to see me.")
rand = random.randint(1, 3)
for _ in range(rand):
self.wave()
self.go(self._left_arm, self.l_home_pose)
rospy.loginfo("Waving")
rospy.sleep(1)
def lookAroundAction(self):
self.say("I'm looking for somebody. Please come closer.")
p = PointStamped()
p.header.stamp = rospy.Time.now()
p.header.frame_id = "/base_link"
p.point.x = 5.0
sign = random.choice([-1, 1])
p.point.y = sign * random.uniform(1.5, 0.5)
p.point.z = random.uniform(1.7, 0.2)
self._head_buff.put((copy.deepcopy(p), 0.1, True))
p.point.y = -1 * sign * random.uniform(1.5, 0.5)
p.point.z = random.uniform(1.7, 0.2)
self._head_buff.put((copy.deepcopy(p), 0.1, True))
p.point.y = sign * random.uniform(1.5, 0.5)
p.point.z = random.uniform(1.7, 0.2)
self._head_buff.put((copy.deepcopy(p), 0.1, True))
p.point.y = -1 * sign * random.uniform(1.5, 0.5)
p.point.z = random.uniform(1.7, 0.2)
self._head_buff.put((copy.deepcopy(p), 0.1, True))
rospy.loginfo("Looking around")
rospy.sleep(1)
def getPointDist(self, pt):
assert(self.face is not None)
# fist, get my position
p = PoseStamped()
p.header.frame_id = "base_link"
p.header.stamp = rospy.Time.now() - rospy.Duration(0.5)
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = 0
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
try:
self._tf.waitForTransform(p.header.frame_id, self._robot_frame, p.header.stamp, rospy.Duration(2))
p = self._tf.transformPose(self._robot_frame, p)
except:
rospy.logerr("TF error!")
return None
return sqrt(pow(p.pose.position.x - pt.point.x, 2) + pow(p.pose.position.y - pt.point.y, 2) + pow(p.pose.position.z - pt.point.z, 2))
def getPoseStamped(self, group, c):
assert(len(c) == 6)
p = PoseStamped()
p.header.frame_id = "base_link"
p.header.stamp = rospy.Time.now() - rospy.Duration(0.5)
p.pose.position.x = c[0]
p.pose.position.y = c[1]
p.pose.position.z = c[2]
quat = tf.transformations.quaternion_from_euler(c[3], c[4], c[5])
p.pose.orientation.x = quat[0]
p.pose.orientation.y = quat[1]
p.pose.orientation.z = quat[2]
p.pose.orientation.w = quat[3]
try:
self._tf.waitForTransform(p.header.frame_id, group.get_pose_reference_frame(), p.header.stamp, rospy.Duration(2))
p = self._tf.transformPose(group.get_pose_reference_frame(), p)
except:
rospy.logerr("TF error!")
return None
return p
def go(self, group, where):
self._move_buff.put((group, where))
def wave(self):
self.go(self._left_arm, self.l_wave_1)
self.go(self._left_arm, self.l_wave_2)
self.go(self._left_arm, self.l_wave_1)
def head(self):
self._head_action_cl.wait_for_server()
while not rospy.is_shutdown():
(target, vel, imp) = self._head_buff.get()
# we don't need to block it here
self._head_buff.task_done()
if (not imp) and (not self._head_buff.empty()):
print "skipping head goal"
# head target can be skipped (follow only the latest one)
continue
# print "head point goal"
# print target
# point PR2's head there (http://wiki.ros.org/pr2_controllers/Tutorials/Moving%20the%20Head)
goal = pr2_controllers_msgs.msg.PointHeadGoal()
goal.target = target
# goal.min_duration = rospy.Duration(3.0)
goal.max_velocity = vel
# goal.pointing_frame = "high_def_frame"
goal.pointing_frame = "head_mount_kinect_rgb_link"
goal.pointing_axis.x = 1
goal.pointing_axis.y = 0
goal.pointing_axis.z = 0
try:
self._head_action_cl.send_goal(goal)
self._head_action_cl.wait_for_result(rospy.Duration.from_sec(5.0))
except:
rospy.logerr("head action error")
#self._head_buff.task_done()
def movements(self):
while not rospy.is_shutdown():
(group, where) = self._move_buff.get()
group.set_start_state_to_current_state()
p = self.getPoseStamped(group, where)
if p is None:
self._move_buff.task_done()
continue
group.set_pose_target(p)
group.set_goal_tolerance(0.1)
group.allow_replanning(True)
self._move_buff.task_done()
group.go(wait=True)
def timer(self, event):
if self._initialized is False:
rospy.loginfo("Moving arms to home positions")
self.init_head()
self.go(self._left_arm, self.l_home_pose)
self.go(self._right_arm, self.r_home_pose)
self._move_buff.join()
self.say("I'm ready for a great job.")
self._initialized = True
if self.face is None:
if (self.no_face_random_delay is None):
delay = random.uniform(30, 10)
self.no_face_random_delay = rospy.Time.now() + rospy.Duration(delay)
rospy.loginfo("Random delay: " + str(delay))
return
else:
if rospy.Time.now() < self.no_face_random_delay:
return
self.init_head()
self.go(self._left_arm, self.l_home_pose)
self.go(self._right_arm, self.r_home_pose)
rospy.loginfo("Starting selected action")
action = self.getRandomFromArray(self.actions)
action()
delay = random.uniform(30, 10)
self.no_face_random_delay = rospy.Time.now() + rospy.Duration(delay)
return
else:
self.no_face_random_delay = None
if self.new_face and (self.last_invited_at + rospy.Duration(10) < rospy.Time.now()):
self.last_invited_at = rospy.Time.now()
self.new_face = False
rospy.loginfo("new person")
self.face_counter = self.face_counter + 1
# cd = getPointDist(self.face)
# TODO decide action based on distance ?
self.go(self._left_arm, self.l_home_pose) # if a person is too close, dont move hand?
self.go(self._right_arm, self.r_advert)
string = self.getRandomFromArray(self.invite_strings)
self.say(string)
# TODO wait some min. time + say something
# after 20 seconds of no detected face, let's continue
if self.face.header.stamp + rospy.Duration(10) < rospy.Time.now():
rospy.loginfo("person left")
string = self.getRandomFromArray(self.goodbye_strings)
self.say(string)
self.init_head()
self.go(self._left_arm, self.l_home_pose)
self.go(self._right_arm, self.r_home_pose)
self.face = None
return
self._head_buff.put((copy.deepcopy(self.face), 0.4, False))
def init_head(self):
p = PointStamped()
p.header.stamp = rospy.Time.now()
p.header.frame_id = "/base_link"
p.point.x = 2.0
p.point.y = 0.0
p.point.z = 1.7
self._head_buff.put((p, 0.1, True))
def face_cb(self, point):
# transform point
try:
self._tf.waitForTransform(point.header.frame_id, self._robot_frame, point.header.stamp, rospy.Duration(2))
pt = self._tf.transformPoint(self._robot_frame, point)
except:
rospy.logerr("Transform error")
return
if self.face is not None:
cd = self.getPointDist(pt) # current distance
dd = fabs(self.face_last_dist - cd) # change in distance
if dd < 0.5:
self.face.header = pt.header
# filter x,y,z values a bit
self.face.point = pt.point
#self.face.point.x = (self.face.point.x + pt.point.x) / 2
#self.face.point.y = (self.face.point.y + pt.point.y) / 2
#self.face.point.z = (self.face.point.z + pt.point.z) / 2
else:
if self._person_prob_left < 2:
self._person_prob_left += 1
else:
self._person_prob_left = 0
print "new face ddist: " + str(dd)
self.new_face = True
self.face = pt
self.face_last_dist = cd
else:
self._person_prob_left = 0
self.new_face = True
self.face = pt
class ArmIntermediary():
def __init__(self, arm):
self.arm = arm
self.tfl = TransformListener()
self.pr2_arm = PR2Arm(self.arm, self.tfl)
self.pr2_gripper = PR2Gripper(self.arm)
rospy.loginfo('Waiting for Pixel_2_3d Service')
try:
rospy.wait_for_service('/pixel_2_3d', 7.0)
self.p23d_proxy = rospy.ServiceProxy('/pixel_2_3d', Pixel23d, True)
rospy.loginfo("Found pixel_2_3d service")
except:
rospy.logwarn("Pixel_2_3d Service Not available")
#Low-level motion requests: pass directly to pr2_arm
rospy.Subscriber("wt_"+self.arm+"_arm_pose_commands", Point,
self.torso_frame_move)
rospy.Subscriber("wt_"+self.arm+"_arm_angle_commands", JointTrajectoryPoint,
self.pr2_arm.send_traj_point)
#More complex motion scripts, built here using pr2_arm functions
rospy.Subscriber("norm_approach_"+self.arm, PoseStamped, self.norm_approach)
rospy.Subscriber("wt_grasp_"+self.arm+"_goal", PoseStamped, self.grasp)
rospy.Subscriber("wt_wipe_"+self.arm+"_goals", PoseStamped, self.wipe)
rospy.Subscriber("wt_swipe_"+self.arm+"_goals", PoseStamped, self.swipe)
rospy.Subscriber("wt_lin_move_"+self.arm, Float32, self.hand_move)
rospy.Subscriber("wt_adjust_elbow_"+self.arm, Float32,
self.pr2_arm.adjust_elbow)
rospy.Subscriber('wt_surf_wipe_right_points', Point,
self.prep_surf_wipe)
rospy.Subscriber("wt_poke_"+self.arm+"_point", PoseStamped, self.poke)
rospy.Subscriber(rospy.get_name()+"/log_out", String, self.repub_log)
rospy.Subscriber("wt_"+self.arm[0]+"_gripper_commands",
Pr2GripperCommand, self.gripper_pos)
rospy.Subscriber("wt_"+self.arm[0]+"_gripper_grab_commands",
Bool, self.gripper_grab)
rospy.Subscriber("wt_"+self.arm[0]+"_gripper_release_commands",
Bool, self.gripper_release)
self.wt_log_out = rospy.Publisher("wt_log_out", String)
self.wipe_started = False
self.surf_wipe_started = False
self.wipe_ends = [PoseStamped(), PoseStamped()]
def repub_log(self, msg):
self.wt_log_out.publish(msg)
def gripper_pos(self, msg):
self.pr2_gripper.gripper_action(msg.position, msg.max_effort)
def gripper_grab(self, msg):
self.pr2_gripper.grab()
def gripper_release(self, msg):
self.pr2_gripper.release()
def torso_frame_move(self, msg):
"""Do linear motion relative to torso frame."""
goal = self.pr2_arm.curr_pose()
goal.pose.position.x += msg.x
goal.pose.position.y += msg.y
goal.pose.position.z += msg.z
self.pr2_arm.blind_move(goal)
def hand_move(self, f32):
"""Do linear motion relative to hand frame."""
hfm_pose = self.pr2_arm.hand_frame_move(f32.data)
self.pr2_arm.blind_move(hfm_pose)
def norm_approach(self, pose):
""" Safe move normal to surface pose at given distance."""
appr = pu.find_approach(pose, 0.25)
self.pr2_arm.move_arm_to(appr)
def grasp(self, ps):
"""Do simple grasp: Normal approch, open, advance, close, retreat."""
rospy.loginfo("Initiating Grasp Sequence")
self.wt_log_out.publish(data="Initiating Grasp Sequence")
approach = pose_utils.find_approach(ps, 0.15)
rospy.loginfo("approach: \r\n %s" %approach)
at_appr = self.pr2_arm.move_arm_to(approach)
rospy.loginfo("arrived at approach: %s" %at_appr)
if at_appr:
opened = self.pr2_arm.gripper(0.09)
if opened:
rospy.loginfo("making linear approach")
hfm_pose = pose_utils.find_approach(ps, -0.02)
self.pr2_arm.blind_move(hfm_pose)
self.pr2_arm.wait_for_stop()
closed = self.pr2_arm.gripper(0.0)
if not closed:
rospy.loginfo("Couldn't close completely:\
Grasp likely successful")
hfm_pose = self.pr2_arm.hand_frame_move(-0.20)
self.pr2_arm.blind_move(hfm_pose)
else:
pass
def prep_surf_wipe(self, point):
pixel_u = point.x
pixel_v = point.y
test_pose = self.p23d_proxy(pixel_u, pixel_v).pixel3d
test_pose = pose_utils.find_approach(test_pose, 0)
(reachable, ik_goal) = self.pr2_arm.full_ik_check(test_pose)
if reachable:
if not self.surf_wipe_started:
start_pose = test_pose
self.surf_wipe_start = [pixel_u, pixel_v, start_pose]
self.surf_wipe_started = True
rospy.loginfo("Received valid starting position for wiping\
action")
self.wt_log_out.publish(data="Received valid starting position\
for wiping action")
return #Return after 1st point, wait for second
else:
rospy.loginfo("Received valid ending position for wiping\
action")
self.wt_log_out.publish(data="Received valid ending position\
for wiping action")
self.surf_wipe_started = False
else:
rospy.loginfo("Cannot reach wipe position, please try another")
self.wt_log_out.publish(data="Cannot reach wipe position,\
please try another")
return #Return on invalid point, wait for another
dist = self.pr2_arm.calc_dist(self.surf_wipe_start[2],test_pose)
print 'dist', dist
num_points = dist/0.02
print 'num_points', num_points
us = np.round(np.linspace(self.surf_wipe_start[0], pixel_u, num_points))
vs = np.round(np.linspace(self.surf_wipe_start[1], pixel_v, num_points))
surf_points = [PoseStamped() for i in xrange(len(us))]
print "Surface Points", [us,vs]
for i in xrange(len(us)):
pose = self.p23d_proxy(us[i],vs[i]).pixel3d
surf_points[i] = pose_utils.find_approach(pose,0)
print i+1, '/', len(us)
self.pr2_arm.blind_move(surf_points[0])
self.pr2_arm.wait_for_stop()
for pose in surf_points:
self.pr2_arm.blind_move(pose,2.5)
rospy.sleep(2)
self.pr2_arm.hand_frame_move(-0.1)
def poke(self, ps):
appr = pose_utils.find_approach(ps,0.15)
prepared = self.pr2_arm.move_arm_to(appr)
if prepared:
pt1 = self.pr2_arm.hand_frame_move(0.155)
self.pr2_arm.blind_move(pt1)
self.pr2_arm.wait_for_stop()
pt2 = self.pr2_arm.hand_frame_move(-0.155)
self.pr2_arm.blind_move(pt2)
def swipe(self, ps):
traj = self.prep_wipe(ps)
if traj is not None:
self.wipe_move(traj, 1)
def wipe(self, ps):
traj = self.prep_wipe(ps)
if traj is not None:
self.wipe_move(traj, 4)
def prep_wipe(self, ps):
#print "Prep Wipe Received: %s" %pa
print "Updating frame to: %s \r\n" %ps
if not self.wipe_started:
self.wipe_appr_seed = ps
self.wipe_ends[0] = pose_utils.find_approach(ps, 0)
print "wipe_end[0]: %s" %self.wipe_ends[0]
(reachable, ik_goal) = self.pr2_arm.full_ik_check(self.wipe_ends[0])
if not reachable:
rospy.loginfo("Cannot find approach for initial wipe position,\
please try another")
self.wt_log_out.publish(data="Cannot find approach for initial\
wipe position, please try another")
return None
else:
self.wipe_started = True
rospy.loginfo("Received starting position for wiping action")
self.wt_log_out.publish(data="Received starting position for\
wiping action")
return None
else:
self.wipe_ends[1] = pose_utils.find_approach(ps, 0)
self.wipe_ends.reverse()
(reachable, ik_goal) = self.pr2_arm.full_ik_check(self.wipe_ends[1])
if not reachable:
rospy.loginfo("Cannot find approach for final wipe position,\
please try another")
self.wt_log_out.publish(data="Cannot find approach for final\
wipe position, please try another")
return None
else:
rospy.loginfo("Received End position for wiping action")
self.wt_log_out.publish(data="Received End position for wiping\
action")
self.wipe_ends[1].header.stamp = rospy.Time.now()
self.tfl.waitForTransform(self.wipe_ends[1].header.frame_id,
'rh_utility_frame',
rospy.Time.now(),
rospy.Duration(3.0))
fin_in_start = self.tfl.transformPose('rh_utility_frame',
self.wipe_ends[1])
ang = math.atan2(-fin_in_start.pose.position.z,
-fin_in_start.pose.position.y)+(math.pi/2)
q_st_rot = transformations.quaternion_about_axis(ang, (1,0,0))
q_st_new = transformations.quaternion_multiply(
[self.wipe_ends[0].pose.orientation.x,
self.wipe_ends[0].pose.orientation.y,
self.wipe_ends[0].pose.orientation.z,
self.wipe_ends[0].pose.orientation.w],
q_st_rot)
self.wipe_ends[0].pose.orientation = Quaternion(*q_st_new)
self.wipe_ends[0].header.stamp = rospy.Time.now()
self.tfl.waitForTransform(self.wipe_ends[0].header.frame_id,
'rh_utility_frame',
rospy.Time.now(),
rospy.Duration(3.0))
start_in_fin = self.tfl.transformPose('rh_utility_frame',
self.wipe_ends[0])
ang = math.atan2(start_in_fin.pose.position.z,
start_in_fin.pose.position.y)+(math.pi/2)
q_st_rot = transformations.quaternion_about_axis(ang, (1,0,0))
q_st_new = transformations.quaternion_multiply(
[self.wipe_ends[1].pose.orientation.x,
self.wipe_ends[1].pose.orientation.y,
self.wipe_ends[1].pose.orientation.z,
self.wipe_ends[1].pose.orientation.w],
q_st_rot)
self.wipe_ends[1].pose.orientation = Quaternion(*q_st_new)
appr = pose_utils.find_approach(self.wipe_appr_seed, 0.15)
appr.pose.orientation = self.wipe_ends[1].pose.orientation
prepared = self.pr2_arm.move_arm_to(appr)
if prepared:
self.pr2_arm.blind_move(self.wipe_ends[1])
traj = self.pr2_arm.build_trajectory(self.wipe_ends[1],
self.wipe_ends[0])
wipe_traj = self.pr2_arm.build_follow_trajectory(traj)
self.pr2_arm.wait_for_stop()
self.wipe_started = False
return wipe_traj
#self.wipe(wipe_traj)
else:
rospy.loginfo("Failure reaching start point,\
please try again")
self.wt_log_out.publish(data="Failure reaching start\
point, please try again")
def wipe_move(self, traj_goal, passes=4):
times = []
for i in range(len(traj_goal.trajectory.points)):
times.append(traj_goal.trajectory.points[i].time_from_start)
count = 0
while count < passes:
traj_goal.trajectory.points.reverse()
for i in range(len(times)):
traj_goal.trajectory.points[i].time_from_start = times[i]
traj_goal.trajectory.header.stamp = rospy.Time.now()
assert traj_goal.trajectory.points[0] != []
self.pr2_arm.r_arm_follow_traj_client.send_goal(traj_goal)
self.pr2_arm.r_arm_follow_traj_client.wait_for_result(
rospy.Duration(20))
rospy.sleep(0.5)# Pause at end of swipe
count += 1
rospy.loginfo("Done Wiping")
self.wt_log_out.publish(data="Done Wiping")
hfm_pose = self.pr2_arm.hand_frame_move(-0.15)
self.pr2_arm.blind_move(hfm_pose)
class ServoingManager(object):
""" Manager for providing test goals to pr2 ar servoing. """
def __init__(self, mode=None):
self.task = 'feeding_quick'
self.model = 'autobed' # options are 'chair' and 'autobed'
self.mode = mode
self.send_task_count = 0
if self.model == 'autobed':
self.bed_state_z = 0.
self.bed_state_head_theta = 0.
self.bed_state_leg_theta = 0.
self.autobed_sub = rospy.Subscriber('/abdout0', FloatArrayBare, self.bed_state_cb)
self.autobed_pub = rospy.Publisher('/abdin0', FloatArrayBare, latch=True)
self.world_B_head = None
self.world_B_ref_model = None
self.world_B_robot = None
self.tfl = TransformListener()
if self.mode == 'manual' or True:
self.base_pose = None
self.object_pose = None
self.raw_head_pose = None
self.raw_base_pose = None
self.raw_object_pose = None
self.raw_head_sub = rospy.Subscriber('/head_frame', PoseStamped, self.head_frame_cb)
self.raw_base_sub = rospy.Subscriber('/robot_frame', PoseStamped, self.robot_frame_cb)
self.raw_object_sub = rospy.Subscriber('/reference', PoseStamped, self.reference_cb)
# self.head_pub = rospy.Publisher('/head_frame', PoseStamped, latch=True)
# self.base_pub = rospy.Publisher('/robot_frame', PoseStamped, latch=True)
# self.object_pub = rospy.Publisher('/reference', PoseStamped, latch=True)
self.base_goal_pub = rospy.Publisher('/base_goal', PoseStamped, latch=True)
# self.robot_location_pub = rospy.Publisher('/robot_location', PoseStamped, latch=True)
# self.navigation = NavigationHelper(robot='/robot_location', target='/base_goal')
self.goal_data_pub = rospy.Publisher("ar_servo_goal_data", ARServoGoalData)
self.servo_goal_pub = rospy.Publisher('servo_goal_pose', PoseStamped, latch=True)
self.reach_goal_pub = rospy.Publisher("arm_reacher/goal_pose", PoseStamped)
# self.test_pub = rospy.Publisher("test_goal_pose", PoseStamped, latch=True)
# self.test_head_pub = rospy.Publisher("test_head_pose", PoseStamped, latch=True)
self.feedback_pub = rospy.Publisher('wt_log_out', String)
self.torso_lift_pub = rospy.Publisher('torso_controller/position_joint_action/goal',
SingleJointPositionActionGoal, latch=True)
self.base_selection_client = rospy.ServiceProxy("select_base_position", BaseMove_multi)
self.reach_service = rospy.ServiceProxy("/base_selection/arm_reach_enable", None_Bool)
self.ui_input_sub = rospy.Subscriber("action_location_goal", String, self.ui_cb)
self.servo_fdbk_sub = rospy.Subscriber("/pr2_ar_servo/state_feedback", Int8, self.servo_fdbk_cb)
self.lock = Lock()
self.head_pose = None
self.goal_pose = None
self.marker_topic = None
rospy.loginfo("[%s] Ready" %rospy.get_name())
self.base_selection_complete = False
self.send_task_count = 0
def servo_fdbk_cb(self, msg):
# if not msg.data == 5:
# return
#self.reach_goal_pub.publish(self.goal_pose)
#self.feedback_pub.publish("Servoing succeeded. Reaching to location.")
#rospy.loginfo("Servoing Succeeded. Sending goal to arm reacher.")
msg = "Servoing Succeeded."
if self.base_selection_complete and self.send_task_count > 1:
movement = False
msg = "Servoing Succeeded. Arms will proceed to move."
self.base_selection_complete = False
self.send_task_count = 0
movement = self.call_arm_reacher()
self.feedback_pub.publish(msg)
rospy.loginfo(msg)
def ui_cb(self, msg):
if self.model == 'chair':
self.send_task_count = 0
if self.send_task_count > 1:
self.send_task_count = 3
self.servo_fdbk_cb(5)
if self.send_task_count > 5:
self.send_task_count = 0
return
if self.model == 'chair':
self.send_task_count = 3
self.base_selection_complete = False
self.head_pose = self.world_B_head
if self.head_pose is None:
log_msg = "Please register your head before sending a task."
self.feedback_pub.publish(String(log_msg))
rospy.loginfo("[%s] %s" % (rospy.get_name(), log_msg))
return
rospy.loginfo("[%s] Found head frame" % rospy.get_name());
# self.test_head_pub.publish(self.head_pose)
loc = msg.data
if loc not in POSES:
log_msg = "Invalid goal location. No Known Pose for %s" % loc
self.feedback_pub.publish(String(log_msg))
rospy.loginfo("[%s]" % rospy.get_name() + log_msg)
return
rospy.loginfo("[%s] Received valid goal location: %s" % (rospy.get_name(), loc))
# pos, quat = POSES[loc]
# goal_ps_ell = PoseStamped()
# goal_ps_ell.header.frame_id = 'head_frame'
# goal_ps_ell.pose.position = Point(*pos)
# goal_ps_ell.pose.orientation = Quaternion(*quat)
now = rospy.Time.now()
# self.tfl.waitForTransform('base_link', goal_ps_ell.header.frame_id, now, rospy.Duration(10))
# goal_ps_ell.header.stamp = now
# goal_ps = self.tfl.transformPose('base_link', goal_ps_ell)
# self.test_pub.publish(goal_ps)
# with self.lock:
# self.action = "touch"
# # self.goal_pose = goal_ps
# self.marker_topic = "r_pr2_ar_pose_marker" # based on location
base_goals = []
configuration_goals = []
goal_array, config_array = self.call_base_selection()
for item in goal_array:
base_goals.append(item)
for item in config_array:
configuration_goals.append(item)
# [0.9], [-0.8], [0.0], [0.14999999999999999], [0.10000000000000001], [1.2217304763960306]
base_goals[0] = .9
base_goals[1] = -.8
base_goals[2] = 0
configuration_goals[0]=0.15
configuration_goals[1]=0.1
configuration_goals[2]=1.221730476396
# print "Base Goals returned:\r\n", base_goals
# if base_goals is None:
# rospy.loginfo("No base goal found")
# return
# base_goals_list = []
# configuration_goals_list = []
# for i in xrange(int(len(base_goals)/7)):
# psm = PoseStamped()
# psm.header.frame_id = '/base_link'
# psm.pose.position.x = base_goals[int(0+7*i)]
# psm.pose.position.y = base_goals[int(1+7*i)]
# psm.pose.position.z = base_goals[int(2+7*i)]
# psm.pose.orientation.x = base_goals[int(3+7*i)]
# psm.pose.orientation.y = base_goals[int(4+7*i)]
# psm.pose.orientation.z = base_goals[int(5+7*i)]
# psm.pose.orientation.w = base_goals[int(6+7*i)]
# base_goals_list.append(copy.copy(psm))
# configuration_goals_list.append([configuration_goals[0+3*i], configuration_goals[1+3*i],
# configuration_goals[2+3*i]])
# Here should publish configuration_goal items to robot Z axis and to Autobed.
# msg.tag_goal_pose.header.frame_id
torso_lift_msg = SingleJointPositionActionGoal()
torso_lift_msg.goal.position = configuration_goals[0]
self.torso_lift_pub.publish(torso_lift_msg)
# Move autobed if we are dealing with autobed. If not autobed, don't move it. Temporarily fixed to True for
# testing
if self.model == 'autobed':
# autobed_goal = FloatArrayBare()
# autobed_goal.data = [configuration_goals[2], configuration_goals[1]+9, self.bed_state_leg_theta]
# self.autobed_pub.publish(autobed_goal)
print 'The autobed should be set to a height of: ', configuration_goals[1]+7
print 'The autobed should be set to a head rest angle of: ', configuration_goals[2]
if self.mode == 'manual':
self.navigation.start_navigate()
elif True:
goal_B_ref_model= np.matrix([[m.cos(base_goals[2]), -m.sin(base_goals[2]), 0., base_goals[0]],
[m.sin(base_goals[2]), m.cos(base_goals[2]), 0., base_goals[1]],
[ 0., 0., 1., 0.],
[ 0., 0., 0., 1.]])
world_B_goal = self.world_B_ref_model*goal_B_ref_model.I
self.base_selection_complete = True
# pub_goal_tf = TF_Goal(world_B_goal, self.tfl)
# if self.servo_to_pose(world_B_goal):
# self.base_selection_complete = True
# print 'At desired location!!'
else:
self.servo_goal_pub.publish(base_goals_list[0])
ar_data = ARServoGoalData()
# 'base_link' in msg.tag_goal_pose.header.frame_id
with self.lock:
ar_data.tag_id = -1
ar_data.marker_topic = self.marker_topic
ar_data.tag_goal_pose = base_goals_list[0]
self.action = None
self.location = None
self.feedback_pub.publish("Base Position Found. Please use servoing tool.")
rospy.loginfo("[%s] Base position found. Sending Servoing goals." % rospy.get_name())
self.send_task_count += 1
# self.goal_data_pub.publish(ar_data)
def servo_to_pose(self, world_B_goal):
# self.tfl.waitForTransform('/base_link', '/r_forearm_cam_optical_frame', rospy.Time(), rospy.Duration(15.0))
# (trans, rot) = self.tf_listener.lookupTransform('/base_link', '/r_forearm_cam_optical_frame', rospy.Time())
#
# self.tfl.waitForTransform(
# ref_model_B_goal = np.matrix([[m.cos(goal_base_pose[2]), -m.sin(goal_base_pose[2]), 0., goal_base_pose[0]],
# [m.sin(goal_base_pose[2]), m.cos(goal_base_pose[2]), 0., goal_base_pose[1]],
# [ 0., 0., 1., 0.],
# [ 0., 0., 0., 1.]])
base_move_pub = rospy.Publisher('/base_controller/command', Twist)
# error_pos = 1
done_moving = False
rate = rospy.Rate(2)
while not done_moving:
done = False
tw = Twist()
tw.linear.x=0
tw.linear.y=0
tw.linear.z=0
tw.angular.x=0
tw.angular.y=0
tw.angular.z=0
# while not rospy.is_shutdown() and np.abs(world_B_goal[0, 3]-self.world_B_robot[0, 3]) > 0.1:
while not done:
error_mat = self.world_B_robot.I*world_B_goal
if np.abs(error_mat[0, 3]) < 0.1:
done = True
else:
tw.linear.x = np.sign(error_mat[0, 3])*0.15
base_move_pub.publish(tw)
rospy.sleep(.1)
rospy.loginfo('Finished moving to X pose!')
print 'Finished moving to X pose!'
done = False
tw = Twist()
tw.linear.x=0
tw.linear.y=0
tw.linear.z=0
tw.angular.x=0
tw.angular.y=0
tw.angular.z=0
while not done:
error_mat = self.world_B_robot.I*world_B_goal
if np.abs(error_mat[1, 3]) < 0.1:
done = True
else:
tw.linear.y = np.sign(error_mat[1, 3])*0.15
base_move_pub.publish(tw)
rospy.sleep(.1)
rospy.loginfo('Finished moving to Y pose!')
print 'Finished moving to Y pose!'
done = False
tw = Twist()
tw.linear.x=0
tw.linear.y=0
tw.linear.z=0
tw.angular.x=0
tw.angular.y=0
tw.angular.z=0
while not done:
error_mat = self.world_B_robot.I*world_B_goal
if np.abs(m.acos(error_mat[0, 0])) < 0.1:
done = True
else:
tw.angular.z = np.sign(m.acos(error_mat[0, 0]))*0.1
base_move_pub.publish(tw)
rospy.sleep(.1)
rospy.loginfo('Finished moving to goal pose!')
print 'Finished moving to goal pose!'
done_moving = True
# error_mat = self.world_B_robot.I*self.world_B_ref_model*ref_model_B_goal
# error_pos = [error_mat[0,3], error_mat[1,3]]
# error_ori = m.acos(error_mat[0,0])
# # while not (rospy.is_shutdown() and (np.linalg.norm(error_pos)>0.1)) and False:
# error_mat = self.world_B_robot.I*self.world_B_ref_model*ref_model_B_goal
# error_pos = [error_mat[0,3], error_mat[1,3]]
# move = np.array([error_mat[0,3],error_mat[1,3],error_mat[2,3]])
# normalized_pos = move / (np.linalg.norm(move))
# tw = Twist()
# tw.linear.x=normalized_pos[0]
# tw.linear.y=normalized_pos[1]
# tw.linear.z=0
# tw.angular.x=0
# tw.angular.y=0
# tw.angular.z=0
# base_move_pub.publish(tw)
# rospy.sleep(.1)
# # rospy.loginfo('Finished moving to X-Y position. Now correcting orientation!')
# # print 'Finished moving to X-Y position. Now correcting orientation!'
# # while not rospy.is_shutdown() and (np.linalg.norm(error_ori)>0.1) and False:
# error_mat = self.world_B_robot.I*self.world_B_ref_model*ref_model_B_goal
# error_ori = m.acos(error_mat[0,0])
# move = -error_ori
# normalized_ori = move / (np.linalg.norm(move))
# tw = Twist()
# tw.linear.x=0
# tw.linear.y=0
# tw.linear.z=0
# tw.angular.x=0
# tw.angular.y=0
# tw.angular.z=normalized_ori
# base_move_pub.publish(tw)
# rospy.sleep(.1)
# # self.world_B_robot
# # self.world_B_head
# # self.world_B_ref_model
# # world_B_ref = createBMatrix(self)
# # error =
# error_mat = self.world_B_robot.I*self.world_B_ref_model*ref_model_B_goal
# error_pos = [error_mat[0,3], error_mat[1,3]]
# error_ori = m.acos(error_mat[0,0])
# if np.linalg.norm(error_pos)<0.05 and np.linalg.norm(error_ori)<0.05:
# done_moving = True
# # rospy.loginfo('Finished moving to goal pose!')
# print 'Finished moving to goal pose!'
return True
def call_arm_reacher(self):
# Place holder return
#bg = PoseStamped()
#bg.header.stamp = rospy.Time.now()
#bg.header.frame_id = 'ar_marker'
#bg.pose.position = Point(0., 0., 0.5)
#q = tft.quaternion_from_euler(0., np.pi/2, 0.)
#bg.pose.orientation = Quaternion(*q)
#return bg
## End Place Holder
self.feedback_pub.publish("Reaching arm to goal, please wait.")
rospy.loginfo("[%s] Calling arm reacher. Please wait." %rospy.get_name())
# bm = BaseMoveRequest()
# bm.model = self.model
# bm.task = self.task
# try:
# resp = self.call_arm_reacher()
# # resp = self.base_selection_client.call(bm)
# except rospy.ServiceException as se:
# rospy.logerr(se)
# self.feedback_pub.publish("Failed to find good base position. Please try again.")
# return None
return self.reach_service()
def call_base_selection(self):
# Place holder return
#bg = PoseStamped()
#bg.header.stamp = rospy.Time.now()
#bg.header.frame_id = 'ar_marker'
#bg.pose.position = Point(0., 0., 0.5)
#q = tft.quaternion_from_euler(0., np.pi/2, 0.)
#bg.pose.orientation = Quaternion(*q)
#return bg
## End Place Holder
self.feedback_pub.publish("Finding a good base location, please wait.")
rospy.loginfo("[%s] Calling base selection. Please wait." %rospy.get_name())
# bm = BaseMoveRequest()
# bm.model = self.model
# bm.task = self.task
try:
resp = self.base_selection_client(self.task, self.model)
# resp = self.base_selection_client.call(bm)
except rospy.ServiceException as se:
rospy.logerr(se)
self.feedback_pub.publish("Failed to find good base position. Please try again.")
return None
return resp.base_goal, resp.configuration_goal
def bed_state_cb(self, data):
self.bed_state_z = data.data[1]
self.bed_state_head_theta = data.data[0]
self.bed_state_leg_theta = data.data[2]
def base_goal_cb(self, data):
goal_trans = [data.pose.position.x,
data.pose.position.y,
data.pose.position.z]
goal_rot = [data.pose.orientation.x,
data.pose.orientation.y,
data.pose.orientation.z,
data.pose.orientation.w]
pr2_B_goal = createBMatrix(goal_trans, goal_rot)
pr2_trans = [data.pose.position.x,
data.pose.position.y,
data.pose.position.z]
pr2_rot = [data.pose.orientation.x,
data.pose.orientation.y,
data.pose.orientation.z,
data.pose.orientation.w]
world_B_pr2 = createBMatrix(pr2_trans, pr2_rot)
world_B_goal = world_B_pr2*pr2_B_goal
# self.head_pose = data
def update_relations(self):
pr2_trans = [self.raw_base_pose.pose.position.x,
self.raw_base_pose.pose.position.y,
self.raw_base_pose.pose.position.z]
pr2_rot = [self.raw_base_pose.pose.orientation.x,
self.raw_base_pose.pose.orientation.y,
self.raw_base_pose.pose.orientation.z,
self.raw_base_pose.pose.orientation.w]
world_B_pr2 = createBMatrix(pr2_trans, pr2_rot)
head_trans = [self.raw_head_pose.pose.position.x,
self.raw_head_pose.pose.position.y,
self.raw_head_pose.pose.position.z]
head_rot = [self.raw_head_pose.pose.orientation.x,
self.raw_head_pose.pose.orientation.y,
self.raw_head_pose.pose.orientation.z,
self.raw_head_pose.pose.orientation.w]
world_B_head = createBMatrix(head_trans, head_rot)
pr2_B_head = world_B_pr2.I*world_B_head
pos, ori = Bmat_to_pos_quat(pr2_B_head)
psm = PoseStamped()
psm.header.frame_id = '/base_link'
psm.pose.position.x = pos[0]
psm.pose.position.y = pos[1]
psm.pose.position.z = pos[2]
psm.pose.orientation.x = ori[0]
psm.pose.orientation.y = ori[1]
psm.pose.orientation.z = ori[2]
psm.pose.orientation.w = ori[3]
self.head_pub(psm)
def head_frame_cb(self, data):
trans = [data.pose.position.x,
data.pose.position.y,
data.pose.position.z]
rot = [data.pose.orientation.x,
data.pose.orientation.y,
data.pose.orientation.z,
data.pose.orientation.w]
self.world_B_head = createBMatrix(trans, rot)
# self.update_relations()
def robot_frame_cb(self, data):
trans = [data.pose.position.x,
data.pose.position.y,
data.pose.position.z]
rot = [data.pose.orientation.x,
data.pose.orientation.y,
data.pose.orientation.z,
data.pose.orientation.w]
self.world_B_robot = createBMatrix(trans, rot)
# self.update_relations()
def reference_cb(self, data):
trans = [data.pose.position.x,
data.pose.position.y,
data.pose.position.z]
rot = [data.pose.orientation.x,
data.pose.orientation.y,
data.pose.orientation.z,
data.pose.orientation.w]
self.world_B_ref_model = createBMatrix(trans, rot)
def get_head_pose(self, head_frame="/head_frame"):
if self.mode == 'manual' or True:
return self.head_pose
else:
try:
now = rospy.Time.now()
self.tfl.waitForTransform("/base_link", head_frame, now, rospy.Duration(15))
pos, quat = self.tfl.lookupTransform("/base_link", head_frame, now)
head_pose = PoseStamped()
head_pose.header.frame_id = "/base_link"
head_pose.header.stamp = now
head_pose.pose.position = Point(*pos)
head_pose.pose.orientation = Quaternion(*quat)
return head_pose
except Exception as e:
rospy.loginfo("TF Exception:\r\n%s" %e)
return None
class Person_Follow(object):
def __init__(self):
rospy.init_node('person_follow')
self.twist=Twist()
self.twist.linear.x = 0; self.twist.linear.y = 0; self.twist.linear.z = 0
self.twist.angular.x = 0; self.twist.angular.y = 0; self.twist.angular.z = 0
self.dist0=0
self.target=1
self.p_angle=.01
self.i_angle=.0005
self.p_d=1
self.i_d=.005
self.error_angle_sum=0
self.error_d_sum=0
self.centroid=Point(x=0,y=0)
self.header_msg=Header(stamp=rospy.Time.now(), frame_id='base_link')
def Find_Pos(self, Data):
d={}
for i in range(len(Data.ranges)):
if(Data.ranges[i]!=0):
d[i]=Data.ranges[i]
d_sum_x=0
d_sum_y=0
d_count=0
for key in d:
if key>345 or key<15:
d_sum_x+=d[key]*math.cos(math.radians(key))
d_sum_y+=d[key]*math.sin(math.radians(key))
d_count+=1
self.dist0=Data.ranges[0]
d_avg_x=d_sum_x/d_count
d_avg_y=d_sum_y/d_count
self.centroid.x = d_avg_x
self.centroid.y = d_avg_y
# print self.centroid.x
def run(self):
rospy.Subscriber("/scan",LaserScan,self.Find_Pos,queue_size=10)
self.pub = rospy.Publisher('cmd_vel',Twist,queue_size=10)
self.t=TransformListener()
self.pub_centroid = rospy.Publisher('/centroid',PointStamped,queue_size=10)
r = rospy.Rate(30)
while not rospy.is_shutdown():
theta=0
if self.centroid.x!=0:
theta = 3*math.atan2(self.centroid.y,self.centroid.x)
print theta
self.twist.angular.z = theta
print self.centroid.x
if self.dist0 == 0:
speed = 1
else:
speed = .1 *(self.dist0-.5)
self.twist.linear.x=speed
self.pub.publish(self.twist)
#print self.t.allFramesAsString()
try:
point_stamped_msg = PointStamped(header=self.header_msg,
point=self.t.waitForTransform('odom','base_link',target.header.stamp, rospy.Duration(0.5))Point(x=-self.centroid.x, y=-self.centroid.y))
self.header_msg.stamp = rospy.Time.now()
#print self.header_msg
self.t.waitForTransform('odom','base_link',self.header_msg.stamp, rospy.Duration(0.5))
point_stamped_msg_odom = self.t.transformPoint('/odom',point_stamped_msg)
#print point_stamped_msg_odom
self.pub_centroid.publish(point_stamped_msg_odom)
except Exception as inst:
print inst
