Feature/manipulation cam gripper

catkin_home/src/human_analyzer/scripts/poseEstimation.py

@@ -31,13 +31,13 @@ def __init__(self):
         rospy.init_node('PoseDetector')
 
         self.imageSub = rospy.Subscriber(
-            '/zed2_up/zed_up_node/rgb/image_rect_color/compressed', CompressedImage, self.image_callback, queue_size=10)
+            '/zed2/zed_node/rgb/image_rect_color/compressed', CompressedImage, self.image_callback, queue_size=10)
 
         self.imageInfo = CameraInfo()
-        self.subscriberInfo = rospy.Subscriber("/zed2_up/zed_up_node/depth/camera_info", CameraInfo, self.infoImageRosCallback)
+        self.subscriberInfo = rospy.Subscriber("/zed2/zed_node/depth/camera_info", CameraInfo, self.infoImageRosCallback)
 
         self.depth_image = []
-        self.subscriberDepth = rospy.Subscriber("/zed2_up/zed_up_node/depth/depth_registered", Image, self.depthImageRosCallback)
+        self.subscriberDepth = rospy.Subscriber("/zed2/zed_node/depth/depth_registered", Image, self.depthImageRosCallback)
 
         self.image_pose_pub = rospy.Publisher(
             '/mediapipe/output', Image, queue_size=10)

catkin_home/src/manipulation/calibration/arm_calibration/launch/arm_calibration.launch

@@ -1,13 +1,13 @@
 <?xml version="1.0"?>
 
 <launch>
-  <arg name="camera_name" default="zed2_up"/>
-  <arg name="camera_frame" default="zed2_up_left_camera_optical_frame" />
-  <arg name="image_topic" default="/zed2_up/zed_up_node/rgb/image_rect_color" />
-  <arg name="info_topic" default="/zed2_up/zed_up_node/rgb/camera_info" />
+  <arg name="camera_name" default="zed2"/>
+  <arg name="camera_frame" default="zed2_left_camera_optical_frame" />
+  <arg name="image_topic" default="/zed2/zed_node/rgb/image_rect_color" />
+  <arg name="info_topic" default="/zed2/zed_node/rgb/camera_info" />
 
-  <arg name="image_topic_synced" default="/zed2_up/zed_up_node/synced_rgb/image_rect" />
-  <arg name="info_topic_synced" default="/zed2_up/zed_up_node/synced_rgb/camera_info" />
+  <arg name="image_topic_synced" default="/zed2/zed_node/synced_rgb/image_rect" />
+  <arg name="info_topic_synced" default="/zed2/zed_node/synced_rgb/camera_info" />
 
   <arg name="tag_file" default="tags.yaml" />
 

catkin_home/src/manipulation/calibration/arm_calibration/src/camera.py

@@ -45,16 +45,16 @@ def __init__(self):
         self.move_arm(self.ARM_CALIBRATION)
         time.sleep(8.5) # Wait for an accurate detection.
         try:
-            # Get the transforms between 'base_footprint', 'apriltag', and 'tag_5'
+            # Get the transforms between 'Base_Gripper_footprint', 'apriltag', and 'tag_5'
             print("Waiting for transforms...")
-            self.listener.waitForTransform('Base', 'apriltag', rospy.Time(0), rospy.Duration(5.0))
-            self.listener.waitForTransform('Base', 'tag_5', rospy.Time(0), rospy.Duration(5.0))
-            self.listener.waitForTransform('Base', 'Cam1', rospy.Time(0), rospy.Duration(5.0))
+            self.listener.waitForTransform('Base_Gripper', 'apriltag', rospy.Time(0), rospy.Duration(5.0))
+            self.listener.waitForTransform('Base_Gripper', 'tag_5', rospy.Time(0), rospy.Duration(5.0))
+            self.listener.waitForTransform('Base_Gripper', 'Cam1', rospy.Time(0), rospy.Duration(5.0))
             self.listener.waitForTransform('Cam1', 'tag_5', rospy.Time(0), rospy.Duration(5.0))
             print("Got transforms!")
