fixed object orientation

detection of several objects at the same time
do not include roscpp, this is only for debug