diff --git a/mir_manipulation/mir_pregrasp_planning/ros/src/mir_pregrasp_planning_ros/orientation_independent_ik.py b/mir_manipulation/mir_pregrasp_planning/ros/src/mir_pregrasp_planning_ros/orientation_independent_ik.py index 3ed110b3b..6f3b14952 100644 --- a/mir_manipulation/mir_pregrasp_planning/ros/src/mir_pregrasp_planning_ros/orientation_independent_ik.py +++ b/mir_manipulation/mir_pregrasp_planning/ros/src/mir_pregrasp_planning_ros/orientation_independent_ik.py @@ -58,7 +58,13 @@ def get_reachable_pose_and_joint_msg_from_point(self, x, y, z, frame_id): :returns: (geometry_msgs.msg.PoseStamped, brics_actuator.JointPositions) or None """ - pitch_ranges = [(0.0, 0.0), (-30.0, 0.0), (-60.0, -30.0), (-90.0, -60.0), (0.0, 10.0)] + # check is_picking_from_shelf from parameter server + self.is_picking_from_shelf = rospy.get_param("/pick_from_shelf_server/pick_statemachine_says_shelf", False) + rospy.loginfo(f"[orientation_independent_ik] shelf picking is set to: {self.is_picking_from_shelf}") + if self.is_picking_from_shelf=='True': + pitch_ranges = [(-20.0, -15.0)] # limiting the pitch range to avoid collisions with shelf and table + else: + pitch_ranges = [(0.0, 0.0), (-30.0, 0.0), (-60.0, -30.0), (-90.0, -60.0), (0.0, 10.0)] return self._get_reachable_pose_and_joint_msg_from_point_and_pitch_ranges( x, y, z, frame_id, pitch_ranges) diff --git a/mir_perception/mir_barrier_tape_detection/CMakeLists.txt b/mir_perception/mir_barrier_tape_detection/CMakeLists.txt index d08882cd3..7dce86109 100644 --- a/mir_perception/mir_barrier_tape_detection/CMakeLists.txt +++ b/mir_perception/mir_barrier_tape_detection/CMakeLists.txt @@ -18,9 +18,7 @@ find_package(OpenCV REQUIRED) find_package(PCL 1.10 REQUIRED) set (CMAKE_CXX_STANDARD 14) -add_compile_options( - -O3 -) +set (CMAKE_BUILD_TYPE Release) generate_dynamic_reconfigure_options( ros/config/BarrierTapeDetection.cfg diff --git a/mir_perception/mir_barrier_tape_detection/ros/launch/barrier_tape_detection.launch b/mir_perception/mir_barrier_tape_detection/ros/launch/barrier_tape_detection.launch index 0a362e6aa..f8b0d97a1 100644 --- a/mir_perception/mir_barrier_tape_detection/ros/launch/barrier_tape_detection.launch +++ b/mir_perception/mir_barrier_tape_detection/ros/launch/barrier_tape_detection.launch @@ -16,11 +16,11 @@ - + - + target_frame: base_laser_front_link # Leave disabled to output scan in pointcloud frame @@ -51,15 +51,15 @@ - + - + - + target_frame: base_laser_rear_link # Leave disabled to output scan in pointcloud frame diff --git a/mir_perception/mir_empty_space_detection/ros/config/params.yaml b/mir_perception/mir_empty_space_detection/ros/config/params.yaml index da149e2cc..97d737821 100644 --- a/mir_perception/mir_empty_space_detection/ros/config/params.yaml +++ b/mir_perception/mir_empty_space_detection/ros/config/params.yaml @@ -9,10 +9,10 @@ passthrough_filter_limit_min: 0.45 # all the min distance starts from the baseli passthrough_filter_limit_max: 0.7 enable_cropbox_filter: True cropbox_filter_min_x: 0.45 -cropbox_filter_max_x: 0.8 -cropbox_filter_min_y: -0.4 -cropbox_filter_max_y: 0.4 -cropbox_filter_min_z: 0.0 +cropbox_filter_max_x: 0.55 +cropbox_filter_min_y: -0.25 +cropbox_filter_max_y: 0.25 +cropbox_filter_min_z: -0.1 cropbox_filter_max_z: 0.6 normal_radius_search: 0.03 use_omp: true @@ -37,10 +37,10 @@ cluster_max_height: 0.09 cluster_max_length: 0.25 cluster_min_distance_to_polygon: 0.04 octree_resolution: 0.01 -object_height_above_workspace: 0.035 +object_height_above_workspace: 0.0 # setting it to 0 because we reduced the gripper height # parameters for finding empty spaces on a plane empty_space_point_count_percentage_threshold: 0.8 -empty_space_radius: 0.045 -num_of_empty_spaces_required: 4 +empty_space_radius: 0.065 +num_of_empty_spaces_required: 2 trial_duration: 3.0 -enable_debug_pc: False \ No newline at end of file +enable_debug_pc: false diff --git a/mir_perception/mir_object_recognition/ros/config/SceneSegmentation.cfg b/mir_perception/mir_object_recognition/ros/config/SceneSegmentation.cfg index 853c97e41..8c71b5e6a 100755 --- a/mir_perception/mir_object_recognition/ros/config/SceneSegmentation.cfg +++ b/mir_perception/mir_object_recognition/ros/config/SceneSegmentation.cfg @@ -58,7 +58,9 @@ pc_os_cluster.add ("pad_cluster", bool_t, 0, "Pad cluster so that it has the sa pc_os_cluster.add ("padded_cluster_size", int_t, 0, "The size of the padded cluster", 2048, 128, 4096) object_pose = gen.add_group("Object pose") -object_pose.add ("object_height_above_workspace", double_t, 0, "The height of the object above the workspace", 0.038, 0, 2.0) +object_pose.add ("use_fixed_heights", bool_t, 0, "Used fixed heights of objects by assuming platforms are 0, 5, 10 or 15 cm exactly", False) +object_pose.add ("height_of_floor", double_t, 0, "The height of floor (or 0cm platform) based on scene segmentation", -0.08, -1.0, 2.0) +object_pose.add ("object_height_above_workspace", double_t, 0, "The height of the object above the workspace", 0.03, -1.0, 2.0) object_pose.add ("container_height", double_t, 0, "The height of the container pose", 0.0335, 0, 2.0) rgb_bbox_proposal = gen.add_group("RGB bbox proposal") @@ -78,6 +80,7 @@ roi.add ("roi_min_bbox_z", double_t, 0, "Min height of objects", 0.03, 0, 1) object_recognizer = gen.add_group("Object recognizer") object_recognizer.add ("enable_rgb_recognizer", bool_t, 0, "Enable rgb object detection and recognition", True) object_recognizer.add ("enable_pc_recognizer", bool_t, 0, "Enable pointcloud object detection and recognition", True) +object_recognizer.add ("obj_category", str_t, 0, "Object category name (eg. atwork, cavity, container)", "atwork") exit (gen.generate (PACKAGE, "mir_object_recognition", "SceneSegmentation")) diff --git a/mir_perception/mir_object_recognition/ros/config/objects.xml b/mir_perception/mir_object_recognition/ros/config/objects.xml index ab2c15633..2b1e445d1 100644 --- a/mir_perception/mir_object_recognition/ros/config/objects.xml +++ b/mir_perception/mir_object_recognition/ros/config/objects.xml @@ -15,9 +15,13 @@ CONTAINER_BOX_REDboxred BEARING_BOXboxgray BEARINGspheregray + BEARING2spheregray AXIScylindergray + ALLENKEYflatgray + WRENCHflatgray + DRILLflatgray BLUE_CONTAINERboxblue RED_CONTAINERboxred - \ No newline at end of file + diff --git a/mir_perception/mir_object_recognition/ros/config/scene_segmentation_constraints.yaml b/mir_perception/mir_object_recognition/ros/config/scene_segmentation_constraints.yaml index ac0c8bff2..dc36b553b 100644 --- a/mir_perception/mir_object_recognition/ros/config/scene_segmentation_constraints.yaml +++ b/mir_perception/mir_object_recognition/ros/config/scene_segmentation_constraints.yaml @@ -13,7 +13,7 @@ multimodal_object_recognition: cropbox_filter_max_x: 0.8 cropbox_filter_min_y: -0.4 cropbox_filter_max_y: 0.4 - cropbox_filter_min_z: 0.0 + cropbox_filter_min_z: -0.1 cropbox_filter_max_z: 0.6 normal_radius_search: 0.03 use_omp: False @@ -41,14 +41,17 @@ multimodal_object_recognition: pad_cluster: True padded_cluster_size: 2048 octree_resolution: 0.0025 - object_height_above_workspace: 0.012 + height_of_floor: -0.083 + use_fixed_heights: False + object_height_above_workspace: -0.008 container_height: 0.07 enable_rgb_recognizer: true enable_pc_recognizer: false + obj_category: 'atwork' # atwork, cavity rgb_roi_adjustment: 2 rgb_bbox_min_diag: 21 - rgb_bbox_max_diag: 175 - rgb_cluster_filter_limit_min: 0.02 # for tower camera configuration (youbot 2) + rgb_bbox_max_diag: 200 + rgb_cluster_filter_limit_min: 0.005 # for tower camera configuration (youbot 2) rgb_cluster_filter_limit_max: 0.35 rgb_cluster_remove_outliers: True enable_roi: True diff --git a/mir_perception/mir_object_recognition/ros/include/mir_object_recognition/multimodal_object_recognition_node.h b/mir_perception/mir_object_recognition/ros/include/mir_object_recognition/multimodal_object_recognition_node.h index da6c30f17..d7204b5fe 100644 --- a/mir_perception/mir_object_recognition/ros/include/mir_object_recognition/multimodal_object_recognition_node.h +++ b/mir_perception/mir_object_recognition/ros/include/mir_object_recognition/multimodal_object_recognition_node.h @@ -111,7 +111,6 @@ class MultimodalObjectRecognitionROS ros::Publisher pub_rgb_object_pose_array_; // Publisher debug ros::Publisher pub_debug_cloud_plane_; - ros::Publisher pub_filtered_rgb_cloud_plane_; std::string horizontal_object_list[9]; // Synchronize callback for image and pointcloud @@ -155,11 +154,13 @@ class MultimodalObjectRecognitionROS // Enable recognizer bool enable_rgb_recognizer_; bool enable_pc_recognizer_ ; + std::string obj_category_; // Visualization BoundingBoxVisualizer bounding_box_visualizer_pc_; ClusteredPointCloudVisualizer cluster_visualizer_rgb_; ClusteredPointCloudVisualizer cluster_visualizer_pc_; + ClusteredPointCloudVisualizer cluster_visualizer_filtered_rgb_; LabelVisualizer label_visualizer_rgb_; LabelVisualizer label_visualizer_pc_; @@ -168,11 +169,14 @@ class MultimodalObjectRecognitionROS std::string target_frame_id_; std::string pointcloud_source_frame_id_; std::set round_objects_; + std::set flat_objects_; ObjectInfo object_info_; std::string object_info_path_; // Dynamic parameter double object_height_above_workspace_; + double height_of_floor_; + bool use_fixed_heights_; double container_height_; int rgb_roi_adjustment_; int rgb_bbox_min_diag_; @@ -238,7 +242,8 @@ class MultimodalObjectRecognitionROS **/ void publishDebug(mas_perception_msgs::ObjectList &combined_object_list, std::vector &clusters_3d, - std::vector &clusters_2d); + std::vector &clusters_2d, + std::vector &filtered_clusters_2d); /** \brief Load qualitative object info * \param[in] Path to the xml object file diff --git a/mir_perception/mir_object_recognition/ros/launch/multimodal_object_recognition.launch b/mir_perception/mir_object_recognition/ros/launch/multimodal_object_recognition.launch index 6cfd48bc3..b1463153d 100644 --- a/mir_perception/mir_object_recognition/ros/launch/multimodal_object_recognition.launch +++ b/mir_perception/mir_object_recognition/ros/launch/multimodal_object_recognition.launch @@ -1,7 +1,7 @@ - + @@ -24,7 +24,7 @@ - + diff --git a/mir_perception/mir_object_recognition/ros/launch/rgb_object_recognition.launch b/mir_perception/mir_object_recognition/ros/launch/rgb_object_recognition.launch index 63933b622..f87f006e6 100644 --- a/mir_perception/mir_object_recognition/ros/launch/rgb_object_recognition.launch +++ b/mir_perception/mir_object_recognition/ros/launch/rgb_object_recognition.launch @@ -1,8 +1,8 @@ - - + + @@ -14,6 +14,10 @@ + + - atwork + - cavity + diff --git a/mir_perception/mir_object_recognition/ros/scripts/coworker_assembly_detection_node b/mir_perception/mir_object_recognition/ros/scripts/coworker_assembly_detection_node new file mode 100755 index 000000000..a76c31b99 --- /dev/null +++ b/mir_perception/mir_object_recognition/ros/scripts/coworker_assembly_detection_node @@ -0,0 +1,338 @@ +#!/usr/bin/env python3 +import os +import random +import time +import cv2 +import numpy as np +import roslib +import rospy +import torch +import math +import csv +from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion +from cv_bridge import CvBridge, CvBridgeError +from std_msgs.msg import ColorRGBA, String +import message_filters +from mas_perception_msgs.msg import ImageList, Object, ObjectList, TimeStampedPose +from sensor_msgs.msg import Image, RegionOfInterest, PointCloud2, PointField +from sensor_msgs import point_cloud2 +from ultralytics import YOLO +import tf +from sklearn.decomposition import PCA +from geometry_msgs.msg import PoseStamped, Pose +from visualization_msgs.msg import Marker +from scipy.optimize import least_squares +from sklearn.mixture import GaussianMixture +import tf.transformations as tr +from scipy.spatial.transform import Rotation as R +import tf2_sensor_msgs +from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud +import tf2_ros +import tf2_py as tf2 +import ros_numpy +from scipy.optimize import least_squares +import pdb + +class CoworkerAssembly(): + def __init__(self, debug_mode=True): + + self.cvbridge = CvBridge() + self.debug = debug_mode + self.pub_debug = rospy.Publisher("/mir_perception/coworker_assembly/output/pick_object_debug", Image, queue_size=1) + self.pub_pose = rospy.Publisher("mcr_perception/object_selector/output/object_pose", PoseStamped, queue_size=1) + + self.tf_buffer = tf2_ros.Buffer(cache_time=rospy.Duration(12)) + self.tf_listener = tf.TransformListener() + self.tf2_listener = tf2_ros.TransformListener(self.tf_buffer) + + rospy.Subscriber("/mir_perception/coworker_assembly/event_in", String, self.event_in_cb) + self.event_out = rospy.Publisher("/mir_perception/coworker_assembly/event_out", String, queue_size=1) + + self.net = 'detection' + self.model_name = 'yolov8' + weights = os.path.join(roslib.packages.get_pkg_dir("mir_rgb_object_recognition_models"), + 'common', 'models', 'yolov8', 'robocup_2023_dataset', "coworker_assembly_model.pt") + self.weights = weights + self.confidence_threshold = 0.8 + self.iou_threshold = 0.45 + self.augment = False + + # Initialize + self.device = torch.device('cpu') + self.half = self.device.type != 'cpu' # half precision only supported on CUDA + + # Load model + if self.model_name == 'yolov8': + self.model = YOLO(weights) + + self.event = None + self.run_once = False + self.pose_sent = False + # self.event_in_cb("e_start") + + + def callback(self, img_msg, point_cloud_msg): + try: + self.run(img_msg, point_cloud_msg) + except Exception as e: + rospy.loginfo("[coworker_assembly_detection_node] killing callback") + + def event_in_cb(self, msg): + """ + Starts a planned motion based on the specified arm position. + + # """ + self.event = msg.data + if self.event.startswith("e_start") and not self.run_once: + # if True: + # Subscribe to image topic + self.sub_img = message_filters.Subscriber("/tower_cam3d_front/rgb/image_raw", Image) + + # subscribe to point cloud topic + self.sub_point_cloud = message_filters.Subscriber("/tower_cam3d_front/depth_registered/points", PointCloud2) + + # synchronize image and point cloud + self.ts = message_filters.ApproximateTimeSynchronizer([self.sub_img, self.sub_point_cloud], queue_size=10, slop=0.1) + self.ts.registerCallback(self.callback) + self.run_once = True + + if self.run_once and self.pose_sent: + self.sub_img.sub.unregister() + self.sub_point_cloud.sub.unregister() + rospy.loginfo("[coworker_assembly_detection_node] Unregistered image and pointcloud subscribers") + + def yolo_detect(self, cv_img): + if self.net == 'detection' and self.model_name == 'yolov8': + predictions = self.model.predict(source=cv_img, + conf=self.confidence_threshold, + iou=self.iou_threshold, + device=self.device, + verbose = False, + ) + # convert results to numpy array + predictions_np = predictions[0].boxes.numpy() + class_ids = predictions_np.cls + class_names = predictions[0].names + class_labels = [class_names[i] for i in class_ids] + class_scores = predictions_np.conf + class_bboxes = predictions_np.xyxy # x, y, w, h + + return class_bboxes, class_scores, class_labels + + def drawAxis(self, img, p_, q_, color, scale): + p = list(p_) + q = list(q_) + + ## [visualization1] + angle = math.atan2(p[1] - q[1], p[0] - q[0]) # angle in radians + hypotenuse = math.sqrt((p[1] - q[1]) * (p[1] - q[1]) + (p[0] - q[0]) * (p[0] - q[0])) + + # Here we lengthen the arrow by a factor of scale + q[0] = p[0] - scale * hypotenuse * math.cos(angle) + q[1] = p[1] - scale * hypotenuse * math.sin(angle) + cv2.line(img, (int(p[0]), int(p[1])), (int(q[0]), int(q[1])), color, 1, cv2.LINE_AA) + + # create the arrow hooks + p[0] = q[0] + 9 * math.cos(angle + math.pi / 4) + p[1] = q[1] + 9 * math.sin(angle + math.pi / 4) + cv2.line(img, (int(p[0]), int(p[1])), (int(q[0]), int(q[1])), color, 1, cv2.LINE_AA) + + p[0] = q[0] + 9 * math.cos(angle - math.pi / 4) + p[1] = q[1] + 9 * math.sin(angle - math.pi / 4) + cv2.line(img, (int(p[0]), int(p[1])), (int(q[0]), int(q[1])), color, 1, cv2.LINE_AA) + return img + + def get_orientation(self, pts, img): + ## [pca] + # Construct a buffer used by the pca analysis + sz = len(pts) + data_pts = np.empty((sz, 2), dtype=np.float64) + for i in range(data_pts.shape[0]): + data_pts[i,0] = pts[i,0,0] + data_pts[i,1] = pts[i,0,1] + + # Perform PCA analysis + mean = np.empty((0)) + mean, eigenvectors, eigenvalues = cv2.PCACompute2(data_pts, mean) + + # Store the center of the object + cntr = (int(mean[0,0]), int(mean[0,1])) + ## [pca] + + ## [visualization] + # Draw the principal components + # cv2.circle(img, cntr, 3, (255, 0, 255), 2) + p1 = (cntr[0] + 0.02 * eigenvectors[0,0] * eigenvalues[0,0], cntr[1] + 0.02 * eigenvectors[0,1] * eigenvalues[0,0]) + p2 = (cntr[0] - 0.02 * eigenvectors[1,0] * eigenvalues[1,0], cntr[1] - 0.02 * eigenvectors[1,1] * eigenvalues[1,0]) + img = self.drawAxis(img, cntr, p1, (255, 255, 0), 1) + # img = self.drawAxis(img, cntr, p2, (0, 0, 255), 5) + + angle = math.atan2(eigenvectors[0,1], eigenvectors[0,0]) # orientation in radians + return angle + + def get_pointcloud_position(self, pointcloud, center): + """ + pointcloud: pointcloud data of current frame with PointCloud2 data type + center: x and y coordinates of center of the bounding box of detected object + """ + + # Get the pointcloud data + pc = np.array(list(point_cloud2.read_points(pointcloud, skip_nans=False, field_names=("x", "y", "z"))), dtype=np.float32) + pc = pc.reshape((480,640,3)) + + # Get the center point of the bounding box + center_x, center_y = center + # center_point = pc[center_y, center_x] + + # Define the radius of the circle + radius = 7 + + # Calculate the coordinates of the circle points within the radius + min_x = max(0, center_x - radius) + max_x = min(pc.shape[1] - 1, center_x + radius) + min_y = max(0, center_y - radius) + max_y = min(pc.shape[0] - 1, center_y + radius) + + circle_points = [] + for y in range(min_y, max_y + 1): + for x in range(min_x, max_x + 1): + distance = math.sqrt((center_x - x) ** 2 + (center_y - y) ** 2) + if distance <= radius: + point = pc[y, x] + if not np.isnan(point).any(): + circle_points.append(point) + + # Determine the centroid of the non-NaN points + if circle_points: + circle_points_pc = np.array([point for point in circle_points]) + center_point = np.mean(circle_points_pc, axis=0) + + # Get the pose of the center point of the bounding box + pose = PoseStamped() + pose.header.frame_id = pointcloud.header.frame_id + pose.header.stamp = pointcloud.header.stamp + pose.pose.position.x = center_point[0] + pose.pose.position.y = center_point[1] + pose.pose.position.z = center_point[2] + pose.pose.orientation.x = 0.0 + pose.pose.orientation.y = 0.0 + pose.pose.orientation.z = 0.0 + pose.pose.orientation.w = 1.0 + + try: + t = self.tf_listener.getLatestCommonTime("base_link", pose.header.frame_id) + pose.header.stamp = t + pose = self.tf_listener.transformPose("base_link", pose) + # if pose doesnot has nan values then append pose to the list + if not math.isnan(pose.pose.position.x): + center_wrt_BL = pose.pose.position.x, pose.pose.position.y, pose.pose.position.z + return center_wrt_BL + except ( + tf.LookupException, + tf.ConnectivityException, + tf.ExtrapolationException, + ) as e: + rospy.logerr("Tf error: %s" % str(e)) + + return None + + def run(self, image, pointcloud): + """ + image: image data of current frame with Image data type + pointcloud: pointcloud data of current frame with PointCloud2 data type + """ + if image: + try: + cv_img = self.cvbridge.imgmsg_to_cv2(image, "bgr8") + bboxes, probs, labels = self.yolo_detect(cv_img) + + # Capatilize labels + labels = [label.upper() for label in labels] + bbox_img = None + + # Fit a Ellipse to the centers of the bounding boxes + for bbox, prob, label in zip(bboxes, probs, labels): + if label == "SCREW_NUT_ASSEMBLY": + # Calculate center of bounding box + center_x = (int(bbox[0]) + int(bbox[2])) // 2 + center_y = (int(bbox[1]) + int(bbox[3])) // 2 + center = np.array([center_x, center_y]) + + # Convert image to grayscale + bbox_img = cv_img[int(bbox[1]):int(bbox[3]), int(bbox[0]):int(bbox[2])] + if bbox_img is None: + continue + gray = cv2.cvtColor(bbox_img, cv2.COLOR_BGR2GRAY) + + # Convert image to binary + _, bw = cv2.threshold(gray, 50, 255, cv2.THRESH_BINARY | cv2.THRESH_OTSU) + + # Find all the contours in the thresholded image + contours, _ = cv2.findContours(bw, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE) + + # get contour with maximum area + max_area = 0 + best_cnt = None + for cnt in contours: + area = cv2.contourArea(cnt) + if area > max_area: + max_area = area + best_cnt = cnt + + yaw_angle = self.get_orientation(best_cnt, bbox_img) + + center_BL = self.get_pointcloud_position(pointcloud, center) + + predicted_pose = PoseStamped() + predicted_pose.header.stamp = pointcloud.header.stamp + predicted_pose.header.frame_id = "base_link_static" + predicted_pose.pose.position.x = center_BL[0] + predicted_pose.pose.position.y = center_BL[1] + predicted_pose.pose.position.z = 0.06 + + # convert yaw_list_CL to yaw_list_BL + yaw_BL = np.pi/2 - yaw_angle # from CL to BL + if yaw_BL > np.pi: + yaw_BL -= 2*np.pi + if yaw_BL < 0: + yaw_BL += np.pi + # now yaw_list is b/w 0 to np.pi (180 degrees) + orientation = tr.quaternion_from_euler(0, 0, yaw_BL) + predicted_pose.pose.orientation.x = orientation[0] + predicted_pose.pose.orientation.y = orientation[1] + predicted_pose.pose.orientation.z = orientation[2] + predicted_pose.pose.orientation.w = orientation[3] + + # publish the pose + self.pub_pose.publish(predicted_pose) + self.pose_sent = True + msg_str = f'e_done' + self.event_out.publish(String(data=msg_str)) + # print("pose e_done sent") + + if self.debug: + # computes the bounding box for the contour, and draws it on the frame, + (x,y,w,h) = cv2.boundingRect(best_cnt) + if w > 40 and h > 40: + cv2.rectangle(bbox_img, (x,y), (x+w,y+h), (255, 0, 0), 2) + + self.publish_debug_img(bbox_img) + + except CvBridgeError as e: + rospy.logerr(e) + return + else: + rospy.logwarn("No image received") + + def publish_debug_img(self, debug_img): + debug_img = np.array(debug_img, dtype=np.uint8) + debug_img = self.cvbridge.cv2_to_imgmsg(debug_img, "bgr8") + self.pub_debug.publish(debug_img) + + +if __name__ == '__main__': + rospy.init_node("coworker_assembly_detection") + rospy.loginfo('Started Coworker assembly detection Node.') + object_recognizer = CoworkerAssembly(debug_mode=True) + rospy.loginfo("Coworker assembly detection node is ready to give pose") + rospy.spin() diff --git a/mir_perception/mir_object_recognition/ros/scripts/hand_gesture_node b/mir_perception/mir_object_recognition/ros/scripts/hand_gesture_node new file mode 100755 index 000000000..47abb9d3c --- /dev/null +++ b/mir_perception/mir_object_recognition/ros/scripts/hand_gesture_node @@ -0,0 +1,156 @@ +#!/usr/bin/env python3 +import os +import random +import time +import cv2 +import numpy as np +import roslib +import rospy +import torch +import math +import csv +from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion +from cv_bridge import CvBridge, CvBridgeError +from std_msgs.msg import ColorRGBA, String +import message_filters +from mas_perception_msgs.msg import ImageList, Object, ObjectList, TimeStampedPose +from sensor_msgs.msg import Image, RegionOfInterest, PointCloud2, PointField +from sensor_msgs import point_cloud2 +from ultralytics import YOLO +import tf +from sklearn.decomposition import PCA +from geometry_msgs.msg import PoseStamped, Pose +from visualization_msgs.msg import Marker +from scipy.optimize import least_squares +from sklearn.mixture import GaussianMixture +import tf.transformations as tr +from scipy.spatial.transform import Rotation as R +import tf2_sensor_msgs +from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud +import tf2_ros +import tf2_py as tf2 +import ros_numpy +from scipy.optimize import least_squares +import pdb + +class GestureRecognition(): + def __init__(self, debug_mode=True): + + self.cvbridge = CvBridge() + self.debug = debug_mode + self.pub_debug = rospy.Publisher("/mir_perception/gesture_recognition/output/gesture_recognition_debug", Image, queue_size=1) + + rospy.Subscriber("/mir_perception/gesture_recognition/event_in", String, self.event_in_cb) + self.event_out = rospy.Publisher("/mir_perception/gesture_recognition/event_out", String, queue_size=1) + + self.net = 'detection' + self.model_name = 'yolov8' + weights = os.path.join(roslib.packages.get_pkg_dir("mir_rgb_object_recognition_models"), + 'common', 'models', 'yolov8', 'robocup_2023_dataset', "hand_gesture_model.pt") + self.weights = weights + self.confidence_threshold = 0.8 + self.iou_threshold = 0.45 + self.augment = False + + # Initialize + self.device = torch.device('cpu') + self.half = self.device.type != 'cpu' # half precision only supported on CUDA + + # Load model + if self.model_name == 'yolov8': + self.model = YOLO(weights) + + self.event = None + self.gesture_sent = False + self.frame_counter = 0 + # self.event_in_cb("e_start") + + + def callback(self, img_msg): + try: + self.run(img_msg) + except Exception as e: + rospy.loginfo("[hand_gesture_node] killing callback") + + def event_in_cb(self, msg): + """ + Starts a planned motion based on the specified arm position. + + # """ + self.event = msg.data + if self.event.startswith("e_start") and not self.gesture_sent: + # if not self.gesture_sent: + # Subscribe to image topic + self.sub_img = rospy.Subscriber("/tower_cam3d_front/rgb/image_raw", Image, self.callback) + + if self.gesture_sent: + self.sub_img.sub.unregister() + rospy.loginfo("[gesture_recognition_node] Unregistered image subscribers") + + def yolo_detect(self, cv_img): + if self.net == 'detection' and self.model_name == 'yolov8': + predictions = self.model.predict(source=cv_img, + conf=self.confidence_threshold, + iou=self.iou_threshold, + device=self.device, + verbose = False, + ) + # convert results to numpy array + predictions_np = predictions[0].boxes.numpy() + class_ids = predictions_np.cls + class_names = predictions[0].names + class_labels = [class_names[i] for i in class_ids] + class_scores = predictions_np.conf + class_bboxes = predictions_np.xyxy # x, y, w, h + + return class_bboxes, class_scores, class_labels, predictions + + def run(self, image): + """ + image: image data of current frame with Image data type + pointcloud: pointcloud data of current frame with PointCloud2 data type + """ + if image: + try: + cv_img = self.cvbridge.imgmsg_to_cv2(image, "bgr8") + bboxes, probs, labels, predictions = self.yolo_detect(cv_img) + + # Capatilize labels + labels = [label.upper() for label in labels] + + # Fit a Ellipse to the centers of the bounding boxes + for bbox, prob, label in zip(bboxes, probs, labels): + if label == "VICTORY": + self.frame_counter += 1 + else: + self.frame_counter = 0 + + if self.frame_counter == 5: + self.gesture_sent = True + msg_str = f'e_done' + self.event_out.publish(String(data=msg_str)) + # print("gesture e_done sent") + + if self.debug: + # Draw bounding boxes and labels of detections + debug_img = predictions[0].plot() + self.publish_debug_img(debug_img) + + except CvBridgeError as e: + rospy.logerr(e) + return + else: + rospy.logwarn("No image received") + + def publish_debug_img(self, debug_img): + debug_img = np.array(debug_img, dtype=np.uint8) + debug_img = self.cvbridge.cv2_to_imgmsg(debug_img, "bgr8") + self.pub_debug.publish(debug_img) + + +if __name__ == '__main__': + rospy.init_node("gesture_recognition") + rospy.loginfo('Started Gesture Recognition Node.') + object_recognizer = GestureRecognition(debug_mode=True) + rospy.loginfo("Gesture Recognition node is ready to recognize gesture") + rospy.spin() diff --git a/mir_perception/mir_object_recognition/ros/scripts/pc_object_recognizer_node b/mir_perception/mir_object_recognition/ros/scripts/pc_object_recognizer_node index 31822be38..4b4eb7ea8 100755 --- a/mir_perception/mir_object_recognition/ros/scripts/pc_object_recognizer_node +++ b/mir_perception/mir_object_recognition/ros/scripts/pc_object_recognizer_node @@ -158,8 +158,9 @@ if __name__ == '__main__': model_dir = rospy.get_param("~model_dir") dataset = rospy.get_param("~dataset") - object_recognizer = PointcloudObjectRecognizer( - model, model_id, dataset, model_dir) - rospy.loginfo('\033[92m'+"PCL Recognizer is ready using %s , model: %s ", - model, model_id) + # commented as we'are not using it + #object_recognizer = PointcloudObjectRecognizer( + # model, model_id, dataset, model_dir) + #rospy.loginfo('\033[92m'+"PCL Recognizer is ready using %s , model: %s ", + # model, model_id) rospy.spin() diff --git a/mir_perception/mir_object_recognition/ros/scripts/rgb_object_recognizer_node b/mir_perception/mir_object_recognition/ros/scripts/rgb_object_recognizer_node index bc0146d74..ec564992a 100755 --- a/mir_perception/mir_object_recognition/ros/scripts/rgb_object_recognizer_node +++ b/mir_perception/mir_object_recognition/ros/scripts/rgb_object_recognizer_node @@ -9,18 +9,13 @@ import torch from cv_bridge import CvBridge, CvBridgeError from mas_perception_msgs.msg import ImageList, Object, ObjectList from sensor_msgs.msg import Image, RegionOfInterest -from rgb_object_recognition.yolov7.models.experimental import attempt_load -from rgb_object_recognition.yolov7.utils.datasets import letterbox -from rgb_object_recognition.yolov7.utils.general import check_img_size, non_max_suppression, apply_classifier, \ - scale_coords, xyxy2xywh -from rgb_object_recognition.yolov7.utils.plots import plot_one_box -from rgb_object_recognition.yolov7.utils.torch_utils import select_device, TracedModel, load_classifier, \ - time_synchronized - +from ultralytics import YOLO class RGBObjectRecognizer(): - def __init__(self, weights, net='detection', model_name='yolov7', - confidence_threshold=0.8, + def __init__(self, weights, + net='detection', + model_name='yolov8', + confidence_threshold=0.65, iou_threshold=0.45, img_size=640, trace=True, @@ -28,9 +23,6 @@ class RGBObjectRecognizer(): augment=False, device='cpu', debug_mode=True): - import sys - sys.path.insert(0, os.path.join(roslib.packages.get_pkg_dir("mir_object_recognition"), - 'common', 'src', 'rgb_object_recognition', 'yolov7')) self.cvbridge = CvBridge() self.debug = debug_mode @@ -45,32 +37,12 @@ class RGBObjectRecognizer(): self.weights = weights self.confidence_threshold = confidence_threshold self.iou_threshold = iou_threshold - self.augment = augment - - # Initialize - self.device = select_device(device) - self.half = self.device.type != 'cpu' # half precision only supported on CUDA + self.device = "cuda" if torch.cuda.is_available() else device # Load model - self.model = attempt_load(weights, map_location=self.device) # load FP32 model - self.names = self.model.module.names if hasattr(self.model, 'module') else self.model.names - self.colors = [[random.randint(0, 255) for _ in range(3)] for _ in self.names] - self.stride = int(self.model.stride.max()) # model stride - img_size = check_img_size(img_size, s=self.stride) # check img_size - self.img_size = img_size - - if trace: - self.model = TracedModel(self.model, self.device, img_size) - - if self.half: - self.model.half() # to FP16 - - # Second-stage classifier - self.classify = classify - if self.classify: - self.modelc = load_classifier(name='resnet101', n=2) # initialize - self.modelc.load_state_dict(torch.load('weights/resnet101.pt', map_location=self.device)['model']).to( - self.device).eval() + if self.model_name == 'yolov8': + self.model_atwork = YOLO(self.weights[0]) + self.model_cavity = YOLO(self.weights[1]) def image_recognition_cb(self, img_msg): if img_msg.images: @@ -82,61 +54,39 @@ class RGBObjectRecognizer(): try: cv_img = self.cvbridge.imgmsg_to_cv2( img_msg.images[0], "bgr8") - # Padded resize - img = letterbox(cv_img, self.img_size, stride=self.stride)[0] - - # Convert - img = img[:, :, ::-1].transpose(2, 0, 1) # BGR to RGB, to 3x416x416 - img = np.ascontiguousarray(img) - - if self.model_name == 'yolov7': - old_img_w = old_img_h = self.img_size - old_img_b = 1 - - img = torch.from_numpy(img).to(self.device) - img = img.half() if self.half else img.float() # uint8 to fp16/32 - img /= 255.0 # 0 - 255 to 0.0 - 1.0 - if img.ndimension() == 3: - img = img.unsqueeze(0) - - # Inference - t1 = time_synchronized() - pred = self.model(img, augment=self.augment)[0] - t2 = time_synchronized() - - # Apply NMS - pred = non_max_suppression(pred, self.confidence_threshold, self.iou_threshold, classes=None, - agnostic=True) - t3 = time_synchronized() - - # Apply Classifier - if self.classify: - pred = apply_classifier(pred, self.modelc, img, cv_img) - - # Process detections - bboxes = [] - probs = [] - labels = [] - for i, det in enumerate(pred): # detections per image - gn = torch.tensor(cv_img.shape)[[1, 0, 1, 0]] # normalization gain whwh - if len(det): - # Rescale boxes from img_size to im0 size - det[:, :4] = scale_coords(img.shape[2:], det[:, :4], cv_img.shape).round() - - # Write results - for *xyxy, conf, cls in reversed(det): - if self.debug: - label = f'{self.names[int(cls)]} {conf:.2f}' - plot_one_box(xyxy, cv_img, label=label, color=self.colors[int(cls)], - line_thickness=1) - #xywh = (xyxy2xywh(torch.tensor(xyxy).view(-1, 4)) / gn).view(-1).tolist() # normalized xywh - bboxes.append(xyxy) - probs.append(conf) - labels.append(self.names[int(cls)]) - if self.debug: - # publish bbox and label - self.publish_debug_img(cv_img) - print(labels) + + if rospy.has_param("/mir_perception/multimodal_object_recognition/obj_category"): + obj_category = rospy.get_param("/mir_perception/multimodal_object_recognition/obj_category") + if obj_category == "cavity": + self.model = self.model_cavity + elif obj_category == "container": + self.model = self.model_container + else: + self.model = self.model_atwork + # print the model name + rospy.loginfo(f"\033[92m" + f"[RGB Recognizer] is using {obj_category} objects category model" + f"\033[0m") + else: + rospy.loginfo(f"\033[92m" + f"[RGB Recognizer] is using atwork objects category model" + f"\033[0m") + + if self.model_name == 'yolov8': + + predictions = self.model.predict(source=cv_img, + conf=self.confidence_threshold, + iou=self.iou_threshold, + device=self.device, + verbose=False + ) + + # convert results to numpy array + predictions_np = predictions[0].boxes.numpy() + class_ids = predictions_np.cls + class_names = predictions[0].names + class_labels = [class_names[i] for i in class_ids] + class_scores = predictions_np.conf + class_bboxes = predictions_np.xyxy # x, y, w, h + + bboxes, probs, labels = class_bboxes, class_scores, class_labels + for i in range(len(labels)): result = Object() result.name = labels[i].upper() @@ -144,15 +94,23 @@ class RGBObjectRecognizer(): roi = RegionOfInterest() roi.x_offset = int(bboxes[i][0]) roi.y_offset = int(bboxes[i][1]) - roi.width = int(bboxes[i][2]) - int(bboxes[i][0]) - roi.height = int(bboxes[i][3]) - int(bboxes[i][1]) + roi.width = int(bboxes[i][2] - bboxes[i][0]) + roi.height = int(bboxes[i][3] - bboxes[i][1]) result.roi = roi + objects.append(result) + if self.debug: - rospy.logdebug("Detected Objects: %s", labels) + # Draw bounding boxes and labels of detections + debug_img = predictions[0].plot() + + # publish bbox and label + self.publish_debug_img(debug_img) + else: - bboxes, probs, labels = self.model.classify(cv_img) + rospy.logerr("[RGB Recognition] Model not supported") + return # Publish result_list result_list.objects = objects @@ -178,11 +136,22 @@ if __name__ == '__main__': classifier_name = rospy.get_param("~classifier") dataset = rospy.get_param("~dataset") model_dir = rospy.get_param("~model_dir") - weights = os.path.join(roslib.packages.get_pkg_dir("mir_rgb_object_recognition_models"), - 'common', 'models', classifier_name, dataset, "best.pt") - + model_category = rospy.get_param("~model_category") + weight_1 = os.path.join(roslib.packages.get_pkg_dir("mir_rgb_object_recognition_models"), + 'common', 'models', classifier_name, dataset, model_category[0]+".pt") + weight_2 = os.path.join(roslib.packages.get_pkg_dir("mir_rgb_object_recognition_models"), + 'common', 'models', classifier_name, dataset, model_category[1]+".pt") + # check if the model exists + if not os.path.isfile(weight_1): + rospy.logerr("[RGB Recognition] Model not found: %s", weight_1) + exit(-1) + if not os.path.isfile(weight_2): + rospy.logerr("[RGB Recognition] Model not found: %s", weight_2) + exit(-1) + weights = [weight_1, weight_2] # 0th index is the default model, eg. atwork object_recognizer = RGBObjectRecognizer( - weights=weights, debug_mode=True) - rospy.loginfo("RGB Recognizer is ready using %s : %s , dataset: %s ", - net, classifier_name, dataset) + weights, + debug_mode=True) + rospy.loginfo("\033[92m" + "RGB Recognizer is ready using %s : %s , dataset: %s " + "\033[0m", + net, classifier_name, dataset) rospy.spin() diff --git a/mir_perception/mir_object_recognition/ros/scripts/rgb_object_recognizer_node_yolov7 b/mir_perception/mir_object_recognition/ros/scripts/rgb_object_recognizer_node_yolov7 new file mode 100755 index 000000000..1c98638e1 --- /dev/null +++ b/mir_perception/mir_object_recognition/ros/scripts/rgb_object_recognizer_node_yolov7 @@ -0,0 +1,188 @@ +#!/usr/bin/env python3 +import os +import random + +import numpy as np +import roslib +import rospy +import torch +from cv_bridge import CvBridge, CvBridgeError +from mas_perception_msgs.msg import ImageList, Object, ObjectList +from sensor_msgs.msg import Image, RegionOfInterest +from rgb_object_recognition.yolov7.models.experimental import attempt_load +from rgb_object_recognition.yolov7.utils.datasets import letterbox +from rgb_object_recognition.yolov7.utils.general import check_img_size, non_max_suppression, apply_classifier, \ + scale_coords, xyxy2xywh +from rgb_object_recognition.yolov7.utils.plots import plot_one_box +from rgb_object_recognition.yolov7.utils.torch_utils import select_device, TracedModel, load_classifier, \ + time_synchronized + + +class RGBObjectRecognizer(): + def __init__(self, weights, net='detection', model_name='yolov7', + confidence_threshold=0.8, + iou_threshold=0.45, + img_size=640, + trace=True, + classify=False, + augment=False, + device='cpu', + debug_mode=True): + import sys + sys.path.insert(0, os.path.join(roslib.packages.get_pkg_dir("mir_object_recognition"), + 'common', 'src', 'rgb_object_recognition', 'yolov7')) + + self.cvbridge = CvBridge() + self.debug = debug_mode + self.pub_debug = rospy.Publisher( + "/mir_perception/multimodal_object_recognition/recognizer/rgb/output/debug_image", Image, queue_size=1) + self.pub_result = rospy.Publisher( + "output/object_list", ObjectList, queue_size=1) + self.sub_img = rospy.Subscriber( + "input/images", ImageList, self.image_recognition_cb) + self.net = net + self.model_name = model_name + self.weights = weights + self.confidence_threshold = confidence_threshold + self.iou_threshold = iou_threshold + self.augment = augment + + # Initialize + self.device = select_device(device) + self.half = self.device.type != 'cpu' # half precision only supported on CUDA + + # Load model + self.model = attempt_load(weights, map_location=self.device) # load FP32 model + self.names = self.model.module.names if hasattr(self.model, 'module') else self.model.names + self.colors = [[random.randint(0, 255) for _ in range(3)] for _ in self.names] + self.stride = int(self.model.stride.max()) # model stride + img_size = check_img_size(img_size, s=self.stride) # check img_size + self.img_size = img_size + + if trace: + self.model = TracedModel(self.model, self.device, img_size) + + if self.half: + self.model.half() # to FP16 + + # Second-stage classifier + self.classify = classify + if self.classify: + self.modelc = load_classifier(name='resnet101', n=2) # initialize + self.modelc.load_state_dict(torch.load('weights/resnet101.pt', map_location=self.device)['model']).to( + self.device).eval() + + def image_recognition_cb(self, img_msg): + if img_msg.images: + result_list = ObjectList() + objects = [] + rospy.loginfo("[{}] images received: {} ".format( + len(img_msg.images), self.model_name)) + if self.net == 'detection': + try: + cv_img = self.cvbridge.imgmsg_to_cv2( + img_msg.images[0], "bgr8") + # Padded resize + img = letterbox(cv_img, self.img_size, stride=self.stride)[0] + + # Convert + img = img[:, :, ::-1].transpose(2, 0, 1) # BGR to RGB, to 3x416x416 + img = np.ascontiguousarray(img) + + if self.model_name == 'yolov7': + old_img_w = old_img_h = self.img_size + old_img_b = 1 + + img = torch.from_numpy(img).to(self.device) + img = img.half() if self.half else img.float() # uint8 to fp16/32 + img /= 255.0 # 0 - 255 to 0.0 - 1.0 + if img.ndimension() == 3: + img = img.unsqueeze(0) + + # Inference + t1 = time_synchronized() + pred = self.model(img, augment=self.augment)[0] + t2 = time_synchronized() + + # Apply NMS + pred = non_max_suppression(pred, self.confidence_threshold, self.iou_threshold, classes=None, + agnostic=True) + t3 = time_synchronized() + + # Apply Classifier + if self.classify: + pred = apply_classifier(pred, self.modelc, img, cv_img) + + # Process detections + bboxes = [] + probs = [] + labels = [] + for i, det in enumerate(pred): # detections per image + gn = torch.tensor(cv_img.shape)[[1, 0, 1, 0]] # normalization gain whwh + if len(det): + # Rescale boxes from img_size to im0 size + det[:, :4] = scale_coords(img.shape[2:], det[:, :4], cv_img.shape).round() + + # Write results + for *xyxy, conf, cls in reversed(det): + if self.debug: + label = f'{self.names[int(cls)]} {conf:.2f}' + plot_one_box(xyxy, cv_img, label=label, color=self.colors[int(cls)], + line_thickness=1) + #xywh = (xyxy2xywh(torch.tensor(xyxy).view(-1, 4)) / gn).view(-1).tolist() # normalized xywh + bboxes.append(xyxy) + probs.append(conf) + labels.append(self.names[int(cls)]) + if self.debug: + # publish bbox and label + self.publish_debug_img(cv_img) + print(labels) + for i in range(len(labels)): + result = Object() + result.name = labels[i].upper() + result.probability = probs[i] + roi = RegionOfInterest() + roi.x_offset = int(bboxes[i][0]) + roi.y_offset = int(bboxes[i][1]) + roi.width = int(bboxes[i][2]) - int(bboxes[i][0]) + roi.height = int(bboxes[i][3]) - int(bboxes[i][1]) + result.roi = roi + objects.append(result) + if self.debug: + rospy.logdebug("Detected Objects: %s", labels) + + else: + bboxes, probs, labels = self.model.classify(cv_img) + + # Publish result_list + result_list.objects = objects + self.pub_result.publish(result_list) + + except CvBridgeError as e: + rospy.logerr(e) + return + + elif self.net == 'classification': + rospy.logwarn("TODO: MobileNet") + + def publish_debug_img(self, debug_img): + debug_img = np.array(debug_img, dtype=np.uint8) + debug_img = self.cvbridge.cv2_to_imgmsg(debug_img, "bgr8") + self.pub_debug.publish(debug_img) + + +if __name__ == '__main__': + rospy.init_node("rgb_object_recognizer") + rospy.loginfo('Started object recognition node.') + net = rospy.get_param("~net") + classifier_name = rospy.get_param("~classifier") + dataset = rospy.get_param("~dataset") + model_dir = rospy.get_param("~model_dir") + weights = os.path.join(roslib.packages.get_pkg_dir("mir_rgb_object_recognition_models"), + 'common', 'models', classifier_name, dataset, "best.pt") #"rc23_bins.pt" + + object_recognizer = RGBObjectRecognizer( + weights=weights, debug_mode=True) + rospy.loginfo("RGB Recognizer is ready using %s : %s , dataset: %s ", + net, classifier_name, dataset) + rospy.spin() diff --git a/mir_perception/mir_object_recognition/ros/src/multimodal_object_recognition_node.cpp b/mir_perception/mir_object_recognition/ros/src/multimodal_object_recognition_node.cpp index 3b4163531..930392027 100644 --- a/mir_perception/mir_object_recognition/ros/src/multimodal_object_recognition_node.cpp +++ b/mir_perception/mir_object_recognition/ros/src/multimodal_object_recognition_node.cpp @@ -44,13 +44,15 @@ MultimodalObjectRecognitionROS::MultimodalObjectRecognitionROS(ros::NodeHandle n bounding_box_visualizer_pc_("output/bounding_boxes", Color(Color::IVORY)), cluster_visualizer_rgb_("output/tabletop_cluster_rgb"), cluster_visualizer_pc_("output/tabletop_cluster_pc"), + cluster_visualizer_filtered_rgb_("output/tabletop_cluster_filtered_rgb"), label_visualizer_rgb_("output/rgb_labels", Color(Color::SEA_GREEN)), label_visualizer_pc_("output/pc_labels", Color(Color::IVORY)), data_collection_(false), enable_roi_(true), rgb_cluster_remove_outliers_(true), enable_rgb_recognizer_(true), - enable_pc_recognizer_(true) + enable_pc_recognizer_(true), + obj_category_("atwork") { tf_listener_.reset(new tf::TransformListener); scene_segmentation_ros_ = SceneSegmentationROSSPtr(new SceneSegmentationROS()); @@ -94,12 +96,10 @@ MultimodalObjectRecognitionROS::MultimodalObjectRecognitionROS(ros::NodeHandle n ROS_WARN_STREAM("[multimodal_object_recognition] target frame: " <("pointcloud_source_frame_id", pointcloud_source_frame_id_, "fixed_camera_link"); - nh_.param("logdir", logdir_, "/tmp/"); + nh_.param("logdir", logdir_, "/home/robocup/perception_debug_data/"); nh_.param("object_info", object_info_path_, "None"); loadObjectInfo(object_info_path_); - pub_filtered_rgb_cloud_plane_ = - nh_.advertise("filtered_rgb_cloud_plane", 1); } MultimodalObjectRecognitionROS::~MultimodalObjectRecognitionROS() @@ -109,9 +109,9 @@ MultimodalObjectRecognitionROS::~MultimodalObjectRecognitionROS() void MultimodalObjectRecognitionROS::synchronizeCallback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::PointCloud2ConstPtr &cloud) { + ROS_INFO("[multimodal_object_recognition_ros] Received enough messages"); if (pointcloud_msg_received_count_ < 1) { - ROS_INFO("[multimodal_object_recognition_ros] Received enough messages"); pointcloud_msg_ = cloud; pointcloud_msg_->header.frame_id = pointcloud_source_frame_id_; pointcloud_msg_received_count_ += 1; @@ -256,7 +256,7 @@ void MultimodalObjectRecognitionROS::recognizeCloudAndImage() filename.append("rgb_raw_"); filename.append(std::to_string(ros::Time::now().toSec())); mpu::object::saveCVImage(raw_cv_image, logdir_, filename); - ROS_INFO_STREAM("Image:" << filename << " saved to " << logdir_); + ROS_DEBUG_STREAM("Image:" << filename << " saved to " << logdir_); } else { @@ -266,6 +266,10 @@ void MultimodalObjectRecognitionROS::recognizeCloudAndImage() return; } + // Reset recognition callback flags + received_recognized_cloud_list_flag_ = false; + received_recognized_image_list_flag_ = false; + // Publish 3D object cluster for recognition if (!cloud_object_list.objects.empty() && enable_pc_recognizer_) { @@ -347,6 +351,7 @@ void MultimodalObjectRecognitionROS::recognizeCloudAndImage() mas_perception_msgs::ObjectList rgb_object_list; mas_perception_msgs::BoundingBoxList bounding_boxes; std::vector clusters_2d; + std::vector filtered_clusters_2d; cv_bridge::CvImagePtr cv_image; if (recognized_image_list_.objects.size() > 0) @@ -372,6 +377,10 @@ void MultimodalObjectRecognitionROS::recognizeCloudAndImage() { object.shape.shape = object.shape.SPHERE; } + else if (flat_objects_.count(recognized_image_list_.objects[i].name)) + { + object.shape.shape = "flat"; + } else { object.shape.shape = object.shape.OTHER; @@ -399,7 +408,15 @@ void MultimodalObjectRecognitionROS::recognizeCloudAndImage() // Remove large 2d misdetected bbox (misdetection) double len_diag = sqrt(powf(roi_2d.width, 2) + powf(roi_2d.height, 2)); - if (len_diag > rgb_bbox_min_diag_ && len_diag < rgb_bbox_max_diag_) + // check if object name has container + bool is_container = false; + if (object.name == "CONTAINER_BOX_BLUE" || object.name == "CONTAINER_BOX_RED") + { + ROS_INFO("Found container object %s", object.name.c_str()); + is_container = true; + } + + if ((len_diag > rgb_bbox_min_diag_ && len_diag < rgb_bbox_max_diag_) || is_container) { PointCloud::Ptr cloud_roi(new PointCloud); bool getROISuccess = mpu::pointcloud::getPointCloudROI(roi_2d, cloud_, cloud_roi, @@ -431,14 +448,19 @@ void MultimodalObjectRecognitionROS::recognizeCloudAndImage() // Publish filtered point cloud from RGB recognizer // ************************************************ - PointCloud filtered_rgb_pointcloud; - filtered_rgb_pointcloud = mpu::object::estimatePose(cloud_roi, pose, object.shape.shape, + // PointCloud filtered_rgb_pointcloud; + PointCloud::Ptr filtered_rgb_pointcloud(new PointCloud); + *filtered_rgb_pointcloud = mpu::object::estimatePose(cloud_roi, pose, object, object.shape.shape, rgb_cluster_filter_limit_min_, - rgb_cluster_filter_limit_max_); + rgb_cluster_filter_limit_max_, + obj_category_); + // append filtered point cloud to filtered_clusters_2d + filtered_clusters_2d.push_back(filtered_rgb_pointcloud); + PointT min_pt; PointT max_pt; - pcl::getMinMax3D(*cloud_roi, min_pt, max_pt); + pcl::getMinMax3D(*filtered_rgb_pointcloud, min_pt, max_pt); rgb_object_list.objects[i].dimensions.vector.z = max_pt.z - min_pt.z; ROS_INFO("[RGB Object Height] Object %s length: %f", object.name.c_str(), rgb_object_list.objects[i].dimensions.vector.z); @@ -448,15 +470,6 @@ void MultimodalObjectRecognitionROS::recognizeCloudAndImage() ROS_INFO("[RGB Object Height] Object %s length is greater than 9cm: %f", object.name.c_str(), max_pt.z); } - sensor_msgs::PointCloud2 ros_filtered_rgb_pointcloud; - pcl::toROSMsg(filtered_rgb_pointcloud, ros_filtered_rgb_pointcloud); - ros_filtered_rgb_pointcloud.header.frame_id = target_frame_id_; - - pub_filtered_rgb_cloud_plane_.publish(ros_filtered_rgb_pointcloud); - - // To visualize the filtered point cloud, sleep for 3 seconds after every point cloud - // ros::Duration(3.0).sleep(); - //********************************* // Transform pose @@ -486,6 +499,7 @@ void MultimodalObjectRecognitionROS::recognizeCloudAndImage() } else { + ROS_WARN("[RGB] BBOX too big or too small"); ROS_DEBUG("[RGB] DECOY"); rgb_object_list.objects[i].name = "DECOY"; rgb_object_list.objects[i].database_id = rgb_object_id_; @@ -523,13 +537,32 @@ void MultimodalObjectRecognitionROS::recognizeCloudAndImage() else { ROS_WARN("No objects to publish"); + if (debug_mode_) + { + ros::Time time_now = ros::Time::now(); + // Save raw image + cv_bridge::CvImagePtr raw_cv_image; + if (mpu::object::getCVImage(image_msg_, raw_cv_image)) + { + std::string filename = ""; + filename = ""; + filename.append("rgb_raw_"); + filename.append(std::to_string(time_now.toSec())); + mpu::object::saveCVImage(raw_cv_image, logdir_, filename); + ROS_DEBUG_STREAM("Image:" << filename << " saved to " << logdir_); + } + else + { + ROS_ERROR("Cannot generate cv image..."); + } + } return; } if (debug_mode_) { - ROS_WARN_STREAM("Debug mode: publishing object information"); - publishDebug(combined_object_list, clusters_3d, clusters_2d); + ROS_DEBUG_STREAM("Debug mode: publishing object information"); + publishDebug(combined_object_list, clusters_3d, clusters_2d, filtered_clusters_2d); ros::Time time_now = ros::Time::now(); @@ -540,7 +573,7 @@ void MultimodalObjectRecognitionROS::recognizeCloudAndImage() filename.append("rgb_debug_"); filename.append(std::to_string(time_now.toSec())); mpu::object::saveCVImage(cv_image, logdir_, filename); - ROS_INFO_STREAM("Image:" << filename << " saved to " << logdir_); + ROS_DEBUG_STREAM("Image:" << filename << " saved to " << logdir_); } else { @@ -555,7 +588,7 @@ void MultimodalObjectRecognitionROS::recognizeCloudAndImage() filename.append("rgb_raw_"); filename.append(std::to_string(time_now.toSec())); mpu::object::saveCVImage(raw_cv_image, logdir_, filename); - ROS_INFO_STREAM("Image:" << filename << " saved to " << logdir_); + ROS_DEBUG_STREAM("Image:" << filename << " saved to " << logdir_); } else { @@ -570,18 +603,17 @@ void MultimodalObjectRecognitionROS::recognizeCloudAndImage() filename.append("pcd_cluster_"); filename.append(std::to_string(time_now.toSec())); mpu::object::savePcd(cluster, logdir_, filename); - ROS_INFO_STREAM("Point cloud:" << filename << " saved to " << logdir_); + ROS_DEBUG_STREAM("Point cloud:" << filename << " saved to " << logdir_); } } } void MultimodalObjectRecognitionROS::publishDebug(mas_perception_msgs::ObjectList &combined_object_list, std::vector &clusters_3d, - std::vector &clusters_2d) + std::vector &clusters_2d, + std::vector &filtered_clusters_2d) { - ROS_INFO_STREAM("Cloud list: " << recognized_cloud_list_.objects.size()); - ROS_INFO_STREAM("RGB list: " << recognized_image_list_.objects.size()); - ROS_INFO_STREAM("Combined object list: "<< combined_object_list.objects.size()); + ROS_INFO_STREAM("Cloud list: " << recognized_cloud_list_.objects.size() << " RGB list: " << recognized_image_list_.objects.size() << " Combined object list: " << combined_object_list.objects.size()); // Compute normal to generate parallel BBOX to the plane const Eigen::Vector3f normal = scene_segmentation_ros_->getPlaneNormal(); @@ -668,6 +700,10 @@ void MultimodalObjectRecognitionROS::publishDebug(mas_perception_msgs::ObjectLis label_visualizer_rgb_.publish(rgb_labels, rgb_object_pose_array); } } + if (filtered_clusters_2d.size() > 0) + { + cluster_visualizer_filtered_rgb_.publish(filtered_clusters_2d, target_frame_id_); + } } void MultimodalObjectRecognitionROS::publishObjectList(mas_perception_msgs::ObjectList &object_list) @@ -709,19 +745,29 @@ void MultimodalObjectRecognitionROS::adjustObjectPose(mas_perception_msgs::Objec { yaw = 0.0; } + if (object_list.objects[i].name == "M30_H" + or object_list.objects[i].name == "M20_H" + or object_list.objects[i].name == "S40_40_V" + or object_list.objects[i].name == "F20_20_V") + { + ROS_WARN("Setting yaw for M30/M20 to zero"); + yaw = 0.0; + } // Update container pose if (object_list.objects[i].name == "CONTAINER_BOX_RED" || object_list.objects[i].name == "CONTAINER_BOX_BLUE") { - if (object_list.objects[i].database_id > 100) + if (object_list.objects[i].database_id >= 100) { - ROS_DEBUG_STREAM("Updating RGB container pose"); + ROS_INFO_STREAM("Updating RGB container pose for " << object_list.objects[i].name); mm_object_recognition_utils_->adjustContainerPose(object_list.objects[i], container_height_); } } - if (object_list.objects[i].dimensions.vector.z > 0.09) + if (object_list.objects[i].dimensions.vector.z > 0.09 and + object_list.objects[i].name != "CONTAINER_BOX_RED" && + object_list.objects[i].name != "CONTAINER_BOX_BLUE") { tf::Quaternion q2; q2.setRPY(0.0, -1.57, 0.0); @@ -739,10 +785,62 @@ void MultimodalObjectRecognitionROS::adjustObjectPose(mas_perception_msgs::Objec object_list.objects[i].pose.pose.orientation.z = q2.z(); object_list.objects[i].pose.pose.orientation.w = q2.w(); - object_list.objects[i].pose.pose.position.z = scene_segmentation_ros_->getWorkspaceHeight() + + double detected_object_height = object_list.objects[i].pose.pose.position.z; + if (obj_category_ == "cavity") + { + ROS_WARN_STREAM("PP01 workstation; not updating height"); + } + else if (object_list.objects[i].name == "CONTAINER_BOX_RED" || + object_list.objects[i].name == "CONTAINER_BOX_BLUE") + { + ROS_WARN_STREAM("Container; not updating height"); + } + else if (use_fixed_heights_ or (std::fabs(detected_object_height - scene_segmentation_ros_->getWorkspaceHeight()) > 0.03)) + { + if (use_fixed_heights_) + { + ROS_WARN_STREAM("Assuming fixed platform heights of 0, 5, 10 and 15 cm"); + } + else + { + ROS_WARN_STREAM("Difference between object height and workspace height is > 3cm"); + } + // do something + bool is_0cm = std::fabs(detected_object_height - height_of_floor_) < 0.01; + bool is_5cm = std::fabs(detected_object_height - (height_of_floor_ + 0.05)) < 0.01; + bool is_10cm = std::fabs(detected_object_height - (height_of_floor_ + 0.1)) < 0.01; + bool is_15cm = std::fabs(detected_object_height - (height_of_floor_ + 0.15)) < 0.01; + if (is_0cm) + { + ROS_WARN_STREAM("Updating height to 0cm"); + object_list.objects[i].pose.pose.position.z = height_of_floor_ + object_height_above_workspace_; + } + if (is_5cm) + { + ROS_WARN_STREAM("Updating height to 5cm"); + object_list.objects[i].pose.pose.position.z = height_of_floor_ + 0.05 + object_height_above_workspace_; + } + if (is_10cm) + { + ROS_WARN_STREAM("Updating height to 10cm"); + object_list.objects[i].pose.pose.position.z = height_of_floor_ + 0.1 + object_height_above_workspace_; + } + if (is_15cm) + { + ROS_WARN_STREAM("Updating height to 15cm"); + object_list.objects[i].pose.pose.position.z = height_of_floor_ + 0.15 + object_height_above_workspace_; + } + + } + else + { + object_list.objects[i].pose.pose.position.z = scene_segmentation_ros_->getWorkspaceHeight() + object_height_above_workspace_; + } + } + /* // Update workspace height if (scene_segmentation_ros_->getWorkspaceHeight() != -1000.0) { @@ -755,9 +853,10 @@ void MultimodalObjectRecognitionROS::adjustObjectPose(mas_perception_msgs::Objec ROS_WARN_STREAM("Updated container height: " << object_list.objects[i].pose.pose.position.z ); } } + */ // Update axis or bolt pose - if (object_list.objects[i].name == "M20_100" || object_list.objects[i].name == "AXIS") + if (object_list.objects[i].name == "M20_100" || object_list.objects[i].name == "AXIS" || object_list.objects[i].name == "SCREWDRIVER") { mm_object_recognition_utils_->adjustAxisBoltPose(object_list.objects[i]); } @@ -785,6 +884,10 @@ void MultimodalObjectRecognitionROS::loadObjectInfo(const std::string &filename) { round_objects_.insert(f.name); } + else if (f.shape == "flat") + { + flat_objects_.insert(f.name); + } object_info_.push_back(f); } } @@ -801,12 +904,14 @@ void MultimodalObjectRecognitionROS::loadObjectInfo(const std::string &filename) void MultimodalObjectRecognitionROS::eventCallback(const std_msgs::String::ConstPtr &msg) { std_msgs::String event_out; + ROS_INFO("Event call back"); + if (msg->data == "e_start") { // Synchronize callback image_sub_ = new message_filters::Subscriber (nh_, "input_image_topic", 1); cloud_sub_ = new message_filters::Subscriber (nh_, "input_cloud_topic", 1); - msg_sync_ = new message_filters::Synchronizer (msgSyncPolicy(10), *image_sub_, *cloud_sub_); + msg_sync_ = new message_filters::Synchronizer (msgSyncPolicy(1), *image_sub_, *cloud_sub_); msg_sync_->registerCallback(boost::bind(&MultimodalObjectRecognitionROS::synchronizeCallback, this, _1, _2)); } else if (msg->data == "e_stop") @@ -859,13 +964,16 @@ void MultimodalObjectRecognitionROS::configCallback(mir_object_recognition::Scen // Object recognizer param enable_rgb_recognizer_ = config.enable_rgb_recognizer; enable_pc_recognizer_ = config.enable_pc_recognizer; + obj_category_ = config.obj_category; // Cluster param center_cluster_ = config.center_cluster; pad_cluster_ = config.pad_cluster; padded_cluster_size_ = config.padded_cluster_size; // Workspace and object height + use_fixed_heights_ = config.use_fixed_heights; object_height_above_workspace_ = config.object_height_above_workspace; + height_of_floor_ = config.height_of_floor; container_height_ = config.container_height; // RGB proposal params rgb_roi_adjustment_ = config.rgb_roi_adjustment; diff --git a/mir_perception/mir_object_recognition/ros/src/multimodal_object_recognition_utils.cpp b/mir_perception/mir_object_recognition/ros/src/multimodal_object_recognition_utils.cpp index ca2413312..c85d315d4 100644 --- a/mir_perception/mir_object_recognition/ros/src/multimodal_object_recognition_utils.cpp +++ b/mir_perception/mir_object_recognition/ros/src/multimodal_object_recognition_utils.cpp @@ -107,7 +107,8 @@ void MultimodalObjectRecognitionUtils::adjustContainerPose(mas_perception_msgs:: // Change the center of object container_object.pose.pose.position.x = centroid[0]; container_object.pose.pose.position.y = centroid[1]; - container_object.pose.pose.position.z = max_pt.z + container_height; + ROS_INFO_STREAM("Updating height from " << container_object.pose.pose.position.z << " to " << (max_pt.z)); + container_object.pose.pose.position.z = max_pt.z; } void MultimodalObjectRecognitionUtils::adjustAxisBoltPose(mas_perception_msgs::Object &object) { @@ -131,7 +132,17 @@ void MultimodalObjectRecognitionUtils::adjustAxisBoltPose(mas_perception_msgs::O unsigned int valid_points = pcl::compute3DCentroid(*point_at_z, centroid); if (object.name == "M20_100") { - ROS_INFO_STREAM("Updating M20_100 pose from object id: " << object.database_id); + ROS_INFO_STREAM("Updating object pose from object id: " << object.database_id); + float midpoint_x = (object.pose.pose.position.x + centroid[0])/2; + float midpoint_y = (object.pose.pose.position.y + centroid[1])/2; + float quarter_x = (object.pose.pose.position.x + midpoint_x)/2; + float quarter_y = (object.pose.pose.position.y + midpoint_y)/2; + object.pose.pose.position.x = quarter_x; + object.pose.pose.position.y = quarter_y; + } + else if (object.name == "SCREWDRIVER") + { + ROS_INFO_STREAM("Updating object pose from object id: " << object.database_id); float midpoint_x = (object.pose.pose.position.x + centroid[0])/2; float midpoint_y = (object.pose.pose.position.y + centroid[1])/2; object.pose.pose.position.x = midpoint_x; diff --git a/mir_perception/mir_perception_utils/ros/include/mir_perception_utils/object_utils_ros.h b/mir_perception/mir_perception_utils/ros/include/mir_perception_utils/object_utils_ros.h index 8f29d39c5..49efd2bc5 100644 --- a/mir_perception/mir_perception_utils/ros/include/mir_perception_utils/object_utils_ros.h +++ b/mir_perception/mir_perception_utils/ros/include/mir_perception_utils/object_utils_ros.h @@ -54,9 +54,11 @@ void estimatePose(const BoundingBox &box, geometry_msgs::PoseStamped &pose); * \param[in] Minimum value of the field z allowed * \param[in] Maximum value of the field z allowed */ -PointCloud estimatePose(const PointCloud::Ptr &xyz_input_cloud, geometry_msgs::PoseStamped &pose, +PointCloud estimatePose(const PointCloud::Ptr &xyz_input_cloud, geometry_msgs::PoseStamped &pose, + mas_perception_msgs::Object &object, std::string shape = "None", float passthrough_lim_min = 0.0060, - float passthrough_lim_max = 0.0); + float passthrough_lim_max = 0.0, + std::string obj_category = "atwork"); /** \brief Transform pose * \param[in] Transform listener diff --git a/mir_perception/mir_perception_utils/ros/src/object_utils_ros.cpp b/mir_perception/mir_perception_utils/ros/src/object_utils_ros.cpp index 18dc28987..75b82f496 100644 --- a/mir_perception/mir_perception_utils/ros/src/object_utils_ros.cpp +++ b/mir_perception/mir_perception_utils/ros/src/object_utils_ros.cpp @@ -52,15 +52,17 @@ void object::estimatePose(const BoundingBox &box, geometry_msgs::PoseStamped &po pose.pose.orientation.w = q.w(); } -PointCloud object::estimatePose(const PointCloud::Ptr &xyz_input_cloud, geometry_msgs::PoseStamped &pose, +PointCloud object::estimatePose(const PointCloud::Ptr &xyz_input_cloud, geometry_msgs::PoseStamped &pose, + mas_perception_msgs::Object &object, std::string shape, float passthrough_lim_min_offset, - float passthrough_lim_max_offset) + float passthrough_lim_max_offset, + std::string obj_category) { // Apply filter to remove points belonging to the plane for non // circular/spherical object // to find its orientation PointCloud filtered_cloud; - if (shape == "sphere") { + if (shape == "sphere" or shape == "flat") { filtered_cloud = *xyz_input_cloud; } else { pcl::PassThrough pass_through; @@ -70,10 +72,38 @@ PointCloud object::estimatePose(const PointCloud::Ptr &xyz_input_cloud, geometry pcl::getMinMax3D(*xyz_input_cloud, min_pt, max_pt); double limit_min = min_pt.z + passthrough_lim_min_offset; double limit_max = max_pt.z + passthrough_lim_max_offset; - - pass_through.setFilterLimits(limit_min, limit_max); - pass_through.setInputCloud(xyz_input_cloud); - pass_through.filter(filtered_cloud); + if (obj_category == "cavity") + { + // print + // std::cout << "[CAVITYYYYYYYYYYYY] min_pt.z: " << min_pt.z << std::endl; + // std::cout << "[CAVITYYYYYYYYYYYY] max_pt.z: " << max_pt.z << std::endl; + limit_max = max_pt.z - 0.015; // TODO: make 0.01 as a dynamic configurable parameter + if (object.name == "M20_H") + { + limit_max = max_pt.z - 0.02; + } + pass_through.setFilterLimits(limit_min, limit_max); + pass_through.setInputCloud(xyz_input_cloud); + pass_through.filter(filtered_cloud); + + // change z value to max(filterpointcloud z) This is only for cavity + for (size_t i = 0; i < filtered_cloud.points.size(); ++i) { + // check if filtered_cloud.points[i].z has not nan or inf value + if (!std::isnan(filtered_cloud.points[i].z) && !std::isinf(filtered_cloud.points[i].z)) { + filtered_cloud.points[i].z = 0.015; // 0.045 is an empirical values, TODO: get 0.045 from target_pose_z_pos from config file + } + } + } + else{ + if (object.name != "M20" && object.name != "M30" && object.name != "F20_20_G") { + pass_through.setFilterLimits(limit_min, limit_max); + pass_through.setInputCloud(xyz_input_cloud); + pass_through.filter(filtered_cloud); + } + else{ + filtered_cloud = *xyz_input_cloud; + } + } } Eigen::Vector4f centroid; diff --git a/mir_perception/mir_ppt_detection/config/object_shape_learned_params.yaml b/mir_perception/mir_ppt_detection/config/object_shape_learned_params.yaml old mode 100755 new mode 100644 diff --git a/mir_perception/mir_ppt_detection/launch/ppt_detector.launch b/mir_perception/mir_ppt_detection/launch/ppt_detector.launch index 6f619fdce..53acf8f68 100644 --- a/mir_perception/mir_ppt_detection/launch/ppt_detector.launch +++ b/mir_perception/mir_ppt_detection/launch/ppt_detector.launch @@ -20,8 +20,10 @@ + + - + diff --git a/mir_perception/mir_ppt_detection/scripts/covariance_data/ppt_train_process_data b/mir_perception/mir_ppt_detection/scripts/covariance_data/ppt_train_process_data old mode 100755 new mode 100644 diff --git a/mir_perception/mir_ppt_detection/scripts/covariance_data/ppt_train_record_data b/mir_perception/mir_ppt_detection/scripts/covariance_data/ppt_train_record_data old mode 100755 new mode 100644 diff --git a/mir_planning/mir_actions/mir_actions/ros/config/youbot_dynamic_params.yaml b/mir_planning/mir_actions/mir_actions/ros/config/youbot_dynamic_params.yaml index 68fc99480..882f75d05 100644 --- a/mir_planning/mir_actions/mir_actions/ros/config/youbot_dynamic_params.yaml +++ b/mir_planning/mir_actions/mir_actions/ros/config/youbot_dynamic_params.yaml @@ -100,3 +100,15 @@ pregrasp_planner_zealous: min_height: 0.0 target_frame: base_link ignore_orientation: True +multimodal_object_recognition_atwork: + /mir_perception/multimodal_object_recognition: + obj_category: atwork + rgb_cluster_filter_limit_max: 0.35 +multimodal_object_recognition_cavity: + /mir_perception/multimodal_object_recognition: + obj_category: cavity + rgb_cluster_filter_limit_max: -0.015 +multimodal_object_recognition_container: + /mir_perception/multimodal_object_recognition: + obj_category: container + rgb_cluster_filter_limit_max: 0.35 \ No newline at end of file diff --git a/mir_planning/mir_actions/mir_insert_cavity/ros/scripts/insert_cavity_client_test.py b/mir_planning/mir_actions/mir_insert_cavity/ros/scripts/insert_cavity_client_test.py index ee0342b08..23b82ef28 100755 --- a/mir_planning/mir_actions/mir_insert_cavity/ros/scripts/insert_cavity_client_test.py +++ b/mir_planning/mir_actions/mir_insert_cavity/ros/scripts/insert_cavity_client_test.py @@ -19,7 +19,7 @@ peg = str(sys.argv[1]).upper() platform = str(sys.argv[2]).upper() goal.parameters.append(KeyValue(key="peg", value=peg)) - goal.parameters.append(KeyValue(key="platform", value=platform)) + goal.parameters.append(KeyValue(key="platform", value=platform)) # stage platform eg: PLATFORM_LEFT goal.parameters.append(KeyValue(key="hole", value="PP01")) rospy.loginfo("Sending following goal to place object server") rospy.loginfo(goal) diff --git a/mir_planning/mir_actions/mir_insert_cavity/ros/scripts/insert_cavity_server.py b/mir_planning/mir_actions/mir_insert_cavity/ros/scripts/insert_cavity_server.py index bc654d84d..0cd4b1dc9 100755 --- a/mir_planning/mir_actions/mir_insert_cavity/ros/scripts/insert_cavity_server.py +++ b/mir_planning/mir_actions/mir_insert_cavity/ros/scripts/insert_cavity_server.py @@ -182,6 +182,41 @@ def execute(self, userdata): return "succeeded" ################# +# =============================================================================== + +class Unstage_to_place(smach.State): + + def __init__(self): + + smach.State.__init__(self, outcomes=["success","failed"],input_keys=["goal","heavy_objects", "platform","object"]) + self.platform = "PLATFORM_MIDDLE" + self.obj = "M20" + + def execute(self,userdata): + + self.platform = Utils.get_value_of(userdata.goal.parameters, "platform") + self.obj = Utils.get_value_of(userdata.goal.parameters, "peg") + + if self.obj is None: + rospy.logwarn('Missing parameter "object". Using default.') + self.obj = "light" + else: + self.obj = "light" + + + self.unstage_client = SimpleActionClient('unstage_object_server', GenericExecuteAction) + self.unstage_client.wait_for_server() + + # Assigning the goal + + goal = GenericExecuteGoal() + goal.parameters.append(KeyValue(key="platform", value=self.platform)) + goal.parameters.append(KeyValue(key="object", value=self.obj)) + + self.unstage_client.send_goal(goal) + self.unstage_client.wait_for_result(rospy.Duration.from_sec(25.0)) + + return "success" # =============================================================================== def transition_cb(*args, **kwargs): @@ -220,22 +255,33 @@ def main(): ) smach.StateMachine.add( - "CHECK_IF_OBJECT_IS_AVAILABLE", + "SELECT_CAVITY", + SelectCavity("/mcr_perception/object_selector/input/object_name", vertical=False), + transitions={ + "succeeded": "GENERATE_OBJECT_POSE", + "failed": "GET_VERTICAL_CAVITY" + }, + ) + + smach.StateMachine.add( + "GET_VERTICAL_CAVITY", + SelectCavity("/mcr_perception/object_selector/input/object_name", vertical=True), + transitions={ + "succeeded": "GENERATE_OBJECT_POSE", + "failed": "OVERALL_FAILED" + }, + ) + + # generates a pose of object + smach.StateMachine.add( + "GENERATE_OBJECT_POSE", gbs.send_and_wait_events_combined( - event_in_list=[ - ("/mcr_perception/cavity_pose_selector/event_in", "e_trigger",) - ], - event_out_list=[ - ( - "/mcr_perception/cavity_pose_selector/event_out", - "e_success", - True, - ) - ], + event_in_list=[("/mcr_perception/object_selector/event_in", "e_trigger")], + event_out_list=[("/mcr_perception/object_selector/event_out", "e_selected", True)], timeout_duration=10, ), transitions={ - "success": "SET_DBC_PARAMS", + "success": "UNSTAGE_FOR_PLACING", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, @@ -267,7 +313,7 @@ def main(): smach.StateMachine.add( "MOVE_ARM_TO_MIDDLE_POSE", - gms.move_arm("ppt_cavity_middle"), + gms.move_arm("pre_place"), transitions={ "succeeded": "PERCEIVE_CAVITY", "failed": "MOVE_ARM_TO_MIDDLE_POSE", @@ -355,7 +401,7 @@ def main(): # move arm to HOLD position smach.StateMachine.add( "MOVE_ARM_TO_HOLD", - gms.move_arm("ppt_cavity_middle"), + gms.move_arm("pre_place"), transitions={ "succeeded": "OVERALL_SUCCESS", "failed": "MOVE_ARM_TO_HOLD", @@ -366,7 +412,7 @@ def main(): sm.register_start_cb(start_cb) # smach viewer - if rospy.get_param("~viewer_enabled", False): + if rospy.get_param("~viewer_enabled", True): sis = IntrospectionServer( "insert_cavity_smach_viewer", sm, "/INSERT_CAVITY_SMACH_VIEWER" ) diff --git a/mir_planning/mir_actions/mir_perceive_cavity/ros/scripts/perceive_cavity_client_test.py b/mir_planning/mir_actions/mir_perceive_cavity/ros/scripts/perceive_cavity_client_test.py index 5b1aba9e8..4ddaa5987 100755 --- a/mir_planning/mir_actions/mir_perceive_cavity/ros/scripts/perceive_cavity_client_test.py +++ b/mir_planning/mir_actions/mir_perceive_cavity/ros/scripts/perceive_cavity_client_test.py @@ -20,7 +20,7 @@ else: location = "PP01" goal.parameters.append(KeyValue(key="location", value=location)) - goal.parameters.append(KeyValue(key="perception_mode", value="three_view")) + goal.parameters.append(KeyValue(key="perception_mode", value="single_view")) rospy.loginfo("Sending following goal to perceive cavity server") rospy.loginfo(goal) diff --git a/mir_planning/mir_actions/mir_perceive_cavity/ros/scripts/perceive_cavity_server.py b/mir_planning/mir_actions/mir_perceive_cavity/ros/scripts/perceive_cavity_server.py index 1669d443d..dde1a0d77 100755 --- a/mir_planning/mir_actions/mir_perceive_cavity/ros/scripts/perceive_cavity_server.py +++ b/mir_planning/mir_actions/mir_perceive_cavity/ros/scripts/perceive_cavity_server.py @@ -63,7 +63,7 @@ def execute(self, userdata): # 2. single_view mode - only perceiving in one direction perception_mode = Utils.get_value_of(userdata.goal.parameters, "perception_mode") if perception_mode is not None and perception_mode == "single_view": - userdata.arm_pose_list = ["ppt_cavity_middle"] + userdata.arm_pose_list = ["pre_place"] else: userdata.arm_pose_list = [ "ppt_cavity_middle", diff --git a/mir_planning/mir_actions/mir_perceive_location/ros/scripts/perceive_location_server.py b/mir_planning/mir_actions/mir_perceive_location/ros/scripts/perceive_location_server.py index d7062b208..1e23927ef 100755 --- a/mir_planning/mir_actions/mir_perceive_location/ros/scripts/perceive_location_server.py +++ b/mir_planning/mir_actions/mir_perceive_location/ros/scripts/perceive_location_server.py @@ -44,6 +44,7 @@ def get_robot_pose(self): return None def execute(self, userdata): + print("[percieve] userdata goal: ", str(userdata.goal.parameters)) target_location = Utils.get_value_of(userdata.goal.parameters, 'location') if target_location is not None: target_pose = Utils.get_pose_from_param_server(target_location) @@ -199,6 +200,31 @@ def execute(self, userdata): # =============================================================================== +class SetPerceptionParams(smach.State): + def __init__(self): + smach.State.__init__( + self, + outcomes=["success", "failure"], + input_keys=["goal"], + ) + self.set_named_config = gbs.set_named_config("multimodal_object_recognition_atwork") + + def execute(self, userdata): + obj_category = Utils.get_value_of(userdata.goal.parameters, "obj_category") + + rospy.loginfo("=============[PERCEIVE_LOCATION] obj_category: %s", obj_category) + + if obj_category: + rospy.loginfo("=============[PERCEIVE_LOCATION] [IFFFF] obj_category: %s", obj_category) + self.set_named_config.execute(userdata, obj_category) + return "success" + else: + rospy.loginfo("=============[PERCEIVE_LOCATION] [ELSEEEE] obj_category: %s", obj_category) + self.set_named_config.execute(userdata) + return "success" + +# =============================================================================== + def transition_cb(*args, **kwargs): userdata = args[0] sm_state = args[1][0] @@ -244,11 +270,22 @@ def main(): sm.userdata.base_pose_index = 0 with sm: - # approach to platform + + # set atwork model + smach.StateMachine.add( + "SET_PERCEPTION_PARAMS", + SetPerceptionParams(), + transitions={ + "success": "SETUP", + "failure": "OVERALL_FAILED", + }, + ) + smach.StateMachine.add( "SETUP", Setup(), transitions={"succeeded": "CHECK_IF_BASE_IS_AT_LOCATION"}, ) + smach.StateMachine.add( "CHECK_IF_BASE_IS_AT_LOCATION", CheckIfBaseCentered(), diff --git a/mir_planning/mir_actions/mir_pick_object/ros/scripts/pick_object_server.py b/mir_planning/mir_actions/mir_pick_object/ros/scripts/pick_object_server.py index 7a0b5c62f..cce5124b2 100755 --- a/mir_planning/mir_actions/mir_pick_object/ros/scripts/pick_object_server.py +++ b/mir_planning/mir_actions/mir_pick_object/ros/scripts/pick_object_server.py @@ -114,6 +114,16 @@ def main(): sm.userdata.reperceive = rospy.get_param("~reperceive", True) with sm: + smach.StateMachine.add( + "SET_PREGRASP_PARAMS", + gbs.set_named_config("pregrasp_planner_no_sampling"), + transitions={ + "success": "SELECT_OBJECT", + "timeout": "OVERALL_FAILED", + "failure": "OVERALL_FAILED", + }, + ) + smach.StateMachine.add( "SELECT_OBJECT", SelectObject("/mcr_perception/object_selector/input/object_name"), diff --git a/mir_scenarios/mir_states/ros/src/mir_states/common/action_states.py b/mir_scenarios/mir_states/ros/src/mir_states/common/action_states.py index cb9c2b1f8..4b263f525 100644 --- a/mir_scenarios/mir_states/ros/src/mir_states/common/action_states.py +++ b/mir_scenarios/mir_states/ros/src/mir_states/common/action_states.py @@ -48,13 +48,15 @@ def execute(self, userdata): class perceive_location(smach.State): - def __init__(self): + def __init__(self, obj_category="multimodal_object_recognition_atwork"): smach.State.__init__(self, outcomes=["success", "failed"]) self.client = SimpleActionClient( "perceive_location_server", GenericExecuteAction ) self.client.wait_for_server() self.goal = GenericExecuteGoal() + self.goal.parameters.append( + KeyValue(key="obj_category", value=str(obj_category))) def execute(self, userdata): self.client.send_goal(self.goal)