From 0fe7c15a4a02d9b5155d0594e4955798a6d9b563 Mon Sep 17 00:00:00 2001 From: Jared Swift Date: Tue, 5 Mar 2024 17:58:50 +0000 Subject: [PATCH 1/4] refactor: drop demo yaml, and add motions. --- skills/config/demo.yaml | 5 ----- skills/config/motions.yaml | 0 2 files changed, 5 deletions(-) delete mode 100644 skills/config/demo.yaml create mode 100644 skills/config/motions.yaml diff --git a/skills/config/demo.yaml b/skills/config/demo.yaml deleted file mode 100644 index 83aa5d563..000000000 --- a/skills/config/demo.yaml +++ /dev/null @@ -1,5 +0,0 @@ -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 } \ No newline at end of file diff --git a/skills/config/motions.yaml b/skills/config/motions.yaml new file mode 100644 index 000000000..e69de29bb From 336cfe5f4181ea54e119daa2cf693823156299ba Mon Sep 17 00:00:00 2001 From: Jared Swift Date: Tue, 5 Mar 2024 17:59:08 +0000 Subject: [PATCH 2/4] feat(WIP): ListenWakeWord and ReceiveObject skills. --- skills/src/lasr_skills/__init__.py | 1 + skills/src/lasr_skills/listen_wake_word.py | 33 +++++++++++ skills/src/lasr_skills/receive_object.py | 69 ++++++++++++++++++++++ 3 files changed, 103 insertions(+) create mode 100644 skills/src/lasr_skills/listen_wake_word.py create mode 100644 skills/src/lasr_skills/receive_object.py diff --git a/skills/src/lasr_skills/__init__.py b/skills/src/lasr_skills/__init__.py index f1a0a65b6..cba4355f6 100755 --- a/skills/src/lasr_skills/__init__.py +++ b/skills/src/lasr_skills/__init__.py @@ -8,3 +8,4 @@ from .look_to_point import LookToPoint from .go_to_location import GoToLocation from .go_to_semantic_location import GoToSemanticLocation +from .listen_wake_word import ListenWakeWord diff --git a/skills/src/lasr_skills/listen_wake_word.py b/skills/src/lasr_skills/listen_wake_word.py new file mode 100644 index 000000000..3c5f871f5 --- /dev/null +++ b/skills/src/lasr_skills/listen_wake_word.py @@ -0,0 +1,33 @@ +import smach +from smach_ros import SimpleActionState + +from lasr_speech_recognition_msgs.msg import ( + TranscribeSpeechAction, + TranscribeSpeechGoal, +) + +from actionlib_msgs.msg import GoalStatus + + +class ListenWakeWord(smach.StateMachine): + def __init__(self, wake_word: str): + smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) + + with self: + + def listen_cb(_, status, result): + if status == GoalStatus.SUCCEEDED: + if wake_word in result.transcription.lower().split(): + return "succeeded" + return "failed" + + smach.StateMachine.add( + "LISTEN", + SimpleActionState( + "transcribe_speech", + TranscribeSpeechAction, + goal=TranscribeSpeechGoal(), + result_cb=listen_cb, + ), + transitions={"succeeded": "succeeded", "failed": "failed"}, + ) diff --git a/skills/src/lasr_skills/receive_object.py b/skills/src/lasr_skills/receive_object.py new file mode 100644 index 000000000..64fdf3a13 --- /dev/null +++ b/skills/src/lasr_skills/receive_object.py @@ -0,0 +1,69 @@ +import smach +from smach_ros import SimpleActionState + +from play_motion_msgs.msg import PlayMotionAction, PlayMotionGoal +from pal_interaction_msgs.msg import TtsGoal, TtsAction, TtsText + +from lasr_skills import ListenWakeWord + + +class ReceiveObject(smach.StateMachine): + + def __init__(self): + smach.StateMachine.__init__( + self, outcomes=["succeeded", "failed"], input_keys=["object_name"] + ) + + with self: + # TODO: clear octomap and look around + smach.StateMachine.add( + "RECEIVE_OBJECT", + SimpleActionState( + "play_motion", + PlayMotionAction, + goal=PlayMotionGoal( + motion_name="receive_object", skip_planning=False + ), + ), + transitions={"succeeded": "succeeded", "aborted": "failed"}, + ) + smach.StateMachine.add( + "SAY", + SimpleActionState( + "tts", + TtsAction, + goal_cb=lambda ud, _: TtsGoal( + rawtext=TtsText( + text=f"Please place the {ud.object_name} in my hand, and say 'done' when you are ready.", + lang_id="en_GB", + ) + ), + input_keys=["object_name"], + ), + ) + smach.StateMachine.add( + "WAIT_FOR_SPEECH", + ListenWakeWord("done"), + transitions={"succeeded": "CLOSE_GRIPPER", "failed": "WAIT_FOR_SPEECH"}, + ) + + smach.StateMachine.add( + "CLOSE_GRIPPER", + SimpleActionState( + "play_motion", + PlayMotionAction, + goal=PlayMotionGoal( + motion_name="close_gripper", skip_planning=False + ), + ), + transitions={"succeeded": "succeeded", "aborted": "failed"}, + ) + smach.StateMachine.add( + "FOLD_ARM", + SimpleActionState( + "play_motion", + PlayMotionAction, + goal=PlayMotionGoal(motion_name="fold_arm", skip_planning=False), + ), + transitions={"succeeded": "succeeded", "aborted": "failed"}, + ) From 8ceabf34341c3b6ebb036c557d0f0ec838ce75d8 Mon Sep 17 00:00:00 2001 From: Jared Swift Date: Tue, 5 Mar 2024 18:04:53 +0000 Subject: [PATCH 3/4] feat: HandoverObject skill, and update dunder init. --- skills/src/lasr_skills/__init__.py | 2 + skills/src/lasr_skills/handover_object.py | 69 +++++++++++++++++++++++ skills/src/lasr_skills/receive_object.py | 2 +- 3 files changed, 72 insertions(+), 1 deletion(-) create mode 100644 skills/src/lasr_skills/handover_object.py diff --git a/skills/src/lasr_skills/__init__.py b/skills/src/lasr_skills/__init__.py index cba4355f6..eb84a0645 100755 --- a/skills/src/lasr_skills/__init__.py +++ b/skills/src/lasr_skills/__init__.py @@ -9,3 +9,5 @@ from .go_to_location import GoToLocation from .go_to_semantic_location import GoToSemanticLocation from .listen_wake_word import ListenWakeWord +from .handover_object import HandoverObject +from .receive_object import ReceiveObject diff --git a/skills/src/lasr_skills/handover_object.py b/skills/src/lasr_skills/handover_object.py new file mode 100644 index 000000000..f385d74e8 --- /dev/null +++ b/skills/src/lasr_skills/handover_object.py @@ -0,0 +1,69 @@ +import smach +from smach_ros import SimpleActionState + +from play_motion_msgs.msg import PlayMotionAction, PlayMotionGoal +from pal_interaction_msgs.msg import TtsGoal, TtsAction, TtsText + +from lasr_skills import ListenWakeWord + + +class HandoverObject(smach.StateMachine): + + def __init__(self): + smach.StateMachine.__init__( + self, outcomes=["succeeded", "failed"], input_keys=["object_name"] + ) + + with self: + # TODO: clear octomap and look around + smach.StateMachine.add( + "HANDOVER_OBJECT", + SimpleActionState( + "play_motion", + PlayMotionAction, + goal=PlayMotionGoal( + motion_name="handover_object", skip_planning=False + ), + ), + transitions={"succeeded": "succeeded", "aborted": "failed"}, + ) + smach.StateMachine.add( + "SAY", + SimpleActionState( + "tts", + TtsAction, + goal_cb=lambda ud, _: TtsGoal( + rawtext=TtsText( + text=f"Please grab the {ud.object_name} in my hand, and say 'done' when you are ready for me to release it.", + lang_id="en_GB", + ) + ), + input_keys=["object_name"], + ), + ) + smach.StateMachine.add( + "WAIT_FOR_SPEECH", + ListenWakeWord("done"), + transitions={"succeeded": "OPEN_GRIPPER", "failed": "failed"}, + ) + + smach.StateMachine.add( + "OPEN_GRIPPER", + SimpleActionState( + "play_motion", + PlayMotionAction, + goal=PlayMotionGoal( + motion_name="open_gripper", skip_planning=False + ), + ), + transitions={"succeeded": "succeeded", "aborted": "failed"}, + ) + smach.StateMachine.add( + "FOLD_ARM", + SimpleActionState( + "play_motion", + PlayMotionAction, + goal=PlayMotionGoal(motion_name="fold_arm", skip_planning=False), + ), + transitions={"succeeded": "succeeded", "aborted": "failed"}, + ) diff --git a/skills/src/lasr_skills/receive_object.py b/skills/src/lasr_skills/receive_object.py index 64fdf3a13..e59f786f0 100644 --- a/skills/src/lasr_skills/receive_object.py +++ b/skills/src/lasr_skills/receive_object.py @@ -44,7 +44,7 @@ def __init__(self): smach.StateMachine.add( "WAIT_FOR_SPEECH", ListenWakeWord("done"), - transitions={"succeeded": "CLOSE_GRIPPER", "failed": "WAIT_FOR_SPEECH"}, + transitions={"succeeded": "CLOSE_GRIPPER", "failed": "failed"}, ) smach.StateMachine.add( From 367ada9af975e010bfbae188e988968b764a6395 Mon Sep 17 00:00:00 2001 From: Jared Swift Date: Fri, 8 Mar 2024 11:07:18 +0000 Subject: [PATCH 4/4] Revert "Spring clean 01 hotfix + slight cleanup (#141)" This reverts commit a16f83b5edcc166ad870a32629e3647bf692760c. --- .../vision/lasr_vision_yolov8/CMakeLists.txt | 2 +- common/vision/lasr_vision_yolov8/package.xml | 2 +- .../lasr_vision_yolov8/requirements.txt | 90 ++++++++++--------- .../src/lasr_vision_yolov8/yolo.py | 59 ++++++++---- skills/src/lasr_skills/detect_3d.py | 1 + skills/src/lasr_skills/detect_3d_in_area.py | 22 ++--- skills/src/lasr_skills/go_to_location.py | 41 ++++++--- .../lasr_skills/go_to_semantic_location.py | 48 +++++----- skills/src/lasr_skills/look_to_point.py | 38 ++++---- skills/src/lasr_skills/wait_for_person.py | 3 + .../lasr_skills/wait_for_person_in_area.py | 9 +- 11 files changed, 182 insertions(+), 133 deletions(-) mode change 100755 => 100644 skills/src/lasr_skills/look_to_point.py diff --git a/common/vision/lasr_vision_yolov8/CMakeLists.txt b/common/vision/lasr_vision_yolov8/CMakeLists.txt index e7599fef7..a65d456c0 100644 --- a/common/vision/lasr_vision_yolov8/CMakeLists.txt +++ b/common/vision/lasr_vision_yolov8/CMakeLists.txt @@ -19,7 +19,7 @@ find_package(catkin REQUIRED catkin_virtualenv) catkin_python_setup() catkin_generate_virtualenv( INPUT_REQUIREMENTS requirements.in - PYTHON_INTERPRETER python3.8 + PYTHON_INTERPRETER python3.10 ) ################################################ diff --git a/common/vision/lasr_vision_yolov8/package.xml b/common/vision/lasr_vision_yolov8/package.xml index cc273a7cb..aeb1be4d2 100644 --- a/common/vision/lasr_vision_yolov8/package.xml +++ b/common/vision/lasr_vision_yolov8/package.xml @@ -51,7 +51,7 @@ catkin catkin_virtualenv lasr_vision_msgs - markers + diff --git a/common/vision/lasr_vision_yolov8/requirements.txt b/common/vision/lasr_vision_yolov8/requirements.txt index e25cdeb1e..99f31cbbf 100644 --- a/common/vision/lasr_vision_yolov8/requirements.txt +++ b/common/vision/lasr_vision_yolov8/requirements.txt @@ -1,53 +1,55 @@ -certifi==2024.2.2 # via requests -charset-normalizer==3.3.2 # via requests -contourpy==1.1.1 # via matplotlib -cycler==0.12.1 # via matplotlib +certifi==2023.7.22 # via requests +charset-normalizer==3.2.0 # via requests +cmake==3.27.4.1 # via triton +contourpy==1.1.0 # via matplotlib +cycler==0.11.0 # via matplotlib dill==0.3.7 # via -r requirements.in -filelock==3.13.1 # via torch, triton -fonttools==4.49.0 # via matplotlib -fsspec==2024.2.0 # via torch -idna==3.6 # via requests -importlib-resources==6.1.2 # via matplotlib -jinja2==3.1.3 # via torch +filelock==3.12.3 # via torch, triton +fonttools==4.42.1 # via matplotlib +idna==3.4 # via requests +jinja2==3.1.2 # via torch kiwisolver==1.4.5 # via matplotlib -markupsafe==2.1.5 # via jinja2 -matplotlib==3.7.5 # via seaborn, ultralytics +lit==16.0.6 # via triton +markupsafe==2.1.3 # via jinja2 +matplotlib==3.7.3 # via seaborn, ultralytics mpmath==1.3.0 # via sympy networkx==3.1 # via torch -numpy==1.24.4 # via contourpy, matplotlib, opencv-python, pandas, scipy, seaborn, torchvision, ultralytics -nvidia-cublas-cu12==12.1.3.1 # via nvidia-cudnn-cu12, nvidia-cusolver-cu12, torch -nvidia-cuda-cupti-cu12==12.1.105 # via torch -nvidia-cuda-nvrtc-cu12==12.1.105 # via torch -nvidia-cuda-runtime-cu12==12.1.105 # via torch -nvidia-cudnn-cu12==8.9.2.26 # via torch -nvidia-cufft-cu12==11.0.2.54 # via torch -nvidia-curand-cu12==10.3.2.106 # via torch -nvidia-cusolver-cu12==11.4.5.107 # via torch -nvidia-cusparse-cu12==12.1.0.106 # via nvidia-cusolver-cu12, torch -nvidia-nccl-cu12==2.19.3 # via torch -nvidia-nvjitlink-cu12==12.4.99 # via nvidia-cusolver-cu12, nvidia-cusparse-cu12 -nvidia-nvtx-cu12==12.1.105 # via torch -opencv-python==4.9.0.80 # via ultralytics -packaging==23.2 # via matplotlib -pandas==2.0.3 # via seaborn, ultralytics -pillow==10.2.0 # via matplotlib, torchvision, ultralytics -psutil==5.9.8 # via ultralytics +numpy==1.25.2 # via contourpy, matplotlib, opencv-python, pandas, scipy, seaborn, torchvision, ultralytics +nvidia-cublas-cu11==11.10.3.66 # via nvidia-cudnn-cu11, nvidia-cusolver-cu11, torch +nvidia-cuda-cupti-cu11==11.7.101 # via torch +nvidia-cuda-nvrtc-cu11==11.7.99 # via torch +nvidia-cuda-runtime-cu11==11.7.99 # via torch +nvidia-cudnn-cu11==8.5.0.96 # via torch +nvidia-cufft-cu11==10.9.0.58 # via torch +nvidia-curand-cu11==10.2.10.91 # via torch +nvidia-cusolver-cu11==11.4.0.1 # via torch +nvidia-cusparse-cu11==11.7.4.91 # via torch +nvidia-nccl-cu11==2.14.3 # via torch +nvidia-nvtx-cu11==11.7.91 # via torch +opencv-python==4.8.0.76 # via ultralytics +packaging==23.1 # via matplotlib +pandas==2.1.0 # via seaborn, ultralytics +pillow==10.0.0 # via matplotlib, torchvision, ultralytics +psutil==5.9.5 # via ultralytics py-cpuinfo==9.0.0 # via ultralytics -pyparsing==3.1.2 # via matplotlib -python-dateutil==2.9.0.post0 # via matplotlib, pandas -pytz==2024.1 # via pandas +pyparsing==3.1.1 # via matplotlib +python-dateutil==2.8.2 # via matplotlib, pandas +pytz==2023.3.post1 # via pandas pyyaml==6.0.1 # via ultralytics -requests==2.31.0 # via ultralytics -scipy==1.10.1 # via ultralytics -seaborn==0.13.2 # via ultralytics +requests==2.31.0 # via torchvision, ultralytics +scipy==1.11.2 # via ultralytics +seaborn==0.12.2 # via ultralytics six==1.16.0 # via python-dateutil sympy==1.12 # via torch -torch==2.2.1 # via torchvision, ultralytics -torchvision==0.17.1 # via ultralytics -tqdm==4.66.2 # via ultralytics -triton==2.2.0 # via torch -typing-extensions==4.10.0 # via torch -tzdata==2024.1 # via pandas +torch==2.0.1 # via torchvision, triton, ultralytics +torchvision==0.15.2 # via ultralytics +tqdm==4.66.1 # via ultralytics +triton==2.0.0 # via torch +typing-extensions==4.7.1 # via filelock, torch +tzdata==2023.3 # via pandas ultralytics==8.0.168 # via -r requirements.in -urllib3==2.2.1 # via requests -zipp==3.17.0 # via importlib-resources +urllib3==2.0.4 # via requests +wheel==0.41.2 # via nvidia-cublas-cu11, nvidia-cuda-cupti-cu11, nvidia-cuda-runtime-cu11, nvidia-curand-cu11, nvidia-cusparse-cu11, nvidia-nvtx-cu11 + +# The following packages are considered to be unsafe in a requirements file: +# setuptools diff --git a/common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py b/common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py index 119ab2645..bdb5ca586 100644 --- a/common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py +++ b/common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py @@ -3,8 +3,10 @@ import cv2_pcl import numpy as np +from PIL import Image from ultralytics import YOLO +from sensor_msgs.msg import Image as SensorImage from lasr_vision_msgs.msg import Detection, Detection3D from lasr_vision_msgs.srv import ( YoloDetectionRequest, @@ -12,13 +14,36 @@ YoloDetection3DRequest, YoloDetection3DResponse, ) - -import markers +import cv2 +import ros_numpy as rnp from geometry_msgs.msg import Point, PointStamped import tf2_ros as tf -import tf2_geometry_msgs # noqa +import tf2_geometry_msgs + +from visualization_msgs.msg import Marker + + +def create_point_marker(x, y, z, idx, g=1.0): + marker_msg = Marker() + marker_msg.header.frame_id = "map" + marker_msg.header.stamp = rospy.Time.now() + marker_msg.id = idx + marker_msg.type = Marker.SPHERE + marker_msg.action = Marker.ADD + marker_msg.pose.position.x = x + marker_msg.pose.position.y = y + marker_msg.pose.position.z = z + marker_msg.pose.orientation.w = 1.0 + marker_msg.scale.x = 0.1 + marker_msg.scale.y = 0.1 + marker_msg.scale.z = 0.1 + marker_msg.color.a = 1.0 + marker_msg.color.r = 0.0 + marker_msg.color.g = g + marker_msg.color.b = 0.0 + return marker_msg # model cache @@ -96,7 +121,14 @@ def detect_3d( """ Run YOLO 3D inference on given detection request """ + + rospy.loginfo("Waiting for transformation to become available") tf_buffer = tf.Buffer() + # Wait for the transformation to become available + while not tf_buffer.can_transform( + "map", request.pcl.header.frame_id, rospy.Time(0) + ): + rospy.sleep(0.01) # Extract rgb image from pointcloud rospy.loginfo("Decoding") @@ -122,31 +154,26 @@ def detect_3d( detection.confidence = float(result.boxes.conf.cpu().numpy()[i]) detection.xywh = result.boxes.xywh[i].cpu().numpy().astype(int).tolist() - # copy segmented mask if available, and estimate 3D position + # copy segmented mask if available if has_segment_masks: detection.xyseg = result.masks.xy[i].flatten().astype(int).tolist() + # xyz_points = [pcl_xyz[x][y] for x, y in indices] centroid = cv2_pcl.seg_to_centroid(request.pcl, np.array(detection.xyseg)) point = Point(*centroid) point_stamped = PointStamped() point_stamped.point = point point_stamped.header.frame_id = request.pcl.header.frame_id - point_stamped.header.stamp = rospy.Time(0) - - # TODO: handle tf errors properly - while not rospy.is_shutdown(): - try: - detection.point = tf_buffer.transform(point_stamped, "map").point - break - except Exception: - continue + detection.point = tf_buffer.transform( + point_stamped, request.target_frame + ).point - # publish to debug topic if debug_point_publisher is not None: - markers.create_and_publish_marker( - debug_point_publisher, detection.point + marker = create_point_marker( + detection.point.x, detection.point.y, detection.point.z, i ) + debug_point_publisher.publish(marker) detected_objects.append(detection) diff --git a/skills/src/lasr_skills/detect_3d.py b/skills/src/lasr_skills/detect_3d.py index 0e8eba925..8d6503c66 100644 --- a/skills/src/lasr_skills/detect_3d.py +++ b/skills/src/lasr_skills/detect_3d.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 import rospy import smach diff --git a/skills/src/lasr_skills/detect_3d_in_area.py b/skills/src/lasr_skills/detect_3d_in_area.py index 95f35cac7..7170928a1 100644 --- a/skills/src/lasr_skills/detect_3d_in_area.py +++ b/skills/src/lasr_skills/detect_3d_in_area.py @@ -1,18 +1,19 @@ +#!/usr/bin/env python3 + import smach from lasr_skills import Detect3D +from lasr_shapely import LasrShapely -from typing import List, Union -from shapely.geometry import Point -from shapely.geometry.polygon import Polygon +from typing import List, Union class Detect3DInArea(smach.StateMachine): class FilterDetections(smach.State): - def __init__(self, area_polygon: Polygon): + def __init__(self, area_polygon): smach.State.__init__( self, outcomes=["succeeded", "failed"], @@ -20,24 +21,25 @@ def __init__(self, area_polygon: Polygon): output_keys=["detections_3d"], ) self.area_polygon = area_polygon + self.shapely = LasrShapely() def execute(self, userdata): - satisfied_points = [ - self.area_polygon.contains(Point(pose[0], pose[1])) - for (_, pose) in userdata.detections_3d - ] + satisfied_points = self.shapely.are_points_in_polygon_2d( + userdata.area_polygon, + [[pose[0], pose[1]] for (_, pose) in userdata.detections_3d], + ).inside filtered_detections = [ userdata.detections_3d[i] for i in range(0, len(userdata.detections_3d)) if satisfied_points[i] ] - userdata.detections_3d = filtered_detections + return "succeeded" def __init__( self, - area_polygon: Polygon, + area_polygon: List[List[float]], depth_topic: str = "/xtion/depth_registered/points", model: str = "yolov8n-seg.pt", filter: Union[List[str], None] = None, diff --git a/skills/src/lasr_skills/go_to_location.py b/skills/src/lasr_skills/go_to_location.py index 5efeae034..00e857e15 100755 --- a/skills/src/lasr_skills/go_to_location.py +++ b/skills/src/lasr_skills/go_to_location.py @@ -1,18 +1,31 @@ -import smach_ros +#!/usr/bin/env python3 -from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from geometry_msgs.msg import PoseStamped -from std_msgs.msg import Header +import rospy +import smach +from tiago_controllers.controllers import Controllers +from geometry_msgs.msg import Point, Quaternion, Pose - -class GoToLocation(smach_ros.SimpleActionState): +class GoToLocation(smach.State): def __init__(self): - super(GoToLocation, self).__init__( - "move_base", - MoveBaseAction, - goal_cb=lambda ud, _: MoveBaseGoal( - target_pose=PoseStamped(pose=ud.location, header=Header(frame_id="map")) - ), - input_keys=["location"], - ) + 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() \ No newline at end of file diff --git a/skills/src/lasr_skills/go_to_semantic_location.py b/skills/src/lasr_skills/go_to_semantic_location.py index 007d801ca..5f1a59a50 100755 --- a/skills/src/lasr_skills/go_to_semantic_location.py +++ b/skills/src/lasr_skills/go_to_semantic_location.py @@ -1,28 +1,30 @@ -import rospy -import smach_ros - -from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion -from std_msgs.msg import Header +#!/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_ros.SimpleActionState): +class GoToSemanticLocation(smach.State): def __init__(self): - super(GoToSemanticLocation, self).__init__( - "move_base", - MoveBaseAction, - goal_cb=lambda ud, _: MoveBaseGoal( - target_pose=PoseStamped( - pose=self._to_pose(rospy.get_param(f"{ud.location}/location")), - header=Header(frame_id="map"), - ) - ), - input_keys=["location"], - ) + smach.State.__init__(self, outcomes=['succeeded', 'failed'], input_keys=['location']) + self.controllers = Controllers() - def _to_pose(self, location): - return Pose( - position=Point(**location["position"]), - orientation=Quaternion(**location["orientation"]), - ) + 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() diff --git a/skills/src/lasr_skills/look_to_point.py b/skills/src/lasr_skills/look_to_point.py old mode 100755 new mode 100644 index 81c633ad7..85790a2ed --- a/skills/src/lasr_skills/look_to_point.py +++ b/skills/src/lasr_skills/look_to_point.py @@ -1,22 +1,20 @@ -import smach_ros -from control_msgs.msg import PointHeadGoal, PointHeadAction -from geometry_msgs.msg import Point, PointStamped -from std_msgs.msg import Header +import smach +import actionlib +from control_msgs.msg import PointHeadGoal +from control_msgs.msg import PointHeadAction +from geometry_msgs.msg import Point - -class LookToPoint(smach_ros.SimpleActionState): +class LookToPoint(smach.State): def __init__(self): - super(LookToPoint, self).__init__( - "/head_controller/point_head_action", - PointHeadAction, - goal_cb=lambda ud, _: PointHeadGoal( - pointing_frame="head_2_link", - pointing_axis=Point(1.0, 0.0, 0.0), - max_velocity=1.0, - target=PointStamped( - header=Header(frame_id="map"), - point=ud.point, - ), - ), - input_keys=["point"], - ) + smach.State.__init__(self, outcomes=['succeeded'], input_keys=['point']) + self.point_head_client = actionlib.SimpleActionClient('/head_controller/point_head_action', PointHeadAction) + + def execute(self, userdata): + ph_goal = PointHeadGoal() + ph_goal.max_velocity = 1.0 + ph_goal.pointing_frame = 'head_2_link' + ph_goal.pointing_axis = Point(1.0, 0.0, 0.0) + ph_goal.target.header.frame_id = 'map' + ph_goal.target.point = userdata.point + self.point_head_client.send_goal_and_wait(ph_goal) + return 'succeeded' \ No newline at end of file diff --git a/skills/src/lasr_skills/wait_for_person.py b/skills/src/lasr_skills/wait_for_person.py index ada04b8a2..85b31d033 100755 --- a/skills/src/lasr_skills/wait_for_person.py +++ b/skills/src/lasr_skills/wait_for_person.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import smach from lasr_skills import Detect @@ -23,6 +25,7 @@ def __init__(self): smach.StateMachine.__init__( self, outcomes=["succeeded", "failed"], + input_keys=["depth_topic"], output_keys=["detections"], ) diff --git a/skills/src/lasr_skills/wait_for_person_in_area.py b/skills/src/lasr_skills/wait_for_person_in_area.py index a203e98b0..604812b3b 100755 --- a/skills/src/lasr_skills/wait_for_person_in_area.py +++ b/skills/src/lasr_skills/wait_for_person_in_area.py @@ -1,9 +1,9 @@ +#!/usr/bin/env python3 + import smach from lasr_skills import Detect3DInArea -from shapely.geometry.polygon import Polygon - class WaitForPersonInArea(smach.StateMachine): @@ -20,17 +20,18 @@ def execute(self, userdata): else: return "not_done" - def __init__(self, area_polygon: Polygon): + def __init__(self): smach.StateMachine.__init__( self, outcomes=["succeeded", "failed"], + input_keys=["depth_topic", "area_polygon"], output_keys=["detections_3d"], ) with self: smach.StateMachine.add( "DETECT_PEOPLE_3D", - Detect3DInArea(area_polygon=area_polygon, filter=["person"]), + Detect3DInArea(), transitions={"succeeded": "CHECK_FOR_PERSON", "failed": "failed"}, ) smach.StateMachine.add(