-            (trans1, rot1) = self.listener.lookupTransform('Base', 'apriltag', rospy.Time(0))
-            (trans2, rot2) = self.listener.lookupTransform('Base', 'tag_5', rospy.Time(0))
-            (trans3, rot3) = self.listener.lookupTransform('Base', 'Cam1', rospy.Time(0))
+            (trans1, rot1) = self.listener.lookupTransform('Base_Gripper', 'apriltag', rospy.Time(0))
+            (trans2, rot2) = self.listener.lookupTransform('Base_Gripper', 'tag_5', rospy.Time(0))
+            (trans3, rot3) = self.listener.lookupTransform('Base_Gripper', 'Cam1', rospy.Time(0))
             (trans4, rot4) = self.listener.lookupTransform('Cam1', 'tag_5', rospy.Time(0))
             print("Got matrices!")
             # Fix Rotation Difference between 'apriltag' and 'tag_5'
@@ -65,9 +65,9 @@ def __init__(self):
             # Publish the camera transform with the rotation fixed.
             broadcasterThread = threading.Thread(target=self.send_transform, args=(trans3, rot_fixed, trans4, rot4))
             broadcasterThread.start()
-            # Get the transform between 'Base' and 'tag_fixed' (the detection with the fixed rotation).
-            self.listener.waitForTransform('Base', 'tag_fixed', rospy.Time(0), rospy.Duration(5.0))
-            (trans6, _) = self.listener.lookupTransform('Base', 'tag_fixed', rospy.Time(0))
+            # Get the transform between 'Base_Gripper' and 'tag_fixed' (the detection with the fixed rotation).
+            self.listener.waitForTransform('Base_Gripper', 'tag_fixed', rospy.Time(0), rospy.Duration(5.0))
+            (trans6, _) = self.listener.lookupTransform('Base_Gripper', 'tag_fixed', rospy.Time(0))
             self.publishRotationFixedTransform = False
             broadcasterThread.join()
 
@@ -76,7 +76,7 @@ def __init__(self):
             trans_cam = np.add(trans3, trans_diff)
 
             # Publish the resultant transform
-            self.broadcaster.sendTransform(trans_cam, rot_fixed, rospy.Time.now(), 'Cam1', 'Base')
+            self.broadcaster.sendTransform(trans_cam, rot_fixed, rospy.Time.now(), 'Cam1', 'Base_Gripper')
 
             # Update the URDF file with the new camera transform.
             rot_euler_fixed = transformations.euler_from_quaternion(rot_fixed)
@@ -113,7 +113,7 @@ def move_arm(self, joint_values):
     def send_transform(self, trans, rot, detection_trans, detection_rot):
         start = time.time()
         while(time.time() - start < 5 and self.publishRotationFixedTransform):
-            self.broadcaster.sendTransform(trans, rot, rospy.Time.now(), 'New_Cam1', 'Base')
+            self.broadcaster.sendTransform(trans, rot, rospy.Time.now(), 'New_Cam1', 'Base_Gripper')
             self.broadcaster.sendTransform(detection_trans, detection_rot, rospy.Time.now(), 'tag_fixed', 'New_Cam1') 
 
 if __name__ == '__main__':

catkin_home/src/manipulation/dashgo_moveit_config/config/dashgo.srdf

@@ -39,12 +39,12 @@
         <joint name="joint6" value="-0.7854"/>
     </group_state>
     <group_state name="arm_grasp" group="arm">
-        <joint name="joint1" value="1.57"/>
-        <joint name="joint2" value="0"/>
-        <joint name="joint3" value="-3.9"/>
+        <joint name="joint1" value="-1.57"/>
+        <joint name="joint2" value="-1.04"/>
+        <joint name="joint3" value="-1.04"/>
         <joint name="joint4" value="0"/>
