merge: pull request #123 from fireblonde/go-to-skills

Go to skills

skills/config/demo.yaml

@@ -0,0 +1,5 @@
+living_room:
+  table:
+    location:
+      position: {x: 1.526343, y: 0.523707, z: 0.0024719}
+      orientation: { x: 0.0, y: 0.0, z: 0.746444684543, w: 0.665447468188 }

skills/launch/demo.launch

@@ -0,0 +1,6 @@
+<launch>
+    <arg name="config" default="demo"/>
+    <rosparam command="load" file="$(find lasr_skills)/config/$(arg config).yaml" />
+    <node pkg="lasr_skills" type="go_to_location.py" name="go_to_location" output="screen" />
+<!--    <node pkg="lasr_skills" type="go_to_semantic_location.py" name="go_to_semantic_location" output="screen" />-->
+</launch>

skills/src/lasr_skills/__init__.py

@@ -6,4 +6,6 @@
 from .detect_people_in_area_3d import DetectPeopleInArea3D
 from .wait_for_person import WaitForPerson
 from .wait_for_person_in_area import WaitForPersonInArea
-from .look_to_point import LookToPoint
+from .look_to_point import LookToPoint
+from .go_to_location import GoToLocation
+from .go_to_semantic_location import GoToSemanticLocation

skills/src/lasr_skills/go_to_location.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python3
+
+import rospy
+import smach
+from tiago_controllers.controllers import Controllers
+from geometry_msgs.msg import Point, Quaternion, Pose
+
+class GoToLocation(smach.State):
+
+    def __init__(self):
+        smach.State.__init__(self, outcomes=['succeeded', 'failed'], input_keys=['location'])
+        self.controllers = Controllers()
+
+    def execute(self, userdata):
+        try:
+            status = self.controllers.base_controller.sync_to_pose(userdata.location)
+            if status:
+                return 'succeeded'
+            return 'failed'
+        except rospy.ERROR as e:
+            rospy.logwarn(f"Unable to go to location. {userdata.location} -> ({str(e)})")
+            return 'failed'
+
+if __name__ == '__main__':
+    rospy.init_node('go_to_location')
+    sm = smach.StateMachine(outcomes=['succeeded', 'failed'])
+    loc = rospy.get_param('/living_room/table/location')
+    sm.userdata.location = Pose(position=Point(**loc['position']), orientation=Quaternion(**loc['orientation']))
+    with sm:
+        smach.StateMachine.add('GoToLocation', GoToLocation(), transitions={'succeeded': 'succeeded', 'failed': 'failed'})
+    sm.execute()

skills/src/lasr_skills/go_to_semantic_location.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python3
+
+import rospy
+import smach
+from tiago_controllers.controllers import Controllers
+from geometry_msgs.msg import Point, Quaternion, Pose
+
+class GoToSemanticLocation(smach.State):
+
+    def __init__(self):
+        smach.State.__init__(self, outcomes=['succeeded', 'failed'], input_keys=['location'])
+        self.controllers = Controllers()
+
+    def execute(self, userdata):
+        loc = rospy.get_param(f"{userdata.location}/location")
+        try:
+            status = self.controllers.base_controller.sync_to_pose(Pose(position=Point(**loc['position']), orientation=Quaternion(**loc['orientation'])))
+            if status:
+                return 'succeeded'
+            return 'failed'
+        except rospy.ERROR as e:
+            rospy.logwarn(f"Unable to go to location. {loc} -> ({str(e)})")
+            return 'failed'
+if __name__ == '__main__':
+    rospy.init_node('go_to_semantic_location')
+    sm = smach.StateMachine(outcomes=['succeeded', 'failed'])
+    sm.userdata.location = '/living_room/table'
+    with sm:
+        smach.StateMachine.add('GoToSemanticLocation', GoToSemanticLocation(), transitions={'succeeded': 'succeeded', 'failed': 'failed'})
+    sm.execute()