#!/usr/bin/env python

from flexbe_core import EventState, Logger
import rospy
from std_msgs.msg import Float64


class SaraSetHeadAngle(EventState):
    """
    Make sara change the angle her the head

    -- angle     Float64      angle of the head

    <= done                what's said is said
    """


    def __init__(self, angle):
        """Constructor"""

        super(SaraSetHeadAngle, self).__init__(outcomes = ['done'])
        self.angle = angle

    def execute(self, userdata):

        Logger.loginfo('Setting head angle to '+str(self.angle))
        pub = rospy.Publisher("/sara_head_pitch_controller/command", Float64, queue_size=1)
        ms = Float64()
        ms.data = self.angle
        pub.publish(ms)

        Logger.loginfo('Publishing done')

        return 'done'
@@ -325,21 +325,26 @@
 _(self, 
-angle
+pitch, yaw
 ):%0A     
@@ -450,21 +450,44 @@
 elf.
-angle = angle
+pitch = pitch%0A        self.yaw = yaw
 %0A%0A  
@@ -518,16 +518,39 @@
 data):%0A%0A
+        ms = Float64()%0A
         
@@ -578,21 +578,84 @@
 ng head 
-angle
+pitch to '+str(self.pitch))%0A        Logger.loginfo('Setting head yaw
  to '+st
@@ -661,21 +661,19 @@
 tr(self.
-angle
+yaw
 ))%0A     
@@ -765,35 +765,155 @@
 )%0A        ms
- = Float64(
+.data = self.pitch%0A        pub.publish(ms)%0A        pub = rospy.Publisher(%22/sara_head_yaw_controller/command%22, Float64, queue_size=1
 )%0A        ms
@@ -925,21 +925,19 @@
  = self.
-angle
+yaw
 %0A       
@@ -1014,12 +1014,13 @@
 eturn 'done'
+%0A

ajoute le yaw à la tête