Skip to content

Commit 73116a2

Browse files
SgtVincentSgtVincent
committed
better grasp detection node memory management
1 parent b3aad67 commit 73116a2

File tree

5 files changed

+138
-5
lines changed

5 files changed

+138
-5
lines changed

‎grasp_detection/src/detectors/config.py‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@ def __init__(self):
2121
# self.max_gripper_width = 0.08 # franka hand
2222
self.max_gripper_width = 0.1 # robotiq 85
2323
# environment-specific parameters
24-
self.table_height = 1.02 # height of the table plane
24+
self.table_height = 0.82 # height of the table plane
2525

2626
# random seed
2727
self.random_seed = 42
@@ -49,6 +49,12 @@ def __init__(self):
4949
self.filter_bbox_3d_margin = 0.15 # in meter
5050
self.filter_table_plane = False # whether to filter table plane
5151

52+
# safety config:
53+
# bound size limit for the input point cloud
54+
self.bound_size_limit = np.array([0.6, 0.6, 0.6])
55+
# max number of points in the input point cloud
56+
self.max_point_num = 100000
57+
5258
self.debug = False # whether to visualize the grasps
5359

5460

‎grasp_detection/src/detectors/detector_anygrasp.py‎

Lines changed: 24 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,13 @@ def detect_callback(self, req: DetectGraspsRequest)->DetectGraspsResponse:
5959
# preprocess perception data: integrate depth images into TSDF volume and point cloud
6060
cloud, min_bound, max_bound = self._preprocess(req.perception_data)
6161

62+
# self.memory_tracker.print_diff()
63+
64+
# safety checking to avoid memory overflow from ill 2D perception
65+
if np.any((max_bound - min_bound) > self.config.bound_size_limit):
66+
rospy.logerr("Bound size too large! Rejecting request...")
67+
return DetectGraspsResponse(grasps=[])
68+
6269
# transform point cloud to the same coordinate frame as the model
6370
# the anygrasp model is trained with the x-axis pointing down
6471
#
@@ -74,12 +81,16 @@ def detect_callback(self, req: DetectGraspsRequest)->DetectGraspsResponse:
7481

7582

7683
# get prediction
84+
rospy.logdebug("Detecting grasps with input points of shape: {}, lims: {}".format(points.shape, lims))
7785
ret = self.model.get_grasp(points, colors, lims)
86+
# make program 50% slower
87+
torch.cuda.empty_cache()
88+
7889
if len(ret) == 2:
7990
gg, cloud = ret
8091
else:
8192
# if no valid grasp found, 3-tuple is returned for debugging (not used here)
82-
print('No Grasp detected by Anygrasp model!')
93+
rospy.logerr('No Grasp detected by Anygrasp model!')
8394
if self.debug:
8495
cloud = cloud.transform(trans_mat)
8596
# create world coordinate frame
@@ -201,7 +212,7 @@ def _preprocess(self, data: Perception)->Tuple[TSDFVolume, np.ndarray]:
201212

202213
tsdf.integrate(depth, intrinsics, extrinsics, rgb_img=rgb, mask_img=mask)
203214

204-
215+
# might get OOM here if the input point cloud is too large
205216
full_cloud: o3d.geometry.PointCloud = tsdf.get_cloud()
206217
cloud = full_cloud
207218

