Ask Your Question

Problem calling filter_trajectory service

asked 2012-08-09 03:35:49 -0500

mbj gravatar image

updated 2012-08-12 22:11:23 -0500


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.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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2012-08-13 14:18:45 -0500

bit-pirate gravatar image

Hi there!

I don't know, how the trajectory filter ensures collision-free motions, but I recommend the use of the planning_environment for collision checking.

Have a look at the tutorials, e.g. "Checking collisions for a given robot state". But be warned, as noted in the overview section of planning_environment, major changes have been made for Electric. I think, the tutorial is only valid for Diamondback.

You could have a look at this piece of code. It should be a working example of collision checking under Electric.

In both the tutorial and the linked code collision checking is done for the robot state, which is defined by the joint states you need to provide. I think that should work for your use case (check the robot state after each increment).

Now, if you are working on Fuerte, I think the Electric version should work (or only minor changes have to be applied). At least that is my personal experience so far regarding changes in arm_navigation for Fuerte.

edit flag offensive delete link more


Hi bit-pirate. Thank you very much for the answer. It has given me a different view on how to solve the problem. However, I haven't solved it yet.

mbj gravatar image mbj  ( 2012-08-14 02:46:27 -0500 )edit

Firstly, I tried to incorporate the code on the update() method of my controller, but I was obtaining always the same result: "[WARN] Incomplete robot state in setPlanningScene" & "[ERROR] Setting planning scene state to NULL".

mbj gravatar image mbj  ( 2012-08-14 02:48:08 -0500 )edit

Worried about the update frequency was troubling the call of the service, I designed a simple program that only call the service once. The result is the same. Any idea? Thanks again.

mbj gravatar image mbj  ( 2012-08-14 02:48:22 -0500 )edit

Which piece of code generates the warning and error above? Please make sure that states for all joints of your robot are published (check the "/joint_states" topic for example).

bit-pirate gravatar image bit-pirate  ( 2012-08-15 14:36:00 -0500 )edit
bit-pirate gravatar image bit-pirate  ( 2012-08-15 20:58:48 -0500 )edit

Hi, I had seen those threads yesterday, however, thanks for the answers. : ) I didn't get any solution so I opened a new thread dedicated to the problem:

mbj gravatar image mbj  ( 2012-08-16 01:25:41 -0500 )edit

Your Answer

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

Add Answer

Question Tools


Asked: 2012-08-09 03:35:49 -0500

Seen: 598 times

Last updated: Aug 13 '12