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()
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()