def __init__(self, program_file=PROGRAM_FILE):
# TODO: Either implement behavior that fixes programs when markers change
# or only let this callback run once
self._markers_sub = rospy.Subscriber(SUB_NAME,
Marker,
callback=self._markers_callback)
self._curr_markers = None
self._tf_listener = tf.TransformListener()
self._arm = fetch_api.Arm()
self._gripper = fetch_api.Gripper()
self._torso = fetch_api.Torso()
self._controller_client = actionlib.SimpleActionClient(
'/query_controller_states', QueryControllerStatesAction)
self._program_file = program_file
self._programs = self._read_in_programs()
self._joint_reader = JointStateReader()
mat = tft.identity_matrix()
mat[:, 0] = np.array([0, 0, -1, 0])
mat[:, 2] = np.array([1, 0, 0, 0])
o = tft.quaternion_from_matrix(mat)
self._constraint_pose = Pose(orientation=Quaternion(*o))
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'gripper_link'
oc.orientation = self._constraint_pose.orientation
oc.weight = 1.0
oc.absolute_z_axis_tolerance = 1.0
oc.absolute_x_axis_tolerance = 1.0
oc.absolute_y_axis_tolerance = 1.0
self._constraint = None
def main():
rospy.init_node('joint_state_republisher')
wait_for_time()
torso_pub = rospy.Publisher('joint_state_republisher/torso_lift_joint',
Float64,
queue_size=10)
head_pan_pub = rospy.Publisher('joint_state_republisher/head_pan_joint',
Float64,
queue_size=10)
head_tilt_pub = rospy.Publisher('joint_state_republisher/head_tilt_joint',
Float64,
queue_size=10)
reader = JointStateReader()
rospy.sleep(0.5)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
torso_joint_val = reader.get_joint('torso_lift_joint')
head_pan_val = reader.get_joint('head_pan_joint')
head_tilt_val = reader.get_joint('head_tilt_joint')
torso_pub.publish(torso_joint_val)
head_pan_pub.publish(head_pan_val)
head_tilt_pub.publish(head_tilt_val)
rate.sleep()
def turn_arm(angle):
reader = JointStateReader()
rospy.sleep(0.5)
names = fetch_api.ArmJoints.names()
arm_vals = reader.get_joints(names)
pprint(arm_vals)
arm_vals[5] = arm_vals[5] + angle
pprint(arm_vals)
arm.move_to_joints(fetch_api.ArmJoints.from_list(arm_vals))
def main():
rospy.init_node('joint_reader_demo')
wait_for_time()
argv = rospy.myargv()
reader = JointStateReader()
rospy.sleep(0.5)
names = fetch_api.ArmJoints.names()
arm_vals = reader.get_joints(names)
for k, v in zip(names, arm_vals):
print '{}\t{}'.format(k, v)
def main():
rospy.init_node('joint_reader_demo')
wait_for_time()
argv = rospy.myargv()
reader = JointStateReader()
rospy.sleep(0.5)
# Fetch Only
names = robot_api.ArmJoints.names()
# Kuri: Browse joints and initialize your own names list
# names = []
arm_vals = reader.get_joints(names)
for k, v in zip(names, arm_vals):
print '{}\t{}'.format(k, v)
def __init__(self, js=None, head_ns=None, eyes_ns=None):
self._js = js
self._tf = TransformListener()
self._head_gh = None
self._head_goal = None
self._head_ac = actionlib.ActionClient(head_ns or self.HEAD_NS, FollowJointTrajectoryAction)
self._eyes_gh = None
self._eyes_goal = None
self._eyes_ac = actionlib.ActionClient(eyes_ns or self.EYES_NS, FollowJointTrajectoryAction)
self.jointReader = JointStateReader()
self.base = Base()
rospy.sleep(0.5)
return
def main():
rospy.init_node('joint_state_republisher')
wait_for_time()
torso_pub = rospy.Publisher('joint_state_republisher/torso_lift_joint',
Float64)
reader = JointStateReader()
rospy.sleep(0.5)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# TODO: get torso joint value
# TODO: publish torso joint value
torso_pub.publish(reader.get_joint(JOINT_NAME))
rate.sleep()
def main():
rospy.init_node('joint_state_republisher')
wait_for_time()
torso_pub = rospy.Publisher('joint_state_republisher/torso_lift_joint',
Float64)
reader = JointStateReader()
rospy.sleep(0.5)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
torso_joint = reader.get_joint('torso_lift_joint')
if torso_joint is not None:
torso_pub.publish(Float64(data=torso_joint))
rate.sleep()
def main():
rospy.init_node('joint_reader_demo')
wait_for_time()
argv = rospy.myargv()
reader = JointStateReader()
rospy.sleep(0.5)
names = fetch_api.ArmJoints.names()
arm_vals = reader.get_joints(names)
for k, v in zip(names, arm_vals):
print '{}\t{}'.format(k, v)
other_joints = ['torso_lift_joint', 'l_gripper_finger_joint', 'r_gripper_finger_joint']
for j in other_joints:
val = reader.get_joint(j)
print '{}\t{}'.format(j, val)
def main():
rospy.init_node('joint_state_republisher')
wait_for_time()
torso_pub = rospy.Publisher('joint_state_republisher/torso_lift_joint',
Float64,
queue_size=10)
reader = JointStateReader()
rospy.sleep(0.5)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# TODO: get torso joint value
torso = reader.get_joints(['torso_lift_joint'])
# TODO: publish torso joint value
torso_pub.publish(torso[0])
rate.sleep()
def main():
rospy.init_node('joint_reader_demo')
wait_for_time()
argv = rospy.myargv()
reader = JointStateReader()
rospy.sleep(0.5)
names = fetch_api.ArmJoints.names()
arm_vals = reader.get_joints(names)
for k, v in zip(names, arm_vals):
print '{}\t{}'.format(k, v)
armList = ['shoulder_pan_joint','shoulder_lift_joint','upperarm_roll_joint','elbow_flex_joint','forearm_roll_joint','wrist_flex_joint','wrist_roll_joint']
value = reader.get_joints(armList)
# print value
# value = reader.get_joint('l_gripper_finger_joint')
print value
def main():
rospy.init_node('joint_state_republisher')
wait_for_time()
torso_pubs = []
reader = JointStateReader()
for joint in reader.jointname:
torso_pubs.append(
rospy.Publisher('joint_state_republisher/' + joint,
Float64,
queue_size=10))
rospy.sleep(0.5)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
for i in range(len(torso_pubs)):
joint_state = reader.get_joint(reader.jointname[i])
torso_pubs[i].publish(joint_state)
rate.sleep()
class ProgramController(object):
def __init__(self, program_file=PROGRAM_FILE):
# TODO: Either implement behavior that fixes programs when markers change
# or only let this callback run once
self._markers_sub = rospy.Subscriber(SUB_NAME,
Marker,
callback=self._markers_callback)
self._curr_markers = None
self._tf_listener = tf.TransformListener()
self._arm = fetch_api.Arm()
self._gripper = fetch_api.Gripper()
self._torso = fetch_api.Torso()
self._controller_client = actionlib.SimpleActionClient(
'/query_controller_states', QueryControllerStatesAction)
self._program_file = program_file
self._programs = self._read_in_programs()
self._joint_reader = JointStateReader()
mat = tft.identity_matrix()
mat[:, 0] = np.array([0, 0, -1, 0])
mat[:, 2] = np.array([1, 0, 0, 0])
o = tft.quaternion_from_matrix(mat)
self._constraint_pose = Pose(orientation=Quaternion(*o))
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'gripper_link'
oc.orientation = self._constraint_pose.orientation
oc.weight = 1.0
oc.absolute_z_axis_tolerance = 1.0
oc.absolute_x_axis_tolerance = 1.0
oc.absolute_y_axis_tolerance = 1.0
self._constraint = None
def __str__(self):
if self._programs:
return "Programs:\n" + "\n".join([
"{}:\n{}".format(name, program)
for name, program in self._programs.items()
])
else:
return "No programs"
def _read_in_programs(self):
try:
with open(self._program_file, 'rb') as file:
p = pickle.load(file)
return p
except IOError:
print 'IOError on {}'.format(self._program_file)
return {}
def _write_out_programs(self):
with open(self._program_file, 'wb') as file:
pickle.dump(self._programs, file)
def _markers_callback(self, msg):
if msg.ns == "tray handle":
self._curr_markers = msg
def remove_step(self, program_name, index):
self._programs[program_name].remove_step(index)
self._write_out_programs()
def relax_arm(self):
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.STOPPED
goal.updates.append(state)
self._controller_client.send_goal(goal)
self._controller_client.wait_for_result()
def start_arm(self):
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.RUNNING
goal.updates.append(state)
self._controller_client.send_goal(goal)
self._controller_client.wait_for_result()
def close(self):
self._gripper.close()
def open(self):
self._gripper.open()
def save_all_joints(self, program_name, index=None):
print "Saving all joints state as a program step"
# need to grab the program as is
curr_program = self._programs.get(program_name)
if curr_program is None:
print("{} does not exist yet".format(program_name))
return
step = ProgramStep()
step.step_type = ProgramStep.MOVE_ALL_JOINTS
step.all_joint_states = self._joint_reader.get_joints(
fetch_api.ArmJoints.names())
curr_program.add_step(step, index)
self._write_out_programs()
def set_constraint(self, program_name, index, has_constraint):
print "setting has_constraint to {} for step at index {} for program {}".format(
has_constraint, index, program_name)
curr_program = self._programs.get(program_name)
if curr_program is None:
print("{} does not exist yet".format(program_name))
return
curr_program.steps[index].has_constraint = has_constraint
def save_joint(self, program_name, joint_name, index=None):
print "Saving next joint state for program {} with name {}".format(
program_name, joint_name)
# need to grab the program as is
curr_program = self._programs.get(program_name)
if curr_program is None:
print("{} does not exist yet".format(program_name))
return
step = ProgramStep()
step.step_type = ProgramStep.MOVE_JOINT
step.joint_name = joint_name
# get current joint state value for joint name, if it exists
joint_state = self._joint_reader.get_joints(
fetch_api.ArmJoints.names())
for i, name in enumerate(fetch_api.ArmJoints.names()):
if step.joint_name == name:
joint_value = joint_state[i]
if joint_value is None:
print "{} is not a value joint name. Try any of these {}".format(
joint_name, fetch_api.ArmJoints.names())
return
step.joint_value = joint_value
curr_program.add_step(step, index)
self._write_out_programs()
def add_torso(self, program_name, index=None):
print "Saving next torso state for program {}".format(program_name)
# need to grab the program as is
curr_program = self._programs.get(program_name)
if curr_program is None:
print("{} does not exist yet".format(program_name))
return
step = ProgramStep()
step.step_type = ProgramStep.MOVE_TORSO
step.torso_height = self._torso.state()
curr_program.add_step(step, index)
self._write_out_programs()
def save_gripper(self, program_name, index=None):
print "Saving gripper state for program {}".format(program_name)
# need to grab the program as is
curr_program = self._programs.get(program_name)
if curr_program is None:
print("{} does not exist yet".format(program_name))
return
step = ProgramStep()
step.step_type = ProgramStep.MOVE_GRIPPER
step.gripper_state = self._gripper.state()
curr_program.add_step(step, index)
self._write_out_programs()
# TODO: gripper status could be used here
def save_program(self,
program_name,
frame_id,
index=None,
has_constraint=False):
print "Saving next position for program {} in {}".format(
program_name, frame_id)
# need to grab the program as is
curr_program = self._programs.get(program_name)
if curr_program is None:
print("{} does not exist yet".format(program_name))
return
# TODO: Complete this
new_pose = PoseStamped()
if frame_id in ID_TO_TAGNAME:
# we know the user is trying to define a pose relative to a tag
# we need to do some transformation stuff
# need to grab the marker from self._curr_markers that matches frame_id
marker_id = ID_TO_TAGNAME[frame_id]
curr_marker = None
if marker_id != self._curr_markers.ns:
print(
'No marker found with frame_id {} and marker_id {} in program {}'
.format(frame_id, marker_id, program_name))
return
else:
# now we have the marker (curr_marker) which has some pose and we can get the robots current position for its arm
# we have to then transform that current position to be relative to the curr_marker.pose
# get position and orientation of the gripper in the base link
position, orientation = self._tf_listener.lookupTransform(
'/base_link', '/gripper_link', rospy.Time(0))
base_link_T_wrist = Pose(Point(*position),
Quaternion(*orientation))
# The transformation from the base_link to this tags frame is just the tags pose
base_link_T_tags = copy.deepcopy(self._curr_markers.pose)
tags_T_wrist = fetch_api.transform(
fetch_api.inverse_pose(base_link_T_tags),
base_link_T_wrist)
new_pose = PoseStamped(pose=tags_T_wrist)
new_pose.header.frame_id = frame_id
else:
# user is trying to define frame_id in some arbitrary frame
# assume base_link for now
if frame_id == 'base_link':
position, orientation = self._tf_listener.lookupTransform(
'/base_link', '/gripper_link', rospy.Time(0))
base_link_T_wrist = Pose(Point(*position),
Quaternion(*orientation))
new_pose = PoseStamped(pose=base_link_T_wrist)
new_pose.header.frame_id = frame_id
else:
print 'Please use base_link'
return
step = ProgramStep()
step.pose = new_pose
step.gripper_state = self._gripper.state()
step.torso_height = self._torso.state()
step.has_constraint = has_constraint
curr_program.add_step(step, index)
self._write_out_programs()
def rename_program(self, program_name, new_program_name):
if program_name not in self._programs:
print '{} does not exist'.format(program_name)
return
if new_program_name in self._programs:
print '{} already exists'.format(new_program_name)
return
self._programs[new_program_name] = self._programs[program_name]
del self._programs[program_name]
def deque_step(self, program_name):
if program_name not in self._programs:
print '{} does not exist'.format(program_name)
return
self._programs[program_name].remove_step(
len(self._programs[program_name].steps) - 1)
self._write_out_programs()
def create_program(self, program_name):
if program_name not in self._programs:
self._programs[program_name] = Program()
print "{} created".format(program_name)
print "the program is {}".format(self._programs[program_name])
else:
print "Trying to create program {} when it already exists".format(
program_name)
self._write_out_programs()
def delete_program(self, program_name):
if program_name in self._programs:
del self._programs[program_name]
print "{} deleted".format(program_name)
self._write_out_programs()
else:
print "{} does not exist".format(program_name)
def run_program(self, program_name):
if program_name not in self._programs:
print "{} does not exist".format(program_name)
return False
else:
self.start_arm()
curr_marker = copy.deepcopy(self._curr_markers)
print 'PRINTING CURR MARKER'
print curr_marker
for i, cur_step in enumerate(self._programs[program_name].steps):
if cur_step.step_type == ProgramStep.MOVE_ARM:
# if cur_step.gripper_state != self._gripper.state():
# if cur_step.gripper_state == fetch_api.Gripper.OPENED:
# self._gripper.open(max_effort=0.7)
# else:
# self._gripper.close(max_effort=0.7)
# # don't move the arm if we move the gripper
# print 'adjust gripper successful'
# continue
pose = cur_step.calc_pose(curr_marker)
if pose is None:
return False
if cur_step.has_constraint:
error = self._arm.move_to_pose(
pose,
orientation_constraint=self._constraint,
allowed_planning_time=25.0,
num_planning_attempts=3,
replan=True)
else:
error = self._arm.move_to_pose(
pose,
allowed_planning_time=25.0,
num_planning_attempts=3,
replan=True)
if error is not None:
print "move arm failed with error {}".format(error)
print "{} failed to run at step #{}".format(
program_name, i + 1)
return False
else:
print 'move arm successful'
# moving a joint
if cur_step.step_type == ProgramStep.MOVE_JOINT:
# grab the current joint state and update the value from
# this program step
joint_state = self._joint_reader.get_joints(
fetch_api.ArmJoints.names())
for i, name in enumerate(fetch_api.ArmJoints.names()):
if cur_step.joint_name == name:
joint_state[i] = cur_step.joint_value
# grab an arm joint object from this list of joint positions
arm_joints = fetch_api.ArmJoints.from_list(joint_state)
res = self._arm.move_to_joints(arm_joints)
if res.error_code != FollowJointTrajectoryResult.SUCCESSFUL:
print 'joint adjustment failed with error code {} and error string {}'.format(
res.error_code, res.error_string)
print "{} failed to run at step #{}".format(
program_name, i + 1)
return False
else:
print 'joint adjustment successful'
# moving all joints
if cur_step.step_type == ProgramStep.MOVE_ALL_JOINTS:
arm_joints = fetch_api.ArmJoints.from_list(
cur_step.all_joint_states)
res = self._arm.move_to_joints(arm_joints)
if res.error_code != FollowJointTrajectoryResult.SUCCESSFUL:
print 'all joints adjustment failed with error code {} and error string {}'.format(
res.error_code, res.error_string)
print "{} failed to run at step #{}".format(
program_name, i + 1)
return False
else:
print 'all joints adjustment successful'
# move torso
if cur_step.step_type == ProgramStep.MOVE_TORSO:
res = self._torso.set_height(cur_step.torso_height)
if res.error_code != FollowJointTrajectoryResult.SUCCESSFUL:
print 'torso adjustment failed with error code {} and error string {}'.format(
res.error_code, res.error_string)
print "{} failed to run at step #{}".format(
program_name, i + 1)
return False
else:
print 'torso adjustment successful'
if cur_step.step_type == ProgramStep.MOVE_GRIPPER:
if cur_step.gripper_state == fetch_api.Gripper.OPENED:
self._gripper.open(max_effort=75, position=0.08)
else:
self._gripper.close(max_effort=75)
print 'gripper adjustment successful'
rospy.sleep(1.5)
return True
@property
def programs(self):
return self._programs
class Head(object):
JOINT_PAN = 'head_1_joint'
JOINT_TILT = 'head_2_joint'
JOINT_EYES = 'eyelids_joint'
JOINT_HEIGHT = 0.405
PAN_LEFT = 0.78
PAN_NEUTRAL = 0
PAN_RIGHT = -PAN_LEFT
TILT_UP = -0.92
TILT_NEUTRAL = 0.0
TILT_DOWN = 0.29
EYES_OPEN = 0.0
EYES_NEUTRAL = 0.1
EYES_CLOSED = 0.41
EYES_HAPPY = -0.16
EYES_SUPER_SAD = 0.15
EYES_CLOSED_BLINK = 0.35
HEAD_NS = '/head_controller/follow_joint_trajectory' # found on real robot, previously thought to use /command
EYES_NS = '/eyelids_controller/follow_joint_trajectory'
EYES_FRAME = "/head_2_link"
BASE_FRAME = "/base_link"
# JS was set to none to try to get demo to work, probably shouldn't be
# Js could stand for something like joint state controller or joint system
def __init__(self, js=None, head_ns=None, eyes_ns=None):
self._js = js
self._tf = TransformListener()
self._head_gh = None
self._head_goal = None
self._head_ac = actionlib.ActionClient(head_ns or self.HEAD_NS, FollowJointTrajectoryAction)
self._eyes_gh = None
self._eyes_goal = None
self._eyes_ac = actionlib.ActionClient(eyes_ns or self.EYES_NS, FollowJointTrajectoryAction)
self.jointReader = JointStateReader()
self.base = Base()
rospy.sleep(0.5)
return
def cancel(self):
head_gh = self._head_gh
eyes_gh = self._eyes_gh
if head_gh:
head_gh.cancel()
self._head_goal = None
self._head_gh = None
if eyes_gh:
eyes_gh.cancel()
self._eyes_goal = None
self._eyes_gh = None
return
def eyes_to(self, radians, duration=1.0, feedback_cb=None, done_cb=None):
"""
Moves the robot's eye lids to the specified location in the duration
specified
:param radians: The eye position. Expected to be between
HeadClient.EYES_HAPPY and HeadClient.EYES_CLOSED
:param duration: The amount of time to take to get the eyes to
the specified location.
:param feedback_cb: Same as send_trajectory's feedback_cb
:param done_cb: Same as send_trajectory's done_cb
"""
if(radians < self.EYES_HAPPY or radians > self.EYES_CLOSED):
return
point = JointTrajectoryPoint()
point.positions.append(float(radians))
point.time_from_start = rospy.Duration(duration)
trajectory = JointTrajectory()
trajectory.points.append(point)
trajectory.joint_names.append(self.JOINT_EYES)
return self.send_trajectory(traj=trajectory, feedback_cb=feedback_cb, done_cb=done_cb)
def is_done(self):
active = {
GoalStatus.PENDING, GoalStatus.RECALLING,
GoalStatus.ACTIVE, GoalStatus.PREEMPTING}
if self._head_gh:
if self._head_gh.get_goal_status() in active:
return False
if self._eyes_gh:
if self._eyes_gh.get_goal_status() in active:
return False
return True
def pan_and_tilt(self, pan, tilt, duration=1.0, feedback_cb=None, done_cb=None):
"""
Moves the robot's head to the point specified in the duration
specified
:param pan: The pan - expected to be between HeadClient.PAN_LEFT
and HeadClient.PAN_RIGHT
:param tilt: The tilt - expected to be between HeadClient.TILT_UP
and HeadClient.TILT_DOWN
:param duration: The amount of time to take to get the head to
the specified location.
:param feedback_cb: Same as send_trajectory's feedback_cb
:param done_cb: Same as send_trajectory's done_cb
"""
if(pan < self.PAN_RIGHT or pan > self.PAN_LEFT):
return
if(tilt > self.TILT_DOWN or tilt < self.TILT_UP):
return
point = JointTrajectoryPoint()
point.positions = [float(pan), float(tilt)]
point.time_from_start = rospy.Duration(duration)
trajectory = JointTrajectory()
trajectory.points = [point]
trajectory.joint_names = [self.JOINT_PAN, self.JOINT_TILT]
return self.send_trajectory(traj=trajectory, feedback_cb=feedback_cb, done_cb=done_cb)
def send_trajectory(self, traj, feedback_cb=None, done_cb=None):
"""
Sends the specified trajectories to the head and eye controllers
:param traj: A trajectory_msgs.msg.JointTrajectory. joint_names
are expected to match HeadClient.JOINT_PAN, JOINT_TILT and JOINT_EYES
:param feedback_cb: A callable that takes one parameter - the feedback
:param done_cb: A callable that takes two parameters - the goal status
the goal handle result
"""
for point in traj.points:
for k in ('velocities', 'accelerations', 'effort'):
if getattr(point, k) is None:
setattr(point, k, [])
if isinstance(point.time_from_start, (int, float)):
point.time_from_start = rospy.Duration(point.time_from_start)
goal = FollowJointTrajectoryGoal(trajectory=traj)
def _handle_transition(gh):
gh_goal = gh.comm_state_machine.action_goal.goal
if done_cb is not None and (id(self._eyes_goal) == id(gh_waitForTransformgoal) or id(self._head_goal) == id(gh_goal)):
if gh.get_comm_state() == CommState.DONE:
waitForTransform
if (id(self._head_goal) == id(gh_goal)):
waitForTransform
self.currentPan = self._head_goal.trajectory.waitForTransformpoints[0].positions[0]
self.currentTilt = self._head_goal.trajectory.points[0].positions[1]
done_cb(gh.get_goal_status(), gh.get_result())
return
def _handle_feedback(gh, feedback):
gh_goal = gh.comm_state_machine.action_goal.goal
if feedback_cb is not None and (id(self._eyes_goal) == id(gh_goal) or id(self._head_goal) == id(gh_goal)):
feedback_cb(feedback)
return
if self.JOINT_EYES in traj.joint_names:
if not self._eyes_ac:
return False
self._eyes_goal = goal
self._eyes_ac.wait_for_server()
self._eyes_gh = self._eyes_ac.send_goal(goal, _handle_transition, _handle_feedback)
else:
if not self._head_ac:
return False
self._head_goal = goal
self._head_ac.wait_for_server()
self._head_gh = self._head_ac.send_goal(goal, _handle_transition, _handle_feedback)
return True
# Gets an equivalent point in the reference frame "destFrame"
def getPointInFrame(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).point
# Move body to face this point (does not change head joints)
def point_base_at(self, pointStamped, fullBody = False):
# Transform the point to the base frame
transformedPoint = self.getPointInFrame(pointStamped, self.BASE_FRAME)
# Get the radians we need to rotate the base in order to point correctly
rotation_rads = atan2(transformedPoint.y, transformedPoint.x)
# Do the rotation
ROTATION_SPEED = 0.5
rospy.loginfo(rotation_rads)
self.base.turn(rotation_rads)
rospy.sleep(ceil(rotation_rads / ROTATION_SPEED)) # Wait for rotation to complete
# Move head to look at a point; move the body if point if out of range of head (and fullBody = true)
# Returns false if point is out of range, true if successful
def look_at(self, pointStamped, fullBody = False):
# Transform the point to the eye frame
eyeFramePoint = self.getPointInFrame(pointStamped, self.EYES_FRAME)
# Get the radians we need to rotate the head
pan_rads = self.jointReader.get_joint(self.JOINT_PAN) + atan2(eyeFramePoint.y, eyeFramePoint.x)
# If the rotation is out of the head's range of rotation
if pan_rads > self.PAN_LEFT or pan_rads < self.PAN_RIGHT:
if not fullBody: return False
# Rotate the body to point toward the point
self.point_base_at(pointStamped)
# Then pan the head to look towards it
eyeFramePoint = self.getPointInFrame(pointStamped, self.EYES_FRAME)
pan_rads = self.jointReader.get_joint(self.JOINT_PAN) + atan2(eyeFramePoint.y, eyeFramePoint.x)
# Get the amount we should tilt the head
tilt_rads = self.jointReader.get_joint(self.JOINT_TILT) - atan2(eyeFramePoint.z, eyeFramePoint.x)
# Return if we aren't allowed to move the body and the tilt is out of range
if (not fullBody) and (tilt_rads < self.TILT_UP or tilt_rads > self.TILT_DOWN): return False
# Otherwise, if the tilt is out of range, move the body backward until the point is in tilt range
while fullBody and (tilt_rads < self.TILT_UP or tilt_rads > self.TILT_DOWN):
self.base.go_forward(-0.1)
rospy.sleep(1) # Wait for movement to complete
eyeFramePoint = self.getPointInFrame(pointStamped, self.EYES_FRAME)
tilt_rads = self.jointReader.get_joint(self.JOINT_TILT) - atan2(eyeFramePoint.z, eyeFramePoint.x)
self.pan_and_tilt(pan_rads, tilt_rads)
return True
def shutdown(self):
self.cancel()
self._head_ac = None
self._eyes_ac = None
return
def wait_for_server(self, timeout=rospy.Duration(0.0)):
return self._head_ac.wait_for_server(timeout) and self._eyes_ac.wait_for_server(timeout)
def wait_for_done(self, timeout):
rate = rospy.Rate(10)
start_time = rospy.Time.now()
while rospy.Time.now() - start_time < rospy.Duration(timeout):
if self.is_done():
return True
rate.sleep()
return False
def main():
rospy.init_node('joint_state_republisher')
wait_for_time()
joint_list = [
'torso_lift_joint', 'head_pan_joint', 'head_tilt_joint',
'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint',
'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint',
'wrist_roll_joint'
]
torso_pub = rospy.Publisher('joint_state_republisher/torso_lift_joint',
Float64)
head_pan_pub = rospy.Publisher('joint_state_republisher/head_pan_joint',
Float64)
head_tilt_pub = rospy.Publisher('joint_state_republisher/head_tilt_joint',
Float64)
shoulder_pan_pub = rospy.Publisher(
'joint_state_republisher/shoulder_pan_joint', Float64)
shoulder_lift_pub = rospy.Publisher(
'joint_state_republisher/shoulder_lift_joint', Float64)
upperarm_roll_pub = rospy.Publisher(
'joint_state_republisher/upperarm_roll_joint', Float64)
elbow_flex_pub = rospy.Publisher(
'joint_state_republisher/elbow_flex_joint', Float64)
forearm_roll_pub = rospy.Publisher(
'joint_state_republisher/forearm_roll_joint', Float64)
wrist_flex_pub = rospy.Publisher(
'joint_state_republisher/wrist_flex_joint', Float64)
wrist_roll_pub = rospy.Publisher(
'joint_state_republisher/wrist_roll_joint', Float64)
reader = JointStateReader()
rospy.sleep(1)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
value = reader.get_joints(joint_list)
message = Float64()
message.data = value[0]
torso_pub.publish(message)
message1 = Float64()
message1.data = value[1]
head_pan_pub.publish(message1)
message2 = Float64()
message2.data = value[2]
head_tilt_pub.publish(message2)
message3 = Float64()
message3.data = value[3]
shoulder_pan_pub.publish(message3)
message4 = Float64()
message4.data = value[4]
shoulder_lift_pub.publish(message4)
message5 = Float64()
message5.data = value[5]
upperarm_roll_pub.publish(message5)
message6 = Float64()
message6.data = value[6]
elbow_flex_pub.publish(message6)
message7 = Float64()
message7.data = value[7]
forearm_roll_pub.publish(message7)
message8 = Float64()
message8.data = value[8]
wrist_flex_pub.publish(message8)
message9 = Float64()
message9.data = value[9]
wrist_roll_pub.publish(message9)
print value
rate.sleep()