ensenso/ros_driver

Can not turn of pointcloud

Closed this issue · 4 comments

I am using an ensenso n35 for deep learning and object detection and localization. I have found that the images produced without the filter are better suited for the object detection as opposed to the images that have the pattern projected on them. I am able to turn of the projector and grab an image. After detection I aim to use the point cloud to find the coordinates of the objects.
I altered the request data to turn the projector of every other picture. I thought that would be able to differentiate between the images with pattern and without by also turning of the point cloud and working with the timing of incoming messages. However the point cloud keeps publishing even when it is supposedly turned of. The request data script contains the following statement:
goal.request_point_cloud = rospy.get_param("~point_cloud", True)
I tried the flowing:

while not rospy.is_shutdown():
    if switch == 0:
        set_parameter.send_goal(SetParameterGoal(parameters=[Parameter(key=Parameter.PROJECTOR, bool_value=False)]))
        goal.request_point_cloud = rospy.get_param("~point_cloud", False)

        switch = 1
    elif switch == 1:
        set_parameter.send_goal(SetParameterGoal(parameters=[Parameter(key=Parameter.PROJECTOR, bool_value=True)]))
        goal.request_point_cloud = rospy.get_param("~point_cloud", True)
        switch = 0

To have the camera switch between publishing a point cloud and not. This however does not seem to work. What would be the proper way of turning the point cloud of and on?

As I have understood you, you want to capture alternating with and without patterns (images without patterns and point cloud with pattern).
Did you maybe leave out the other booleans in request_data and only request the goal.request_point_cloud? If so, then the issue is easily fixed.
In General: If no flag is specified (e.g. all flags are false), the point cloud is calculated by default. Usually you want to have the point cloud and giving the client an empty goal will result in calculating and publishing the point cloud.
All requested results of the action are also published by default if you do not specify anything for "publish_results" or "include_results_in_response".

One way the approach your goal would be to call the following lines for switch == 0: (Only capture an image without pattern).

goal.request_rectified_images = True

And for switch == 1: (capture the point cloud with pattern).

goal.request_point_cloud = True

And maybe you also want to set those booleans with other things than rospy.get_param, because it will only be called when initializing and does not change over runtime. In your case it only defines, whether the loop starts with an image, or a point cloud.

In your case you do not have to set any parameters, though. The projector pattern is set automatically on or off. If the point cloud is requested, the projector will be set on. If everything other than the point cloud (or normals, disparity map, depth image) is requested, the projector will be off and the front light (if available) will be set on.

So concluding, it should be:

while not rospy.is_shutdown():
        # get images capture without pattern
        goal.request_rectified_images = True
        goal.request_point_cloud = False  #only needed when you reuse the same goal
        ... send goal and wait for result ...
        ... do something with the result ...

        # get the point cloud capture with pattern
        goal.request_point_cloud = True
        goal.request_rectified_images = False  #only needed when you reuse the same goal
        ... send goal and wait for result ...
        ... do something with the result ...

Please tell me if I misunderstood you.
Yasin

Thank you for the response. It seems to work. I am however encoutering a different problem now. I am currently letting the camera adjust it's exposure. This however does not seem to be able to adjust fast enough. I recon this can only be solved by manually setting the exposure times?

Yes, that's true. In that case you have to set the parameters before requesting the point cloud or the rectified image. The best way is to define parameter set with a name beforehand (before going into the ros loop). The parameter_sets can be loaded within the request, given the set's name. Then you could define a parameter_set for requesting the point cloud like this:

set_parameter_client.send_goal(SetParameterGoal(parameter_set="name_for_set_with_pattern", parameters=[
        Parameter(key=Parameter.PROJECTOR, bool_value=True),
        Parameter(key=Parameter.EXPOSURE, float_value=12345) # Exchange that value
    ]))
set_parameter_client.wait_for_result()

and one parameter_set for requesting the image without the projector pattern like this:

set_parameter_client.send_goal(SetParameterGoal(parameter_set="name_for_set_withOUT_pattern" parameters=[
        Parameter(key=Parameter.PROJECTOR, bool_value=False),
        Parameter(key=Parameter.EXPOSURE, float_value=67890) # Exchange that value
    ]))
set_parameter_client.wait_for_result()

Then you have to adjust the exposure time yourself, of course.
In the loop however, you can request the data like before with sending the request goals
with the parameter_set you defined with a name:

while not rospy.is_shutdown():
        # get images capture without pattern
        goal.request_rectified_images = True
        goal.parameter_set = "name_for_set_withOUT_pattern"
        goal.request_point_cloud = False  #only needed when you reuse the same goal
        ... send goal and wait for result ...
        ... do something with the result ...

        # get the point cloud capture with pattern
        goal.request_point_cloud = True
        goal.parameter_set = "name_for_set_with_pattern"
        goal.request_rectified_images = False  #only needed when you reuse the same goal
        ... send goal and wait for result ...
        ... do something with the result ...

As a sidenote, tough. We are currently working on the feature that the exposure for different parameter_sets will be defined automatically (or with a little help). So this topic will have some improvements in the near future.

Your solution worked, thanks. I have now configured the script to readjust the exposure time once in every 25 images. This works just fine. I cannot find how i would close this issue but in my eyes it is resolved.