Gazebo Object 가져오는 Topic코드

ROS kinetic, Ubuntu 16.04, Gazebo 7.0?

 

Gazebo 환경은 환경 내의 모든 물체들을 한번에 /gazebo/model_states라는 이름으로 담아 전달한다.

 

약 1000hz의 속도인데다, 쓸데없는 바닥 요소도 포함하고 있으므로 이 중에서 특정 물체들만을 골라 전달하도록 하는 코드를 작성했다.

 

#!/usr/bin/env python
import rospy
import rospkg
import roslib
from gazebo_msgs.msg import ModelStates
from std_msgs.msg import Float32MultiArray
from geometry_msgs.msg import Twist
 
 
def callback(msg):
    obj_pub_node = rospy.Publisher("object_info", Float32MultiArray, queue_size=100)
    rate = rospy.Rate(10)#10 hz
    #After Receive the Topic --> make new Topic which is customized
 
    get_obj_list = [1,3]#model_states[1], model_states[3] object topic
    global pos_x, pos_y, pos_z, ori_x, ori_y, ori_z, ori_w
    global linear_x, linear_y, linear_z, angular_x, angular_y, angular_z
 
    id = 100
    while not rospy.is_shutdown():
        obj = [0, 0, 0, 0, 0, 0, 0]
        for i in range(len(msg.pose)):
            if i in get_obj_list:
                obj_id = id
                #position
                pos_x = msg.pose[i].position.x
                pos_y = msg.pose[i].position.y
                pos_z = msg.pose[i].position.z
 
                #orientation
                ori_x = msg.pose[i].orientation.x
                ori_y = msg.pose[i].orientation.y
                ori_z = msg.pose[i].orientation.z
                ori_w = msg.pose[i].orientation.w
 
                #twist
                linear_x = msg.twist[i].linear.x
                linear_y = msg.twist[i].linear.y
                linear_z = msg.twist[i].linear.z
 
                angular_x = msg.twist[i].angular.x
                angular_y = msg.twist[i].angular.y
                angular_z = msg.twist[i].angular.z
 
 
                #Real Sending Topic define
 
                obj[0] = pos_x
                obj[1] = pos_y
                obj[2] = pos_z
                obj[3] = angular_x
                obj[4] = angular_y
                obj[5] = angular_z
                obj[6] = id
 
 
                #print(obj)
                publish_obj = Float32MultiArray(data=obj)
 
                obj_pub_node.publish(publish_obj)
                rate.sleep()
 
                id += 1
            else:
                continue
 
 
 
def subscriber():
 
    rospy.init_node('objectPerception', anonymous=True)
 
    rospy.Subscriber("/gazebo/model_states", ModelStates, callback)
 
    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()
 
 
if __name__ == '__main__':
 
    try:
        subscriber()
    except rospy.ROSInterruptException:
        pass
 

필요한 부분은 position에 해당되는 x,y,z좌표이며 orientation값 자체가 quaternion값으로 나오게 되는데 우리는 euler angle값이 필요하므로 

ModelStates() 형식 중 euler값 alpha, beta, gamma만을 담아 float32multiarray형태의 topic으로 감싸서 보내주도록 하였다.

gazebo/ModelStates Documentation

 

마지막으로 각 물체들마다 id값으로 구분해야 하기 때문에 어쩔수 없이 100, 101, 102같은 어설픈 id값을 줬다.

 

이걸 받아 Cm측에 전달하는 코드는 추후 추가.

 

BBOX 가져오는 코드는 이걸 쓰면 될듯?

GetBoundingBox() values not changing after scale in gazebo (using : visual_plugin) - Gazebo: Q&A Forum