diff --git a/common/vision/lasr_vision_yolov8/CMakeLists.txt b/common/vision/lasr_vision_yolov8/CMakeLists.txt
index a65d456c0..e7599fef7 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.10
+ PYTHON_INTERPRETER python3.8
)
################################################
diff --git a/common/vision/lasr_vision_yolov8/package.xml b/common/vision/lasr_vision_yolov8/package.xml
index aeb1be4d2..cc273a7cb 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 99f31cbbf..e25cdeb1e 100644
--- a/common/vision/lasr_vision_yolov8/requirements.txt
+++ b/common/vision/lasr_vision_yolov8/requirements.txt
@@ -1,55 +1,53 @@
-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
+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
dill==0.3.7 # via -r requirements.in
-filelock==3.12.3 # via torch, triton
-fonttools==4.42.1 # via matplotlib
-idna==3.4 # via requests
-jinja2==3.1.2 # via torch
+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
kiwisolver==1.4.5 # via matplotlib
-lit==16.0.6 # via triton
-markupsafe==2.1.3 # via jinja2
-matplotlib==3.7.3 # via seaborn, ultralytics
+markupsafe==2.1.5 # via jinja2
+matplotlib==3.7.5 # via seaborn, ultralytics
mpmath==1.3.0 # via sympy
networkx==3.1 # via torch
-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
+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
py-cpuinfo==9.0.0 # via ultralytics
-pyparsing==3.1.1 # via matplotlib
-python-dateutil==2.8.2 # via matplotlib, pandas
-pytz==2023.3.post1 # via pandas
+pyparsing==3.1.2 # via matplotlib
+python-dateutil==2.9.0.post0 # via matplotlib, pandas
+pytz==2024.1 # via pandas
pyyaml==6.0.1 # via ultralytics
-requests==2.31.0 # via torchvision, ultralytics
-scipy==1.11.2 # via ultralytics
-seaborn==0.12.2 # via ultralytics
+requests==2.31.0 # via ultralytics
+scipy==1.10.1 # via ultralytics
+seaborn==0.13.2 # via ultralytics
six==1.16.0 # via python-dateutil
sympy==1.12 # via torch
-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
+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
ultralytics==8.0.168 # via -r requirements.in
-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
+urllib3==2.2.1 # via requests
+zipp==3.17.0 # via importlib-resources
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 bdb5ca586..119ab2645 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,10 +3,8 @@
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,
@@ -14,36 +12,13 @@
YoloDetection3DRequest,
YoloDetection3DResponse,
)
-import cv2
-import ros_numpy as rnp
+
+import markers
from geometry_msgs.msg import Point, PointStamped
import tf2_ros as tf
-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
+import tf2_geometry_msgs # noqa
# model cache
@@ -121,14 +96,7 @@ 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")
@@ -154,26 +122,31 @@ 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
+ # copy segmented mask if available, and estimate 3D position
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
- detection.point = tf_buffer.transform(
- point_stamped, request.target_frame
- ).point
+ 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
+ # publish to debug topic
if debug_point_publisher is not None:
- marker = create_point_marker(
- detection.point.x, detection.point.y, detection.point.z, i
+ markers.create_and_publish_marker(
+ debug_point_publisher, detection.point
)
- 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 2e0d7e016..4d8bc1c71 100644
--- a/skills/src/lasr_skills/detect_3d.py
+++ b/skills/src/lasr_skills/detect_3d.py
@@ -1,4 +1,3 @@
-#!/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 600613707..7a8d78de3 100644
--- a/skills/src/lasr_skills/detect_3d_in_area.py
+++ b/skills/src/lasr_skills/detect_3d_in_area.py
@@ -1,19 +1,18 @@
-#!/usr/bin/env python3
-
import smach
from lasr_skills import Detect3D
-from lasr_shapely import LasrShapely
-
from typing import List
+from shapely.geometry import Point
+from shapely.geometry.polygon import Polygon
+
class Detect3DInArea(smach.StateMachine):
class FilterDetections(smach.State):
- def __init__(self, area_polygon):
+ def __init__(self, area_polygon: Polygon):
smach.State.__init__(
self,
outcomes=["succeeded", "failed"],
@@ -21,25 +20,24 @@ def __init__(self, area_polygon):
output_keys=["detections_3d"],
)
self.area_polygon = area_polygon
- self.shapely = LasrShapely()
def execute(self, userdata):
- satisfied_points = self.shapely.are_points_in_polygon_2d(
- userdata.area_polygon,
- [[pose[0], pose[1]] for (_, pose) in userdata.detections_3d],
- ).inside
+ satisfied_points = [
+ self.area_polygon.contains(Point(pose[0], pose[1]))
+ for (_, pose) in userdata.detections_3d
+ ]
filtered_detections = [
userdata.detections_3d[i]
for i in range(0, len(userdata.detections_3d))
if satisfied_points[i]
]
- userdata.detections_3d = filtered_detections
+ userdata.detections_3d = filtered_detections
return "succeeded"
def __init__(
self,
- area_polygon: List[List[float]],
+ area_polygon: Polygon,
depth_topic: str = "/xtion/depth_registered/points",
model: str = "yolov8n-seg.pt",
filter: 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 00e857e15..5efeae034 100755
--- a/skills/src/lasr_skills/go_to_location.py
+++ b/skills/src/lasr_skills/go_to_location.py
@@ -1,31 +1,18 @@
-#!/usr/bin/env python3
+import smach_ros
-import rospy
-import smach
-from tiago_controllers.controllers import Controllers
-from geometry_msgs.msg import Point, Quaternion, Pose
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from geometry_msgs.msg import PoseStamped
+from std_msgs.msg import Header
-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'
+class GoToLocation(smach_ros.SimpleActionState):
-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
+ 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"],
+ )
diff --git a/skills/src/lasr_skills/go_to_semantic_location.py b/skills/src/lasr_skills/go_to_semantic_location.py
index 5f1a59a50..007d801ca 100755
--- a/skills/src/lasr_skills/go_to_semantic_location.py
+++ b/skills/src/lasr_skills/go_to_semantic_location.py
@@ -1,30 +1,28 @@
-#!/usr/bin/env python3
-
import rospy
-import smach
-from tiago_controllers.controllers import Controllers
-from geometry_msgs.msg import Point, Quaternion, Pose
+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
+
-class GoToSemanticLocation(smach.State):
+class GoToSemanticLocation(smach_ros.SimpleActionState):
def __init__(self):
- smach.State.__init__(self, outcomes=['succeeded', 'failed'], input_keys=['location'])
- self.controllers = Controllers()
+ 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"],
+ )
- 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()
+ def _to_pose(self, location):
+ return Pose(
+ position=Point(**location["position"]),
+ orientation=Quaternion(**location["orientation"]),
+ )
diff --git a/skills/src/lasr_skills/look_to_point.py b/skills/src/lasr_skills/look_to_point.py
old mode 100644
new mode 100755
index 85790a2ed..81c633ad7
--- a/skills/src/lasr_skills/look_to_point.py
+++ b/skills/src/lasr_skills/look_to_point.py
@@ -1,20 +1,22 @@
-import smach
-import actionlib
-from control_msgs.msg import PointHeadGoal
-from control_msgs.msg import PointHeadAction
-from geometry_msgs.msg import Point
+import smach_ros
+from control_msgs.msg import PointHeadGoal, PointHeadAction
+from geometry_msgs.msg import Point, PointStamped
+from std_msgs.msg import Header
-class LookToPoint(smach.State):
- def __init__(self):
- 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
+class LookToPoint(smach_ros.SimpleActionState):
+ 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"],
+ )
diff --git a/skills/src/lasr_skills/wait_for_person.py b/skills/src/lasr_skills/wait_for_person.py
index 85b31d033..ada04b8a2 100755
--- a/skills/src/lasr_skills/wait_for_person.py
+++ b/skills/src/lasr_skills/wait_for_person.py
@@ -1,5 +1,3 @@
-#!/usr/bin/env python3
-
import smach
from lasr_skills import Detect
@@ -25,7 +23,6 @@ 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 604812b3b..a203e98b0 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,18 +20,17 @@ def execute(self, userdata):
else:
return "not_done"
- def __init__(self):
+ def __init__(self, area_polygon: Polygon):
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(),
+ Detect3DInArea(area_polygon=area_polygon, filter=["person"]),
transitions={"succeeded": "CHECK_FOR_PERSON", "failed": "failed"},
)
smach.StateMachine.add(