-        <joint name="joint5" value="2.33"/>
-        <joint name="joint6" value="-1.57"/>
+        <joint name="joint5" value="2.09"/>
+        <joint name="joint6" value="-2.356"/>
     </group_state>
     <group_state name="arm_calibration" group="arm">
         <joint name="joint1" value="-1.57"/>
@@ -107,6 +107,13 @@
     <disable_collisions link1="Cam1" link2="Loca2" reason="Never"/>
     <disable_collisions link1="Cam1" link2="link5" reason="Never"/>
     <disable_collisions link1="Cam1" link2="link_base" reason="Never"/>
+    <disable_collisions link1="Cam1" link2="link_eef" reason="Never"/>
+    <disable_collisions link1="Cam1" link2="Dedo1" reason="Adjacent"/>
+    <disable_collisions link1="Cam1" link2="Dedo2" reason="Adjacent"/>
+    <disable_collisions link1="Cam1" link2="Servo1" reason="Never"/>
+    <disable_collisions link1="Cam1" link2="Servo2" reason="Never"/>
+    <disable_collisions link1="Cam1" link2="link6" reason="Never"/>
+    <disable_collisions link1="Cam1" link2="Base_Gripper" reason="Never"/>
     <disable_collisions link1="Dedo1" link2="Dedo2" reason="Never"/>
     <disable_collisions link1="Dedo1" link2="Servo1" reason="Adjacent"/>
     <disable_collisions link1="Dedo1" link2="Servo2" reason="Never"/>
@@ -197,4 +204,5 @@
 
     <disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
     <disable_collisions link1="link4" link2="link6" reason="Never"/>
+
 </robot>

catkin_home/src/manipulation/dashgo_moveit_config/launch/rvizManipulation.rviz

@@ -523,7 +523,7 @@ Visualization Manager:
       Size (Pixels): 3
       Size (m): 0.009999999776482582
       Style: Flat Squares
-      Topic: /zed2_up/zed_up_node/point_cloud/cloud_registered
+      Topic: /zed2/zed_node/point_cloud/cloud_registered
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true

catkin_home/src/manipulation/dashgo_moveit_config/src/OctomapInput.cpp

