def larm():
names = ['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll']
pub = rospy.Publisher('/joint_angles', JointAnglesWithSpeed, queue_size=1)
rospy.init_node('nao_larm', anonymous=True)
rate = rospy.Rate(10) # 10hz
with open(filename, 'rU') as csvfile:
csvreader = csv.reader(csvfile)
for row in csvreader:
cols = []
angles = []
for col in row:
cols.append(col)
for i in range(2, 6):
angles.append(float(cols[i]))
print names, angles
msg = JointAnglesWithSpeed()
msg.joint_names = names
msg.joint_angles = angles
msg.speed = 1.0
msg.header.frame_id = 'Fixed Frame'
pub.publish(msg)
rate.sleep()
def callback_waving(self, location):
'''
this function takes the information that waving has been detected and decides whether Pepper has to turn his head
(when the waving is not in the center of view)
or if he publishes the currect head
'''
a,b=location.data.split(" ")
self.counter2 = 0
locationlist=[int(a),int(b)]
if locationlist[0] < 0 and locationlist[1] < 0:
newpos= abs(locationlist[0]-locationlist[1])
if newpos > 10:
newpos = 10
msg = JointAnglesWithSpeed()
msg.joint_names=["HeadYaw"]
msg.joint_angles=[newpos*almath.TO_RAD]
msg.speed=0.05
msg.relative=1
self.joint_angles_pub.publish(msg)
elif locationlist[0] > 0 and locationlist[1] > 0:
newpos= abs(locationlist[0]-locationlist[1])
if newpos > 10:
newpos = 10
msg = JointAnglesWithSpeed()
msg.joint_names=["HeadYaw"]
msg.joint_angles=[-newpos*almath.TO_RAD]
msg.speed=0.05
msg.relative=1
self.joint_angles_pub.publish(msg)
else:
self.counter = self.counter +1
#this is supposed to tell you that a waving person has been found in the direction he's looking into
#the numbers at the end of the sent String are supposed to tell you the current angle of Peppers head, not the direction he's supposed to turn to
#it uses a type of costum message that hasn't been defined yet
msg = PositionCommand()
msg.command= "go"
msg.location= "waving " + str(self.position)
self.go_to_pub.publish(msg)
def publish_joints(self, names, angles):
m = JointAnglesWithSpeed()
m.header.stamp = rospy.Time.now()
m.joint_names = names
m.joint_angles = angles
m.relative = 0
m.speed = 1
self.__pub_joints.publish(m)
def stand(self):
print "STANDING"
self.__call_service(
"/naoqi_driver/robot_posture/go_to_posture", GoToPosture,
GoToPostureRequest(GoToPostureRequest.STAND_INIT, 0.5))
j = JointAnglesWithSpeed()
j.joint_names = ['HeadYaw', 'HeadPitch']
j.joint_angles = [0., -.5]
j.speed = .05
self.joints.publish(j)
def idle(self):
j = JointAnglesWithSpeed()
j.joint_names = ['HeadYaw', 'HeadPitch']
j.joint_angles = [.5, -.5]
j.speed = .05
pub = rospy.Publisher("/joint_angles",
JointAnglesWithSpeed,
queue_size=10)
while not rospy.is_shutdown() and self.is_idle:
try:
rospy.wait_for_message("/naoqi_driver_node/people_detected",
PersonDetectedArray,
timeout=5.)
except rospy.ROSException, rospy.ROSInterruptException:
# j.joint_angles[0] *= -1.
# j.joint_angles[1] = np.random.rand()-.5
j.joint_angles = [
np.random.rand() - .5, -(np.random.rand() * .6)
]
pub.publish(j)
