Problem calling filter_trajectory service

edit

Hello.

I'm writing my own cartesian controller for my Powerball arm following this tutorials. After a few days working with it I get a correct control and now, my next step is get a correct movement without collisions (in a prefixed environment) and auto-collisions.

As you know, there is wizard in the arm_navigation package that allows to generate automatically the kinematics services and so on related with the arm movement. I executed it and generated the files with success. After a study of the generated files and the packages involved, I concluded that the correct way to avoid the collisions is the use of the "trajectory_filter_server" node. My idea is call this service (trajectory_filter_server/filter_trajectory_with_constraints) every time that I make a position increment, and obtain from this a answer containing if the movement is valid or not.

However, after several test, I haven't be able to make a correct call to the service. Anyone can tell me which are the necessary and essential parameters to correctly launch the service? I "only" filled the stamp, frame_id, trajectory.joint_names, trajectory.points, robot_state.start_state.name, robot_state.start_state.position and the group_name.

[Added 13/08/2012]

When debugging with GDB I observed that when I call the service it fails in the line 84 of the spline_smoother_utils.cpp ( spline_segment = spline.segments.back(); ).

  if(time >= segment_end_time)
  {
    ROS_DEBUG("Did not find spline segment corresponding to input time: %f",time);
    segment_time = segment_end_time - segment_start_time;
    spline_segment = spline.segments.back();
    return true;
  }

I think that all the necessaries services are launched because I get before the next output:

[INFO] [1344842134.839221386, 118.688000000]: Successfully connected to planning scene action server for /trajectory_filter_server

[INFO] [1344842134.846775320, 118.768000000]: waitForService: Service [/trajectory_filter_server/filter_trajectory_with_constraints] is now available.

[INFO] [1344842134.851041043, 118.799000000]: Sending Planning Scene....

[INFO] [1344842134.851441300, 118.813000000]: Reverting planning scene to default.

[INFO] [1344842134.856230667, 118.827000000]: Planning scene sent.

[INFO] [1344842134.859664450, 118.857000000]: Initialized

[INFO] [1344842134.859908300, 118.864000000]: Selecting planning group 0

[INFO] [1344842134.861566525, 118.864000000]: Planning group selected.

P.S: Do you think that my "idea" is correct or exist any node more appropriately for this purpose.