#!/usr/bin/env python

import rospy
import smach


class GuideInterface(smach.State):
    def __init__(self, waypointset):
        smach.State.__init__(
            self,
            outcomes=['move_to_point', 'aborted', 'killall'],
            input_keys=['current_waypoint'],
            output_keys=['waypoint']
        )
        self.waypointset = waypointset

    def execute(self, userdata):
        rospy.loginfo("Showing guide interface")
        rospy.sleep(1)
        # Guide interface returning the next waypoint

        waypoints = self.waypointset['slow']
        rospy.loginfo("I am at: " + userdata.current_waypoint)
        next_waypoint = ""
        for elem in waypoints.items():
            if elem[1] == userdata.current_waypoint:
                key = str(int(elem[0])+1)
                if not key in waypoints.keys():
                    key = "1"
                next_waypoint = waypoints[key]

        rospy.loginfo("I will go to: " + next_waypoint)
        userdata.waypoint = next_waypoint
        return 'move_to_point'

@@ -42,16 +42,97 @@
 t smach%0A
+import actionlib%0Afrom aaf_walking_group.msg import InterfaceAction, InterfaceGoal
 %0A%0Aclass 
@@ -439,16 +439,272 @@
 intset%0A%0A
+        rospy.loginfo(%22Creating guide interface client...%22)%0A        self._client = actionlib.SimpleActionClient(%0A            'interface_server',%0A            InterfaceAction%0A        )%0A        self._client.wait_for_server()%0A        rospy.loginfo(%22...done%22)%0A%0A
     def 
@@ -800,16 +800,17 @@
 leep(1)%0A
+%0A
         
@@ -855,17 +855,16 @@
 aypoint%0A
-%0A
         
@@ -1250,16 +1250,493 @@
 s%5Bkey%5D%0A%0A
+        # Getting the next waypoint from guide interface%0A        rospy.loginfo(%22Opening the guide interface...%22)%0A        goal = InterfaceGoal()%0A        goal.possible_points = waypoints%0A        goal.next_point = next_waypoint%0A        rospy.loginfo(%22Sending a goal to interface server...%22)%0A        self._client.send_goal_and_wait(goal)%0A        result = self._client.get_result()%0A        rospy.loginfo(%22Got the chosen next waypoint.%22)%0A        next_waypoint = result.chosen_point%0A%0A
         

add action server to GuideInterface class