@@ -253,7 +264,17 @@ def _preprocess(self, data: Perception)->Tuple[TSDFVolume, np.ndarray]:
253264
# filter the cloud by the table plane height
254265
height = self.config.table_height
255266
cloud = cloud.select_by_index(np.where(np.asarray(cloud.points)[:,2] > height)[0])
256-
267+
268+
# if cloud is too large, downsample it
269+
if len(cloud.points) > self.config.max_point_num:
270+
num_points = len(cloud.points)
271+
every_k_points = int(num_points // self.config.max_point_num) + 1
272+
cloud = cloud.uniform_down_sample(every_k_points)
273+
rospy.logwarn(f"Anygrasp get more input points ({num_points}) than its limit ({self.config.max_point_num}). Downsample {len(cloud.points)} points")
274+
275+
del tsdf
276+
del full_cloud
277+
257278
return cloud, min_bound, max_bound
258279

259280

‎grasp_detection/src/detectors/detector_base.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ def __init__(self, service_name, config):
1414
self.cv_bridge = cv_bridge.CvBridge()
1515

1616
# initialize ROS service
17-
self.service = rospy.Service(self.service_name, DetectGrasps, self.detect_callback)
17+
self.service = rospy.Service(self.service_name, DetectGrasps, self.detect_callback, buff_size=1)
1818
rospy.loginfo(f'DetectGrasps service {self.service_name} listener is ready.')
1919

2020
def detect_callback(self, req: DetectGraspsRequest)->DetectGraspsResponse:

‎instruct_to_policy/scripts/bash/eval_pick_and_place_perception.sh‎

Lines changed: 102 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,102 @@
1+
# This bash file is used to evaluate code genearted under the ground truth grounding problem setting.
2+
3+
# ROS Installation workspace path
4+
CATKIN_WS="/home/junting/catkin_ws"
5+
CATKIN_CONDA_ENV="catkin_ws"
6+
7+
# activate catkin workspace
8+
cd $CATKIN_WS
9+
mamba activate $CATKIN_CONDA_ENV
10+
source devel/setup.bash
11+
12+
# Define script arguments
13+
robot_name="panda"
14+
package_root="$(rospack find instruct_to_policy)"
15+
benchmark_data_dir="$(rospack find instruct_to_policy)/data/benchmark"
16+
verbose=false
17+
use_gt_planning_scene=false
18+
19+
models_names=(
20+
"panda_toy_figure"
21+
"Nintendo_Mario_toy_figure"
22+
"wood_block"
23+
"grey_medication_bottle"
24+
"white_and_brown_box"
25+
"brown_medication_bottle"
26+
"blue_medication_bottle"
27+
"brown_ceramic_cup"
28+
"toy_bus"
29+
"dog_figure"
30+
)
31+
32+
# function to kill all descendant processes of a given pid
33+
kill_descendant_processes() {
34+
local pid="$1"
35+
local and_self="${2:-false}"
36+
if children="$(pgrep -P "$pid")"; then
37+
for child in $children; do
38+
kill_descendant_processes "$child" true
39+
done
40+
fi
41+
if [[ "$and_self" == true ]]; then
42+
kill -9 "$pid"
43+
fi
44+
}
45+
46+
# function to evaluate code in one benchmark world
47+
function eval_model_in_world() {
48+
local model_name=$1
49+
local robot_name=$2
50+
local verbose=$3
51+
52+
# launch_grasp_detection_node
53+
# the script is in the same directory as this script
54+
gnome-terminal -- bash -c "source $(rospack find instruct_to_policy)/scripts/bash/run_grasp_detection.sh; exec bash"
55+
56+
# eval all configs
57+
echo "-----------------Evaluating $model_name -------------------------------------"
58+
rosrun instruct_to_policy eval_model_pick_and_place.py \
59+
_config_to_eval:=perception_few_shot_gpt_4 \
60+
_model_name:=$model_name \
61+
_use_gt_planning_scene:=$use_gt_planning_scene
62+
echo "-----------------Evaluating $model_name End ....--------------------------------------------------------"
63+
64+
# kill all process in this function
65+
66+
67+
}
68+
69+
roscore &
70+
killall -9 python
71+
killall -9 rviz
72+
killall -9 gzserver
73+
74+
# roslaunch gazebo and moveit
75+
roslaunch instruct_to_policy run_${robot_name}_moveit_gazebo.launch \
76+
--wait \
77+
world:=$package_root/worlds/table_cabinet_plate.world \
78+
moveit_rviz:=true \
79+
gazebo_gui:=true \
80+
use_gt_planning_scene:=$use_gt_planning_scene \
81+
enable_moveit_sensor_plugins:=$(if $use_gt_planning_scene; then echo "false"; else echo "true"; fi) \
82+
publish_pointcloud:=$(if $use_gt_planning_scene; then echo "false"; else echo "true"; fi) \
83+
verbose:=$verbose &
84+
85+
# Wait for the Gazebo world to load
86+
sleep 15
87+
88+
# iterate through all the worlds under data/benchmark/worlds
89+
for model_name in "${models_names[@]}"
90+
do
91+
92+
sleep 5
93+
94+
# evaluate code of all configs in the world
95+
# also save all logs to a log file
96+
eval_model_in_world $model_name $robot_name $verbose 2>&1 | tee -a eval_pick_and_place_perception.log
97+
98+
done
99+
100+
# kill all descendant processes of this script
101+
kill_descendant_processes $$
102+

‎instruct_to_policy/scripts/bash/run_grasp_detection.sh‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,10 @@ cd $GRASP_DETECTION_WS
99
mamba activate $GRASP_DETECTION_CONDA_ENV
1010
source devel/setup.bash
1111

12+
rosnode kill /grasp_detection_node
13+
14+
sleep 3
15+
1216
# roslaunch grasp detection node in the background
1317
roslaunch grasp_detection run_node.launch \
1418
model_name:=$model_name \

0 commit comments

Comments
 (0)