def filter( self, trajectory ):
# rospy.loginfo( 'filtering trajectory:\n %s'%trajectory )
request = FilterJointTrajectoryWithConstraintsRequest()
request.allowed_time = rospy.Duration.from_sec(5.0);
request.trajectory = trajectory
try:
response = self.proxy( request )
if response.error_code.val == response.error_code.SUCCESS:
return response.trajectory
else:
rospy.logerr( 'Trajectory was not filtered! (error: %s)'%response.error_code.val )
return None
except rospy.ServiceException, e:
rospy.logerr( 'service call failed: %s'%e)
return None
