Liang-Ting Jiang's profile - activity

Hi,

Is there any working service server/client example code like the AddTwoInts tutorial for rospy and roscpp? I am able to run publisher/subscriber in rosjava, but struggling making a rosjava service server node works.

What I am trying is a simple service server which takes empty request and print something on the screen.

import com.google.common.base.Preconditions;
import org.ros.node.DefaultNodeFactory;
import org.ros.node.Node;
import org.ros.node.NodeConfiguration;
import org.ros.node.NodeMain;
import org.ros.node.service.ServiceServer;
import org.ros.internal.node.service.ServiceResponseBuilder;
import org.ros.service.std_srvs.Empty;

public class TestServiceServer implements NodeMain {

  private static final String SERVICE_NAME = "/test_service"; 
  private static final String SERVICE_TYPE = "std_srvs/Empty";
  private Node node;

  @Override
  public void main(NodeConfiguration configuration) {
    Preconditions.checkState(node == null);
    Preconditions.checkNotNull(configuration);
    try {
      System.out.println("Starting a Testing Service Node........");
      node = new DefaultNodeFactory().newNode("test_service_server", configuration);

      ServiceServer<Empty.Request, Empty.Response> server =
          node.newServiceServer(SERVICE_NAME, SERVICE_TYPE,
              new ServiceResponseBuilder<Empty.Request, Empty.Response>() {
                @Override
                public Empty.Response build(Empty.Request request) {
                  Empty.Response response = new Empty.Response();
                  System.out.println("called!!");
                  return response;
                }
              });
      //server.awaitRegistration();
    } catch (Exception e) {
      if (node != null) {
        node.getLog().fatal(e);
      } else {
        e.printStackTrace();
      }
    } 
  @Override
  public void shutdown() {
    node.shutdown();
    node = null;
  }

}

When I call the service:

rosservice call /test_service

These error shows up:

Traceback (most recent call last):
  File "/opt/ros/electric/ros/bin/rosservice", line 46, in <module>
    rosservice.rosservicemain()
  File "/opt/ros/electric/stacks/ros_comm/tools/rosservice/src/rosservice.py", line     731, in rosservicemain
    _rosservice_cmd_call(argv)
  File "/opt/ros/electric/stacks/ros_comm/tools/rosservice/src/rosservice.py", line  586, in _rosservice_cmd_call
    service_class = get_service_class_by_name(service_name)
  File "/opt/ros/electric/stacks/ros_comm/tools/rosservice/src/rosservice.py", line   357, in get_service_class_by_name
    service_type = get_service_type(service_name)
  File "/opt/ros/electric/stacks/ros_comm/tools/rosservice/src/rosservice.py", line 141, in get_service_type
    return get_service_headers(service_name, service_uri).get('type', None)
  File "/opt/ros/electric/stacks/ros_comm/tools/rosservice/src/rosservice.py", line  113, in get_service_headers
    return roslib.network.read_ros_handshake_header(s, cStringIO.StringIO(), 2048)
  File "/opt/ros/electric/ros/core/roslib/src/roslib/network.py", line 367, in   read_ros_handshake_header
    raise ROSHandshakeException("connection from sender terminated before handshake    header received. %s bytes were received. Please check sender for additional   details."%b.tell())
roslib.network.ROSHandshakeException: connection from sender terminated before    handshake header received. 0 bytes were received. Please check sender for additional  details.

Could someone point what's the problem? Thanks.

If you are using a kinect you could first try to find the sphere or bound the search space using the rgb image.

More generally, a feature set could be constructed and searched for. The features might include color, radius, and normal distribution.

One issue will be getting proper normals for such a small sphere. I think using normals from the kinect won't work from far away. The farther it is from a 2cm sphere, the fewer points will give you the normal you want (and need) to ID a sphere.

Try it from closer up, with fewer neighbors in your normal extraction step.

The radius of the sphere I am detecting is about 2cm (very small). I tried to modify the PCL tutorial which does cylinder model segmentation (http://www.pointclouds.org/documentation/tutorials/cylinder_segmentation.php#cylinder-segmentation). I have tried different sets of parameters. After I increase the MaxIteration to a large number (100000), I am able to detect larger ball (r = 15cm), but not a small ball (r = 2cm).

The pointclouds are from Kinect using openni_kinect driver.

This is the code I use:

pcl::ModelCoefficients coefficients;
pcl::PointIndices inliers;
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients (false);
seg.setModelType (pcl::SACMODEL_SPHERE); //detecting SPHERE
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.001);
seg.setRadiusLimits(0.001, 0.20);
seg.setMaxIterations(100000);
seg.setInputCloud (filteredCloud.makeShared ());
seg.segment (inliers, coefficients);
ROS_INFO_STREAM("# Inliers points: " << inliers.indices.size());

Is Normal Estimation Required for doing sphere fitting? Or is there any parameter I didn't setup well?

I am also wondering if there is a better method to detect a small ball in Pointclouds other than SAC_Segmentation since randomly picking up points in the clouds might not be ideal for finding such small objects, besides I have a specific ball to detect.

Any help would be greatly appreciated!

-LT