diff --git a/CMakeLists.txt b/CMakeLists.txt index 134368a..e6938dd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(deep-grasping-ros) +project(deep_grasping_ros) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -11,6 +11,8 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs + message_generation + geometry_msgs ) ## System dependencies are found with CMake's conventions @@ -54,11 +56,11 @@ find_package(catkin REQUIRED COMPONENTS # ) ## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) +add_service_files( + FILES + GetTarget6dofGrasp.srv + GetTargetPos.srv +) ## Generate actions in the 'action' folder # add_action_files( @@ -68,10 +70,11 @@ find_package(catkin REQUIRED COMPONENTS # ) ## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs +) ################################################ ## Declare ROS dynamic reconfigure parameters ## @@ -104,7 +107,7 @@ find_package(catkin REQUIRED COMPONENTS ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES deep-grasping-ros +# LIBRARIES deep_grasping_ros # CATKIN_DEPENDS roscpp rospy std_msgs # DEPENDS system_lib ) @@ -122,7 +125,7 @@ include_directories( ## Declare a C++ library # add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/deep-grasping-ros.cpp +# src/${PROJECT_NAME}/deep_grasping_ros.cpp # ) ## Add cmake target dependencies of the library @@ -133,7 +136,7 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/deep-grasping-ros_node.cpp) +# add_executable(${PROJECT_NAME}_node src/deep_grasping_ros_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -197,7 +200,7 @@ include_directories( ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_deep-grasping-ros.cpp) +# catkin_add_gtest(${PROJECT_NAME}-test test/test_deep_grasping_ros.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() diff --git a/README.md b/README.md index 7dee8c2..958e5bf 100644 --- a/README.md +++ b/README.md @@ -35,14 +35,14 @@ ros27 && ROS_NAMESPACE=azure1 roslaunch azure_kinect_ros_driver driver.launch co 6-dof-graspnet server ``` -ros && conda activate 6dofgraspnet && cd ~/catkin_ws/src/deep-grasping-ros/src \ +ros && conda activate 6dofgraspnet && cd ~/catkin_ws/src/deep_grasping_ros/src \ && python 6dgn_ros_server.py ``` 6-dof-graspnet client ``` ros && conda activate 6dofgraspnet && \ - cd ~/catkin_ws/src/deep-grasping-ros/src/6dof-graspnet \ + cd ~/catkin_ws/src/deep_grasping_ros/src/6dof-graspnet \ && python -m demo.6dgn_client --vae_checkpoint_folder checkpoints/npoints_1024_train_evaluator_0_allowed_categories__ngpus_1_/ ``` @@ -62,16 +62,14 @@ ros27 && ROS_NAMESPACE=azure1 roslaunch azure_kinect_ros_driver driver.launch co contact graspnet server ``` -ros && conda activate contact_graspnet_env \ - && cd ~/catkin_ws/src/deep-grasping-ros/src \ - && python contact_grasp_server.py +ros27 && rosrun deep_grasping_ros contact_grasp_server.py ``` contact graspnet client ``` ros && conda activate contact_graspnet_env \ - && cd ~/catkin_ws/src/deep-grasping-ros/src/contact_graspnet \ + && cd ~/catkin_ws/src/deep_grasping_ros/src/contact_graspnet \ && CUDA_VISIBLE_DEVICES=1 python contact_graspnet/contact_grasp_client.py ``` diff --git a/package.xml b/package.xml index 92d986b..08b01e6 100644 --- a/package.xml +++ b/package.xml @@ -1,8 +1,8 @@ - deep-grasping-ros + deep_grasping_ros 0.0.0 - The deep-grasping-ros package + The deep_grasping_ros package @@ -19,7 +19,7 @@ - + diff --git a/src/6dgn_ros_server.py b/src/6dgn_ros_server.py old mode 100644 new mode 100755 diff --git a/src/color_detector.py b/src/color_detector.py new file mode 100755 index 0000000..e4ce414 --- /dev/null +++ b/src/color_detector.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python + +## sudo apt install ros-melodic-ros-numpy +## pip install open3d==0.9.0, pyrsistent=0.16 +## pip install open3d-ros-helper + + +import rospy +import cv2, cv_bridge +import numpy as np +from sensor_msgs.msg import Image, PointCloud2, CameraInfo +from geometry_msgs.msg import Point +from open3d_ros_helper import open3d_ros_helper as orh +import copy +import tf2_ros +from visualization_msgs.msg import Marker, MarkerArray +from panda_control.srv import get_target_pos + + +class ColorDetector(): + + def __init__(self): + + rospy.init_node("color_detector") + rospy.loginfo("Starting color_detector node") + + + self.cv_bridge = cv_bridge.CvBridge() + self.result_pub = rospy.Publisher("/color_detect", Image, queue_size=1) + # RED + self.lower_bound = np.array([155, 25, 0]) # hsv + self.upper_bound = np.array([179, 255, 255]) # hsv + self.tf_buffer = tf2_ros.Buffer(rospy.Duration(1.0)) + self.listener = tf2_ros.TransformListener(self.tf_buffer) + + # get K and H from camera to robot base + camera_info = rospy.wait_for_message("/azure1/rgb/camera_info", CameraInfo) + self.K = np.array(camera_info.K).reshape(3, 3) + get_XYZ_to_pick_srv = rospy.Service('/get_XYZ_to_pick', get_target_pos, self.get_XYZ_to_pick) + self.XYZ_to_pick_markers_pub = rospy.Publisher('/XYZ_to_Pick', MarkerArray, queue_size=1) + + + def get_transform_cam_to_world(self): + while True: + try: + transform_cam_to_world = self.tf_buffer.lookup_transform("world", "azure1_rgb_camera_link", rospy.Time(), rospy.Duration(5.0)) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + print(e) + rospy.sleep(0.5) + else: + self.H_cam_to_world = orh.msg_to_se3(transform_cam_to_world) + print("Got transform from camera to the robot base") + break + + def get_XYZ_to_pick(self, msg): + + rospy.loginfo("Wating for /azure1/rgb/image_raw") + # get rgb + rgb_msg = rospy.wait_for_message("/azure1/rgb/image_raw", Image) + rgb = self.cv_bridge.imgmsg_to_cv2(rgb_msg, desired_encoding='bgr8') + # get point cloud + pc_msg = rospy.wait_for_message("/azure1/points2", PointCloud2) + cloud_cam = orh.rospc_to_o3dpc(pc_msg, remove_nans=True) + + self.get_transform_cam_to_world() + + + # red region detection in 2D + rgb = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV) + mask = cv2.inRange(rgb, self.lower_bound, self.upper_bound) + ys, xs = np.where(mask) + xy_to_pick = (int(np.median(xs)), int(np.median(ys))) + rospy.loginfo("xy_to_pick: {}".format(xy_to_pick)) + + # get corresponding 3D pointcloud and get the median position + cloud_center = orh.crop_with_2dmask(copy.deepcopy(cloud_cam), np.array(mask, dtype=bool), self.K) + cloud_center = cloud_center.transform(self.H_cam_to_world) + cloud_center_npy = np.asarray(cloud_center.points) + valid_mask = (np.nan_to_num(cloud_center_npy) != 0).any(axis=1) + cloud_center_npy = cloud_center_npy[valid_mask] + XYZ_to_pick = np.median(cloud_center_npy, axis=0) + + rospy.loginfo("XYZ_to_pick: {}".format(XYZ_to_pick)) + self.publish_markers(XYZ_to_pick) + + result = cv2.bitwise_and(rgb, rgb, mask=mask) + result = cv2.cvtColor(result, cv2.COLOR_HSV2BGR) + result = cv2.circle(result, xy_to_pick, 50, (0, 255, 0), 10) + + # convert opencv img to ros imgmsg + img_msg = self.cv_bridge.cv2_to_imgmsg(result, encoding='bgr8') + + # publish it as topic + self.result_pub.publish(img_msg) + rospy.loginfo_once("Published the result as topic. check /color_detect") + + pos = Point() + pos.x = XYZ_to_pick[0] + pos.y = XYZ_to_pick[1] + pos.z = XYZ_to_pick[2] + + return pos + + def publish_markers(self, XYZ): + + # Delete all existing markers + markers = MarkerArray() + marker = Marker() + marker.action = Marker.DELETEALL + markers.markers.append(marker) + self.XYZ_to_pick_markers_pub.publish(markers) + + # Object markers + markers = MarkerArray() + marker = Marker() + marker.header.frame_id = "world" + marker.header.stamp = rospy.Time() + marker.action = Marker.ADD + marker.pose.position.x = XYZ[0] + marker.pose.position.y = XYZ[1] + marker.pose.position.z = XYZ[2] + marker.pose.orientation.x = 0.0 + marker.pose.orientation.y = 0.0 + marker.pose.orientation.z = 0.0 + marker.pose.orientation.w = 1.0 + marker.color.r = 1 + marker.color.g = 0 + marker.color.b = 0 + marker.color.a = 0.5 + marker.ns = "XYZ_to_pick" + marker.id = 0 + marker.type = Marker.SPHERE + marker.scale.x = 0.1 + marker.scale.y = 0.1 + marker.scale.z = 0.1 + markers.markers.append(marker) + self.XYZ_to_pick_markers_pub.publish(markers) + + +if __name__ == '__main__': + + color_detector = ColorDetector() + rospy.spin() \ No newline at end of file diff --git a/src/contact_grasp_server.py b/src/contact_grasp_server.py old mode 100644 new mode 100755 index 906b472..d34b599 --- a/src/contact_grasp_server.py +++ b/src/contact_grasp_server.py @@ -6,11 +6,23 @@ import numpy as np import message_filters +import tf2_ros from sensor_msgs.msg import Image, CameraInfo, PointCloud2 - from easy_tcp_python2_3 import socket_utils as su -import PIL.Image # @UnresolvedImport -import numpy +from open3d_ros_helper import open3d_ros_helper as orh +from visualization_msgs.msg import MarkerArray, Marker +from deep_grasping_ros.srv import GetTarget6dofGrasp + +panda_gripper_coords = { + "left_center_indent": [0.041489, 0, 0.1034], + "left_center_flat": [0.0399, 0, 0.1034], + "right_center_indent": [-0.041489, 0, 0.1034], + "right_center_flat": [-0.0399, 0, 0.1034], + "left_tip_indent": [0.041489, 0, 0.112204], + "left_tip_flat": [0.0399, 0, 0.112204], + "right_tip_indent": [-0.041489, 0, 0.112204], + "right_tip_flat": [-0.0399, 0, 0.112204], +} class ContactGraspNet(): @@ -27,32 +39,142 @@ def __init__(self): self.data = {} self.data["intrinsics_matrix"] = np.array(self.camera_info.K).reshape(3, 3) - rgb_sub = message_filters.Subscriber("/azure1/rgb/image_raw", Image, buff_size=2048*1536*3) - depth_sub = message_filters.Subscriber("/azure1/depth_to_rgb/image_raw", Image, buff_size=2048*1536*3) - point_sub = message_filters.Subscriber("/azure1/points2", PointCloud2, buff_size=2048*1536*3) - self.ts = message_filters.ApproximateTimeSynchronizer([rgb_sub, depth_sub, point_sub], queue_size=1, slop=1) - self.ts.registerCallback(self.inference) + self.bridge = cv_bridge.CvBridge() + # rgb_sub = message_filters.Subscriber("/azure1/rgb/image_raw", Image, buff_size=2048*1536*3) + # depth_sub = message_filters.Subscriber("/azure1/depth_to_rgb/image_raw", Image, buff_size=2048*1536*3) + # self.ts = message_filters.ApproximateTimeSynchronizer([rgb_sub, depth_sub], queue_size=1, slop=1) + # self.ts.registerCallback(self.inference) + + self.grasp_srv = rospy.Service('/get_target_grasp_pose', GetTarget6dofGrasp, self.get_target_grasp_pose) + + # tf publisher + self.br = tf2_ros.TransformBroadcaster() + self.tf_buffer = tf2_ros.Buffer(rospy.Duration(1.0)) + self.listener = tf2_ros.TransformListener(self.tf_buffer) + self.marker_pub = rospy.Publisher("target_grasp", MarkerArray, queue_size = 1) - def inference(self, rgb, depth, pcl_msg): + def inference(self, rgb, depth): # convert ros imgmsg to opencv img - rgb = np.frombuffer(rgb.data, dtype=np.uint8).reshape(rgb.height, rgb.width, -1) + # rgb = np.frombuffer(rgb.data, dtype=np.uint8).reshape(rgb.height, rgb.width, -1) + # depth = np.frombuffer(depth.data, dtype=np.float32).reshape(depth.height, depth.width) + rgb = self.bridge.imgmsg_to_cv2(rgb, desired_encoding='bgr8') + depth = self.bridge.imgmsg_to_cv2(depth, desired_encoding='32FC1') + + + self.data["rgb"] = rgb + self.data["depth"] = depth - depth = np.frombuffer(depth.data, dtype=np.float32).reshape(depth.height, depth.width) - # depth_img_raw = cv2.imdecode(np.fromstring(depth.data, np.uint8), cv2.CV_LOAD_IMAGE_UNCHANGED) - # depth = self.cv_bridge.imgmsg_to_cv2(depth, desired_encoding='32FC1') + # send image to dnn client + rospy.loginfo_once("Sending rgb, depth to Contact-GraspNet client") + su.sendall_pickle(self.sock, self.data) + pred_grasps_cam, scores, contact_pts = su.recvall_pickle(self.sock) + if pred_grasps_cam is None: + return + for k in pred_grasps_cam.keys(): + if len(scores[k]) == 0: + return + best_grasp = pred_grasps_cam[k][np.argmax(scores[k])] + if k == 1: + print(k, pred_grasps_cam[k], scores[k], contact_pts[k]) + exit() + + # publish as tf + while True: + try: + T_base_to_cam = self.tf_buffer.lookup_transform("world", "azure1_rgb_camera_link", rospy.Time(), rospy.Duration(5.0)) + T_base_to_cam = orh.msg_to_se3(T_base_to_cam) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + print(e) + rospy.sleep(0.5) + else: + break + T_cam_to_grasp = best_grasp + T_base_to_grasp = np.matmul(T_base_to_cam, T_cam_to_grasp) + t_target_grasp = orh.se3_to_transform_stamped(T_base_to_grasp, "panda_link0", "grasp_pose") + self.br.sendTransform(t_target_grasp) + + self.publish_marker() + def get_target_grasp_pose(self, msg): + + rgb = rospy.wait_for_message("/azure1/rgb/image_raw", Image) + depth = rospy.wait_for_message("/azure1/depth_to_rgb/image_raw", Image) + rgb = self.bridge.imgmsg_to_cv2(rgb, desired_encoding='bgr8') + depth = self.bridge.imgmsg_to_cv2(depth, desired_encoding='32FC1') self.data["rgb"] = rgb self.data["depth"] = depth # send image to dnn client - rospy.loginfo_once("Sending rgb, depth, pcl to 6 dof graspnet client") + rospy.loginfo_once("Sending rgb, depth to Contact-GraspNet client") su.sendall_pickle(self.sock, self.data) - pred_grasps_cam = su.recvall_pickle(self.sock) + pred_grasps_cam, scores, contact_pts = su.recvall_pickle(self.sock) + if pred_grasps_cam is None: + return + for k in pred_grasps_cam.keys(): + if len(scores[k]) == 0: + return + best_grasp = pred_grasps_cam[k][np.argmax(scores[k])] + if k == 1: + print(k, pred_grasps_cam[k], scores[k], contact_pts[k]) + exit() + + # publish as tf + while True: + try: + T_base_to_cam = self.tf_buffer.lookup_transform("world", "azure1_rgb_camera_link", rospy.Time(), rospy.Duration(5.0)) + T_base_to_cam = orh.msg_to_se3(T_base_to_cam) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + print(e) + rospy.sleep(0.5) + else: + break + T_cam_to_grasp = best_grasp + T_base_to_grasp = np.matmul(T_base_to_cam, T_cam_to_grasp) + t_target_grasp = orh.se3_to_transform_stamped(T_base_to_grasp, "panda_link0", "target_grasp_pose") + self.br.sendTransform(t_target_grasp) + self.publish_marker("target_grasp_pose") + return t_target_grasp + + def publish_marker(self, frame_id): + + # Delete all existing markers + marker_array = MarkerArray() + marker = Marker() + marker.action = Marker.DELETEALL + marker_array.markers.append(marker) + self.marker_pub.publish(marker_array) + markers = [] + for k in panda_gripper_coords.keys(): + marker = Marker() + marker.header.frame_id = frame_id + marker.header.stamp = rospy.Time() + marker.type = marker.SPHERE + marker.action = marker.ADD + marker.ns = k + marker.pose.position.x = panda_gripper_coords[k][0] + marker.pose.position.y = panda_gripper_coords[k][1] + marker.pose.position.z = panda_gripper_coords[k][2] + marker.pose.orientation.x = 0.0 + marker.pose.orientation.y = 0.0 + marker.pose.orientation.z = 0.0 + marker.pose.orientation.w = 1.0 + marker.color.r = 1 + marker.color.g = 0 + marker.color.b = 0 + marker.color.a = 1.0 + marker.id = 0 + marker.type = Marker.SPHERE + marker.scale.x = 0.02 + marker.scale.y = 0.02 + marker.scale.z = 0.02 + markers.append(marker) + marker_array = MarkerArray() + marker_array.markers = markers + self.marker_pub.publish(marker_array) - if __name__ == '__main__': grasp_net = ContactGraspNet() diff --git a/srv/GetTarget6dofGrasp.srv b/srv/GetTarget6dofGrasp.srv new file mode 100644 index 0000000..c635b22 --- /dev/null +++ b/srv/GetTarget6dofGrasp.srv @@ -0,0 +1,2 @@ +--- +geometry_msgs/TransformStamped target_pose \ No newline at end of file diff --git a/srv/GetTargetPos.srv b/srv/GetTargetPos.srv new file mode 100644 index 0000000..301238e --- /dev/null +++ b/srv/GetTargetPos.srv @@ -0,0 +1,2 @@ +--- +geometry_msgs/Point position \ No newline at end of file