feat(GPSR): object properties (count/weight/size) (#151)

Co-authored-by: Jared Swift <j.w.swift@outlook.com>

tasks/gpsr/config/adjust_height.yaml

@@ -0,0 +1,38 @@
+play_motion:
+  motions:
+    higher_3:
+      joints: [torso_lift_joint]
+      points:
+      - positions: [0.30]
+        time_from_start: 0.0
+    higher_2:
+      joints: [torso_lift_joint]
+      points:
+      - positions: [0.26]
+        time_from_start: 0.0
+    higher_1:
+      joints: [torso_lift_joint]
+      points:
+      - positions: [0.24]
+        time_from_start: 0.0
+    middle:
+      joints: [torso_lift_joint]
+      points:
+      - positions: [0.22]
+        time_from_start: 0.0 
+    lower_1:
+      joints: [torso_lift_joint]
+      points:
+      - positions: [0.20]
+        time_from_start: 0.0
+    lower_2:
+      joints: [torso_lift_joint]
+      points:
+      - positions: [0.18]
+        time_from_start: 0.0
+    lower_3:
+      joints: [torso_lift_joint]
+      points:
+      - positions: [0.16]
+        time_from_start: 0.0
+

tasks/gpsr/config/find_object.yaml

@@ -0,0 +1,23 @@
+LOCATION_0:
+  pose: 
+    position: {x: 0.8552300795613897, y: 1.429070868095394, z: 0.0}
+    orientation: {x: 0.0, y: 0.0, z: 0.9133881598650153, w: 0.40708975597330027}
+
+
+LOCATION_1:
+  pose: 
+    position: {x: 2.9409194762572146,y: -3.712802433807799,z: 0.0}
+    orientation: {x: 0.0, y: 0.0, z: -0.611918859029574, w: 0.7909205459235108} 
+
+
+
+LOCATION_2:
+  pose: 
+    position: {x: 0.8491182859434898, y: -3.0076084920640334, z: 0.0}
+    orientation: { x: 0.0, y: 0.0, z: -0.6636264920996592, w: 0.7480640874841546}
+
+LOCATION_3:
+  pose: 
+    position: {x: 0.8479247023925237, y: 1.432409182340313, z: 0.0}
+    orientation: {x: 0.0, y: 0.0, z: 0.8476927158365988, w: 0.530487567731395}
+

tasks/gpsr/config/guiding_people.yaml

@@ -0,0 +1,40 @@
+Start:
+  pose: 
+    position: {x: 4.229935896326249, y: 5.989767766362657, z: 0.0}
+    orientation: {x: 0.0, y: 0.0, z: 0.8755557432741014, w: 0.4831171083906427}
+
+
+WaitPoint1:
+  pose: 
+    position: {x: 2.3428986531653817, y: 3.3629245249591597, z: 0.0}
+    orientation: {x: 0.0, y: 0.0, z: -0.5548983151957669, w: 0.8319181809486431} 
+
+TurnAround1:
+  pose: 
+    position: {x: 2.3362042642374203,y: 3.2752834061488105,z: 0.0}
+    orientation: {x: 0.0, y: 0.0, z: 0.6376807566089671, w: 0.7703007546735335} 
+
+
+WaitPoint2:
+  pose: 
+    position: {x: 2.17519261886225, y: -0.36151674936209355, z: 0.0}
+    orientation: { x: 0.0, y: 0.0, z: -0.7375323735298429, w: 0.6753117783627324}
+
+
+TurnAround2:
+  pose: 
+    position: {x: 2.157761847255999,y: -0.462103856858122,z: 0.0}
+    orientation: {x: 0.0, y: 0.0, z: 0.8652281383725151, w: 0.5013783686682463} 
+
+
+Destination:
+  pose: 
+    position: {x: 2.5710323093426086, y: -2.9194003635449, z: 0.0}
+    orientation: {x: 0.0, y: 0.0, z: -0.7864753977372808, w: 0.6176216064500868}
+
+
+TurnAroundDestination: 
+  pose: 
+    position: {x: 2.546711405659906, y: -3.012970231861997, z: 0.0}
+    orientation: {x: 0.0, y: 0.0, z: 0.7369260641412018, w: 0.6759733544965788}
+

tasks/gpsr/config/object_weight.yaml

@@ -0,0 +1,55 @@
+Object_list:
+  Object:
+    Cleaning_supplies:
+      sponge: 50
+      clanser: 500
+
+    Toys:
+      tennis_ball: 58 
+      soccer_ball: 430
+      dice: 8
+      baseball: 145
+      rubiks_cube: 98
+
+    Dishes:
+      fork: 30
+      knife: 80
+      spoon: 50
+      plate: 500
+      bowl: 300
+      cup: 250
+      bottle: 300
+
+    Drinks:
+      red_wine: 1200
+      cola: 380
+      juice_pack: 200
+      tropical_juice: 1200
+      milk: 1000
+      iced_tea: 340
+      orange_juice: 1200
+
+    Food:
+      cereal: 500
+      tuna: 165
+      spam: 340
+      mustard: 250
+      strawberry_jello: 85
+      chocolate_jello: 85
+      coffee_grounds: 250
+      sugar: 1000
+
+    Fruits:
+      apple: 150
+      orange: 200
+      pear: 180
+      banana: 120
+      strawberry: 10
+      lemon: 110
+      peach: 150
+      plum: 80
+
+    Snacks:
+      pringles: 150
+      cornflakes: 500
+      cheezit: 200

tasks/gpsr/config/play_motion.yaml

@@ -0,0 +1,45 @@
+play_motion:
+  motions:
+    look_down:
+      joints: [head_1_joint, head_2_joint]
+      points:
+      - positions: [0.0, -0.2]
+        time_from_start: 0.0
+    # look_left:
+    #   joints: [head_1_joint, head_2_joint]
+    #   points:
+    #   - positions: [0.5, -0.25]
+    #     time_from_start: 0.0 
+    look_down_left:
+      joints: [head_1_joint, head_2_joint]
+      points:
+      - positions: [0.5, -0.55]
+        time_from_start: 0.0
+    # look_right:
+    #   joints: [head_1_joint, head_2_joint]
+    #   points:
+    #   - positions: [-0.5, -0.25]
+    #     time_from_start: 0.0 
+    look_down_right:
+      joints: [head_1_joint, head_2_joint]
+      points:
+      - positions: [-0.5, -0.55]
+        time_from_start: 0.0
+
+    look_center:
+      joints: [head_1_joint, head_2_joint]
+      points:
+      - positions: [0.0, 0.0]
+        time_from_start: 0.0
+
+    look_left:
+      joints: [head_1_joint, head_2_joint]
+      points:
+      - positions: [0.5, 0.0]
+        time_from_start: 0.0
+
+    look_right:
+      joints: [head_1_joint, head_2_joint]
+      points:
+      - positions: [-0.5, 0.0]
+        time_from_start: 0.0

tasks/gpsr/src/gpsr/states/adjust_base.py

@@ -0,0 +1,146 @@
+#!/usr/bin/env python3
+import rospy
+import smach
+from geometry_msgs.msg import Twist
+from lasr_skills.vision import GetImage
+from lasr_vision_msgs.srv import BodyPixDetection, BodyPixDetectionRequest
+from lasr_vision_msgs.msg import BodyPixMaskRequest
+from time import sleep
+
+
+class Rotate(smach.State):
+    def __init__(self, angular_speed, duration=1):
+        smach.State.__init__(self, outcomes=["succeeded", "failed"])
+        self.angular_speed = angular_speed
+        self.duration = duration  # Duration for which to rotate
+
+    def execute(self, userdata):
+        rospy.loginfo("Rotating")
+        pub = rospy.Publisher(
+            "/mobile_base_controller/cmd_vel", Twist, queue_size=10, latch=True
+        )
+
+        # Construct the twist message with the desired angular speed
+        twist = Twist()
+        twist.angular.z = self.angular_speed
+
+        try:
+            # Rotate for the specified duration
+            start_time = rospy.Time.now()
+            while (rospy.Time.now() - start_time).to_sec() < self.duration:
+                pub.publish(twist)
+                sleep(0.1)  # Publish at 10 Hz
+
+            # Stop the robot after rotating
+            twist.angular.z = 0
+            pub.publish(twist)
+            return "succeeded"
+        except rospy.ROSInterruptException:
+            rospy.loginfo("ROS Interrupt Exception")
+            return "failed"
+
+
+class DetectHipCenter(smach.State):
+    """
+    State for detecting gestures.
+    """
+
+    def __init__(self, gesture_to_detect: str = "raising_left_hand"):
+        self.gesture_to_detect = gesture_to_detect
+        smach.State.__init__(
+            self,
+            outcomes=["move_left", "move_right", "stay_same", "failed"],
+            input_keys=["img_msg"],
+        )
+
+        self.body_pix_client = rospy.ServiceProxy("/bodypix/detect", BodyPixDetection)
+
+    def execute(self, userdata):
+
+        body_pix_masks = BodyPixMaskRequest()
+        body_pix_masks.parts = ["left_hip", "right_hip"]
+        masks = [body_pix_masks]
+
+        req = BodyPixDetectionRequest()
+        req.image_raw = userdata.img_msg
+        req.masks = masks
+        req.dataset = "resnet50"
+        req.confidence = 0.7
+
+        try:
+            res = self.body_pix_client(req)
+        except Exception as e:
+            print(e)
+            return "failed"
+
+        part_info = {}
+        poses = res.poses
+
+        if not poses:
+            rospy.logerr("No poses available in the response.")
+            return "failed"
+
+        for pose in poses:
+            for keypoint in pose.keypoints:
+                if pose is None or pose.keypoints is None:
+                    rospy.logerr("Pose or keypoints data is missing.")
+                    return "failed"  # Skip this iteration if data is missing
+                else:
+                    part_info[keypoint.part] = {
+                        "x": keypoint.xy[0],
+                        "y": keypoint.xy[1],
+                        "score": keypoint.score,
+                    }
+
+        print(part_info["leftHip"]["x"])
+        print(part_info["rightHip"]["x"])
+        if (part_info["leftHip"]["x"] + part_info["rightHip"]["x"]) / 2 < 300:
+            return "move_left"
+        elif (part_info["leftHip"]["x"] + part_info["rightHip"]["x"]) / 2 > 340:
+            return "move_right"
+        elif 300 < (part_info["leftHip"]["x"] + part_info["rightHip"]["x"]) / 2 < 340:
+            return "stay_same"
+        else:
+            return "failed"
+
+
+class AdjustBase(smach.StateMachine):
+    def __init__(self):
+        smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
+        with self:
+            smach.StateMachine.add(
+                "GET_IMAGE",
+                GetImage(),
+                transitions={"succeeded": "BODY_PIX_DETECTION", "failed": "failed"},
+            )
+
+            smach.StateMachine.add(
+                "BODY_PIX_DETECTION",
+                DetectHipCenter(),
+                transitions={
+                    "move_left": "ADJUST_BASE_LEFT",
+                    "move_right": "ADJUST_BASE_RIGHT",
+                    "stay_same": "succeeded",
+                    "failed": "GET_IMAGE",
+                },
+            )
+
+            smach.StateMachine.add(
+                "ADJUST_BASE_LEFT",
+                Rotate(0.1),
+                transitions={"succeeded": "GET_IMAGE", "failed": "failed"},
+            )
+
+            smach.StateMachine.add(
+                "ADJUST_BASE_RIGHT",
+                Rotate(-0.1),
+                transitions={"succeeded": "GET_IMAGE", "failed": "failed"},
+            )
+
+
+if __name__ == "__main__":
+    rospy.init_node("adjust_base")
+    while not rospy.is_shutdown():
+        sm = AdjustBase()
+        sm.execute()
+    rospy.spin()