getJointVelocityLimits(..) crashes while trying to stream a filtered trajectory

[Google Code Exporter]

> What steps will reproduce the problem?
1. Run a 'planning_scene_warehouse_viewer_ROBOTNAME_real.launch' with an actual 
robot using trajectory STREAMING nodes
2. Create a new trajectory using the dialogs and RVIZ
3. Press the 'filter trajectory' button after planning it
4. Press 'execute on robot'

> What is the expected output? What do you see instead?

Expected: motion_streaming_interface node should transmit the new trajectory to 
the robot, which should follow it.

Actual result: the motion_interface_streaming node crashes in 
param_utils::getJointVelocityLimits(..), line 116. It seems to be trying to 
iterate beyond the last element in model.joints_, resulting in a SEGFAULT.

A snippet from my console log, after adding some logging statements to 
getJointVelocityLimits(..):

-------------------------------------------------------------------

[ INFO] [1362933950.615050313, 1362933950.613281965] [/environment_server]: 
Successfully connected to planning scene action server for 
/trajectory_filter_server
[ INFO] [1362933988.950569726, 1362933988.941057920] 
[/motion_streaming_interface]: Receiving joint trajectory message
[ INFO] [1362933988.955415037, 1362933988.951634883] 
[/motion_streaming_interface]: Looking for: robot_description
[ INFO] [1362933988.955459177, 1362933988.951634883] 
[/motion_streaming_interface]: joints size: 6
[ INFO] [1362933988.955501222, 1362933988.951634883] 
[/motion_streaming_interface]: Looking at: joint_1
[ INFO] [1362933988.955532930, 1362933988.951634883] 
[/motion_streaming_interface]: Looking at: joint_2
[ INFO] [1362933988.955561775, 1362933988.951634883] 
[/motion_streaming_interface]: Looking at: joint_3
[ INFO] [1362933988.955589362, 1362933988.951634883] 
[/motion_streaming_interface]: Looking at: joint_4
[ INFO] [1362933988.955615622, 1362933988.951634883] 
[/motion_streaming_interface]: Looking at: joint_5
[ INFO] [1362933988.955643559, 1362933988.951634883] 
[/motion_streaming_interface]: Looking at: joint_5-tool0
motion_streaming_interface: /usr/include/boost/smart_ptr/shared_ptr.hpp:418: T* 
boost::shared_ptr<T>::operator->() const [with T = urdf::JointLimits]: 
Assertion `px != 0' failed.
[motion_streaming_interface-15] process has died [pid 5844, exit code -6, cmd 
/home/user/ros/industrial_fuerte_dev/fanuc/fanuc_common/bin/motion_streaming_int
erface __name:=motion_streaming_interface 
__log:=/home/user/.ros/log/f2b9e04a-89a1-11e2-bf77-0025b3c07e72/motion_streaming
_interface-15.log].
log file: 
/home/user/.ros/log/f2b9e04a-89a1-11e2-bf77-0025b3c07e72/motion_streaming_interf
ace-15*.log

-------------------------------------------------------------------

Original issue reported on code.google.com by colaed11 on 10 Mar 2013 at 5:04

[Google Code Exporter]

That last joint (joint_5-tool0) looks a little odd.  Is it fixed?  I don't 
think a fixed joint should be in the list.  This might cause problems with the 
look up into the joint velocity limits.  Of course the failure should be a 
little bit more graceful than a SEGFAULT.

Original comment by [e-mail] on 10 Mar 2013 at 8:17

[Google Code Exporter]

Right, I was wondering whether that (joint-5_tool0) could be the cause somehow. 
I'll look into it; has been a while since I wrote that URDF.

And I agree: shouldn't SEGFAULT in any case.

Original comment by colaed11 on 11 Mar 2013 at 7:32

[Google Code Exporter]

That is indeed the issue.

getJointVelocityLimits() scans through all joints defined in the URDF and 
pushes the velocity limits onto a vector for returning to the calling code.  
This routine is overly simplistic and doesn't handle gracefully the case where 
a joint is defined without any velocity limits.

I'm busy this morning, but can try to put together a quick patch later today.  
It should be a simple 1-line fix.

Thanks for the catch!

Original comment by [e-mail] on 11 Mar 2013 at 2:13

• Changed state: Accepted

[Google Code Exporter]

Fixed in r1203.

Added check to ensure that velocity limit exists (and is non-zero) before 
adding to list.

Original comment by [e-mail] on 11 Mar 2013 at 11:28

• Changed state: Fixed