Ask Your Question
0

Hi, i wanto to meake a program that give the position of a red ball in 2D with rviz

asked 2017-10-22 22:19:00 -0500

ANDRO gravatar image

I already have a code in OpenCv for the detection and it works, i just dont know how to proceed with rviz. I see the camera in rviz by subcribing in the topic. But after that i dont know how to see it on GRID. Please does anyone can help me?

edit retag flag offensive close merge delete

Comments

You need to tell RVIZ the position of your camera. You can do it with simple markers. But I would recommend doing it by publishing tf transforms of your camera (you'll probably need that anyway in the future).

l4ncelot gravatar image l4ncelot  ( 2017-10-23 02:09:57 -0500 )edit

Have a look at tf_broadcaster tutorial.

l4ncelot gravatar image l4ncelot  ( 2017-10-23 02:10:13 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-10-23 07:44:53 -0500

RDaneelOlivaw gravatar image

So here you have very rudimentary solution. Of course you will need to add calibration procedures, and take extra thing into account like the blob area to get the distance value, but this I hope gives you a starting point.

So essentially you need to publish a Marker topic , with a Sphere Type of colour red. This marker will have to be referenced to your camera link or similar. I've done this Video as an example of how it could be done: Video

The core about solving this is creating a script that retrieves your Detection Data and Publishes a Marker with it. Here you have the script I've used to publish it:

#!/usr/bin/env python
import rospy
from cmvision.msg import Blobs, Blob
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
from sensor_msgs.msg import CameraInfo


class MarkerBasics(object):

    def __init__(self):
        self.marker_objectlisher = rospy.Publisher('/marker_redball', Marker, queue_size=1)
        self.rate = rospy.Rate(1)
        self.init_marker(index=0,z_val=0)

    def init_marker(self,index=0, z_val=0):
        self.marker_object = Marker()
        self.marker_object.header.frame_id = "/camera_link"
        self.marker_object.header.stamp    = rospy.get_rostime()
        self.marker_object.ns = "mira"
        self.marker_object.id = index
        self.marker_object.type = Marker.SPHERE
        self.marker_object.action = Marker.ADD

        my_point = Point()
        my_point.z = z_val
        self.marker_object.pose.position = my_point

        self.marker_object.pose.orientation.x = 0
        self.marker_object.pose.orientation.y = 0
        self.marker_object.pose.orientation.z = 0.0
        self.marker_object.pose.orientation.w = 1.0
        self.marker_object.scale.x = 0.05
        self.marker_object.scale.y = 0.05
        self.marker_object.scale.z = 0.05

        self.marker_object.color.r = 1.0
        self.marker_object.color.g = 0.0
        self.marker_object.color.b = 0.0
        # This has to be otherwise it will be transparent
        self.marker_object.color.a = 1.0

        # If we want it for ever, 0, otherwise seconds before desapearing
        self.marker_object.lifetime = rospy.Duration(0)

    def update_position(self,position):        
        self.marker_object.pose.position = position
        self.marker_objectlisher.publish(self.marker_object)


class BallDetector(object):
    def __init__(self):        
        self.rate = rospy.Rate(1)
        self.save_camera_values()        
        rospy.Subscriber('/blobs', Blobs, self.redball_detect_callback)        
        self.markerbasics_object = MarkerBasics()

    def save_camera_values(self):
        data_camera_info = None
        while data_camera_info is None:
            data_camera_info = rospy.wait_for_message('/mira/mira/camera1/camera_info', CameraInfo, timeout=5)
            rospy.loginfo("No Camera info found, trying again")


        self.cam_height_y = data_camera_info.height
        self.cam_width_x = data_camera_info.width
        rospy.loginfo("CAMERA INFO:: Image width=="+str(self.cam_width_x)+", Image Height=="+str(self.cam_height_y))

    def redball_detect_callback(self,data):

        if(len(data.blobs)):

            for obj in data.blobs:
                if obj.name == "RedBall":
                    rospy.loginfo("Blob <"+str(obj.name)+"> Detected!")
                    redball_point = Point()
                    # There is a diference in the axis from blobs and the camera link frame.
                    # We convert to percent of the screen
                    # TODO: Take into account the Depth distance and camera cone.
                    rospy.loginfo("self.cam_width_x="+str(self.cam_width_x))
                    rospy.loginfo("self.cam_width_x="+str(self.cam_height_y))
                    rospy.loginfo("obj.x="+str(obj.x))
                    rospy.loginfo("obj.y="+str(obj.y))

                    middle_width = float(self.cam_width_x)/2.0 
                    middle_height = float(self.cam_height_y)/2.0

                    redball_point.x = (obj.x - middle_width) / float(self.cam_width_x ...
(more)
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-10-22 22:19:00 -0500

Seen: 801 times

Last updated: Oct 23 '17