How to get bounding box coordinates from Gazebo with Python?

asked 2019-06-21 04:36:41 -0500

Mihai gravatar image

Hello! I am writing a Python script to detect when an object falls on the ground plane in the Gazebo simulator. Communication with Gazebo is done through ROS services.

My intention is to use the object bounding box for this: if an object is on the ground, then its bounding box should should have a corner on the ground (bounding_box.z = 0) or below it (for a bounding box with a fixed orientation wrt the object).

I can query for the object centroid using GetModelState (see code below). How can I get the positions of the corners of the object bounding box from Gazebo? Otherwise, is there a better way to detect when an object is supported by the ground plane?

System setup: ROS Kinectic Ubuntu 16.04

Sample code:

import sys
import roslib
import rospy
import rosservice
import geometry_msgs.msg
import gazebo_msgs.msg

import math
import tf

from std_msgs.msg import String
from gazebo_msgs.srv import GetModelState

import time
import os

...

try:
    model_state_getter = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState, persistent=True)
except rospy.ServiceException as e:
    print("Service call failed: %s"%e)
else:
    print("Service call succeeded")

...

model_name = 'object_1'
state_object = model_state_getter(model_name, 'world')
object_position = state_object.pose.position
print("Object Z position: {0}:".format(object_position.z))

Thank you in advance.

edit retag flag offensive close merge delete