@@ -16,7 +16,7 @@ class OctomapService
   {
     tf_listener = new tf::TransformListener();
     state_ = true;
-    sub_ = nh_.subscribe("/zed2_up/zed_up_node/point_cloud/ds_cloud_registered", 1, &OctomapService::cloudCallback, this);
+    sub_ = nh_.subscribe("/zed2/zed_node/point_cloud/ds_cloud_registered", 1, &OctomapService::cloudCallback, this);
     pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/octomap/points", 10);
     service_ = nh_.advertiseService("/toggle_octomap", &OctomapService::toggleOctomap, this);
     state_pub_ = nh_.advertise<std_msgs::Bool>("/octomap/state", 10);
@@ -40,7 +40,7 @@ class OctomapService
     {
       sensor_msgs::PointCloud2 t_pc;
       try {
-        tf_listener->waitForTransform("base_link", "zed2_up_left_camera_frame", ros::Time(0), ros::Duration(5.0));
+        tf_listener->waitForTransform("base_link", "zed2_left_camera_frame", ros::Time(0), ros::Duration(5.0));
       } catch (tf::TransformException ex) {
         return;
       }

catkin_home/src/manipulation/object_detector/launch/object_2D.tmr2023.launch

@@ -1,25 +1,25 @@
 <launch>
     <!-- ObjectDetection -->
-    <arg name="SOURCE" default="/zed2_up/zed_up_node/rgb/image_rect_color" /> 
-    <arg name="DEPTH_INPUT" default="/zed2_up/zed_up_node/depth/depth_registered" />
-    <arg name="CAMERA_INFO" default="/zed2_up/zed_up_node/depth/camera_info" />
+    <arg name="SOURCE" default="/zed2/zed_node/rgb/image_rect_color" /> 
+    <arg name="DEPTH_INPUT" default="/zed2/zed_node/depth/depth_registered" />
+    <arg name="CAMERA_INFO" default="/zed2/zed_node/depth/camera_info" />
     <arg name="ROS_INPUT" default="True" />
     <arg name="USE_ACTIVE_FLAG" default="False" />
     <arg name="VERBOSE" default="False" />
     <arg name="DEPTH_ACTIVE" default="True" />
-    <arg name="USE_YOLO" default="True" />
+    <arg name="USE_YOLO8" default="False" />
     <arg name="YOLO_MODEL_PATH" default="$(find object_detector)/models/tmr2023/yolo11classes.pt" />
     <arg name="MODELS_PATH" default="$(find object_detector)/models/gazebo/" />
     <arg name="LABELS_PATH" default="$(find object_detector)/models/gazebo/label_map.pbtxt" />
-    <arg name="CAMERA_FRAME" default="zed2_up_left_camera_optical_frame" />
+    <arg name="CAMERA_FRAME" default="zed2_left_camera_optical_frame" />
     <arg name="USE_FAKE_DETECTOR" default="False" />
 
     <node name="Detection2D" pkg="object_detector" type="Detection2D.py" respawn="true" output="screen" unless="$(arg USE_FAKE_DETECTOR)">
         <param name="SOURCE" value="$(arg SOURCE)" />
         <param name="ROS_INPUT" value="$(arg ROS_INPUT)" />
         <param name="USE_ACTIVE_FLAG" value="$(arg USE_ACTIVE_FLAG)" />
         <param name="VERBOSE" value="$(arg VERBOSE)" />
-        <param name="USE_YOLO" value="$(arg USE_YOLO)" />
+        <param name="USE_YOLO8" value="$(arg USE_YOLO8)" />
         <param name="YOLO_MODEL_PATH" value="$(arg YOLO_MODEL_PATH)" />
         <param name="MODELS_PATH" value="$(arg MODELS_PATH)" />
         <param name="LABELS_PATH" value="$(arg LABELS_PATH)" />

catkin_home/src/manipulation/object_detector/launch/object_3D.tmr2023.launch

@@ -1,10 +1,10 @@
 <launch>
     <arg name="MAP_FRAME" default="map" />
-    <arg name="CAMERA_FRAME" default="zed2_up_left_camera_frame" />
+    <arg name="CAMERA_FRAME" default="zed2_left_camera_frame" />
     <arg name="BASE_FRAME" default="base_link" />
     <arg name="map_to_odom" default="true"/>
     <arg name="odom_to_base_footprint" default="true"/>
-    <arg name="POINT_CLOUD_TOPIC" default="/zed2_up/zed_up_node/point_cloud/ds_cloud_registered" />
+    <arg name="POINT_CLOUD_TOPIC" default="/zed2/zed_node/point_cloud/ds_cloud_registered" />
     <node 
         pkg="tf" 
         type="static_transform_publisher" 

catkin_home/src/manipulation/object_detector/src/Detection2D.py

@@ -24,6 +24,8 @@
 from vision_utils import *
 import tf2_ros
 import tf2_geometry_msgs
+import imutils
+from ultralytics import YOLO
 
 SOURCES = {
     "VIDEO": str(pathlib.Path(__file__).parent) + "/../resources/test.mp4",
@@ -40,10 +42,10 @@
     "CAMERA_INFO": "/camera/depth/camera_info",
     "MODELS_PATH": str(pathlib.Path(__file__).parent) + "/../models/",
     "LABELS_PATH": str(pathlib.Path(__file__).parent) + "/../models/label_map.pbtxt",
-    "MIN_SCORE_THRESH": 0.6,
+    "MIN_SCORE_THRESH": 0.2,
     "VERBOSE": True,
     "CAMERA_FRAME": "xtion_rgb_optical_frame",
-    "USE_YOLO": False,
+    "USE_YOLO8": False,
     "YOLO_MODEL_PATH": str(pathlib.Path(__file__).parent) + "/../models/yolov5s.pt",
 }
 
@@ -62,6 +64,19 @@ def loadTfModel():
 
         def loadYoloModel():
             self.model = torch.hub.load('ultralytics/yolov5', 'custom', path=ARGS["YOLO_MODEL_PATH"], force_reload=False)
+
+        def yolov8_warmup(model, repetitions=1, verbose=False):
+            # Warmup model
+            startTime = time.time()
+            # create an empty frame to warmup the model
+            for i in range(repetitions):
+                warmupFrame = np.zeros((360, 640, 3), dtype=np.uint8)
+                model.predict(source=warmupFrame, verbose=verbose)
+            rospy.logdebug(f"Model warmed up in {time.time() - startTime} seconds")
+
+        def loadYolov8Model():
+            self.model = YOLO(ARGS["YOLO_MODEL_PATH"])
+            yolov8_warmup(self.model, repetitions=10, verbose=False)
 
         self.category_index = {
             1 : 'CocaCola',
@@ -73,14 +88,14 @@ def loadYoloModel():
             7 : 'Harpic',
         }
 
-        if ARGS["USE_YOLO"]:
-            print("[INFO] Loading Yolo Model")
-            loadYoloModel()
-            print("[INFO] Yolo Model Loaded")
+        if ARGS["USE_YOLO8"]:
+            print("[INFO] Loading Yolov8 Model")
+            loadYolov8Model()
+            print("[INFO] Yolov8 Model Loaded")
         else:
-            print("[INFO] Loading TF Model")
-            loadTfModel()
-            print("[INFO] TF Model Loaded")
+            print("[INFO] Loading Yolov5 Model")
+            loadYoloModel()
+            print("[INFO] Yolov5 Model Loaded")
 
         self.activeFlag = not ARGS["USE_ACTIVE_FLAG"]
         self.runThread = None
@@ -175,6 +190,8 @@ def infoImageRosCallback(self, data):
     # Process a frame only when the script finishes the process of the previous frame, rejecting frames to keep real-time idea.
     def imageCallback(self, img):
         self.rgb_image = img
+        # flip image
+        # img = imutils.rotate(img, 180)
         if not self.activeFlag:
             self.detections_frame = img
         elif self.runThread == None or not self.runThread.is_alive():
@@ -197,6 +214,7 @@ def callFps(self):
 
     def yolo_run_inference_on_image(self, frame):
         frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+        #cv2.imshow('frame', frame)
         results = self.model(frame)
         output = {
             'detection_boxes': [],  # Normalized ymin, xmin, ymax, xmax
@@ -227,6 +245,47 @@ def yolo_run_inference_on_image(self, frame):
         output['detection_classes'] = np.array(output['detection_classes'])
         output['detection_scores'] = np.array(output['detection_scores'])
         return output
+
+    def yolov8_run_inference_on_image(self, frame):
+        frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+        cv2.imshow('frame', frame)
+        results = self.model(frame)
+        output = {
+            'detection_boxes': [],  # Normalized ymin, xmin, ymax, xmax
+            'detection_classes': [], # ClassID 
+            'detection_scores': [] # Confidence
+        }
+        height = frame.shape[0]
+        width = frame.shape[1]
+
+        print(f"Height: {height} Width: {width}")
+
+        boxes = []
+        confidences = []
+        classids = []
+
+        for out in results:
+                for box in out.boxes:
+                    x1, y1, x2, y2 = [round(x) for x in box.xyxy[0].tolist()]
+                    x1 = x1/width
+                    x2 = x2/width
+                    y1 = y1/height
+                    y2 = y2/height
+
+                    class_id = box.cls[0].item()
+                    prob = round(box.conf[0].item(), 2)
+
+                    boxes.append([x1, y1, x2, y2])
+                    confidences.append(float(prob))
+                    classids.append(class_id)
+                    print(f"Found {class_id} in {x1} {y1} {x2} {y2}")
+                    print("------------------------------")
+                    print()
+
+        output['detection_boxes'] = np.array(boxes)
+        output['detection_classes'] = np.array(classids)
+        output['detection_scores'] = np.array(confidences)
+        return output
 
     # Function to run the detection model.
     def run_inference_on_image(self, frame):
@@ -261,10 +320,10 @@ def run_inference_on_image(self, frame):
 
     # Handle the detection model input/output.
     def compute_result(self, frame):
-        if ARGS["USE_YOLO"]:
-            detections = self.yolo_run_inference_on_image(frame)
+        if ARGS["USE_YOLO8"]:
+            detections = self.yolov8_run_inference_on_image(frame)
         else:
-            detections = self.run_inference_on_image(frame)
+            detections = self.yolo_run_inference_on_image(frame)
         return self.get_objects(detections["detection_boxes"],
                                 detections["detection_scores"],
                                 detections["detection_classes"],
@@ -396,6 +455,7 @@ def run(self, frame):
 
         self.detections_frame = frame
 
+        print("PUBLISHED DATA")
         self.publisher.publish(objectDetectionArray(detections=detected_objects))
         self.fps.update()
 

catkin_home/src/manipulation/pick_and_place/config/dashgo_eigen_params.cfg

@@ -33,7 +33,7 @@ image_size = 60
 image_num_channels = 15 
 
 # (OpenVINO) Path to directory that contains neural network parameters
-weights_file = /home/emilianh/Documents/Home_2023/robocup-home/catkin_home/lib/gpd/models/lenet/15channels/params/
+weights_file = /home/emilianh/roborregos/Home_2023/robocup-home/catkin_home/lib/gpd/models/lenet/15channels/params/
 
 # Preprocessing of point cloud
 #   voxelize: if the cloud gets voxelized/downsampled

catkin_home/src/manipulation/pick_and_place/config/params_DASHGO.yaml

@@ -4,9 +4,9 @@ ARM_GROUP: "arm"
 ARM_JOINTS: [joint1, joint2, joint3, joint4, joint5, joint6]
 # ARM_GRASP: [-1.57, 0.0, 0.0, -1.57, -1.57]
 ARM_GRASP: [-1.57, -0.785, -0.785, 0, 1.57, 0]
-ARM_INIT: [-1.57, -1.39626, -0.174533, 0.0, 1.57, -0.7854]
+ARM_INIT: [-1.57, 0.0, -3.1416, 0.0, 1.57, 0.7854]
 ARM_HOME: [0.0, 0.0, 0.0, 0.0, 0.0, -0.7854]
-ARM_PREGRASP: [-1.57, -0.1745, -0.262, 0.0, 0.0, -0.7854]
+ARM_PREGRASP: [-1.57, -1.04, -1.04, 0, 2.09, -2.356]
 HEAD_ENABLE: false
 HEAD_GROUP: "head"
 HEAD_JOINTS: [Cam1Rev1, Cam1Rev2]

catkin_home/src/manipulation/pick_and_place/launch/manipulation.dashgo.launch

@@ -27,11 +27,11 @@
   <!-- 3D Detector -->
   <include file="$(find object_detector)/launch/object_3D_BAG.launch">
     <arg name="MAP_FRAME" default="map" />
-    <arg name="CAMERA_FRAME" default="zed2_up_left_camera_frame" />
+    <arg name="CAMERA_FRAME" default="zed2_left_camera_frame" />
     <arg name="BASE_FRAME" default="base_link" />
     <arg name="map_to_odom"  default="true"/>
     <arg name="odom_to_base_footprint"  default="true"/>
-    <arg name="POINT_CLOUD_TOPIC" default="/zed2_up/zed_up_node/point_cloud/ds_cloud_registered" />
+    <arg name="POINT_CLOUD_TOPIC" default="/zed2/zed_node/point_cloud/ds_cloud_registered" />
   </include>
 
   <!-- Manipulation Server -->